diff --git a/.cspell-partial.json b/.cspell-partial.json index e231eddd712ea..13849ef298019 100644 --- a/.cspell-partial.json +++ b/.cspell-partial.json @@ -5,5 +5,5 @@ "perception/bytetrack/lib/**" ], "ignoreRegExpList": [], - "words": ["dltype", "tvmgen", "quantizer", "imageio", "mimsave"] + "words": ["dltype", "tvmgen"] } diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index e298dccefd827..f7603af847d1f 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,17 +1,17 @@ -### Copied from .github/CODEOWNERS-manual ### - ### Automatically generated from package.xml ### -common/autoware_ad_api_specs/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp +common/autoware_ad_api_specs/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp common/autoware_auto_common/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_auto_geometry/** satoshi.ota@tier4.jp -common/autoware_auto_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp +common/autoware_auto_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp common/autoware_auto_tf2/** jit.ray.c@gmail.com satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +common/autoware_overlay_rviz_plugin/autoware_overlay_msgs/** team-spatzenhirn@uni-ulm.de +common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** khalil@leodrive.ai common/autoware_point_types/** taichi.higashide@tier4.jp common/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp -common/component_interface_specs/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp yukihiro.saito@tier4.jp -common/component_interface_tools/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp -common/component_interface_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp yukihiro.saito@tier4.jp +common/component_interface_specs/** isamu.takagi@tier4.jp yukihiro.saito@tier4.jp +common/component_interface_tools/** isamu.takagi@tier4.jp +common/component_interface_utils/** isamu.takagi@tier4.jp yukihiro.saito@tier4.jp common/cuda_utils/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp common/fake_test_node/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/geography_utils/** koji.minoda@tier4.jp @@ -19,31 +19,32 @@ 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/object_recognition_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yusuke.muramatsu@tier4.jp +common/mission_planner_rviz_plugin/** isamu.takagi@tier4.jp +common/motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +common/object_recognition_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yoshi.ri@tier4.jp common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp -common/path_distance_calculator/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp -common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yusuke.muramatsu@tier4.jp +common/path_distance_calculator/** isamu.takagi@tier4.jp +common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp common/polar_grid/** yukihiro.saito@tier4.jp common/qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp common/signal_processing/** ali.boyali@tier4.jp fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -common/tensorrt_common/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato.hirabayashi@tier4.jp +common/tensorrt_common/** dan.umeda@tier4.jp manato.hirabayashi@tier4.jp common/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp -common/tier4_api_utils/** isamu.takagi@tier4.jp 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_api_utils/** isamu.takagi@tier4.jp +common/tier4_automatic_goal_rviz_plugin/** dawid.moszynski@robotec.ai kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp +common/tier4_autoware_utils/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp common/tier4_calibration_rviz_plugin/** tomoya.kimura@tier4.jp +common/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_datetime_rviz_plugin/** isamu.takagi@tier4.jp common/tier4_debug_rviz_plugin/** takayuki.murooka@tier4.jp -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_localization_rviz_plugin/** isamu.takagi@tier4.jp takamasa.horibe@tier4.jp common/tier4_logging_level_configure_rviz_plugin/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp common/tier4_perception_rviz_plugin/** yukihiro.saito@tier4.jp -common/tier4_planning_rviz_plugin/** takayuki.murooka@tier4.jp yukihiro.saito@tier4.jp 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 @@ -53,19 +54,18 @@ common/tier4_traffic_light_rviz_plugin/** satoshi.ota@tier4.jp common/tier4_vehicle_rviz_plugin/** yukihiro.saito@tier4.jp common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp -common/traffic_light_utils/** mingyu.li@tier4.jp shunsuke.miura@tier4.jp -common/trtexec_vendor/** daisuke.nishimatsu@tier4.jp yusuke.muramatsu@tier4.jp +common/traffic_light_utils/** kotaro.uetake@tier4.jp satoshi.ota@tier4.jp shunsuke.miura@tier4.jp common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp -control/autonomous_emergency_braking/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp +control/autonomous_emergency_braking/** mamoru.sobue@tier4.jp 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/control_validator/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/external_cmd_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/joy_controller/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/lane_departure_checker/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp control/mpc_lateral_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/obstacle_collision_checker/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/operation_mode_transition_manager/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp -control/pid_longitudinal_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/pid_longitudinal_controller/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/predicted_path_checker/** berkay@leodrive.ai control/pure_pursuit/** takamasa.horibe@tier4.jp control/shift_decider/** takamasa.horibe@tier4.jp @@ -75,117 +75,130 @@ control/vehicle_cmd_gate/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp evaluator/diagnostic_converter/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp evaluator/localization_evaluator/** dominik.jargot@robotec.ai koji.minoda@tier4.jp +evaluator/perception_online_evaluator/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp evaluator/planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp -launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp +evaluator/tier4_metrics_rviz_plugin/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp +launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp launch/tier4_control_launch/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -launch/tier4_localization_launch/** 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_localization_launch/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +launch/tier4_map_launch/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp launch/tier4_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/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/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/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 -localization/yabloc/yabloc_common/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp -localization/yabloc/yabloc_image_processing/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp -localization/yabloc/yabloc_monitor/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp -localization/yabloc/yabloc_particle_filter/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp -localization/yabloc/yabloc_pose_initializer/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp -map/map_height_fitter/** isamu.takagi@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -map/map_loader/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp -map/map_projection_loader/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -map/map_tf_generator/** azumi.suzuki@tier4.jp -map/util/lanelet2_map_preprocessor/** ryohsuke.mitsudome@tier4.jp +localization/ekf_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp +localization/geo_pose_projector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/gyro_odometer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/landmark_based_localizer/ar_tag_based_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/landmark_based_localizer/landmark_manager/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/localization_error_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/ndt_scan_matcher/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/pose2twist/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/pose_estimator_arbiter/** kento.yabuuchi.2@tier4.jp +localization/pose_initializer/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/pose_instability_detector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/stop_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/tree_structured_parzen_estimator/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/twist2accel/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/yabloc/yabloc_common/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/yabloc/yabloc_image_processing/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/yabloc/yabloc_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/yabloc/yabloc_particle_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/yabloc/yabloc_pose_initializer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +map/map_height_fitter/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +map/map_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +map/map_projection_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +map/map_tf_generator/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +map/util/lanelet2_map_preprocessor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp perception/bytetrack/** manato.hirabayashi@tier4.jp perception/cluster_merger/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp perception/compare_map_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp yukihiro.saito@tier4.jp perception/crosswalk_traffic_light_estimator/** satoshi.ota@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/detected_object_feature_remover/** tomoya.kimura@tier4.jp -perception/detected_object_validation/** 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/detected_object_validation/** dai.nguyen@tier4.jp shintaro.tomie@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp +perception/detection_by_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/elevation_map_loader/** kosuke.takeuchi@tier4.jp shintaro.tomie@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 koji.minoda@tier4.jp kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@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/** kenzo.lobos@tier4.jp koji.minoda@tier4.jp perception/lidar_centerpoint_tvm/** carl.liu@autocore.ai xinyu.wang@tier4.jp -perception/map_based_prediction/** kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp yutaka.shimizu@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/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/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/object_merger/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/object_range_splitter/** yukihiro.saito@tier4.jp perception/object_velocity_splitter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp perception/occupancy_grid_map_outlier_filter/** abrahammonrroy@yahoo.com yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/probabilistic_occupancy_grid_map/** mamoru.sobue@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/radar_crossing_objects_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp -perception/radar_fusion_to_detected_object/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp -perception/radar_object_clustering/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp -perception/radar_object_tracker/** satoshi.tanaka@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp +perception/radar_crossing_objects_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp +perception/radar_fusion_to_detected_object/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp +perception/radar_object_clustering/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp +perception/radar_object_tracker/** satoshi.tanaka@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp perception/shape_estimation/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/simple_object_merger/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp -perception/tensorrt_classifier/** mingyu.li@tier4.jp -perception/tensorrt_yolo/** daisuke.nishimatsu@tier4.jp -perception/tensorrt_yolox/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato.hirabayashi@tier4.jp -perception/tracking_object_merger/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/tensorrt_classifier/** kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp +perception/tensorrt_yolox/** dan.umeda@tier4.jp manato.hirabayashi@tier4.jp +perception/tracking_object_merger/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/traffic_light_arbiter/** kenzo.lobos@tier4.jp shunsuke.miura@tier4.jp perception/traffic_light_classifier/** shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp -perception/traffic_light_fine_detector/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp -perception/traffic_light_map_based_detector/** yukihiro.saito@tier4.jp +perception/traffic_light_fine_detector/** shintaro.tomie@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp +perception/traffic_light_map_based_detector/** shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp perception/traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp -perception/traffic_light_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 shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_avoidance_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_dynamic_avoidance_module/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp +planning/behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_goal_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_path_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_planner_common/** daniel.sanchez@tier4.jp fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_sampling_planner_module/** daniel.sanchez@tier4.jp maxime.clement@tier4.jp +planning/behavior_path_side_shift_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_path_start_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_crosswalk_module/** kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_crosswalk_module/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yuki.takagi@tier4.jp planning/behavior_velocity_detection_area_module/** kyoichi.sugahara@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_dynamic_obstacle_stop_module/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp planning/behavior_velocity_intersection_module/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_no_drivable_lane_module/** ahmed.ebrahim@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp 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/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_stop_line_module/** 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 mamoru.sobue@tier4.jp ryohsuke.mitsudome@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp +planning/objects_of_interest_marker_interface/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp zulfaqar.azmi@tier4.jp planning/obstacle_avoidance_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -planning/obstacle_cruise_planner/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp -planning/obstacle_stop_planner/** berkay@leodrive.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp +planning/obstacle_cruise_planner/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp +planning/obstacle_stop_planner/** berkay@leodrive.ai bnk@leodrive.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp planning/path_smoother/** maxime.clement@tier4.jp takayuki.murooka@tier4.jp -planning/planning_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 satoshi.ota@tier4.jp taiki.tanaka@tier4.jp planning/rtc_replayer/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp planning/sampling_based_planner/bezier_sampler/** maxime.clement@tier4.jp planning/sampling_based_planner/frenet_planner/** maxime.clement@tier4.jp @@ -194,42 +207,45 @@ planning/sampling_based_planner/sampler_common/** maxime.clement@tier4.jp planning/scenario_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/static_centerline_optimizer/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp planning/surround_obstacle_checker/** satoshi.ota@tier4.jp -sensing/gnss_poser/** koji.minoda@tier4.jp yamato.ando@tier4.jp +sensing/gnss_poser/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp sensing/image_diagnostics/** dai.nguyen@tier4.jp sensing/image_transport_decompressor/** yukihiro.saito@tier4.jp sensing/imu_corrector/** koji.minoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp sensing/livox/livox_tag_filter/** ryohsuke.mitsudome@tier4.jp -sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp -sensing/radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp -sensing/radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp -sensing/radar_threshold_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp -sensing/radar_tracks_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp -sensing/tier4_pcl_extensions/** ryu.yamamoto@tier4.jp +sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp +sensing/radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp +sensing/radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp +sensing/radar_threshold_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp +sensing/radar_tracks_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp +sensing/tier4_pcl_extensions/** david.wong@tier4.jp kenzo.lobos@tier4.jp ryu.yamamoto@tier4.jp sensing/vehicle_velocity_converter/** ryu.yamamoto@tier4.jp simulator/dummy_perception_publisher/** yukihiro.saito@tier4.jp simulator/fault_injection/** keisuke.shima@tier4.jp simulator/simple_planning_simulator/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp system/autoware_auto_msgs_adapter/** isamu.takagi@tier4.jp mfc@leodrive.ai system/bluetooth_monitor/** fumihito.ito@tier4.jp -system/component_state_monitor/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp -system/default_ad_api/** 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_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/component_state_monitor/** isamu.takagi@tier4.jp +system/default_ad_api/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp +system/default_ad_api_helpers/ad_api_adaptors/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp +system/default_ad_api_helpers/ad_api_visualizers/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp +system/default_ad_api_helpers/automatic_pose_initializer/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp +system/diagnostic_graph_aggregator/** isamu.takagi@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/system_diagnostic_graph/** isamu.takagi@tier4.jp +system/duplicated_node_checker/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp uken.ryu@tier4.jp +system/emergency_handler/** makoto.kurihara@tier4.jp ryuta.kambe@tier4.jp tetsuhiro.kawaguchi@tier4.jp +system/hazard_status_converter/** isamu.takagi@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/mrm_handler/** makoto.kurihara@tier4.jp ryuta.kambe@tier4.jp tetsuhiro.kawaguchi@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 -vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/** taiki.tanaka@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp +vehicle/accel_brake_map_calibrator/** taiki.tanaka@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp vehicle/external_cmd_converter/** takamasa.horibe@tier4.jp vehicle/raw_vehicle_cmd_converter/** makoto.kurihara@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp vehicle/steer_offset_estimator/** taiki.tanaka@tier4.jp vehicle/vehicle_info_util/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp + +### Copied from .github/CODEOWNERS-manual ### diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index e95f4d46a147b..e4231a12a6add 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -24,7 +24,6 @@ jobs: rosdistro: - humble container-suffix: - - "" - -cuda include: - rosdistro: humble @@ -36,6 +35,9 @@ jobs: with: fetch-depth: 0 + - name: Check disk space before build + run: df -h + - name: Remove exec_depend uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 @@ -69,37 +71,5 @@ jobs: verbose: true flags: differential - clang-tidy-differential: - runs-on: [self-hosted, linux, X64] - container: ghcr.io/autowarefoundation/autoware-universe:humble-latest-cuda - needs: build-and-test-differential - steps: - - name: Check out repository - uses: actions/checkout@v3 - with: - fetch-depth: 0 - - - name: Remove exec_depend - uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 - - - name: Get modified packages - id: get-modified-packages - uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 - - - name: Get modified files - id: get-modified-files - uses: tj-actions/changed-files@v35 - with: - files: | - **/*.cpp - **/*.hpp - - - name: Run clang-tidy - if: ${{ steps.get-modified-files.outputs.all_changed_files != '' }} - uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 - with: - rosdistro: humble - target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} - target-files: ${{ steps.get-modified-files.outputs.all_changed_files }} - clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy - build-depends-repos: build_depends.repos + - name: Check disk space after build + run: df -h diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml index 7568c89908fd7..a6eea766cd80d 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.github/workflows/build-and-test.yaml @@ -9,7 +9,7 @@ on: jobs: build-and-test: if: ${{ github.event_name != 'push' || github.ref_name == github.event.repository.default_branch }} - runs-on: ubuntu-latest + runs-on: [self-hosted, linux, X64] container: ${{ matrix.container }}${{ matrix.container-suffix }} strategy: fail-fast: false @@ -27,14 +27,6 @@ jobs: - name: Check out repository uses: actions/checkout@v3 - - name: Free disk space (Ubuntu) - uses: jlumbroso/free-disk-space@v1.2.0 - with: - tool-cache: false - dotnet: false - swap-storage: false - large-packages: false - - name: Remove exec_depend uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 diff --git a/.github/workflows/cancel-previous-workflows.yaml b/.github/workflows/cancel-previous-workflows.yaml index f9c29b81b6e6c..1da4d24966de9 100644 --- a/.github/workflows/cancel-previous-workflows.yaml +++ b/.github/workflows/cancel-previous-workflows.yaml @@ -8,7 +8,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Cancel previous runs - uses: styfle/cancel-workflow-action@0.11.0 + uses: styfle/cancel-workflow-action@0.12.0 with: workflow_id: all all_but_latest: true diff --git a/.github/workflows/github-release.yaml b/.github/workflows/github-release.yaml index 95ebb8725f62b..b426d0cba6614 100644 --- a/.github/workflows/github-release.yaml +++ b/.github/workflows/github-release.yaml @@ -30,7 +30,7 @@ jobs: echo "tag-name=${REF_NAME#beta/}" >> $GITHUB_OUTPUT - name: Check out repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: fetch-depth: 0 ref: ${{ steps.set-tag-name.outputs.ref-name }} diff --git a/.github/workflows/json-schema-check.yaml b/.github/workflows/json-schema-check.yaml index 3d0c0b83d8746..77ce4576b4952 100644 --- a/.github/workflows/json-schema-check.yaml +++ b/.github/workflows/json-schema-check.yaml @@ -2,12 +2,26 @@ name: json-schema-check on: pull_request: - paths: - - "**/schema/*.schema.json" - - "**/config/*.param.yaml" + workflow_dispatch: jobs: + check-if-relevant-files-changed: + runs-on: ubuntu-latest + outputs: + run-check: ${{ steps.paths_filter.outputs.json_or_yaml }} + steps: + - uses: actions/checkout@v3 + - uses: dorny/paths-filter@v3 + id: paths_filter + with: + filters: | + json_or_yaml: + - '**/schema/*.schema.json' + - '**/config/*.param.yaml' + json-schema-check: + needs: check-if-relevant-files-changed + if: needs.check-if-relevant-files-changed.outputs.run-check == 'true' runs-on: ubuntu-latest steps: - name: Check out repository @@ -15,3 +29,11 @@ jobs: - name: Run json-schema-check uses: autowarefoundation/autoware-github-actions/json-schema-check@v1 + + no-relevant-changes: + needs: check-if-relevant-files-changed + if: needs.check-if-relevant-files-changed.outputs.run-check == 'false' + runs-on: ubuntu-latest + steps: + - name: Dummy step + run: echo "No relevant changes, passing check" diff --git a/.github/workflows/pre-commit-optional.yaml b/.github/workflows/pre-commit-optional.yaml index e754ecab24f85..38738196a0bd3 100644 --- a/.github/workflows/pre-commit-optional.yaml +++ b/.github/workflows/pre-commit-optional.yaml @@ -8,7 +8,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: fetch-depth: 0 diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml index b231dbda87938..33c00ee1066ae 100644 --- a/.github/workflows/pre-commit.yaml +++ b/.github/workflows/pre-commit.yaml @@ -16,7 +16,7 @@ jobs: private_key: ${{ secrets.PRIVATE_KEY }} - name: Check out repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: ref: ${{ github.event.pull_request.head.ref }} 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/.github/workflows/spell-check-differential.yaml b/.github/workflows/spell-check-differential.yaml deleted file mode 100644 index eb18ccdba38d0..0000000000000 --- a/.github/workflows/spell-check-differential.yaml +++ /dev/null @@ -1,16 +0,0 @@ -name: spell-check-differential - -on: - pull_request: - -jobs: - spell-check-differential: - runs-on: ubuntu-latest - steps: - - name: Check out repository - uses: actions/checkout@v3 - - - name: Run spell-check - uses: autowarefoundation/autoware-github-actions/spell-check@v1 - with: - cspell-json-url: https://raw.githubusercontent.com/tier4/autoware-spell-check-dict/main/.cspell.json diff --git a/README.md b/README.md index d429cc035df1d..23d0b172554fd 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,16 @@ -# autoware.universe +# Autoware Universe -For Autoware's general documentation, see [Autoware Documentation](https://autowarefoundation.github.io/autoware-documentation/). +## Welcome to Autoware Universe -For detailed documents of Autoware Universe components, see [Autoware Universe Documentation](https://autowarefoundation.github.io/autoware.universe/). +Autoware Universe serves as a foundational pillar within the Autoware ecosystem, playing a critical role in enhancing the core functionalities of autonomous driving technologies. +This repository is a pivotal element of the Autoware Core/Universe concept, managing a wide array of packages that significantly extend the capabilities of autonomous vehicles. ---- +![autoware_universe_front](docs/assets/images/autoware_universe_front.png) + +## Getting Started + +To dive into the vast world of Autoware and understand how Autoware Universe fits into the bigger picture, we recommend starting with the [Autoware Documentation](https://autowarefoundation.github.io/autoware-documentation/). This resource provides a thorough overview of the Autoware ecosystem, guiding you through its components, functionalities, and how to get started with development. + +### Explore Autoware Universe documentation + +For those looking to explore the specifics of Autoware Universe components, the [Autoware Universe Documentation](https://autowarefoundation.github.io/autoware.universe/), deployed with MKDocs, offers detailed insights. diff --git a/common/autoware_ad_api_specs/package.xml b/common/autoware_ad_api_specs/package.xml index d4d82faf4ee78..0d14ccff758b0 100644 --- a/common/autoware_ad_api_specs/package.xml +++ b/common/autoware_ad_api_specs/package.xml @@ -5,8 +5,6 @@ 0.0.0 The autoware_ad_api_specs package Takagi, Isamu - yabuta - Kah Hooi Tan Ryohsuke Mitsudome Apache License 2.0 diff --git a/common/autoware_auto_common/design/comparisons.md b/common/autoware_auto_common/design/comparisons.md index f3a7eb5e58429..3d00f77c1b0c3 100644 --- a/common/autoware_auto_common/design/comparisons.md +++ b/common/autoware_auto_common/design/comparisons.md @@ -22,8 +22,8 @@ The `exclusive_or` function will test whether two values cast to different boole ## Example Usage ```c++ -#include "common/bool_comparisons.hpp" -#include "common/float_comparisons.hpp" +#include "autoware_auto_common/common/bool_comparisons.hpp" +#include "autoware_auto_common/common/float_comparisons.hpp" #include diff --git a/common/autoware_auto_common/include/common/type_traits.hpp b/common/autoware_auto_common/include/autoware_auto_common/common/type_traits.hpp similarity index 96% rename from common/autoware_auto_common/include/common/type_traits.hpp rename to common/autoware_auto_common/include/autoware_auto_common/common/type_traits.hpp index 7087ed1e81181..66f382f081b33 100644 --- a/common/autoware_auto_common/include/common/type_traits.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/common/type_traits.hpp @@ -14,15 +14,15 @@ // // Developed by Apex.AI, Inc. -#include -#include +#include "autoware_auto_common/common/types.hpp" +#include "autoware_auto_common/common/visibility_control.hpp" #include #include #include -#ifndef COMMON__TYPE_TRAITS_HPP_ -#define COMMON__TYPE_TRAITS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__COMMON__TYPE_TRAITS_HPP_ +#define AUTOWARE_AUTO_COMMON__COMMON__TYPE_TRAITS_HPP_ namespace autoware { @@ -219,4 +219,4 @@ struct intersect } // namespace common } // namespace autoware -#endif // COMMON__TYPE_TRAITS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__COMMON__TYPE_TRAITS_HPP_ diff --git a/common/autoware_auto_common/include/autoware_auto_common/common/types.hpp b/common/autoware_auto_common/include/autoware_auto_common/common/types.hpp new file mode 100644 index 0000000000000..1c7dfe48c7ec8 --- /dev/null +++ b/common/autoware_auto_common/include/autoware_auto_common/common/types.hpp @@ -0,0 +1,125 @@ +// Copyright 2017-2020 the Autoware Foundation, Arm Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. +/// \file +/// \brief This file includes common type definition + +#ifndef AUTOWARE_AUTO_COMMON__COMMON__TYPES_HPP_ +#define AUTOWARE_AUTO_COMMON__COMMON__TYPES_HPP_ + +#include "autoware_auto_common/common/visibility_control.hpp" +#include "autoware_auto_common/helper_functions/float_comparisons.hpp" + +#include +#include +#include + +namespace autoware +{ +namespace common +{ +namespace types +{ +// Aliases to conform to MISRA C++ Rule 3-9-2 (Directive 4.6 in MISRA C). +// Similarly, the stdint typedefs should be used instead of plain int, long etc. types. +// We don't currently require code to comply to MISRA, but we should try to where it is +// easily possible. +using bool8_t = bool; +using char8_t = char; +using uchar8_t = unsigned char; +// If we ever compile on a platform where this is not true, float32_t and float64_t definitions +// need to be adjusted. +static_assert(sizeof(float) == 4, "float is assumed to be 32-bit"); +using float32_t = float; +static_assert(sizeof(double) == 8, "double is assumed to be 64-bit"); +using float64_t = double; + +/// pi = tau / 2 +constexpr float32_t PI = 3.14159265359F; +/// pi/2 +constexpr float32_t PI_2 = 1.5707963267948966F; +/// tau = 2 pi +constexpr float32_t TAU = 6.283185307179586476925286766559F; + +struct COMMON_PUBLIC PointXYZIF +{ + float32_t x{0}; + float32_t y{0}; + float32_t z{0}; + float32_t intensity{0}; + uint16_t id{0}; + static constexpr uint16_t END_OF_SCAN_ID = 65535u; + friend bool operator==(const PointXYZIF & p1, const PointXYZIF & p2) noexcept + { + using autoware::common::helper_functions::comparisons::rel_eq; + const auto epsilon = std::numeric_limits::epsilon(); + return rel_eq(p1.x, p2.x, epsilon) && rel_eq(p1.y, p2.y, epsilon) && + rel_eq(p1.z, p2.z, epsilon) && rel_eq(p1.intensity, p2.intensity, epsilon) && + (p1.id == p2.id); + } +}; + +struct COMMON_PUBLIC PointXYZF +{ + float32_t x{0}; + float32_t y{0}; + float32_t z{0}; + uint16_t id{0}; + static constexpr uint16_t END_OF_SCAN_ID = 65535u; + friend bool operator==(const PointXYZF & p1, const PointXYZF & p2) noexcept + { + using autoware::common::helper_functions::comparisons::rel_eq; + const auto epsilon = std::numeric_limits::epsilon(); + return rel_eq(p1.x, p2.x, epsilon) && rel_eq(p1.y, p2.y, epsilon) && + rel_eq(p1.z, p2.z, epsilon) && (p1.id == p2.id); + } +}; + +struct COMMON_PUBLIC PointXYZI +{ + float32_t x{0.0F}; + float32_t y{0.0F}; + float32_t z{0.0F}; + float32_t intensity{0.0F}; + friend bool operator==(const PointXYZI & p1, const PointXYZI & p2) noexcept + { + return helper_functions::comparisons::rel_eq( + p1.x, p2.x, std::numeric_limits::epsilon()) && + + helper_functions::comparisons::rel_eq( + p1.y, p2.y, std::numeric_limits::epsilon()) && + + helper_functions::comparisons::rel_eq( + p1.z, p2.z, std::numeric_limits::epsilon()) && + + helper_functions::comparisons::rel_eq( + p1.intensity, p2.intensity, std::numeric_limits::epsilon()); + } +}; + +using PointBlock = std::vector; +using PointPtrBlock = std::vector; +/// \brief Stores basic configuration information, does some simple validity checking +static constexpr uint16_t POINT_BLOCK_CAPACITY = 512U; + +// TODO(yunus.caliskan): switch to std::void_t when C++17 is available +/// \brief `std::void_t<> implementation +template +using void_t = void; +} // namespace types +} // namespace common +} // namespace autoware + +#endif // AUTOWARE_AUTO_COMMON__COMMON__TYPES_HPP_ diff --git a/common/autoware_auto_common/include/autoware_auto_common/common/visibility_control.hpp b/common/autoware_auto_common/include/autoware_auto_common/common/visibility_control.hpp new file mode 100644 index 0000000000000..33834fd9ccfed --- /dev/null +++ b/common/autoware_auto_common/include/autoware_auto_common/common/visibility_control.hpp @@ -0,0 +1,38 @@ +// Copyright 2017-2019 the Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#ifndef AUTOWARE_AUTO_COMMON__COMMON__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE_AUTO_COMMON__COMMON__VISIBILITY_CONTROL_HPP_ + +#if defined(_MSC_VER) && defined(_WIN64) +#if defined(COMMON_BUILDING_DLL) || defined(COMMON_EXPORTS) +#define COMMON_PUBLIC __declspec(dllexport) +#define COMMON_LOCAL +#else // defined(COMMON_BUILDING_DLL) || defined(COMMON_EXPORTS) +#define COMMON_PUBLIC __declspec(dllimport) +#define COMMON_LOCAL +#endif // defined(COMMON_BUILDING_DLL) || defined(COMMON_EXPORTS) +#elif defined(__GNUC__) && defined(__linux__) +#define COMMON_PUBLIC __attribute__((visibility("default"))) +#define COMMON_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__GNUC__) && defined(__APPLE__) +#define COMMON_PUBLIC __attribute__((visibility("default"))) +#define COMMON_LOCAL __attribute__((visibility("hidden"))) +#else // !(defined(__GNUC__) && defined(__APPLE__)) +#error "Unsupported Build Configuration" +#endif // _MSC_VER + +#endif // AUTOWARE_AUTO_COMMON__COMMON__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/angle_utils.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/angle_utils.hpp similarity index 89% rename from common/autoware_auto_common/include/helper_functions/angle_utils.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/angle_utils.hpp index 6cca2440d5680..ea974774dd9d5 100644 --- a/common/autoware_auto_common/include/helper_functions/angle_utils.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/angle_utils.hpp @@ -14,8 +14,8 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ -#define HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ #include #include @@ -63,4 +63,4 @@ constexpr T wrap_angle(T angle) noexcept } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/bool_comparisons.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/bool_comparisons.hpp similarity index 85% rename from common/autoware_auto_common/include/helper_functions/bool_comparisons.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/bool_comparisons.hpp index c6bf09365af4f..45da098ad0066 100644 --- a/common/autoware_auto_common/include/helper_functions/bool_comparisons.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/bool_comparisons.hpp @@ -18,10 +18,10 @@ // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. -#ifndef HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ -#define HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ -#include "common/types.hpp" +#include "autoware_auto_common/common/types.hpp" namespace autoware { @@ -47,4 +47,4 @@ types::bool8_t exclusive_or(const T & a, const T & b) } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/byte_reader.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/byte_reader.hpp similarity index 90% rename from common/autoware_auto_common/include/helper_functions/byte_reader.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/byte_reader.hpp index d9e55c4ecfbfe..3852361caebeb 100644 --- a/common/autoware_auto_common/include/helper_functions/byte_reader.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/byte_reader.hpp @@ -16,8 +16,8 @@ /// \file /// \brief This file includes common helper functions -#ifndef HELPER_FUNCTIONS__BYTE_READER_HPP_ -#define HELPER_FUNCTIONS__BYTE_READER_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BYTE_READER_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BYTE_READER_HPP_ #include #include @@ -70,4 +70,4 @@ class ByteReader } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__BYTE_READER_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BYTE_READER_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/crtp.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/crtp.hpp similarity index 88% rename from common/autoware_auto_common/include/helper_functions/crtp.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/crtp.hpp index 0e8110a9a3bb9..e75674eb73c70 100644 --- a/common/autoware_auto_common/include/helper_functions/crtp.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/crtp.hpp @@ -16,8 +16,8 @@ /// \file /// \brief This file includes common helper functions -#ifndef HELPER_FUNCTIONS__CRTP_HPP_ -#define HELPER_FUNCTIONS__CRTP_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__CRTP_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__CRTP_HPP_ namespace autoware { @@ -49,4 +49,4 @@ class crtp } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__CRTP_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__CRTP_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/float_comparisons.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/float_comparisons.hpp similarity index 95% rename from common/autoware_auto_common/include/helper_functions/float_comparisons.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/float_comparisons.hpp index de1f459f21d0a..1a64fe71a1720 100644 --- a/common/autoware_auto_common/include/helper_functions/float_comparisons.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/float_comparisons.hpp @@ -18,8 +18,8 @@ // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. -#ifndef HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ -#define HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ #include #include @@ -146,4 +146,4 @@ bool approx_eq(const T & a, const T & b, const T & abs_eps, const T & rel_eps) } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/mahalanobis_distance.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/mahalanobis_distance.hpp similarity index 92% rename from common/autoware_auto_common/include/helper_functions/mahalanobis_distance.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/mahalanobis_distance.hpp index fb9bdccf32b25..70c29eaf220d8 100644 --- a/common/autoware_auto_common/include/helper_functions/mahalanobis_distance.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/mahalanobis_distance.hpp @@ -14,8 +14,8 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ -#define HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ #include @@ -69,4 +69,4 @@ types::float32_t calculate_mahalanobis_distance( } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/message_adapters.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp similarity index 94% rename from common/autoware_auto_common/include/helper_functions/message_adapters.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp index 352ef7c7b65d5..d3bda57b3374f 100644 --- a/common/autoware_auto_common/include/helper_functions/message_adapters.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp @@ -16,8 +16,8 @@ /// \file /// \brief This file includes common helper functions -#ifndef HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ -#define HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ #include @@ -112,4 +112,4 @@ TimeStamp get_stamp(const T & msg) noexcept } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/template_utils.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/template_utils.hpp similarity index 91% rename from common/autoware_auto_common/include/helper_functions/template_utils.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/template_utils.hpp index b39931a3fd15a..0360908509618 100644 --- a/common/autoware_auto_common/include/helper_functions/template_utils.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/template_utils.hpp @@ -14,10 +14,10 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ -#define HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ -#include +#include "autoware_auto_common/common/types.hpp" #include @@ -72,4 +72,4 @@ struct expression_valid_with_return< } // namespace helper_functions } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/type_name.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/type_name.hpp similarity index 84% rename from common/autoware_auto_common/include/helper_functions/type_name.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/type_name.hpp index a95834373d552..106cede1f2f5a 100644 --- a/common/autoware_auto_common/include/helper_functions/type_name.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/type_name.hpp @@ -14,10 +14,10 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef HELPER_FUNCTIONS__TYPE_NAME_HPP_ -#define HELPER_FUNCTIONS__TYPE_NAME_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TYPE_NAME_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TYPE_NAME_HPP_ -#include +#include "autoware_auto_common/common/visibility_control.hpp" #include #include @@ -53,4 +53,4 @@ COMMON_PUBLIC std::string get_type_name(const T &) } // namespace helper_functions } // namespace autoware -#endif // HELPER_FUNCTIONS__TYPE_NAME_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TYPE_NAME_HPP_ diff --git a/common/autoware_auto_common/include/common/types.hpp b/common/autoware_auto_common/include/common/types.hpp deleted file mode 100644 index 3f3e444c1aa8c..0000000000000 --- a/common/autoware_auto_common/include/common/types.hpp +++ /dev/null @@ -1,125 +0,0 @@ -// Copyright 2017-2020 the Autoware Foundation, Arm Limited -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/// \file -/// \brief This file includes common type definition - -#ifndef COMMON__TYPES_HPP_ -#define COMMON__TYPES_HPP_ - -#include "common/visibility_control.hpp" -#include "helper_functions/float_comparisons.hpp" - -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace types -{ -// Aliases to conform to MISRA C++ Rule 3-9-2 (Directive 4.6 in MISRA C). -// Similarly, the stdint typedefs should be used instead of plain int, long etc. types. -// We don't currently require code to comply to MISRA, but we should try to where it is -// easily possible. -using bool8_t = bool; -using char8_t = char; -using uchar8_t = unsigned char; -// If we ever compile on a platform where this is not true, float32_t and float64_t definitions -// need to be adjusted. -static_assert(sizeof(float) == 4, "float is assumed to be 32-bit"); -using float32_t = float; -static_assert(sizeof(double) == 8, "double is assumed to be 64-bit"); -using float64_t = double; - -/// pi = tau / 2 -constexpr float32_t PI = 3.14159265359F; -/// pi/2 -constexpr float32_t PI_2 = 1.5707963267948966F; -/// tau = 2 pi -constexpr float32_t TAU = 6.283185307179586476925286766559F; - -struct COMMON_PUBLIC PointXYZIF -{ - float32_t x{0}; - float32_t y{0}; - float32_t z{0}; - float32_t intensity{0}; - uint16_t id{0}; - static constexpr uint16_t END_OF_SCAN_ID = 65535u; - friend bool operator==(const PointXYZIF & p1, const PointXYZIF & p2) noexcept - { - using autoware::common::helper_functions::comparisons::rel_eq; - const auto epsilon = std::numeric_limits::epsilon(); - return rel_eq(p1.x, p2.x, epsilon) && rel_eq(p1.y, p2.y, epsilon) && - rel_eq(p1.z, p2.z, epsilon) && rel_eq(p1.intensity, p2.intensity, epsilon) && - (p1.id == p2.id); - } -}; - -struct COMMON_PUBLIC PointXYZF -{ - float32_t x{0}; - float32_t y{0}; - float32_t z{0}; - uint16_t id{0}; - static constexpr uint16_t END_OF_SCAN_ID = 65535u; - friend bool operator==(const PointXYZF & p1, const PointXYZF & p2) noexcept - { - using autoware::common::helper_functions::comparisons::rel_eq; - const auto epsilon = std::numeric_limits::epsilon(); - return rel_eq(p1.x, p2.x, epsilon) && rel_eq(p1.y, p2.y, epsilon) && - rel_eq(p1.z, p2.z, epsilon) && (p1.id == p2.id); - } -}; - -struct COMMON_PUBLIC PointXYZI -{ - float32_t x{0.0F}; - float32_t y{0.0F}; - float32_t z{0.0F}; - float32_t intensity{0.0F}; - friend bool operator==(const PointXYZI & p1, const PointXYZI & p2) noexcept - { - return helper_functions::comparisons::rel_eq( - p1.x, p2.x, std::numeric_limits::epsilon()) && - - helper_functions::comparisons::rel_eq( - p1.y, p2.y, std::numeric_limits::epsilon()) && - - helper_functions::comparisons::rel_eq( - p1.z, p2.z, std::numeric_limits::epsilon()) && - - helper_functions::comparisons::rel_eq( - p1.intensity, p2.intensity, std::numeric_limits::epsilon()); - } -}; - -using PointBlock = std::vector; -using PointPtrBlock = std::vector; -/// \brief Stores basic configuration information, does some simple validity checking -static constexpr uint16_t POINT_BLOCK_CAPACITY = 512U; - -// TODO(yunus.caliskan): switch to std::void_t when C++17 is available -/// \brief `std::void_t<> implementation -template -using void_t = void; -} // namespace types -} // namespace common -} // namespace autoware - -#endif // COMMON__TYPES_HPP_ diff --git a/common/autoware_auto_common/include/common/visibility_control.hpp b/common/autoware_auto_common/include/common/visibility_control.hpp deleted file mode 100644 index 0a988d6407dfa..0000000000000 --- a/common/autoware_auto_common/include/common/visibility_control.hpp +++ /dev/null @@ -1,38 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef COMMON__VISIBILITY_CONTROL_HPP_ -#define COMMON__VISIBILITY_CONTROL_HPP_ - -#if defined(_MSC_VER) && defined(_WIN64) -#if defined(COMMON_BUILDING_DLL) || defined(COMMON_EXPORTS) -#define COMMON_PUBLIC __declspec(dllexport) -#define COMMON_LOCAL -#else // defined(COMMON_BUILDING_DLL) || defined(COMMON_EXPORTS) -#define COMMON_PUBLIC __declspec(dllimport) -#define COMMON_LOCAL -#endif // defined(COMMON_BUILDING_DLL) || defined(COMMON_EXPORTS) -#elif defined(__GNUC__) && defined(__linux__) -#define COMMON_PUBLIC __attribute__((visibility("default"))) -#define COMMON_LOCAL __attribute__((visibility("hidden"))) -#elif defined(__GNUC__) && defined(__APPLE__) -#define COMMON_PUBLIC __attribute__((visibility("default"))) -#define COMMON_LOCAL __attribute__((visibility("hidden"))) -#else // !(defined(__GNUC__) && defined(__APPLE__)) -#error "Unsupported Build Configuration" -#endif // _MSC_VER - -#endif // COMMON__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_auto_common/test/test_angle_utils.cpp b/common/autoware_auto_common/test/test_angle_utils.cpp index 031ae144139aa..810c302845daf 100644 --- a/common/autoware_auto_common/test/test_angle_utils.cpp +++ b/common/autoware_auto_common/test/test_angle_utils.cpp @@ -14,8 +14,8 @@ // // Developed by Apex.AI, Inc. -#include -#include +#include "autoware_auto_common/common/types.hpp" +#include "autoware_auto_common/helper_functions/angle_utils.hpp" #include diff --git a/common/autoware_auto_common/test/test_bool_comparisons.cpp b/common/autoware_auto_common/test/test_bool_comparisons.cpp index 67c1c8ddbf9aa..a84d65e569692 100644 --- a/common/autoware_auto_common/test/test_bool_comparisons.cpp +++ b/common/autoware_auto_common/test/test_bool_comparisons.cpp @@ -18,7 +18,7 @@ // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. -#include "helper_functions/bool_comparisons.hpp" +#include "autoware_auto_common/helper_functions/bool_comparisons.hpp" #include diff --git a/common/autoware_auto_common/test/test_byte_reader.cpp b/common/autoware_auto_common/test/test_byte_reader.cpp index c83d06c6e8132..a5ab8743f7ed4 100644 --- a/common/autoware_auto_common/test/test_byte_reader.cpp +++ b/common/autoware_auto_common/test/test_byte_reader.cpp @@ -14,9 +14,10 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include "common/types.hpp" -#include "gtest/gtest.h" -#include "helper_functions/byte_reader.hpp" +#include "autoware_auto_common/common/types.hpp" +#include "autoware_auto_common/helper_functions/byte_reader.hpp" + +#include #include diff --git a/common/autoware_auto_common/test/test_float_comparisons.cpp b/common/autoware_auto_common/test/test_float_comparisons.cpp index d292dc0a0cb20..37d3afdc177d5 100644 --- a/common/autoware_auto_common/test/test_float_comparisons.cpp +++ b/common/autoware_auto_common/test/test_float_comparisons.cpp @@ -18,7 +18,7 @@ // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. -#include "helper_functions/float_comparisons.hpp" +#include "autoware_auto_common/helper_functions/float_comparisons.hpp" #include diff --git a/common/autoware_auto_common/test/test_mahalanobis_distance.cpp b/common/autoware_auto_common/test/test_mahalanobis_distance.cpp index 262599180abb6..2015a85bc2bc8 100644 --- a/common/autoware_auto_common/test/test_mahalanobis_distance.cpp +++ b/common/autoware_auto_common/test/test_mahalanobis_distance.cpp @@ -13,8 +13,8 @@ // limitations under the License. // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include -#include +#include "autoware_auto_common/common/types.hpp" +#include "autoware_auto_common/helper_functions/mahalanobis_distance.hpp" #include diff --git a/common/autoware_auto_common/test/test_message_field_adapters.cpp b/common/autoware_auto_common/test/test_message_field_adapters.cpp index 34974c1cda9a6..c35f0ff826995 100644 --- a/common/autoware_auto_common/test/test_message_field_adapters.cpp +++ b/common/autoware_auto_common/test/test_message_field_adapters.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "autoware_auto_common/helper_functions/message_adapters.hpp" #include diff --git a/common/autoware_auto_common/test/test_template_utils.cpp b/common/autoware_auto_common/test/test_template_utils.cpp index 9c9ca9ae4b5f2..a670aaab83cfa 100644 --- a/common/autoware_auto_common/test/test_template_utils.cpp +++ b/common/autoware_auto_common/test/test_template_utils.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "autoware_auto_common/helper_functions/template_utils.hpp" #include diff --git a/common/autoware_auto_common/test/test_type_name.cpp b/common/autoware_auto_common/test/test_type_name.cpp index 1fb60d838f307..4ada59b4fb2e1 100644 --- a/common/autoware_auto_common/test/test_type_name.cpp +++ b/common/autoware_auto_common/test/test_type_name.cpp @@ -14,8 +14,8 @@ // // Developed by Apex.AI, Inc. -#include -#include +#include "autoware_auto_common/common/types.hpp" +#include "autoware_auto_common/helper_functions/type_name.hpp" #include diff --git a/common/autoware_auto_common/test/test_type_traits.cpp b/common/autoware_auto_common/test/test_type_traits.cpp index 7203ab8d649ee..92d01b3d84d51 100644 --- a/common/autoware_auto_common/test/test_type_traits.cpp +++ b/common/autoware_auto_common/test/test_type_traits.cpp @@ -14,8 +14,8 @@ // // Developed by Apex.AI, Inc. -#include -#include +#include "autoware_auto_common/common/type_traits.hpp" +#include "autoware_auto_common/common/types.hpp" #include diff --git a/common/autoware_auto_geometry/CMakeLists.txt b/common/autoware_auto_geometry/CMakeLists.txt index 454e0e7ef044f..ee819b7cd797c 100644 --- a/common/autoware_auto_geometry/CMakeLists.txt +++ b/common/autoware_auto_geometry/CMakeLists.txt @@ -5,9 +5,9 @@ find_package(autoware_cmake REQUIRED) autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED - include/geometry/spatial_hash.hpp - include/geometry/intersection.hpp - include/geometry/spatial_hash_config.hpp + include/autoware_auto_geometry/spatial_hash.hpp + include/autoware_auto_geometry/intersection.hpp + include/autoware_auto_geometry/spatial_hash_config.hpp src/spatial_hash.cpp src/bounding_box.cpp ) diff --git a/common/autoware_auto_geometry/design/interval.md b/common/autoware_auto_geometry/design/interval.md index 26260ba8d8e67..4fe65ff8e0310 100644 --- a/common/autoware_auto_geometry/design/interval.md +++ b/common/autoware_auto_geometry/design/interval.md @@ -39,7 +39,7 @@ See 'Example Usage' below. ## Example Usage ```c++ -#include "geometry/interval.hpp" +#include "autoware_auto_geometry/interval.hpp" #include diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box/bounding_box_common.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/bounding_box_common.hpp similarity index 95% rename from common/autoware_auto_geometry/include/geometry/bounding_box/bounding_box_common.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/bounding_box_common.hpp index d1dee63f73b56..0f3a68e14d442 100644 --- a/common/autoware_auto_geometry/include/geometry/bounding_box/bounding_box_common.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/bounding_box_common.hpp @@ -17,11 +17,11 @@ /// \file /// \brief Common functionality for bounding box computation algorithms -#ifndef GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ -#define GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ -#include -#include +#include "autoware_auto_geometry/common_2d.hpp" +#include "autoware_auto_geometry/visibility_control.hpp" #include #include @@ -185,4 +185,4 @@ std::vector GEOMETRY_PUBLIC get_transformed_corners } // namespace common } // namespace autoware -#endif // GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/eigenbox_2d.hpp similarity index 96% rename from common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/eigenbox_2d.hpp index f050628f32f25..e0f2e66e87ee5 100644 --- a/common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/eigenbox_2d.hpp @@ -19,10 +19,10 @@ /// bounding box // cspell: ignore eigenbox, EIGENBOX -#ifndef GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ -#define GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ -#include +#include "autoware_auto_geometry/bounding_box/bounding_box_common.hpp" #include #include @@ -244,4 +244,4 @@ BoundingBox eigenbox_2d(const IT begin, const IT end) } // namespace common } // namespace autoware -#endif // GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box/lfit.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/lfit.hpp similarity index 97% rename from common/autoware_auto_geometry/include/geometry/bounding_box/lfit.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/lfit.hpp index 9b8991a7c7132..07fd6c989cedc 100644 --- a/common/autoware_auto_geometry/include/geometry/bounding_box/lfit.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/lfit.hpp @@ -20,10 +20,10 @@ // cspell: ignore LFIT, lfit // LFIT means "L-Shape Fitting" -#ifndef GEOMETRY__BOUNDING_BOX__LFIT_HPP_ -#define GEOMETRY__BOUNDING_BOX__LFIT_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__LFIT_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__LFIT_HPP_ -#include +#include "autoware_auto_geometry/bounding_box/eigenbox_2d.hpp" #include #include @@ -278,4 +278,4 @@ BoundingBox lfit_bounding_box_2d(const IT begin, const IT end) } // namespace common } // namespace autoware -#endif // GEOMETRY__BOUNDING_BOX__LFIT_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__LFIT_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box/rotating_calipers.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/rotating_calipers.hpp similarity index 96% rename from common/autoware_auto_geometry/include/geometry/bounding_box/rotating_calipers.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/rotating_calipers.hpp index 5bc05810bb1b0..fb75384eb07cb 100644 --- a/common/autoware_auto_geometry/include/geometry/bounding_box/rotating_calipers.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/rotating_calipers.hpp @@ -17,11 +17,11 @@ /// \file /// \brief This file implements the rotating calipers algorithm for minimum oriented bounding boxes -#ifndef GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ -#define GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ -#include -#include -#include +#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ +#include "autoware_auto_geometry/bounding_box/bounding_box_common.hpp" +#include "autoware_auto_geometry/common_2d.hpp" +#include "autoware_auto_geometry/convex_hull.hpp" #include #include @@ -277,4 +277,4 @@ BoundingBox minimum_perimeter_bounding_box(std::list & list) } // namespace geometry } // namespace common } // namespace autoware -#endif // GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box_2d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box_2d.hpp new file mode 100644 index 0000000000000..c4c52928ac19a --- /dev/null +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box_2d.hpp @@ -0,0 +1,34 @@ +// Copyright 2017-2020 the Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. +/// \file +/// \brief Main header for user-facing bounding box algorithms: functions and types +#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX_2D_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX_2D_HPP_ + +#include "autoware_auto_geometry/bounding_box/eigenbox_2d.hpp" +#include "autoware_auto_geometry/bounding_box/lfit.hpp" +#include "autoware_auto_geometry/bounding_box/rotating_calipers.hpp" + +namespace autoware +{ +namespace common +{ +namespace geometry +{ +} // namespace geometry +} // namespace common +} // namespace autoware +#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/common_2d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp similarity index 98% rename from common/autoware_auto_geometry/include/geometry/common_2d.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp index fd045003521ea..e3c2e58c9f80e 100644 --- a/common/autoware_auto_geometry/include/geometry/common_2d.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp @@ -16,12 +16,12 @@ /// \file /// \brief This file includes common functionality for 2D geometry, such as dot products -#ifndef GEOMETRY__COMMON_2D_HPP_ -#define GEOMETRY__COMMON_2D_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__COMMON_2D_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__COMMON_2D_HPP_ -#include "geometry/interval.hpp" +#include "autoware_auto_geometry/interval.hpp" -#include +#include #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" @@ -584,4 +584,4 @@ bool is_point_inside_polygon_2d( } // namespace common } // namespace autoware -#endif // GEOMETRY__COMMON_2D_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__COMMON_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/common_3d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/common_3d.hpp similarity index 93% rename from common/autoware_auto_geometry/include/geometry/common_3d.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/common_3d.hpp index 4477b010e7eba..74cd210dec586 100644 --- a/common/autoware_auto_geometry/include/geometry/common_3d.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/common_3d.hpp @@ -16,10 +16,10 @@ /// \file /// \brief This file includes common functionality for 3D geometry, such as dot products -#ifndef GEOMETRY__COMMON_3D_HPP_ -#define GEOMETRY__COMMON_3D_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__COMMON_3D_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__COMMON_3D_HPP_ -#include +#include "autoware_auto_geometry/common_2d.hpp" namespace autoware { @@ -74,4 +74,4 @@ inline OUT distance_3d(const T1 & a, const T2 & b) } // namespace common } // namespace autoware -#endif // GEOMETRY__COMMON_3D_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__COMMON_3D_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/convex_hull.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp similarity index 97% rename from common/autoware_auto_geometry/include/geometry/convex_hull.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp index e690c4d07441b..ae81c55ad6b55 100644 --- a/common/autoware_auto_geometry/include/geometry/convex_hull.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp @@ -18,11 +18,12 @@ /// \brief This file implements the monotone chain algorithm to compute 2D convex hulls on linked /// lists of points -#ifndef GEOMETRY__CONVEX_HULL_HPP_ -#define GEOMETRY__CONVEX_HULL_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__CONVEX_HULL_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__CONVEX_HULL_HPP_ -#include -#include +#include "autoware_auto_geometry/common_2d.hpp" + +#include // lint -e537 NOLINT pclint vs cpplint #include @@ -191,4 +192,4 @@ typename std::list::const_iterator convex_hull(std::list & list) } // namespace common } // namespace autoware -#endif // GEOMETRY__CONVEX_HULL_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__CONVEX_HULL_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/hull_pockets.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp similarity index 94% rename from common/autoware_auto_geometry/include/geometry/hull_pockets.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp index 6e8caa0df1e80..cd9fd49f71635 100644 --- a/common/autoware_auto_geometry/include/geometry/hull_pockets.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp @@ -18,11 +18,12 @@ /// \brief This file implements an algorithm for getting a list of "pockets" in the convex /// hull of a non-convex simple polygon. -#ifndef GEOMETRY__HULL_POCKETS_HPP_ -#define GEOMETRY__HULL_POCKETS_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__HULL_POCKETS_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__HULL_POCKETS_HPP_ -#include -#include +#include "autoware_auto_geometry/common_2d.hpp" + +#include #include #include @@ -107,4 +108,4 @@ typename std::vector::value_typ } // namespace common } // namespace autoware -#endif // GEOMETRY__HULL_POCKETS_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__HULL_POCKETS_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/intersection.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/intersection.hpp similarity index 98% rename from common/autoware_auto_geometry/include/geometry/intersection.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/intersection.hpp index 87dc32b0190d0..08c70c3a5a6be 100644 --- a/common/autoware_auto_geometry/include/geometry/intersection.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/intersection.hpp @@ -14,11 +14,11 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef GEOMETRY__INTERSECTION_HPP_ -#define GEOMETRY__INTERSECTION_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__INTERSECTION_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__INTERSECTION_HPP_ -#include -#include +#include "autoware_auto_geometry/common_2d.hpp" +#include "autoware_auto_geometry/convex_hull.hpp" #include #include @@ -309,4 +309,4 @@ common::types::float32_t convex_intersection_over_union_2d( } // namespace common } // namespace autoware -#endif // GEOMETRY__INTERSECTION_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__INTERSECTION_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/interval.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp similarity index 97% rename from common/autoware_auto_geometry/include/geometry/interval.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp index 59c26f27cc454..54be2c32b1d05 100644 --- a/common/autoware_auto_geometry/include/geometry/interval.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp @@ -18,11 +18,11 @@ // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. -#ifndef GEOMETRY__INTERVAL_HPP_ -#define GEOMETRY__INTERVAL_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ -#include "common/types.hpp" -#include "helper_functions/float_comparisons.hpp" +#include +#include #include #include @@ -355,4 +355,4 @@ T Interval::clamp_to(const Interval & i, T val) } // namespace common } // namespace autoware -#endif // GEOMETRY__INTERVAL_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/lookup_table.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp similarity index 96% rename from common/autoware_auto_geometry/include/geometry/lookup_table.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp index e23a8c31b60f8..7b8867ca096ae 100644 --- a/common/autoware_auto_geometry/include/geometry/lookup_table.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp @@ -17,11 +17,12 @@ /// \file /// \brief This file contains a 1D linear lookup table implementation -#ifndef GEOMETRY__LOOKUP_TABLE_HPP_ -#define GEOMETRY__LOOKUP_TABLE_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__LOOKUP_TABLE_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__LOOKUP_TABLE_HPP_ -#include "common/types.hpp" -#include "geometry/interval.hpp" +#include "autoware_auto_geometry/interval.hpp" + +#include #include #include @@ -175,4 +176,4 @@ class LookupTable1D } // namespace common } // namespace autoware -#endif // GEOMETRY__LOOKUP_TABLE_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__LOOKUP_TABLE_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/spatial_hash.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp similarity index 97% rename from common/autoware_auto_geometry/include/geometry/spatial_hash.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp index 78626548e5c74..8936e2c17d779 100644 --- a/common/autoware_auto_geometry/include/geometry/spatial_hash.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp @@ -17,12 +17,13 @@ /// \brief This file implements a spatial hash for efficient fixed-radius near neighbor queries in /// 2D -#ifndef GEOMETRY__SPATIAL_HASH_HPP_ -#define GEOMETRY__SPATIAL_HASH_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_HPP_ -#include -#include -#include +#include "autoware_auto_geometry/spatial_hash_config.hpp" +#include "autoware_auto_geometry/visibility_control.hpp" + +#include #include #include @@ -328,4 +329,4 @@ using SpatialHash3d = SpatialHash; } // namespace common } // namespace autoware -#endif // GEOMETRY__SPATIAL_HASH_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/spatial_hash_config.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp similarity index 98% rename from common/autoware_auto_geometry/include/geometry/spatial_hash_config.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp index e118ec24c7759..24c4d6e879d4a 100644 --- a/common/autoware_auto_geometry/include/geometry/spatial_hash_config.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp @@ -17,14 +17,14 @@ /// \brief This file implements a spatial hash for efficient fixed-radius near neighbor queries in /// 2D -#ifndef GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ -#define GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ -#include "helper_functions/crtp.hpp" +#include "autoware_auto_geometry/common_2d.hpp" +#include "autoware_auto_geometry/visibility_control.hpp" -#include -#include -#include +#include +#include #include #include @@ -447,4 +447,4 @@ class GEOMETRY_PUBLIC Config3d : public Config } // namespace common } // namespace autoware -#endif // GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/visibility_control.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/visibility_control.hpp new file mode 100644 index 0000000000000..8972246997997 --- /dev/null +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/visibility_control.hpp @@ -0,0 +1,41 @@ +// Copyright 2017-2019 the Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#ifndef AUTOWARE_AUTO_GEOMETRY__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__VISIBILITY_CONTROL_HPP_ + +//////////////////////////////////////////////////////////////////////////////// +#if defined(__WIN32) +#if defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS) +#define GEOMETRY_PUBLIC __declspec(dllexport) +#define GEOMETRY_LOCAL +#else // defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS) +#define GEOMETRY_PUBLIC __declspec(dllimport) +#define GEOMETRY_LOCAL +#endif // defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS) +#elif defined(__linux__) +#define GEOMETRY_PUBLIC __attribute__((visibility("default"))) +#define GEOMETRY_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) +#define GEOMETRY_PUBLIC __attribute__((visibility("default"))) +#define GEOMETRY_LOCAL __attribute__((visibility("hidden"))) +#elif defined(QNX) +#define GEOMETRY_PUBLIC __attribute__((visibility("default"))) +#define GEOMETRY_LOCAL __attribute__((visibility("hidden"))) +#else // defined(__linux__) +#error "Unsupported Build Configuration" +#endif // defined(__WIN32) +#endif // AUTOWARE_AUTO_GEOMETRY__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box_2d.hpp b/common/autoware_auto_geometry/include/geometry/bounding_box_2d.hpp deleted file mode 100644 index d1d84122889c9..0000000000000 --- a/common/autoware_auto_geometry/include/geometry/bounding_box_2d.hpp +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright 2017-2020 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/// \file -/// \brief Main header for user-facing bounding box algorithms: functions and types -#ifndef GEOMETRY__BOUNDING_BOX_2D_HPP_ -#define GEOMETRY__BOUNDING_BOX_2D_HPP_ - -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -} // namespace geometry -} // namespace common -} // namespace autoware -#endif // GEOMETRY__BOUNDING_BOX_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/visibility_control.hpp b/common/autoware_auto_geometry/include/geometry/visibility_control.hpp deleted file mode 100644 index 96efe9aa6e27b..0000000000000 --- a/common/autoware_auto_geometry/include/geometry/visibility_control.hpp +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef GEOMETRY__VISIBILITY_CONTROL_HPP_ -#define GEOMETRY__VISIBILITY_CONTROL_HPP_ - -//////////////////////////////////////////////////////////////////////////////// -#if defined(__WIN32) -#if defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS) -#define GEOMETRY_PUBLIC __declspec(dllexport) -#define GEOMETRY_LOCAL -#else // defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS) -#define GEOMETRY_PUBLIC __declspec(dllimport) -#define GEOMETRY_LOCAL -#endif // defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS) -#elif defined(__linux__) -#define GEOMETRY_PUBLIC __attribute__((visibility("default"))) -#define GEOMETRY_LOCAL __attribute__((visibility("hidden"))) -#elif defined(__APPLE__) -#define GEOMETRY_PUBLIC __attribute__((visibility("default"))) -#define GEOMETRY_LOCAL __attribute__((visibility("hidden"))) -#elif defined(QNX) -#define GEOMETRY_PUBLIC __attribute__((visibility("default"))) -#define GEOMETRY_LOCAL __attribute__((visibility("hidden"))) -#else // defined(__linux__) -#error "Unsupported Build Configuration" -#endif // defined(__WIN32) -#endif // GEOMETRY__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_auto_geometry/src/bounding_box.cpp b/common/autoware_auto_geometry/src/bounding_box.cpp index 225ea099e4e79..3a4ea96a151a2 100644 --- a/common/autoware_auto_geometry/src/bounding_box.cpp +++ b/common/autoware_auto_geometry/src/bounding_box.cpp @@ -14,13 +14,14 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. +#include "autoware_auto_geometry/bounding_box/bounding_box_common.hpp" +#include "autoware_auto_geometry/bounding_box/eigenbox_2d.hpp" + #include -#include -#include // cspell: ignore eigenbox -#include +#include "autoware_auto_geometry/bounding_box/lfit.hpp" // cspell: ignore lfit -#include +#include "autoware_auto_geometry/bounding_box/rotating_calipers.hpp" #include diff --git a/common/autoware_auto_geometry/src/spatial_hash.cpp b/common/autoware_auto_geometry/src/spatial_hash.cpp index 024cca48b8ee7..ffd91aaa08b3a 100644 --- a/common/autoware_auto_geometry/src/spatial_hash.cpp +++ b/common/autoware_auto_geometry/src/spatial_hash.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "autoware_auto_geometry/spatial_hash.hpp" #include // lint -e537 NOLINT repeated include file due to cpplint rule diff --git a/common/autoware_auto_geometry/test/include/test_bounding_box.hpp b/common/autoware_auto_geometry/test/include/test_bounding_box.hpp index 5e42622b19ce9..a179fbde5f151 100644 --- a/common/autoware_auto_geometry/test/include/test_bounding_box.hpp +++ b/common/autoware_auto_geometry/test/include/test_bounding_box.hpp @@ -17,9 +17,9 @@ #ifndef TEST_BOUNDING_BOX_HPP_ #define TEST_BOUNDING_BOX_HPP_ -#include "geometry/bounding_box/lfit.hpp" +#include "autoware_auto_geometry/bounding_box/lfit.hpp" // cspell: ignore lfit -#include "geometry/bounding_box/rotating_calipers.hpp" +#include "autoware_auto_geometry/bounding_box/rotating_calipers.hpp" #include diff --git a/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp b/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp index 50e946c416270..fc2d97c257b95 100644 --- a/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp +++ b/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp @@ -17,7 +17,7 @@ #ifndef TEST_SPATIAL_HASH_HPP_ #define TEST_SPATIAL_HASH_HPP_ -#include "geometry/spatial_hash.hpp" +#include "autoware_auto_geometry/spatial_hash.hpp" #include diff --git a/common/autoware_auto_geometry/test/src/lookup_table.cpp b/common/autoware_auto_geometry/test/src/lookup_table.cpp index e7533518d7f49..81865656c55b5 100644 --- a/common/autoware_auto_geometry/test/src/lookup_table.cpp +++ b/common/autoware_auto_geometry/test/src/lookup_table.cpp @@ -14,8 +14,9 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include -#include +#include "autoware_auto_geometry/lookup_table.hpp" + +#include #include diff --git a/common/autoware_auto_geometry/test/src/test_area.cpp b/common/autoware_auto_geometry/test/src/test_area.cpp index d722314dada83..bc9c28682ed44 100644 --- a/common/autoware_auto_geometry/test/src/test_area.cpp +++ b/common/autoware_auto_geometry/test/src/test_area.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "autoware_auto_geometry/common_2d.hpp" #include diff --git a/common/autoware_auto_geometry/test/src/test_common_2d.cpp b/common/autoware_auto_geometry/test/src/test_common_2d.cpp index 642e396bdce31..baf6967edd47e 100644 --- a/common/autoware_auto_geometry/test/src/test_common_2d.cpp +++ b/common/autoware_auto_geometry/test/src/test_common_2d.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "autoware_auto_geometry/common_2d.hpp" #include #include diff --git a/common/autoware_auto_geometry/test/src/test_convex_hull.cpp b/common/autoware_auto_geometry/test/src/test_convex_hull.cpp index 43e3a3ad08adf..8b9bbce36c428 100644 --- a/common/autoware_auto_geometry/test/src/test_convex_hull.cpp +++ b/common/autoware_auto_geometry/test/src/test_convex_hull.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include "geometry/convex_hull.hpp" +#include "autoware_auto_geometry/convex_hull.hpp" #include diff --git a/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp b/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp index 2b79d4ff0f22b..9477a83a488ed 100644 --- a/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp +++ b/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp @@ -14,8 +14,8 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include "geometry/convex_hull.hpp" -#include "geometry/hull_pockets.hpp" +#include "autoware_auto_geometry/convex_hull.hpp" +#include "autoware_auto_geometry/hull_pockets.hpp" #include diff --git a/common/autoware_auto_geometry/test/src/test_intersection.cpp b/common/autoware_auto_geometry/test/src/test_intersection.cpp index 69c54960d4fc5..1036c69573c49 100644 --- a/common/autoware_auto_geometry/test/src/test_intersection.cpp +++ b/common/autoware_auto_geometry/test/src/test_intersection.cpp @@ -14,8 +14,8 @@ // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include -#include +#include "autoware_auto_geometry/convex_hull.hpp" +#include "autoware_auto_geometry/intersection.hpp" #include diff --git a/common/autoware_auto_geometry/test/src/test_interval.cpp b/common/autoware_auto_geometry/test/src/test_interval.cpp index ba8d5742dadc5..266ab123f5203 100644 --- a/common/autoware_auto_geometry/test/src/test_interval.cpp +++ b/common/autoware_auto_geometry/test/src/test_interval.cpp @@ -18,7 +18,7 @@ // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. -#include "geometry/interval.hpp" +#include "autoware_auto_geometry/interval.hpp" #include diff --git a/common/autoware_auto_perception_rviz_plugin/CMakeLists.txt b/common/autoware_auto_perception_rviz_plugin/CMakeLists.txt index dd5f88d4caae8..8d0469e78c3ac 100644 --- a/common/autoware_auto_perception_rviz_plugin/CMakeLists.txt +++ b/common/autoware_auto_perception_rviz_plugin/CMakeLists.txt @@ -13,18 +13,18 @@ set(OD_PLUGIN_LIB_SRC ) set(OD_PLUGIN_LIB_HEADERS - include/visibility_control.hpp + include/autoware_auto_perception_rviz_plugin/visibility_control.hpp ) set(OD_PLUGIN_LIB_HEADERS_TO_WRAP - include/object_detection/detected_objects_display.hpp - include/object_detection/tracked_objects_display.hpp - include/object_detection/predicted_objects_display.hpp + include/autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp + include/autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp + include/autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp ) set(COMMON_HEADERS - include/common/color_alpha_property.hpp - include/object_detection/object_polygon_detail.hpp - include/object_detection/object_polygon_display_base.hpp + include/autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp + include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp + include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp ) set(COMMON_SRC diff --git a/common/autoware_auto_perception_rviz_plugin/include/common/color_alpha_property.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp similarity index 84% rename from common/autoware_auto_perception_rviz_plugin/include/common/color_alpha_property.hpp rename to common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp index 8c43192a26bd6..10dc46e55ec70 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/common/color_alpha_property.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp @@ -11,13 +11,14 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef COMMON__COLOR_ALPHA_PROPERTY_HPP_ -#define COMMON__COLOR_ALPHA_PROPERTY_HPP_ +#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_ +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_ + +#include "autoware_auto_perception_rviz_plugin/visibility_control.hpp" #include #include #include -#include #include @@ -55,4 +56,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ColorAlphaProperty } // namespace rviz_plugins } // namespace autoware -#endif // COMMON__COLOR_ALPHA_PROPERTY_HPP_ +#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp similarity index 76% rename from common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp rename to common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp index 67dac25c796bb..97479fb68ca9b 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp @@ -11,10 +11,10 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ -#define OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ +#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ -#include +#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" #include @@ -43,4 +43,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay } // namespace rviz_plugins } // namespace autoware -#endif // OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ +#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp new file mode 100644 index 0000000000000..8c6ade475217a --- /dev/null +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp @@ -0,0 +1,296 @@ +// Copyright 2021 Apex.AI, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/// \brief This file defines some helper functions used by ObjectPolygonDisplayBase class +#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ + +#include "autoware_auto_perception_rviz_plugin/visibility_control.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace autoware +{ +namespace rviz_plugins +{ +namespace object_detection +{ +namespace detail +{ +// Struct to define all the configurable visual properties of an object of a particular +// classification type +struct ObjectPropertyValues +{ + // Classified type of the object + std::string label; + // Color for the type of the object + std::array color; + // Alpha values for the type of the object + float alpha{0.999F}; +}; + +// Map defining colors according to value of label field in ObjectClassification msg +const std::map< + autoware_auto_perception_msgs::msg::ObjectClassification::_label_type, ObjectPropertyValues> + // Color map is based on cityscapes color + kDefaultObjectPropertyValues = { + {autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN, + {"UNKNOWN", {255, 255, 255}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::CAR, {"CAR", {30, 144, 255}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::BUS, {"BUS", {30, 144, 255}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN, + {"PEDESTRIAN", {255, 192, 203}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, {"CYCLIST", {119, 11, 32}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, + {"MOTORCYCLE", {119, 11, 32}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER, + {"TRAILER", {30, 144, 255}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, {"TRUCK", {30, 144, 255}}}}; + +/// \brief Convert the given polygon into a marker representing the shape in 3d +/// \param shape_msg Shape msg to be converted. Corners should be in object-local frame +/// \param centroid Centroid position of the shape in Object.header.frame_id frame +/// \param orientation Orientation of the shape in Object.header.frame_id frame +/// \param color_rgba Color and alpha values to use for the marker +/// \param line_width Line thickness around the object +/// \return Marker ptr. Id and header will have to be set by the caller +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_shape_marker_ptr( + const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, + const bool & is_orientation_available = true); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_2d_shape_marker_ptr( + const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, + const bool & is_orientation_available = true); + +/// \brief Convert the given polygon into a marker representing the shape in 3d +/// \param centroid Centroid position of the shape in Object.header.frame_id frame +/// \return Marker ptr. Id and header will have to be set by the caller +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_label_marker_ptr( + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const std::string label, const std_msgs::msg::ColorRGBA & color_rgba); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_existence_probability_marker_ptr( + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const float existence_probability, const std_msgs::msg::ColorRGBA & color_rgba); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_uuid_marker_ptr( + const std::string & uuid, const geometry_msgs::msg::Point & centroid, + const std_msgs::msg::ColorRGBA & color_rgba); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_pose_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const double & confidence_interval_coefficient); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_yaw_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & length, + const double & confidence_interval_coefficient, const double & line_width); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_velocity_text_marker_ptr( + const geometry_msgs::msg::Twist & twist, const geometry_msgs::msg::Point & vis_pos, + const std_msgs::msg::ColorRGBA & color_rgba); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_acceleration_text_marker_ptr( + const geometry_msgs::msg::Accel & accel, const geometry_msgs::msg::Point & vis_pos, + const std_msgs::msg::ColorRGBA & color_rgba); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_twist_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_twist_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & confidence_interval_coefficient); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_yaw_rate_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_yaw_rate_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & confidence_interval_coefficient, const double & line_width); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_predicted_path_marker_ptr( + const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const std_msgs::msg::ColorRGBA & predicted_path_color, const bool is_simple = false); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_path_confidence_marker_ptr( + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const std_msgs::msg::ColorRGBA & path_confidence_color); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_arc_line_strip( + const double start_angle, const double end_angle, const double radius, + std::vector & points); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_line_list_from_points( + const double point_list[][3], const int point_pairs[][2], const int & num_pairs, + std::vector & points); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_covariance_eigen_vectors( + const Eigen::Matrix2d & matrix, double & sigma1, double & sigma2, double & yaw); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_direction_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_orientation_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_direction_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_orientation_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_cylinder_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_cylinder_bottom_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_circle_line_list( + const geometry_msgs::msg::Point center, const double radius, + std::vector & points, const int n); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_polygon_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_polygon_bottom_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_path_line_list( + const autoware_auto_perception_msgs::msg::PredictedPath & paths, + std::vector & points, const bool is_simple = false); + +/// \brief Convert Point32 to Point +/// \param val Point32 to be converted +/// \return Point type +inline AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Point to_point( + const geometry_msgs::msg::Point32 & val) +{ + geometry_msgs::msg::Point ret; + ret.x = static_cast(val.x); + ret.y = static_cast(val.y); + ret.z = static_cast(val.z); + return ret; +} + +/// \brief Convert to Pose from Point and Quaternion +/// \param point +/// \param orientation +/// \return Pose type +inline AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Pose to_pose( + const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Quaternion & orientation) +{ + geometry_msgs::msg::Pose ret; + ret.position = point; + ret.orientation = orientation; + return ret; +} + +inline AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Pose initPose() +{ + geometry_msgs::msg::Pose pose; + pose.position.x = 0.0; + pose.position.y = 0.0; + pose.position.z = 0.0; + pose.orientation.x = 0.0; + pose.orientation.y = 0.0; + pose.orientation.z = 0.0; + pose.orientation.w = 1.0; + return pose; +} + +/// \brief Get the best classification from the list of classifications based on max probability +/// \tparam ClassificationContainerT List type with ObjectClassificationMsg +/// \param labels List of ObjectClassificationMsg objects +/// \param logger_name Name to use for logger in case of a warning (if labels is empty) +/// \return Id of the best classification, Unknown if there is no best label +template +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC + autoware_auto_perception_msgs::msg::ObjectClassification::_label_type + get_best_label(ClassificationContainerT labels, const std::string & logger_name) +{ + const auto best_class_label = std::max_element( + labels.begin(), labels.end(), + [](const auto & a, const auto & b) -> bool { return a.probability < b.probability; }); + if (best_class_label == labels.end()) { + RCLCPP_WARN( + rclcpp::get_logger(logger_name), + "Empty classification field. " + "Treating as unknown"); + return autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + } + return best_class_label->label; +} + +} // namespace detail +} // namespace object_detection +} // namespace rviz_plugins +} // namespace autoware + +#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp new file mode 100644 index 0000000000000..4290fdff49bb3 --- /dev/null +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp @@ -0,0 +1,567 @@ +// Copyright 2021 Apex.AI, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ + +#include "autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp" +#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp" +#include "autoware_auto_perception_rviz_plugin/visibility_control.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace autoware +{ +namespace rviz_plugins +{ +namespace object_detection +{ +/// \brief Base rviz plugin class for all object msg types. The class defines common properties +/// for the plugin and also defines common helper functions that can be used by its derived +/// classes. +/// \tparam MsgT PredictedObjects or TrackedObjects or DetectedObjects type +template +class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase +: public rviz_common::RosTopicDisplay +{ +public: + using Color = std::array; + using Marker = visualization_msgs::msg::Marker; + using MarkerCommon = rviz_default_plugins::displays::MarkerCommon; + using ObjectClassificationMsg = autoware_auto_perception_msgs::msg::ObjectClassification; + using RosTopicDisplay = rviz_common::RosTopicDisplay; + + using PolygonPropertyMap = + std::unordered_map; + + explicit ObjectPolygonDisplayBase(const std::string & default_topic) + : m_marker_common(this), + // m_display_type_property{"Polygon Type", "3d", "Type of the polygon to display object", this}, + m_display_label_property{"Display Label", true, "Enable/disable label visualization", this}, + m_display_uuid_property{"Display UUID", true, "Enable/disable uuid visualization", this}, + m_display_velocity_text_property{ + "Display Velocity", true, "Enable/disable velocity text visualization", this}, + m_display_acceleration_text_property{ + "Display Acceleration", true, "Enable/disable acceleration text visualization", this}, + m_display_pose_covariance_property{ + "Display Pose Covariance", true, "Enable/disable pose covariance visualization", this}, + m_display_yaw_covariance_property{ + "Display Yaw Covariance", false, "Enable/disable yaw covariance visualization", this}, + m_display_twist_property{"Display Twist", true, "Enable/disable twist visualization", this}, + m_display_twist_covariance_property{ + "Display Twist Covariance", false, "Enable/disable twist covariance visualization", this}, + m_display_yaw_rate_property{ + "Display Yaw Rate", false, "Enable/disable yaw rate visualization", this}, + m_display_yaw_rate_covariance_property{ + "Display Yaw Rate Covariance", false, "Enable/disable yaw rate covariance visualization", + this}, + m_display_predicted_paths_property{ + "Display Predicted Paths", true, "Enable/disable predicted paths visualization", this}, + m_display_path_confidence_property{ + "Display Predicted Path Confidence", true, "Enable/disable predicted paths visualization", + this}, + + m_display_existence_probability_property{ + "Display Existence Probability", false, "Enable/disable existence probability visualization", + this}, + + m_line_width_property{"Line Width", 0.03, "Line width of object-shape", this}, + m_default_topic{default_topic} + { + m_display_type_property = new rviz_common::properties::EnumProperty( + "Polygon Type", "3d", "Type of the polygon to display object.", this); + // Option values here must correspond to indices in palette_textures_ array in onInitialize() + // below. + m_display_type_property->addOption("3d", 0); + m_display_type_property->addOption("2d", 1); + m_display_type_property->addOption("Disable", 2); + m_simple_visualize_mode_property = new rviz_common::properties::EnumProperty( + "Visualization Type", "Normal", "Simplicity of the polygon to display object.", this); + m_simple_visualize_mode_property->addOption("Normal", 0); + m_simple_visualize_mode_property->addOption("Simple", 1); + // Confidence interval property + m_confidence_interval_property = new rviz_common::properties::EnumProperty( + "Confidence Interval", "95%", "Confidence interval of state estimations.", this); + m_confidence_interval_property->addOption("70%", 0); + m_confidence_interval_property->addOption("85%", 1); + m_confidence_interval_property->addOption("95%", 2); + m_confidence_interval_property->addOption("99%", 3); + + // iterate over default values to create and initialize the properties. + for (const auto & map_property_it : detail::kDefaultObjectPropertyValues) { + const auto & class_property_values = map_property_it.second; + const auto & color = class_property_values.color; + // This is just a parent property to contain the necessary properties for the given class: + m_class_group_properties.emplace_back( + class_property_values.label.c_str(), QVariant(), + "Groups polygon properties for the given class", this); + auto & parent_property = m_class_group_properties.back(); + // Associate a color and opacity property for the given class and attach them to the + // parent property of the class so they can have a drop down view from the label property: + m_polygon_properties.emplace( + std::piecewise_construct, std::forward_as_tuple(map_property_it.first), + std::forward_as_tuple( + QColor{color[0], color[1], color[2]}, class_property_values.alpha, &parent_property)); + } + init_color_list(predicted_path_colors); + } + + void onInitialize() override + { + RosTopicDisplay::RTDClass::onInitialize(); + m_marker_common.initialize(this->context_, this->scene_node_); + QString message_type = QString::fromStdString(rosidl_generator_traits::name()); + this->topic_property_->setMessageType(message_type); + this->topic_property_->setValue(m_default_topic.c_str()); + this->topic_property_->setDescription("Topic to subscribe to."); + } + + void load(const rviz_common::Config & config) override + { + RosTopicDisplay::Display::load(config); + m_marker_common.load(config); + } + + void update(float wall_dt, float ros_dt) override { m_marker_common.update(wall_dt, ros_dt); } + + void reset() override + { + RosTopicDisplay::reset(); + m_marker_common.clearMarkers(); + } + + void clear_markers() { m_marker_common.clearMarkers(); } + + void add_marker(visualization_msgs::msg::Marker::ConstSharedPtr marker_ptr) + { + m_marker_common.addMessage(marker_ptr); + } + + void add_marker(visualization_msgs::msg::MarkerArray::ConstSharedPtr markers_ptr) + { + m_marker_common.addMessage(markers_ptr); + } + + void deleteMarker(rviz_default_plugins::displays::MarkerID marker_id) + { + m_marker_common.deleteMarker(marker_id); + } + +protected: + /// \brief Convert given shape msg into a Marker + /// \tparam ClassificationContainerT List type with ObjectClassificationMsg + /// \param shape_msg Shape msg to be converted + /// \param centroid Centroid position of the shape in Object.header.frame_id frame + /// \param orientation Orientation of the shape in Object.header.frame_id frame + /// \param labels List of ObjectClassificationMsg objects + /// \param line_width Line thickness around the object + /// \return Marker ptr. Id and header will have to be set by the caller + template + std::optional get_shape_marker_ptr( + const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const ClassificationContainerT & labels, const double & line_width, + const bool & is_orientation_available) const + { + const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels); + if (m_display_type_property->getOptionInt() == 0) { + return detail::get_shape_marker_ptr( + shape_msg, centroid, orientation, color_rgba, line_width, is_orientation_available); + } else if (m_display_type_property->getOptionInt() == 1) { + return detail::get_2d_shape_marker_ptr( + shape_msg, centroid, orientation, color_rgba, line_width, is_orientation_available); + } else { + return std::nullopt; + } + } + + template + visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( + const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, + const bool & is_orientation_available); + + /// \brief Convert given shape msg into a Marker to visualize label name + /// \tparam ClassificationContainerT List type with ObjectClassificationMsg + /// \param centroid Centroid position of the shape in Object.header.frame_id frame + /// \param labels List of ObjectClassificationMsg objects + /// \return Marker ptr. Id and header will have to be set by the caller + template + std::optional get_label_marker_ptr( + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const ClassificationContainerT & labels) const + { + if (m_display_label_property.getBool()) { + const std::string label = get_best_label(labels); + const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels); + return detail::get_label_marker_ptr(centroid, orientation, label, color_rgba); + } else { + return std::nullopt; + } + } + template + std::optional get_existence_probability_marker_ptr( + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const float existence_probability, const ClassificationContainerT & labels) const + { + if (m_display_existence_probability_property.getBool()) { + const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels); + return detail::get_existence_probability_marker_ptr( + centroid, orientation, existence_probability, color_rgba); + } else { + return std::nullopt; + } + } + + template + std::optional get_uuid_marker_ptr( + const unique_identifier_msgs::msg::UUID & uuid, const geometry_msgs::msg::Point & centroid, + const ClassificationContainerT & labels) const + { + if (m_display_uuid_property.getBool()) { + const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels); + const std::string uuid_str = uuid_to_string(uuid); + return detail::get_uuid_marker_ptr(uuid_str, centroid, color_rgba); + } else { + return std::nullopt; + } + } + + std::optional get_pose_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) const + { + if (m_display_pose_covariance_property.getBool()) { + return detail::get_pose_covariance_marker_ptr(pose_with_covariance, get_confidence_region()); + } else { + return std::nullopt; + } + } + + std::optional get_yaw_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & length, + const double & line_width) const + { + if (m_display_yaw_covariance_property.getBool()) { + return detail::get_yaw_covariance_marker_ptr( + pose_with_covariance, length, get_confidence_interval(), line_width); + } else { + return std::nullopt; + } + } + + template + std::optional get_velocity_text_marker_ptr( + const geometry_msgs::msg::Twist & twist, const geometry_msgs::msg::Point & vis_pos, + const ClassificationContainerT & labels) const + { + if (m_display_velocity_text_property.getBool()) { + const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels); + return detail::get_velocity_text_marker_ptr(twist, vis_pos, color_rgba); + } else { + return std::nullopt; + } + } + + template + std::optional get_acceleration_text_marker_ptr( + const geometry_msgs::msg::Accel & accel, const geometry_msgs::msg::Point & vis_pos, + const ClassificationContainerT & labels) const + { + if (m_display_acceleration_text_property.getBool()) { + const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels); + return detail::get_acceleration_text_marker_ptr(accel, vis_pos, color_rgba); + } else { + return std::nullopt; + } + } + + std::optional get_twist_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & line_width) const + { + if (m_display_twist_property.getBool()) { + return detail::get_twist_marker_ptr(pose_with_covariance, twist_with_covariance, line_width); + } else { + return std::nullopt; + } + } + + std::optional get_twist_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance) const + { + if (m_display_twist_covariance_property.getBool()) { + return detail::get_twist_covariance_marker_ptr( + pose_with_covariance, twist_with_covariance, get_confidence_region()); + } else { + return std::nullopt; + } + } + + std::optional get_yaw_rate_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & line_width) const + { + if (m_display_yaw_rate_property.getBool()) { + return detail::get_yaw_rate_marker_ptr( + pose_with_covariance, twist_with_covariance, line_width); + } else { + return std::nullopt; + } + } + + std::optional get_yaw_rate_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & line_width) const + { + if (m_display_yaw_rate_covariance_property.getBool()) { + return detail::get_yaw_rate_covariance_marker_ptr( + pose_with_covariance, twist_with_covariance, get_confidence_interval(), line_width); + } else { + return std::nullopt; + } + } + + std::optional get_predicted_path_marker_ptr( + const unique_identifier_msgs::msg::UUID & uuid, + const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path) const + { + if (m_display_predicted_paths_property.getBool()) { + const std::string uuid_str = uuid_to_string(uuid); + const std_msgs::msg::ColorRGBA predicted_path_color = get_color_from_uuid(uuid_str); + return detail::get_predicted_path_marker_ptr( + shape, predicted_path, predicted_path_color, + m_simple_visualize_mode_property->getOptionInt() == 1); + } else { + return std::nullopt; + } + } + + std::optional get_path_confidence_marker_ptr( + const unique_identifier_msgs::msg::UUID & uuid, + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path) const + { + if (m_display_path_confidence_property.getBool()) { + const std::string uuid_str = uuid_to_string(uuid); + const std_msgs::msg::ColorRGBA path_confidence_color = get_color_from_uuid(uuid_str); + return detail::get_path_confidence_marker_ptr(predicted_path, path_confidence_color); + } else { + return std::nullopt; + } + } + + /// \brief Get color and alpha values based on the given list of classification values + /// \tparam ClassificationContainerT Container of ObjectClassification + /// \param labels list of classifications + /// \return Color and alpha for the best class in the given list. Unknown class is used in + /// degenerate cases + template + std_msgs::msg::ColorRGBA get_color_rgba(const ClassificationContainerT & labels) const + { + static const std::string kLoggerName("ObjectPolygonDisplayBase"); + const auto label = detail::get_best_label(labels, kLoggerName); + auto it = m_polygon_properties.find(label); + if (it == m_polygon_properties.end()) { + it = m_polygon_properties.find(ObjectClassificationMsg::UNKNOWN); + } + return it->second; + } + + /// \brief Get color and alpha values based on the given list of classification values + /// \tparam ClassificationContainerT Container of ObjectClassification + /// \param labels list of classifications + /// \return best label string + template + std::string get_best_label(const ClassificationContainerT & labels) const + { + static const std::string kLoggerName("ObjectPolygonDisplayBase"); + const auto label = detail::get_best_label(labels, kLoggerName); + auto it = detail::kDefaultObjectPropertyValues.find(label); + if (it == detail::kDefaultObjectPropertyValues.end()) { + it = detail::kDefaultObjectPropertyValues.find(ObjectClassificationMsg::UNKNOWN); + } + return (it->second).label; + } + std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & u) const + { + std::stringstream ss; + for (auto i = 0; i < 16; ++i) { + ss << std::hex << std::setfill('0') << std::setw(2) << +u.uuid[i]; + } + return ss.str(); + } + + std_msgs::msg::ColorRGBA AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC + get_color_from_uuid(const std::string & uuid) const + { + int i = (static_cast(uuid.at(0)) * 4 + static_cast(uuid.at(1))) % + static_cast(predicted_path_colors.size()); + + std_msgs::msg::ColorRGBA color; + color.r = predicted_path_colors.at(i).r; + color.g = predicted_path_colors.at(i).g; + color.b = predicted_path_colors.at(i).b; + return color; + } + + void init_color_list(std::vector & colors) const + { + std_msgs::msg::ColorRGBA sample_color; + sample_color.r = 1.0; + sample_color.g = 0.65; + sample_color.b = 0.0; + colors.push_back(sample_color); // orange + sample_color.r = 1.0; + sample_color.g = 1.0; + sample_color.b = 0.0; + colors.push_back(sample_color); // yellow + sample_color.r = 0.69; + sample_color.g = 1.0; + sample_color.b = 0.18; + colors.push_back(sample_color); // green yellow + sample_color.r = 0.59; + sample_color.g = 1.0; + sample_color.b = 0.59; + colors.push_back(sample_color); // pale green + sample_color.r = 0.5; + sample_color.g = 1.0; + sample_color.b = 0.0; + colors.push_back(sample_color); // chartreuse green + sample_color.r = 0.0; + sample_color.g = 1.0; + sample_color.b = 1.0; + colors.push_back(sample_color); // cyan + sample_color.r = 0.53; + sample_color.g = 0.81; + sample_color.b = 0.98; + colors.push_back(sample_color); // light skyblue + sample_color.r = 1.0; + sample_color.g = 0.41; + sample_color.b = 0.71; + colors.push_back(sample_color); // hot pink + } + + double get_line_width() { return m_line_width_property.getFloat(); } + + double get_confidence_interval() const + { + switch (m_confidence_interval_property->getOptionInt()) { + case 0: + // 70% + return 1.036; + case 1: + // 85% + return 1.440; + case 2: + // 95% + return 1.960; + case 3: + // 99% + return 2.576; + default: + return 1.960; + } + } + + double get_confidence_region() const + { + switch (m_confidence_interval_property->getOptionInt()) { + case 0: + // 70% + return 1.552; + case 1: + // 85% + return 1.802; + case 2: + // 95% + return 2.448; + case 3: + // 99% + return 3.035; + default: + return 2.448; + } + } + +private: + // All rviz plugins should have this. Should be initialized with pointer to this class + MarkerCommon m_marker_common; + // List is used to store the properties for classification in case we need to access them: + std::list m_class_group_properties; + // Map to store class labels and its corresponding properties + PolygonPropertyMap m_polygon_properties; + // Property to choose type of visualization polygon + rviz_common::properties::EnumProperty * m_display_type_property; + // Property to choose simplicity of visualization polygon + rviz_common::properties::EnumProperty * m_simple_visualize_mode_property; + // Property to set confidence interval of state estimations + rviz_common::properties::EnumProperty * m_confidence_interval_property; + // Property to enable/disable label visualization + rviz_common::properties::BoolProperty m_display_label_property; + // Property to enable/disable uuid visualization + rviz_common::properties::BoolProperty m_display_uuid_property; + // Property to enable/disable velocity text visualization + rviz_common::properties::BoolProperty m_display_velocity_text_property; + // Property to enable/disable acceleration text visualization + rviz_common::properties::BoolProperty m_display_acceleration_text_property; + // Property to enable/disable pose with covariance visualization + rviz_common::properties::BoolProperty m_display_pose_covariance_property; + // Property to enable/disable yaw covariance visualization + rviz_common::properties::BoolProperty m_display_yaw_covariance_property; + // Property to enable/disable twist visualization + rviz_common::properties::BoolProperty m_display_twist_property; + // Property to enable/disable twist covariance visualization + rviz_common::properties::BoolProperty m_display_twist_covariance_property; + // Property to enable/disable yaw rate visualization + rviz_common::properties::BoolProperty m_display_yaw_rate_property; + // Property to enable/disable yaw rate covariance visualization + rviz_common::properties::BoolProperty m_display_yaw_rate_covariance_property; + // Property to enable/disable predicted paths visualization + rviz_common::properties::BoolProperty m_display_predicted_paths_property; + // Property to enable/disable predicted path confidence visualization + rviz_common::properties::BoolProperty m_display_path_confidence_property; + + rviz_common::properties::BoolProperty m_display_existence_probability_property; + + // Property to decide line width of object shape + rviz_common::properties::FloatProperty m_line_width_property; + // Default topic name to be visualized + std::string m_default_topic; + + std::vector predicted_path_colors; +}; +} // namespace object_detection +} // namespace rviz_plugins +} // namespace autoware + +#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp similarity index 92% rename from common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp rename to common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp index 2896286970217..775c18db6ba5c 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp @@ -11,10 +11,10 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ -#define OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ +#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ -#include +#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" #include @@ -153,4 +153,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay } // namespace rviz_plugins } // namespace autoware -#endif // OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ +#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp similarity index 78% rename from common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp rename to common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp index 9f00a2cb1cde2..4e86a5ee93fd8 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp @@ -11,10 +11,10 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ -#define OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ +#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ -#include +#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" #include @@ -39,11 +39,23 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay Q_OBJECT public: + using TrackedObject = autoware_auto_perception_msgs::msg::TrackedObject; using TrackedObjects = autoware_auto_perception_msgs::msg::TrackedObjects; TrackedObjectsDisplay(); +protected: + uint get_object_dynamics_to_visualize() + { + return m_select_object_dynamics_property->getOptionInt(); + } + + static bool is_object_to_show(const uint showing_dynamic_status, const TrackedObject & object); + private: + // Property to choose object dynamics to visualize + rviz_common::properties::EnumProperty * m_select_object_dynamics_property; + void processMessage(TrackedObjects::ConstSharedPtr msg) override; boost::uuids::uuid to_boost_uuid(const unique_identifier_msgs::msg::UUID & uuid_msg) @@ -102,4 +114,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay } // namespace rviz_plugins } // namespace autoware -#endif // OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ +#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/visibility_control.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/visibility_control.hpp new file mode 100644 index 0000000000000..47cb8383fdada --- /dev/null +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/visibility_control.hpp @@ -0,0 +1,43 @@ +// Copyright 2019 the Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ + +#if defined(__WIN32) +#if defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || \ + defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_EXPORTS) +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC __declspec(dllexport) +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_LOCAL +// defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || +// defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_EXPORTS) +#else +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC __declspec(dllimport) +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_LOCAL +// defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || +// defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_EXPORTS) +#endif +#elif defined(__linux__) +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC __attribute__((visibility("default"))) +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC __attribute__((visibility("default"))) +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_LOCAL __attribute__((visibility("hidden"))) +#else // defined(_LINUX) +#error "Unsupported Build Configuration" +#endif // defined(_WINDOWS) + +#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp deleted file mode 100644 index 5d99b8a463711..0000000000000 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp +++ /dev/null @@ -1,235 +0,0 @@ -// Copyright 2021 Apex.AI, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -/// \brief This file defines some helper functions used by ObjectPolygonDisplayBase class -#ifndef OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ -#define OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace autoware -{ -namespace rviz_plugins -{ -namespace object_detection -{ -namespace detail -{ -// Struct to define all the configurable visual properties of an object of a particular -// classification type -struct ObjectPropertyValues -{ - // Classified type of the object - std::string label; - // Color for the type of the object - std::array color; - // Alpha values for the type of the object - float alpha{0.999F}; -}; - -// Map defining colors according to value of label field in ObjectClassification msg -const std::map< - autoware_auto_perception_msgs::msg::ObjectClassification::_label_type, ObjectPropertyValues> - // Color map is based on cityscapes color - kDefaultObjectPropertyValues = { - {autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN, - {"UNKNOWN", {255, 255, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::CAR, {"CAR", {30, 144, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::BUS, {"BUS", {30, 144, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN, - {"PEDESTRIAN", {255, 192, 203}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, {"CYCLIST", {119, 11, 32}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, - {"MOTORCYCLE", {119, 11, 32}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER, - {"TRAILER", {30, 144, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, {"TRUCK", {30, 144, 255}}}}; - -/// \brief Convert the given polygon into a marker representing the shape in 3d -/// \param shape_msg Shape msg to be converted. Corners should be in object-local frame -/// \param centroid Centroid position of the shape in Object.header.frame_id frame -/// \param orientation Orientation of the shape in Object.header.frame_id frame -/// \param color_rgba Color and alpha values to use for the marker -/// \param line_width Line thickness around the object -/// \return Marker ptr. Id and header will have to be set by the caller -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, - const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_2d_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, - const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width); - -/// \brief Convert the given polygon into a marker representing the shape in 3d -/// \param centroid Centroid position of the shape in Object.header.frame_id frame -/// \return Marker ptr. Id and header will have to be set by the caller -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_label_marker_ptr( - const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const std::string label, const std_msgs::msg::ColorRGBA & color_rgba); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_uuid_marker_ptr( - const std::string & uuid, const geometry_msgs::msg::Point & centroid, - const std_msgs::msg::ColorRGBA & color_rgba); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_pose_with_covariance_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_velocity_text_marker_ptr( - const geometry_msgs::msg::Twist & twist, const geometry_msgs::msg::Point & vis_pos, - const std_msgs::msg::ColorRGBA & color_rgba); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_acceleration_text_marker_ptr( - const geometry_msgs::msg::Accel & accel, const geometry_msgs::msg::Point & vis_pos, - const std_msgs::msg::ColorRGBA & color_rgba); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_twist_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, - const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_predicted_path_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const std_msgs::msg::ColorRGBA & predicted_path_color, const bool is_simple = false); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_path_confidence_marker_ptr( - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const std_msgs::msg::ColorRGBA & path_confidence_color); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, - std::vector & points); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, - std::vector & points); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_cylinder_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, - std::vector & points); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_cylinder_bottom_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, - std::vector & points); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_circle_line_list( - const geometry_msgs::msg::Point center, const double radius, - std::vector & points, const int n); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_polygon_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, - std::vector & points); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_polygon_bottom_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, - std::vector & points); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_path_line_list( - const autoware_auto_perception_msgs::msg::PredictedPath & paths, - std::vector & points, const bool is_simple = false); - -/// \brief Convert Point32 to Point -/// \param val Point32 to be converted -/// \return Point type -inline AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Point to_point( - const geometry_msgs::msg::Point32 & val) -{ - geometry_msgs::msg::Point ret; - ret.x = static_cast(val.x); - ret.y = static_cast(val.y); - ret.z = static_cast(val.z); - return ret; -} - -/// \brief Convert to Pose from Point and Quaternion -/// \param point -/// \param orientation -/// \return Pose type -inline AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Pose to_pose( - const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Quaternion & orientation) -{ - geometry_msgs::msg::Pose ret; - ret.position = point; - ret.orientation = orientation; - return ret; -} - -inline AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Pose initPose() -{ - geometry_msgs::msg::Pose pose; - pose.position.x = 0.0; - pose.position.y = 0.0; - pose.position.z = 0.0; - pose.orientation.x = 0.0; - pose.orientation.y = 0.0; - pose.orientation.z = 0.0; - pose.orientation.w = 1.0; - return pose; -} - -/// \brief Get the best classification from the list of classifications based on max probability -/// \tparam ClassificationContainerT List type with ObjectClassificationMsg -/// \param labels List of ObjectClassificationMsg objects -/// \param logger_name Name to use for logger in case of a warning (if labels is empty) -/// \return Id of the best classification, Unknown if there is no best label -template -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC - autoware_auto_perception_msgs::msg::ObjectClassification::_label_type - get_best_label(ClassificationContainerT labels, const std::string & logger_name) -{ - const auto best_class_label = std::max_element( - labels.begin(), labels.end(), - [](const auto & a, const auto & b) -> bool { return a.probability < b.probability; }); - if (best_class_label == labels.end()) { - RCLCPP_WARN( - rclcpp::get_logger(logger_name), - "Empty classification field. " - "Treating as unknown"); - return autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; - } - return best_class_label->label; -} - -} // namespace detail -} // namespace object_detection -} // namespace rviz_plugins -} // namespace autoware - -#endif // OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp deleted file mode 100644 index 10bf11847c2f5..0000000000000 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ /dev/null @@ -1,427 +0,0 @@ -// Copyright 2021 Apex.AI, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#ifndef OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ -#define OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ - -#include "rviz_common/properties/enum_property.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include - -namespace autoware -{ -namespace rviz_plugins -{ -namespace object_detection -{ -/// \brief Base rviz plugin class for all object msg types. The class defines common properties -/// for the plugin and also defines common helper functions that can be used by its derived -/// classes. -/// \tparam MsgT PredictedObjects or TrackedObjects or DetectedObjects type -template -class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase -: public rviz_common::RosTopicDisplay -{ -public: - using Color = std::array; - using Marker = visualization_msgs::msg::Marker; - using MarkerCommon = rviz_default_plugins::displays::MarkerCommon; - using ObjectClassificationMsg = autoware_auto_perception_msgs::msg::ObjectClassification; - using RosTopicDisplay = rviz_common::RosTopicDisplay; - - using PolygonPropertyMap = - std::unordered_map; - - explicit ObjectPolygonDisplayBase(const std::string & default_topic) - : m_marker_common(this), - // m_display_type_property{"Polygon Type", "3d", "Type of the polygon to display object", this}, - m_display_label_property{"Display Label", true, "Enable/disable label visualization", this}, - m_display_uuid_property{"Display UUID", true, "Enable/disable uuid visualization", this}, - m_display_pose_with_covariance_property{ - "Display PoseWithCovariance", true, "Enable/disable pose with covariance visualization", - this}, - m_display_velocity_text_property{ - "Display Velocity", true, "Enable/disable velocity text visualization", this}, - m_display_acceleration_text_property{ - "Display Acceleration", true, "Enable/disable acceleration text visualization", this}, - m_display_twist_property{"Display Twist", true, "Enable/disable twist visualization", this}, - m_display_predicted_paths_property{ - "Display Predicted Paths", true, "Enable/disable predicted paths visualization", this}, - m_display_path_confidence_property{ - "Display Predicted Path Confidence", true, "Enable/disable predicted paths visualization", - this}, - m_line_width_property{"Line Width", 0.03, "Line width of object-shape", this}, - m_default_topic{default_topic} - { - m_display_type_property = new rviz_common::properties::EnumProperty( - "Polygon Type", "3d", "Type of the polygon to display object.", this); - // Option values here must correspond to indices in palette_textures_ array in onInitialize() - // below. - m_display_type_property->addOption("3d", 0); - m_display_type_property->addOption("2d", 1); - m_display_type_property->addOption("Disable", 2); - m_simple_visualize_mode_property = new rviz_common::properties::EnumProperty( - "Visualization Type", "Normal", "Simplicity of the polygon to display object.", this); - m_simple_visualize_mode_property->addOption("Normal", 0); - m_simple_visualize_mode_property->addOption("Simple", 1); - // iterate over default values to create and initialize the properties. - for (const auto & map_property_it : detail::kDefaultObjectPropertyValues) { - const auto & class_property_values = map_property_it.second; - const auto & color = class_property_values.color; - // This is just a parent property to contain the necessary properties for the given class: - m_class_group_properties.emplace_back( - class_property_values.label.c_str(), QVariant(), - "Groups polygon properties for the given class", this); - auto & parent_property = m_class_group_properties.back(); - // Associate a color and opacity property for the given class and attach them to the - // parent property of the class so they can have a drop down view from the label property: - m_polygon_properties.emplace( - std::piecewise_construct, std::forward_as_tuple(map_property_it.first), - std::forward_as_tuple( - QColor{color[0], color[1], color[2]}, class_property_values.alpha, &parent_property)); - } - init_color_list(predicted_path_colors); - } - - void onInitialize() override - { - RosTopicDisplay::RTDClass::onInitialize(); - m_marker_common.initialize(this->context_, this->scene_node_); - QString message_type = QString::fromStdString(rosidl_generator_traits::name()); - this->topic_property_->setMessageType(message_type); - this->topic_property_->setValue(m_default_topic.c_str()); - this->topic_property_->setDescription("Topic to subscribe to."); - } - - void load(const rviz_common::Config & config) override - { - RosTopicDisplay::Display::load(config); - m_marker_common.load(config); - } - - void update(float wall_dt, float ros_dt) override { m_marker_common.update(wall_dt, ros_dt); } - - void reset() override - { - RosTopicDisplay::reset(); - m_marker_common.clearMarkers(); - } - - void clear_markers() { m_marker_common.clearMarkers(); } - - void add_marker(visualization_msgs::msg::Marker::ConstSharedPtr marker_ptr) - { - m_marker_common.addMessage(marker_ptr); - } - - void add_marker(visualization_msgs::msg::MarkerArray::ConstSharedPtr markers_ptr) - { - m_marker_common.addMessage(markers_ptr); - } - - void deleteMarker(rviz_default_plugins::displays::MarkerID marker_id) - { - m_marker_common.deleteMarker(marker_id); - } - -protected: - /// \brief Convert given shape msg into a Marker - /// \tparam ClassificationContainerT List type with ObjectClassificationMsg - /// \param shape_msg Shape msg to be converted - /// \param centroid Centroid position of the shape in Object.header.frame_id frame - /// \param orientation Orientation of the shape in Object.header.frame_id frame - /// \param labels List of ObjectClassificationMsg objects - /// \param line_width Line thickness around the object - /// \return Marker ptr. Id and header will have to be set by the caller - template - std::optional get_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, - const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const ClassificationContainerT & labels, const double & line_width) const - { - const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels); - if (m_display_type_property->getOptionInt() == 0) { - return detail::get_shape_marker_ptr(shape_msg, centroid, orientation, color_rgba, line_width); - } else if (m_display_type_property->getOptionInt() == 1) { - return detail::get_2d_shape_marker_ptr( - shape_msg, centroid, orientation, color_rgba, line_width); - } else { - return std::nullopt; - } - } - - template - visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, - const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width); - - /// \brief Convert given shape msg into a Marker to visualize label name - /// \tparam ClassificationContainerT List type with ObjectClassificationMsg - /// \param centroid Centroid position of the shape in Object.header.frame_id frame - /// \param labels List of ObjectClassificationMsg objects - /// \return Marker ptr. Id and header will have to be set by the caller - template - std::optional get_label_marker_ptr( - const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const ClassificationContainerT & labels) const - { - if (m_display_label_property.getBool()) { - const std::string label = get_best_label(labels); - const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels); - return detail::get_label_marker_ptr(centroid, orientation, label, color_rgba); - } else { - return std::nullopt; - } - } - - template - std::optional get_uuid_marker_ptr( - const unique_identifier_msgs::msg::UUID & uuid, const geometry_msgs::msg::Point & centroid, - const ClassificationContainerT & labels) const - { - if (m_display_uuid_property.getBool()) { - const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels); - const std::string uuid_str = uuid_to_string(uuid); - return detail::get_uuid_marker_ptr(uuid_str, centroid, color_rgba); - } else { - return std::nullopt; - } - } - - std::optional get_pose_with_covariance_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) const - { - if (m_display_pose_with_covariance_property.getBool()) { - return detail::get_pose_with_covariance_marker_ptr(pose_with_covariance); - } else { - return std::nullopt; - } - } - - template - std::optional get_velocity_text_marker_ptr( - const geometry_msgs::msg::Twist & twist, const geometry_msgs::msg::Point & vis_pos, - const ClassificationContainerT & labels) const - { - if (m_display_velocity_text_property.getBool()) { - const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels); - return detail::get_velocity_text_marker_ptr(twist, vis_pos, color_rgba); - } else { - return std::nullopt; - } - } - - template - std::optional get_acceleration_text_marker_ptr( - const geometry_msgs::msg::Accel & accel, const geometry_msgs::msg::Point & vis_pos, - const ClassificationContainerT & labels) const - { - if (m_display_acceleration_text_property.getBool()) { - const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels); - return detail::get_acceleration_text_marker_ptr(accel, vis_pos, color_rgba); - } else { - return std::nullopt; - } - } - - std::optional get_twist_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, - const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance) const - { - if (m_display_twist_property.getBool()) { - return detail::get_twist_marker_ptr(pose_with_covariance, twist_with_covariance); - } else { - return std::nullopt; - } - } - - std::optional get_predicted_path_marker_ptr( - const unique_identifier_msgs::msg::UUID & uuid, - const autoware_auto_perception_msgs::msg::Shape & shape, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path) const - { - if (m_display_predicted_paths_property.getBool()) { - const std::string uuid_str = uuid_to_string(uuid); - const std_msgs::msg::ColorRGBA predicted_path_color = get_color_from_uuid(uuid_str); - return detail::get_predicted_path_marker_ptr( - shape, predicted_path, predicted_path_color, - m_simple_visualize_mode_property->getOptionInt() == 1); - } else { - return std::nullopt; - } - } - - std::optional get_path_confidence_marker_ptr( - const unique_identifier_msgs::msg::UUID & uuid, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path) const - { - if (m_display_path_confidence_property.getBool()) { - const std::string uuid_str = uuid_to_string(uuid); - const std_msgs::msg::ColorRGBA path_confidence_color = get_color_from_uuid(uuid_str); - return detail::get_path_confidence_marker_ptr(predicted_path, path_confidence_color); - } else { - return std::nullopt; - } - } - - /// \brief Get color and alpha values based on the given list of classification values - /// \tparam ClassificationContainerT Container of ObjectClassification - /// \param labels list of classifications - /// \return Color and alpha for the best class in the given list. Unknown class is used in - /// degenerate cases - template - std_msgs::msg::ColorRGBA get_color_rgba(const ClassificationContainerT & labels) const - { - static const std::string kLoggerName("ObjectPolygonDisplayBase"); - const auto label = detail::get_best_label(labels, kLoggerName); - auto it = m_polygon_properties.find(label); - if (it == m_polygon_properties.end()) { - it = m_polygon_properties.find(ObjectClassificationMsg::UNKNOWN); - } - return it->second; - } - - /// \brief Get color and alpha values based on the given list of classification values - /// \tparam ClassificationContainerT Container of ObjectClassification - /// \param labels list of classifications - /// \return best label string - template - std::string get_best_label(const ClassificationContainerT & labels) const - { - static const std::string kLoggerName("ObjectPolygonDisplayBase"); - const auto label = detail::get_best_label(labels, kLoggerName); - auto it = detail::kDefaultObjectPropertyValues.find(label); - if (it == detail::kDefaultObjectPropertyValues.end()) { - it = detail::kDefaultObjectPropertyValues.find(ObjectClassificationMsg::UNKNOWN); - } - return (it->second).label; - } - - std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & u) const - { - std::stringstream ss; - for (auto i = 0; i < 16; ++i) { - ss << std::hex << std::setfill('0') << std::setw(2) << +u.uuid[i]; - } - return ss.str(); - } - - std_msgs::msg::ColorRGBA AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC - get_color_from_uuid(const std::string & uuid) const - { - int i = (static_cast(uuid.at(0)) * 4 + static_cast(uuid.at(1))) % - static_cast(predicted_path_colors.size()); - - std_msgs::msg::ColorRGBA color; - color.r = predicted_path_colors.at(i).r; - color.g = predicted_path_colors.at(i).g; - color.b = predicted_path_colors.at(i).b; - return color; - } - - void init_color_list(std::vector & colors) const - { - std_msgs::msg::ColorRGBA sample_color; - sample_color.r = 1.0; - sample_color.g = 0.65; - sample_color.b = 0.0; - colors.push_back(sample_color); // orange - sample_color.r = 1.0; - sample_color.g = 1.0; - sample_color.b = 0.0; - colors.push_back(sample_color); // yellow - sample_color.r = 0.69; - sample_color.g = 1.0; - sample_color.b = 0.18; - colors.push_back(sample_color); // green yellow - sample_color.r = 0.59; - sample_color.g = 1.0; - sample_color.b = 0.59; - colors.push_back(sample_color); // pale green - sample_color.r = 0.5; - sample_color.g = 1.0; - sample_color.b = 0.0; - colors.push_back(sample_color); // chartreuse green - sample_color.r = 0.0; - sample_color.g = 1.0; - sample_color.b = 1.0; - colors.push_back(sample_color); // cyan - sample_color.r = 0.53; - sample_color.g = 0.81; - sample_color.b = 0.98; - colors.push_back(sample_color); // light skyblue - sample_color.r = 1.0; - sample_color.g = 0.41; - sample_color.b = 0.71; - colors.push_back(sample_color); // hot pink - } - - double get_line_width() { return m_line_width_property.getFloat(); } - -private: - // All rviz plugins should have this. Should be initialized with pointer to this class - MarkerCommon m_marker_common; - // List is used to store the properties for classification in case we need to access them: - std::list m_class_group_properties; - // Map to store class labels and its corresponding properties - PolygonPropertyMap m_polygon_properties; - // Property to choose type of visualization polygon - rviz_common::properties::EnumProperty * m_display_type_property; - // Property to choose simplicity of visualization polygon - rviz_common::properties::EnumProperty * m_simple_visualize_mode_property; - // Property to enable/disable label visualization - rviz_common::properties::BoolProperty m_display_label_property; - // Property to enable/disable uuid visualization - rviz_common::properties::BoolProperty m_display_uuid_property; - // Property to enable/disable pose with covariance visualization - rviz_common::properties::BoolProperty m_display_pose_with_covariance_property; - // Property to enable/disable velocity text visualization - rviz_common::properties::BoolProperty m_display_velocity_text_property; - // Property to enable/disable acceleration text visualization - rviz_common::properties::BoolProperty m_display_acceleration_text_property; - // Property to enable/disable twist visualization - rviz_common::properties::BoolProperty m_display_twist_property; - // Property to enable/disable predicted paths visualization - rviz_common::properties::BoolProperty m_display_predicted_paths_property; - // Property to enable/disable predicted path confidence visualization - rviz_common::properties::BoolProperty m_display_path_confidence_property; - // Property to decide line width of object shape - rviz_common::properties::FloatProperty m_line_width_property; - // Default topic name to be visualized - std::string m_default_topic; - - std::vector predicted_path_colors; -}; -} // namespace object_detection -} // namespace rviz_plugins -} // namespace autoware - -#endif // OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/visibility_control.hpp b/common/autoware_auto_perception_rviz_plugin/include/visibility_control.hpp deleted file mode 100644 index bce7351572375..0000000000000 --- a/common/autoware_auto_perception_rviz_plugin/include/visibility_control.hpp +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright 2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef VISIBILITY_CONTROL_HPP_ -#define VISIBILITY_CONTROL_HPP_ - -#if defined(__WIN32) -#if defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || \ - defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_EXPORTS) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC __declspec(dllexport) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_LOCAL -// defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || -// defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_EXPORTS) -#else -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC __declspec(dllimport) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_LOCAL -// defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || -// defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_EXPORTS) -#endif -#elif defined(__linux__) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC __attribute__((visibility("default"))) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_LOCAL __attribute__((visibility("hidden"))) -#elif defined(__APPLE__) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC __attribute__((visibility("default"))) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_LOCAL __attribute__((visibility("hidden"))) -#else // defined(_LINUX) -#error "Unsupported Build Configuration" -#endif // defined(_WINDOWS) - -#endif // VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/package.xml b/common/autoware_auto_perception_rviz_plugin/package.xml index c1ad3e3df140e..2033239824d95 100644 --- a/common/autoware_auto_perception_rviz_plugin/package.xml +++ b/common/autoware_auto_perception_rviz_plugin/package.xml @@ -8,7 +8,8 @@ Satoshi Tanaka Taiki Tanaka Takeshi Miura - + Shunsuke Miura + Yoshi Ri Apache 2.0 ament_cmake diff --git a/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp b/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp index ac9c5af4ddeef..b3e542a02243b 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp" #include diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp index 067173288e685..53e935fa1850a 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp" #include @@ -38,7 +38,9 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) auto shape_marker = get_shape_marker_ptr( object.shape, object.kinematics.pose_with_covariance.pose.position, object.kinematics.pose_with_covariance.pose.orientation, object.classification, - get_line_width()); + get_line_width(), + object.kinematics.orientation_availability == + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE); if (shape_marker) { auto shape_marker_ptr = shape_marker.value(); shape_marker_ptr->header = msg->header; @@ -57,6 +59,43 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) add_marker(label_marker_ptr); } + // Get marker for pose covariance + auto pose_with_covariance_marker = + get_pose_covariance_marker_ptr(object.kinematics.pose_with_covariance); + if (pose_with_covariance_marker) { + auto marker_ptr = pose_with_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = id++; + add_marker(marker_ptr); + } + + // Get marker for yaw covariance + auto yaw_covariance_marker = get_yaw_covariance_marker_ptr( + object.kinematics.pose_with_covariance, object.shape.dimensions.x * 0.65, + get_line_width() * 0.5); + if (yaw_covariance_marker) { + auto marker_ptr = yaw_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = id++; + add_marker(marker_ptr); + } + + // Get marker for existence probability + geometry_msgs::msg::Point existence_probability_position; + existence_probability_position.x = object.kinematics.pose_with_covariance.pose.position.x + 0.5; + existence_probability_position.y = object.kinematics.pose_with_covariance.pose.position.y; + existence_probability_position.z = object.kinematics.pose_with_covariance.pose.position.z + 0.5; + const float existence_probability = object.existence_probability; + auto existence_prob_marker = get_existence_probability_marker_ptr( + existence_probability_position, object.kinematics.pose_with_covariance.pose.orientation, + existence_probability, object.classification); + if (existence_prob_marker) { + auto existence_prob_marker_ptr = existence_prob_marker.value(); + existence_prob_marker_ptr->header = msg->header; + existence_prob_marker_ptr->id = id++; + add_marker(existence_prob_marker_ptr); + } + // Get marker for velocity text geometry_msgs::msg::Point vel_vis_position; vel_vis_position.x = object.kinematics.pose_with_covariance.pose.position.x - 0.5; @@ -73,13 +112,46 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) // Get marker for twist auto twist_marker = get_twist_marker_ptr( - object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance); + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, + get_line_width()); if (twist_marker) { auto twist_marker_ptr = twist_marker.value(); twist_marker_ptr->header = msg->header; twist_marker_ptr->id = id++; add_marker(twist_marker_ptr); } + + // Get marker for twist covariance + auto twist_covariance_marker = get_twist_covariance_marker_ptr( + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance); + if (twist_covariance_marker) { + auto marker_ptr = twist_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = id++; + add_marker(marker_ptr); + } + + // Get marker for yaw rate + auto yaw_rate_marker = get_yaw_rate_marker_ptr( + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, + get_line_width() * 0.4); + if (yaw_rate_marker) { + auto marker_ptr = yaw_rate_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = id++; + add_marker(marker_ptr); + } + + // Get marker for yaw rate covariance + auto yaw_rate_covariance_marker = get_yaw_rate_covariance_marker_ptr( + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, + get_line_width() * 0.3); + if (yaw_rate_covariance_marker) { + auto marker_ptr = yaw_rate_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = id++; + add_marker(marker_ptr); + } } } diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp index 894e377a6f851..8be9d1c01332e 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License.. +#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp" + #include #include -#include #include @@ -56,7 +57,7 @@ visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr( marker_ptr->type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; marker_ptr->ns = std::string("path confidence"); marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); marker_ptr->scale.x = 0.5; marker_ptr->scale.y = 0.5; marker_ptr->scale.z = 0.5; @@ -77,42 +78,41 @@ visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; marker_ptr->ns = std::string("path"); marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); marker_ptr->pose = initPose(); marker_ptr->color = predicted_path_color; marker_ptr->color.a = 0.6; marker_ptr->scale.x = 0.015; calc_path_line_list(predicted_path, marker_ptr->points, is_simple); for (size_t k = 0; k < marker_ptr->points.size(); ++k) { - marker_ptr->points.at(k).z -= shape.dimensions.z / 2.0; + marker_ptr->points.at(k).z -= shape.dimensions.z * 0.5; } return marker_ptr; } visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, - const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance) + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width) { auto marker_ptr = std::make_shared(); marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; marker_ptr->ns = std::string("twist"); - marker_ptr->scale.x = 0.03; + marker_ptr->scale.x = line_width; marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose = pose_with_covariance.pose; - geometry_msgs::msg::Point pt_s; - pt_s.x = 0.0; - pt_s.y = 0.0; - pt_s.z = 0.0; - marker_ptr->points.push_back(pt_s); - - geometry_msgs::msg::Point pt_e; - pt_e.x = twist_with_covariance.twist.linear.x; - pt_e.y = twist_with_covariance.twist.linear.y; - pt_e.z = twist_with_covariance.twist.linear.z; - marker_ptr->points.push_back(pt_e); + // velocity line + geometry_msgs::msg::Point point; + point.x = 0.0; + point.y = 0.0; + point.z = 0.0; + marker_ptr->points.push_back(point); + point.x = twist_with_covariance.twist.linear.x; + point.y = twist_with_covariance.twist.linear.y; + point.z = twist_with_covariance.twist.linear.z; + marker_ptr->points.push_back(point); - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); marker_ptr->color.a = 0.999; marker_ptr->color.r = 1.0; marker_ptr->color.g = 0.0; @@ -121,6 +121,168 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr( return marker_ptr; } +visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & confidence_interval_coefficient) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::CYLINDER; + marker_ptr->ns = std::string("twist covariance"); + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = pose_with_covariance.pose; + + // position is the tip of the velocity vector + const double velocity = std::sqrt( + twist_with_covariance.twist.linear.x * twist_with_covariance.twist.linear.x + + twist_with_covariance.twist.linear.y * twist_with_covariance.twist.linear.y); + const double velocity_angle = + std::atan2(twist_with_covariance.twist.linear.y, twist_with_covariance.twist.linear.x); + const double pos_yaw_angle = 2.0 * std::atan2( + pose_with_covariance.pose.orientation.z, + pose_with_covariance.pose.orientation.w); // [rad] + marker_ptr->pose.position.x += velocity * std::cos(pos_yaw_angle + velocity_angle); + marker_ptr->pose.position.y += velocity * std::sin(pos_yaw_angle + velocity_angle); + + // velocity covariance + // extract eigen values and eigen vectors + Eigen::Matrix2d eigen_twist_covariance; + eigen_twist_covariance << twist_with_covariance.covariance[0], + twist_with_covariance.covariance[1], twist_with_covariance.covariance[6], + twist_with_covariance.covariance[7]; + double phi, sigma1, sigma2; + calc_covariance_eigen_vectors(eigen_twist_covariance, sigma1, sigma2, phi); + phi = pos_yaw_angle + phi; + double area = sigma1 * sigma2; + double alpha = std::min(0.5, 1.0 / area); + alpha = std::max(0.1, alpha); + + // ellipse orientation + marker_ptr->pose.orientation.x = 0.0; + marker_ptr->pose.orientation.y = 0.0; + marker_ptr->pose.orientation.z = std::sin(phi * 0.5); + marker_ptr->pose.orientation.w = std::cos(phi * 0.5); + + // ellipse size + marker_ptr->scale.x = sigma1 * confidence_interval_coefficient; + marker_ptr->scale.y = sigma2 * confidence_interval_coefficient; + marker_ptr->scale.z = 0.05; + + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); + marker_ptr->color.a = alpha; + marker_ptr->color.r = 0.2; + marker_ptr->color.g = 0.4; + marker_ptr->color.b = 0.9; + + return marker_ptr; +} + +visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::LINE_STRIP; + marker_ptr->ns = std::string("yaw rate"); + marker_ptr->scale.x = line_width; + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = pose_with_covariance.pose; + + // yaw rate + const double yaw_rate = twist_with_covariance.twist.angular.z; + const double velocity = std::sqrt( + twist_with_covariance.twist.linear.x * twist_with_covariance.twist.linear.x + + twist_with_covariance.twist.linear.y * twist_with_covariance.twist.linear.y + + twist_with_covariance.twist.linear.z * twist_with_covariance.twist.linear.z); + const double velocity_angle = + std::atan2(twist_with_covariance.twist.linear.y, twist_with_covariance.twist.linear.x); + const double yaw_mark_length = velocity * 0.8; + + geometry_msgs::msg::Point point; + // first point + point.x = 0; + point.y = 0; + point.z = 0; + marker_ptr->points.push_back(point); + // yaw rate arc + calc_arc_line_strip( + velocity_angle, velocity_angle + yaw_rate, yaw_mark_length, marker_ptr->points); + // last point + point.x = 0; + point.y = 0; + point.z = 0; + marker_ptr->points.push_back(point); + + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); + marker_ptr->color.a = 0.9; + marker_ptr->color.r = 1.0; + marker_ptr->color.g = 0.0; + marker_ptr->color.b = 0.0; + + return marker_ptr; +} + +visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & confidence_interval_coefficient, const double & line_width) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + marker_ptr->ns = std::string("yaw rate covariance"); + marker_ptr->scale.x = line_width; + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = pose_with_covariance.pose; + + // yaw rate covariance + const double yaw_rate_covariance = twist_with_covariance.covariance[35]; + const double yaw_rate_sigma = std::sqrt(yaw_rate_covariance) * confidence_interval_coefficient; + const double yaw_rate = twist_with_covariance.twist.angular.z; + const double velocity = std::sqrt( + twist_with_covariance.twist.linear.x * twist_with_covariance.twist.linear.x + + twist_with_covariance.twist.linear.y * twist_with_covariance.twist.linear.y + + twist_with_covariance.twist.linear.z * twist_with_covariance.twist.linear.z); + const double velocity_angle = + std::atan2(twist_with_covariance.twist.linear.y, twist_with_covariance.twist.linear.x); + const double yaw_mark_length = velocity * 0.8; + const double bar_width = std::max(velocity * 0.05, 0.1); + const double velocity_yaw_angle = velocity_angle + yaw_rate; + const double velocity_yaw_p_sigma_angle = velocity_yaw_angle + yaw_rate_sigma; + const double velocity_yaw_n_sigma_angle = velocity_yaw_angle - yaw_rate_sigma; + + const double point_list[7][3] = { + {yaw_mark_length * std::cos(velocity_yaw_angle), yaw_mark_length * std::sin(velocity_yaw_angle), + 0}, + {yaw_mark_length * std::cos(velocity_yaw_p_sigma_angle), + yaw_mark_length * std::sin(velocity_yaw_p_sigma_angle), 0}, + {yaw_mark_length * std::cos(velocity_yaw_n_sigma_angle), + yaw_mark_length * std::sin(velocity_yaw_n_sigma_angle), 0}, + {(yaw_mark_length + bar_width) * std::cos(velocity_yaw_p_sigma_angle), + (yaw_mark_length + bar_width) * std::sin(velocity_yaw_p_sigma_angle), 0}, + {(yaw_mark_length - bar_width) * std::cos(velocity_yaw_p_sigma_angle), + (yaw_mark_length - bar_width) * std::sin(velocity_yaw_p_sigma_angle), 0}, + {(yaw_mark_length + bar_width) * std::cos(velocity_yaw_n_sigma_angle), + (yaw_mark_length + bar_width) * std::sin(velocity_yaw_n_sigma_angle), 0}, + {(yaw_mark_length - bar_width) * std::cos(velocity_yaw_n_sigma_angle), + (yaw_mark_length - bar_width) * std::sin(velocity_yaw_n_sigma_angle), 0}, + }; + const int point_pairs[4][2] = { + {0, 1}, + {0, 2}, + {3, 4}, + {5, 6}, + }; + calc_line_list_from_points(point_list, point_pairs, 4, marker_ptr->points); + + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); + marker_ptr->color.a = 0.9; + marker_ptr->color.r = 1.0; + marker_ptr->color.g = 0.2; + marker_ptr->color.b = 0.4; + + return marker_ptr; +} + visualization_msgs::msg::Marker::SharedPtr get_velocity_text_marker_ptr( const geometry_msgs::msg::Twist & twist, const geometry_msgs::msg::Point & vis_pos, const std_msgs::msg::ColorRGBA & color_rgba) @@ -137,7 +299,7 @@ visualization_msgs::msg::Marker::SharedPtr get_velocity_text_marker_ptr( marker_ptr->text = std::to_string(static_cast(vel * 3.6)) + std::string("[km/h]"); marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose.position = vis_pos; - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); marker_ptr->color = color_rgba; return marker_ptr; } @@ -158,52 +320,122 @@ visualization_msgs::msg::Marker::SharedPtr get_acceleration_text_marker_ptr( marker_ptr->text = getRoundedDoubleString(acc) + std::string("[m/s^2]"); marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose.position = vis_pos; - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); marker_ptr->color = color_rgba; return marker_ptr; } -visualization_msgs::msg::Marker::SharedPtr get_pose_with_covariance_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) +void calc_arc_line_strip( + const double start_angle, const double end_angle, const double radius, + std::vector & points) +{ + geometry_msgs::msg::Point point; + // arc points + const double maximum_delta_angle = 10.0 * M_PI / 180.0; + const int num_points = + std::max(3, static_cast(std::abs(end_angle - start_angle) / maximum_delta_angle)); + for (int i = 0; i < num_points; ++i) { + const double angle = start_angle + (end_angle - start_angle) * static_cast(i) / + static_cast(num_points - 1); + point.x = radius * std::cos(angle); + point.y = radius * std::sin(angle); + point.z = 0; + points.push_back(point); + } +} + +void calc_covariance_eigen_vectors( + const Eigen::Matrix2d & matrix, double & sigma1, double & sigma2, double & yaw) +{ + Eigen::SelfAdjointEigenSolver solver(matrix); + Eigen::Vector2d eigen_values = solver.eigenvalues(); + // eigen values + sigma1 = std::sqrt(eigen_values.x()); + sigma2 = std::sqrt(eigen_values.y()); + // orientation of covariance ellipse + Eigen::Vector2d e1 = solver.eigenvectors().col(0); + yaw = std::atan2(e1.y(), e1.x()); +} + +visualization_msgs::msg::Marker::SharedPtr get_pose_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const double & confidence_interval_coefficient) { auto marker_ptr = std::make_shared(); - marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + marker_ptr->type = visualization_msgs::msg::Marker::CYLINDER; marker_ptr->ns = std::string("position covariance"); - marker_ptr->scale.x = 0.03; marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose = pose_with_covariance.pose; + + // position covariance + // extract eigen values and eigen vectors + Eigen::Matrix2d eigen_pose_covariance; + eigen_pose_covariance << pose_with_covariance.covariance[0], pose_with_covariance.covariance[1], + pose_with_covariance.covariance[6], pose_with_covariance.covariance[7]; + double yaw, sigma1, sigma2; + calc_covariance_eigen_vectors(eigen_pose_covariance, sigma1, sigma2, yaw); + + // ellipse orientation marker_ptr->pose.orientation.x = 0.0; marker_ptr->pose.orientation.y = 0.0; - marker_ptr->pose.orientation.z = 0.0; - marker_ptr->pose.orientation.w = 1.0; + marker_ptr->pose.orientation.z = std::sin(yaw * 0.5); + marker_ptr->pose.orientation.w = std::cos(yaw * 0.5); + + // ellipse size + marker_ptr->scale.x = sigma1 * confidence_interval_coefficient; + marker_ptr->scale.y = sigma2 * confidence_interval_coefficient; + marker_ptr->scale.z = 0.05; + + // ellipse color density + double area = sigma1 * sigma2; + double alpha = std::min(0.5, 3.0 / area); + alpha = std::max(0.1, alpha); + + // marker configuration + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); + marker_ptr->color.a = alpha; + marker_ptr->color.r = 0.8; + marker_ptr->color.g = 0.8; + marker_ptr->color.b = 0.8; + return marker_ptr; +} + +visualization_msgs::msg::Marker::SharedPtr get_yaw_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & length, + const double & confidence_interval_coefficient, const double & line_width) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::LINE_STRIP; + marker_ptr->ns = std::string("yaw covariance"); + marker_ptr->scale.x = line_width; + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = pose_with_covariance.pose; geometry_msgs::msg::Point point; - Eigen::Matrix2d eigen_pose_with_covariance; - eigen_pose_with_covariance << pose_with_covariance.covariance[0], - pose_with_covariance.covariance[1], pose_with_covariance.covariance[6], - pose_with_covariance.covariance[7]; - Eigen::SelfAdjointEigenSolver solver(eigen_pose_with_covariance); - double sigma1 = 2.448 * std::sqrt(solver.eigenvalues().x()); // 2.448 sigma is 95% - double sigma2 = 2.448 * std::sqrt(solver.eigenvalues().y()); // 2.448 sigma is 95% - Eigen::Vector2d e1 = solver.eigenvectors().col(0); - Eigen::Vector2d e2 = solver.eigenvectors().col(1); - point.x = -e1.x() * sigma1; - point.y = -e1.y() * sigma1; - point.z = 0; - marker_ptr->points.push_back(point); - point.x = e1.x() * sigma1; - point.y = e1.y() * sigma1; - point.z = 0; - marker_ptr->points.push_back(point); - point.x = -e2.x() * sigma2; - point.y = -e2.y() * sigma2; + + // orientation covariance + double yaw_vector_length = std::max(length, 1.0); + double yaw_sigma = + std::sqrt(pose_with_covariance.covariance[35]) * confidence_interval_coefficient; + // get arc points + if (yaw_sigma > M_PI) { + yaw_vector_length = 1.0; + } + // first point + point.x = 0; + point.y = 0; point.z = 0; marker_ptr->points.push_back(point); - point.x = e2.x() * sigma2; - point.y = e2.y() * sigma2; + // arc points + calc_arc_line_strip(-yaw_sigma, yaw_sigma, yaw_vector_length, marker_ptr->points); + // last point + point.x = 0; + point.y = 0; point.z = 0; marker_ptr->points.push_back(point); - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); - marker_ptr->color.a = 0.999; + + // marker configuration + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); + marker_ptr->color.a = 0.9; marker_ptr->color.r = 1.0; marker_ptr->color.g = 1.0; marker_ptr->color.b = 1.0; @@ -237,7 +469,24 @@ visualization_msgs::msg::Marker::SharedPtr get_label_marker_ptr( marker_ptr->text = label; marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose = marker_ptr->pose = to_pose(centroid, orientation); - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); + marker_ptr->color = color_rgba; + return marker_ptr; +} + +visualization_msgs::msg::Marker::SharedPtr get_existence_probability_marker_ptr( + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const float existence_probability, const std_msgs::msg::ColorRGBA & color_rgba) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker_ptr->ns = std::string("existence probability"); + marker_ptr->scale.x = 0.5; + marker_ptr->scale.z = 0.5; + marker_ptr->text = std::to_string(existence_probability); + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = marker_ptr->pose = to_pose(centroid, orientation); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); marker_ptr->color = color_rgba; return marker_ptr; } @@ -245,7 +494,8 @@ visualization_msgs::msg::Marker::SharedPtr get_label_marker_ptr( visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width) + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, + const bool & is_orientation_available) { auto marker_ptr = std::make_shared(); marker_ptr->ns = std::string("shape"); @@ -254,6 +504,11 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( if (shape_msg.type == Shape::BOUNDING_BOX) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; calc_bounding_box_line_list(shape_msg, marker_ptr->points); + if (is_orientation_available) { + calc_bounding_box_direction_line_list(shape_msg, marker_ptr->points); + } else { + calc_bounding_box_orientation_line_list(shape_msg, marker_ptr->points); + } } else if (shape_msg.type == Shape::CYLINDER) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; calc_cylinder_line_list(shape_msg, marker_ptr->points); @@ -267,7 +522,7 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose = to_pose(centroid, orientation); - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); marker_ptr->scale.x = line_width; marker_ptr->color = color_rgba; @@ -277,7 +532,8 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width) + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, + const bool & is_orientation_available) { auto marker_ptr = std::make_shared(); marker_ptr->ns = std::string("shape"); @@ -286,6 +542,11 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( if (shape_msg.type == Shape::BOUNDING_BOX) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; calc_2d_bounding_box_bottom_line_list(shape_msg, marker_ptr->points); + if (is_orientation_available) { + calc_2d_bounding_box_bottom_direction_line_list(shape_msg, marker_ptr->points); + } else { + calc_2d_bounding_box_bottom_orientation_line_list(shape_msg, marker_ptr->points); + } } else if (shape_msg.type == Shape::CYLINDER) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; calc_2d_cylinder_bottom_line_list(shape_msg, marker_ptr->points); @@ -299,170 +560,177 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose = to_pose(centroid, orientation); - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); marker_ptr->scale.x = line_width; marker_ptr->color = color_rgba; return marker_ptr; } +void calc_line_list_from_points( + const double point_list[][3], const int point_pairs[][2], const int & num_pairs, + std::vector & points) +{ + geometry_msgs::msg::Point point; + for (int i = 0; i < num_pairs; ++i) { + point.x = point_list[point_pairs[i][0]][0]; + point.y = point_list[point_pairs[i][0]][1]; + point.z = point_list[point_pairs[i][0]][2]; + points.push_back(point); + point.x = point_list[point_pairs[i][1]][0]; + point.y = point_list[point_pairs[i][1]][1]; + point.z = point_list[point_pairs[i][1]][2]; + points.push_back(point); + } +} + void calc_bounding_box_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) { + const double length_half = shape.dimensions.x * 0.5; + const double width_half = shape.dimensions.y * 0.5; + const double height_half = shape.dimensions.z * 0.5; + geometry_msgs::msg::Point point; + + // bounding box corner points + // top and bottom surface, clockwise + const double point_list[8][3] = { + {length_half, width_half, height_half}, {length_half, -width_half, height_half}, + {-length_half, -width_half, height_half}, {-length_half, width_half, height_half}, + {length_half, width_half, -height_half}, {length_half, -width_half, -height_half}, + {-length_half, -width_half, -height_half}, {-length_half, width_half, -height_half}, + }; + const int point_pairs[12][2] = { + {0, 1}, {1, 2}, {2, 3}, {3, 0}, {4, 5}, {5, 6}, {6, 7}, {7, 4}, {0, 4}, {1, 5}, {2, 6}, {3, 7}, + }; + calc_line_list_from_points(point_list, point_pairs, 12, points); +} + +void calc_bounding_box_direction_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + // direction triangle + const double length_half = shape.dimensions.x * 0.5; + const double width_half = shape.dimensions.y * 0.5; + const double height_half = shape.dimensions.z * 0.5; + const double triangle_size_half = std::min(width_half * 1.4, shape.dimensions.x); + geometry_msgs::msg::Point point; + + // triangle-shaped direction indicator + const double point_list[6][3] = { + {length_half, 0, height_half}, + {length_half - triangle_size_half, width_half, height_half}, + {length_half - triangle_size_half, -width_half, height_half}, + {length_half, 0, -height_half}, + {length_half, width_half, height_half}, + {length_half, -width_half, height_half}, + }; + const int point_pairs[5][2] = { + {0, 1}, {1, 2}, {0, 2}, {3, 4}, {3, 5}, + }; + calc_line_list_from_points(point_list, point_pairs, 5, points); +} + +void calc_bounding_box_orientation_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + const double length_half = shape.dimensions.x * 0.5; + const double width_half = shape.dimensions.y * 0.5; + const double height_half = shape.dimensions.z * 0.5; + const double tick_width = width_half * 0.5; + const double tick_length = std::min(tick_width, length_half); geometry_msgs::msg::Point point; - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - // up surface - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - - // down surface - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); + + // front corner cuts for orientation + const double point_list[4][3] = { + {length_half, width_half - tick_width, height_half}, + {length_half - tick_length, width_half, height_half}, + {length_half, -width_half + tick_width, height_half}, + {length_half - tick_length, -width_half, height_half}, + }; + const int point_pairs[2][2] = { + {0, 1}, + {2, 3}, + }; + calc_line_list_from_points(point_list, point_pairs, 2, points); } void calc_2d_bounding_box_bottom_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) { + const double length_half = shape.dimensions.x * 0.5; + const double width_half = shape.dimensions.y * 0.5; + const double height_half = shape.dimensions.z * 0.5; + geometry_msgs::msg::Point point; + + // bounding box corner points + // top surface, clockwise + const double point_list[4][3] = { + {length_half, width_half, -height_half}, + {length_half, -width_half, -height_half}, + {-length_half, -width_half, -height_half}, + {-length_half, width_half, -height_half}, + }; + const int point_pairs[4][2] = { + {0, 1}, + {1, 2}, + {2, 3}, + {3, 0}, + }; + calc_line_list_from_points(point_list, point_pairs, 4, points); +} + +void calc_2d_bounding_box_bottom_direction_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + const double length_half = shape.dimensions.x * 0.5; + const double width_half = shape.dimensions.y * 0.5; + const double height_half = shape.dimensions.z * 0.5; + const double triangle_size_half = std::min(width_half * 1.4, shape.dimensions.x); geometry_msgs::msg::Point point; - // down surface - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); + + // triangle-shaped direction indicator + const double point_list[6][3] = { + {length_half, 0, -height_half}, + {length_half - triangle_size_half, width_half, -height_half}, + {length_half - triangle_size_half, -width_half, -height_half}, + }; + const int point_pairs[3][2] = { + {0, 1}, + {1, 2}, + {0, 2}, + }; + calc_line_list_from_points(point_list, point_pairs, 3, points); +} + +void calc_2d_bounding_box_bottom_orientation_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + const double length_half = shape.dimensions.x * 0.5; + const double width_half = shape.dimensions.y * 0.5; + const double height_half = shape.dimensions.z * 0.5; + const double tick_width = width_half * 0.5; + const double tick_length = std::min(tick_width, length_half); + geometry_msgs::msg::Point point; + + // front corner cuts for orientation + const double point_list[4][3] = { + {length_half, width_half - tick_width, height_half}, + {length_half - tick_length, width_half, height_half}, + {length_half, -width_half + tick_width, height_half}, + {length_half - tick_length, -width_half, height_half}, + }; + const int point_pairs[2][2] = { + {0, 1}, + {2, 3}, + }; + calc_line_list_from_points(point_list, point_pairs, 2, points); } void calc_cylinder_line_list( @@ -570,7 +838,7 @@ void calc_polygon_line_list( geometry_msgs::msg::Point point; point.x = shape.footprint.points.at(i).x; point.y = shape.footprint.points.at(i).y; - point.z = shape.dimensions.z / 2.0; + point.z = shape.dimensions.z * 0.5; points.push_back(point); point.x = shape.footprint.points .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) @@ -578,14 +846,14 @@ void calc_polygon_line_list( point.y = shape.footprint.points .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) .y; - point.z = shape.dimensions.z / 2.0; + point.z = shape.dimensions.z * 0.5; points.push_back(point); } for (size_t i = 0; i < shape.footprint.points.size(); ++i) { geometry_msgs::msg::Point point; point.x = shape.footprint.points.at(i).x; point.y = shape.footprint.points.at(i).y; - point.z = -shape.dimensions.z / 2.0; + point.z = -shape.dimensions.z * 0.5; points.push_back(point); point.x = shape.footprint.points .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) @@ -593,18 +861,18 @@ void calc_polygon_line_list( point.y = shape.footprint.points .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) .y; - point.z = -shape.dimensions.z / 2.0; + point.z = -shape.dimensions.z * 0.5; points.push_back(point); } for (size_t i = 0; i < shape.footprint.points.size(); ++i) { geometry_msgs::msg::Point point; point.x = shape.footprint.points.at(i).x; point.y = shape.footprint.points.at(i).y; - point.z = shape.dimensions.z / 2.0; + point.z = shape.dimensions.z * 0.5; points.push_back(point); point.x = shape.footprint.points.at(i).x; point.y = shape.footprint.points.at(i).y; - point.z = -shape.dimensions.z / 2.0; + point.z = -shape.dimensions.z * 0.5; points.push_back(point); } } @@ -623,7 +891,7 @@ void calc_2d_polygon_bottom_line_list( geometry_msgs::msg::Point point; point.x = shape.footprint.points.at(i).x; point.y = shape.footprint.points.at(i).y; - point.z = -shape.dimensions.z / 2.0; + point.z = -shape.dimensions.z * 0.5; points.push_back(point); point.x = shape.footprint.points .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) @@ -631,7 +899,7 @@ void calc_2d_polygon_bottom_line_list( point.y = shape.footprint.points .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) .y; - point.z = -shape.dimensions.z / 2.0; + point.z = -shape.dimensions.z * 0.5; points.push_back(point); } } diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp index 2cc5397d18721..b42ffe1804246 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp" #include #include @@ -77,12 +77,12 @@ std::vector PredictedObjectsDisplay: auto shape_marker = get_shape_marker_ptr( object.shape, object.kinematics.initial_pose_with_covariance.pose.position, object.kinematics.initial_pose_with_covariance.pose.orientation, object.classification, - get_line_width()); + get_line_width(), true); if (shape_marker) { - auto shape_marker_ptr = shape_marker.value(); - shape_marker_ptr->header = msg->header; - shape_marker_ptr->id = uuid_to_marker_id(object.object_id); - markers.push_back(shape_marker_ptr); + auto marker_ptr = shape_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(marker_ptr); } // Get marker for label @@ -90,10 +90,10 @@ std::vector PredictedObjectsDisplay: object.kinematics.initial_pose_with_covariance.pose.position, object.kinematics.initial_pose_with_covariance.pose.orientation, object.classification); if (label_marker) { - auto label_marker_ptr = label_marker.value(); - label_marker_ptr->header = msg->header; - label_marker_ptr->id = uuid_to_marker_id(object.object_id); - markers.push_back(label_marker_ptr); + auto marker_ptr = label_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(marker_ptr); } // Get marker for id @@ -111,16 +111,46 @@ std::vector PredictedObjectsDisplay: markers.push_back(id_marker_ptr); } - // Get marker for pose with covariance + // Get marker for pose covariance auto pose_with_covariance_marker = - get_pose_with_covariance_marker_ptr(object.kinematics.initial_pose_with_covariance); + get_pose_covariance_marker_ptr(object.kinematics.initial_pose_with_covariance); if (pose_with_covariance_marker) { - auto pose_with_covariance_marker_ptr = pose_with_covariance_marker.value(); - pose_with_covariance_marker_ptr->header = msg->header; - pose_with_covariance_marker_ptr->id = uuid_to_marker_id(object.object_id); - markers.push_back(pose_with_covariance_marker_ptr); + auto marker_ptr = pose_with_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(marker_ptr); } + // Get marker for yaw covariance + auto yaw_covariance_marker = get_yaw_covariance_marker_ptr( + object.kinematics.initial_pose_with_covariance, object.shape.dimensions.x * 0.65, + get_line_width() * 0.5); + if (yaw_covariance_marker) { + auto marker_ptr = yaw_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(marker_ptr); + } + + // Get marker for existence probability + geometry_msgs::msg::Point existence_probability_position; + existence_probability_position.x = + object.kinematics.initial_pose_with_covariance.pose.position.x + 0.5; + existence_probability_position.y = + object.kinematics.initial_pose_with_covariance.pose.position.y; + existence_probability_position.z = + object.kinematics.initial_pose_with_covariance.pose.position.z + 0.5; + const float existence_probability = object.existence_probability; + auto existence_prob_marker = get_existence_probability_marker_ptr( + existence_probability_position, + object.kinematics.initial_pose_with_covariance.pose.orientation, existence_probability, + object.classification); + if (existence_prob_marker) { + auto existence_prob_marker_ptr = existence_prob_marker.value(); + existence_prob_marker_ptr->header = msg->header; + existence_prob_marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(existence_prob_marker_ptr); + } // Get marker for velocity text geometry_msgs::msg::Point vel_vis_position; vel_vis_position.x = uuid_vis_position.x - 0.5; @@ -154,7 +184,7 @@ std::vector PredictedObjectsDisplay: // Get marker for twist auto twist_marker = get_twist_marker_ptr( object.kinematics.initial_pose_with_covariance, - object.kinematics.initial_twist_with_covariance); + object.kinematics.initial_twist_with_covariance, get_line_width()); if (twist_marker) { auto twist_marker_ptr = twist_marker.value(); twist_marker_ptr->header = msg->header; @@ -162,6 +192,39 @@ std::vector PredictedObjectsDisplay: markers.push_back(twist_marker_ptr); } + // Get marker for twist covariance + auto twist_covariance_marker = get_twist_covariance_marker_ptr( + object.kinematics.initial_pose_with_covariance, + object.kinematics.initial_twist_with_covariance); + if (twist_covariance_marker) { + auto marker_ptr = twist_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(marker_ptr); + } + + // Get marker for yaw rate + auto yaw_rate_marker = get_yaw_rate_marker_ptr( + object.kinematics.initial_pose_with_covariance, + object.kinematics.initial_twist_with_covariance, get_line_width() * 0.4); + if (yaw_rate_marker) { + auto marker_ptr = yaw_rate_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(marker_ptr); + } + + // Get marker for twist covariance + auto yaw_rate_covariance_marker = get_yaw_rate_covariance_marker_ptr( + object.kinematics.initial_pose_with_covariance, + object.kinematics.initial_twist_with_covariance, get_line_width() * 0.3); + if (yaw_rate_covariance_marker) { + auto marker_ptr = yaw_rate_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(marker_ptr); + } + // Add marker for each candidate path int32_t path_count = 0; for (const auto & predicted_path : object.kinematics.predicted_paths) { diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp index 00a1199c697ce..504b51f850444 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp" #include @@ -26,6 +26,24 @@ namespace object_detection { TrackedObjectsDisplay::TrackedObjectsDisplay() : ObjectPolygonDisplayBase("tracks") { + // Option for selective visualization by object dynamics + m_select_object_dynamics_property = new rviz_common::properties::EnumProperty( + "Dynamic Status", "All", "Selectively visualize objects by its dynamic status.", this); + m_select_object_dynamics_property->addOption("Dynamic", 0); + m_select_object_dynamics_property->addOption("Static", 1); + m_select_object_dynamics_property->addOption("All", 2); +} + +bool TrackedObjectsDisplay::is_object_to_show( + const uint showing_dynamic_status, const TrackedObject & object) +{ + if (showing_dynamic_status == 0 && object.kinematics.is_stationary) { + return false; // Show only moving objects + } + if (showing_dynamic_status == 1 && !object.kinematics.is_stationary) { + return false; // Show only stationary objects + } + return true; } void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) @@ -33,12 +51,17 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) clear_markers(); update_id_map(msg); + const auto showing_dynamic_status = get_object_dynamics_to_visualize(); for (const auto & object : msg->objects) { + // Filter by object dynamic status + if (!is_object_to_show(showing_dynamic_status, object)) continue; // Get marker for shape auto shape_marker = get_shape_marker_ptr( object.shape, object.kinematics.pose_with_covariance.pose.position, object.kinematics.pose_with_covariance.pose.orientation, object.classification, - get_line_width()); + get_line_width(), + object.kinematics.orientation_availability == + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE); if (shape_marker) { auto shape_marker_ptr = shape_marker.value(); shape_marker_ptr->header = msg->header; @@ -72,16 +95,42 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) add_marker(id_marker_ptr); } - // Get marker for pose with covariance + // Get marker for pose covariance auto pose_with_covariance_marker = - get_pose_with_covariance_marker_ptr(object.kinematics.pose_with_covariance); + get_pose_covariance_marker_ptr(object.kinematics.pose_with_covariance); if (pose_with_covariance_marker) { - auto pose_with_covariance_marker_ptr = pose_with_covariance_marker.value(); - pose_with_covariance_marker_ptr->header = msg->header; - pose_with_covariance_marker_ptr->id = uuid_to_marker_id(object.object_id); - add_marker(pose_with_covariance_marker_ptr); + auto marker_ptr = pose_with_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(marker_ptr); + } + + // Get marker for yaw covariance + auto yaw_covariance_marker = get_yaw_covariance_marker_ptr( + object.kinematics.pose_with_covariance, object.shape.dimensions.x * 0.65, + get_line_width() * 0.5); + if (yaw_covariance_marker) { + auto marker_ptr = yaw_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(marker_ptr); } + // Get marker for existence probability + geometry_msgs::msg::Point existence_probability_position; + existence_probability_position.x = object.kinematics.pose_with_covariance.pose.position.x + 0.5; + existence_probability_position.y = object.kinematics.pose_with_covariance.pose.position.y; + existence_probability_position.z = object.kinematics.pose_with_covariance.pose.position.z + 0.5; + const float existence_probability = object.existence_probability; + auto existence_prob_marker = get_existence_probability_marker_ptr( + existence_probability_position, object.kinematics.pose_with_covariance.pose.orientation, + existence_probability, object.classification); + if (existence_prob_marker) { + auto existence_prob_marker_ptr = existence_prob_marker.value(); + existence_prob_marker_ptr->header = msg->header; + existence_prob_marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(existence_prob_marker_ptr); + } // Get marker for velocity text geometry_msgs::msg::Point vel_vis_position; vel_vis_position.x = uuid_vis_position.x - 0.5; @@ -113,13 +162,46 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) // Get marker for twist auto twist_marker = get_twist_marker_ptr( - object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance); + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, + get_line_width()); if (twist_marker) { auto twist_marker_ptr = twist_marker.value(); twist_marker_ptr->header = msg->header; twist_marker_ptr->id = uuid_to_marker_id(object.object_id); add_marker(twist_marker_ptr); } + + // Get marker for twist covariance + auto twist_covariance_marker = get_twist_covariance_marker_ptr( + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance); + if (twist_covariance_marker) { + auto marker_ptr = twist_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(marker_ptr); + } + + // Get marker for yaw rate + auto yaw_rate_marker = get_yaw_rate_marker_ptr( + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, + get_line_width() * 0.4); + if (yaw_rate_marker) { + auto marker_ptr = yaw_rate_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(marker_ptr); + } + + // Get marker for twist covariance + auto yaw_rate_covariance_marker = get_yaw_rate_covariance_marker_ptr( + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, + get_line_width() * 0.5); + if (yaw_rate_covariance_marker) { + auto marker_ptr = yaw_rate_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(marker_ptr); + } } } diff --git a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp index b35eb6e93ce6e..50c16ba8eaf7d 100644 --- a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp +++ b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp @@ -17,7 +17,7 @@ #ifndef AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_ #define AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_ -#include +#include #include #include diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_msgs/CHANGELOG.rst b/common/autoware_overlay_rviz_plugin/autoware_overlay_msgs/CHANGELOG.rst new file mode 100644 index 0000000000000..e7672ee001955 --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_msgs/CHANGELOG.rst @@ -0,0 +1,24 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rviz_2d_overlay_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.3.0 (2023-05-18) +------------------ +* Removed old position message fields +* Contributors: Dominik, Jonas Otto + +1.2.1 (2022-09-30) +------------------ + +1.2.0 (2022-09-27) +------------------ +* Rename package from overlay_rviz_msgs to rviz_2d_overlay_msgs +* Contributors: Jonas Otto + +1.1.0 (2022-09-11) +------------------ + +1.0.0 (2022-08-30) +------------------ +* Create OverlayText message (currently same as jsk_rviz_plugins) +* Contributors: Jonas Otto, Dominik Authaler diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_msgs/CMakeLists.txt b/common/autoware_overlay_rviz_plugin/autoware_overlay_msgs/CMakeLists.txt new file mode 100644 index 0000000000000..e23a4e755cbc4 --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_msgs/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.5) +project(autoware_overlay_msgs) + +if (NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif () + + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/OverlayText.msg" + DEPENDENCIES + std_msgs +) + +ament_package() diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_msgs/msg/OverlayText.msg b/common/autoware_overlay_rviz_plugin/autoware_overlay_msgs/msg/OverlayText.msg new file mode 100644 index 0000000000000..db3cf628b895b --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_msgs/msg/OverlayText.msg @@ -0,0 +1,31 @@ +uint8 ADD = 0 +uint8 DELETE = 1 + +# constants for the horizontal and vertical alignment +uint8 LEFT = 0 +uint8 RIGHT = 1 +uint8 CENTER = 2 +uint8 TOP = 3 +uint8 BOTTOM = 4 + +uint8 action + +int32 width +int32 height +# Position: Positive values move the overlay towards the center of the window, +# for center alignment positive values move the overlay towards the bottom right +int32 horizontal_distance # Horizontal distance from left/right border or center, depending on alignment +int32 vertical_distance # Vertical distance between from top/bottom border or center, depending on alignment + +# Alignment of the overlay withing RVIZ +uint8 horizontal_alignment # one of LEFT, CENTER, RIGHT +uint8 vertical_alignment # one of TOP, CENTER, BOTTOM + +std_msgs/ColorRGBA bg_color + +int32 line_width +float32 text_size +string font +std_msgs/ColorRGBA fg_color + +string text diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_msgs/package.xml b/common/autoware_overlay_rviz_plugin/autoware_overlay_msgs/package.xml new file mode 100644 index 0000000000000..4881b126ffffb --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_msgs/package.xml @@ -0,0 +1,24 @@ + + + + autoware_overlay_msgs + 1.3.0 + Messages describing 2D overlays for RVIZ, extracted/derived from the jsk_visualization package. + Team Spatzenhirn + BSD-3-Clause + + ament_cmake + rosidl_default_generators + + autoware_auto_vehicle_msgs + autoware_perception_msgs + std_msgs + unique_identifier_msgs + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..afb12bffeeaa7 --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt @@ -0,0 +1,140 @@ +cmake_minimum_required(VERSION 3.8) +project(autoware_overlay_rviz_plugin) + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(autoware_auto_vehicle_msgs REQUIRED) +find_package(tier4_planning_msgs REQUIRED) +find_package(autoware_perception_msgs REQUIRED) +ament_auto_find_build_dependencies() + +find_package(autoware_overlay_msgs REQUIRED) + +find_package(rviz_common REQUIRED) +find_package(rviz_rendering REQUIRED) +find_package(rviz_ogre_vendor REQUIRED) +find_package(std_msgs REQUIRED) + +set( + headers_to_moc + include/overlay_text_display.hpp + include/signal_display.hpp + +) + +set( + headers_to_not_moc + include/steering_wheel_display.hpp + include/gear_display.hpp + include/speed_display.hpp + include/turn_signals_display.hpp + include/traffic_display.hpp + include/speed_limit_display.hpp +) + + + +foreach(header "${headers_to_moc}") + qt5_wrap_cpp(display_moc_files "${header}") +endforeach() + +set( + display_source_files + src/overlay_text_display.cpp + src/overlay_utils.cpp + src/signal_display.cpp + src/steering_wheel_display.cpp + src/gear_display.cpp + src/speed_display.cpp + src/turn_signals_display.cpp + src/traffic_display.cpp + src/speed_limit_display.cpp + +) + +add_library( + ${PROJECT_NAME} SHARED + ${display_moc_files} + ${headers_to_not_moc} + ${display_source_files} +) + +set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 17) +target_compile_options(${PROJECT_NAME} PRIVATE -Wall -Wextra -Wpedantic) + +target_include_directories( + ${PROJECT_NAME} PUBLIC + $ + $ + ${Qt5Widgets_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} +) + +target_link_libraries( + ${PROJECT_NAME} PUBLIC + rviz_ogre_vendor::OgreMain + rviz_ogre_vendor::OgreOverlay +) + + +target_compile_definitions(${PROJECT_NAME} PRIVATE "${PROJECT_NAME}_BUILDING_LIBRARY") + +# prevent pluginlib from using boost +target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file(rviz_common plugins_description.xml) + +ament_target_dependencies( + ${PROJECT_NAME} + PUBLIC + rviz_common + rviz_rendering + autoware_overlay_msgs + autoware_auto_vehicle_msgs + tier4_planning_msgs + autoware_perception_msgs +) + +ament_export_include_directories(include) +ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + rviz_common + rviz_ogre_vendor +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS + DESTINATION lib/${PROJECT_NAME} +) + +# Copy the assets directory to the installation directory +install( + DIRECTORY assets/ + DESTINATION share/${PROJECT_NAME}/assets +) + +add_definitions(-DQT_NO_KEYWORDS) + +ament_package( + CONFIG_EXTRAS "autoware_overlay_rviz_plugin-extras.cmake" +) diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/LICENSE b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/LICENSE new file mode 100644 index 0000000000000..a2fe2d3d1c7ed --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/LICENSE @@ -0,0 +1,12 @@ +Copyright (c) 2022, Team Spatzenhirn +Copyright (c) 2014, JSK Lab + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/README.md b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/README.md new file mode 100644 index 0000000000000..0d0def1a46997 --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/README.md @@ -0,0 +1,54 @@ +# autoware_overlay_rviz_plugin + +Plugin for displaying 2D overlays over the RViz2 3D scene. + +Based on the [jsk_visualization](https://github.com/jsk-ros-pkg/jsk_visualization) +package, under the 3-Clause BSD license. + +## Purpose + +This plugin provides a visual and easy-to-understand display of vehicle speed, turn signal, steering status and gears. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------------------------------------- | ------------------------------------------------------- | ------------------------------------ | +| `/vehicle/status/velocity_status` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | The topic is vehicle velocity | +| `/vehicle/status/turn_indicators_status` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport` | The topic is status of turn signal | +| `/vehicle/status/hazard_status` | `autoware_auto_vehicle_msgs::msg::HazardReport` | The topic is status of hazard | +| `/vehicle/status/steering_status` | `autoware_auto_vehicle_msgs::msg::SteeringReport` | The topic is status of steering | +| `/vehicle/status/gear_status` | `autoware_auto_vehicle_msgs::msg::GearReport` | The topic is status of gear | +| `/planning/scenario_planning/current_max_velocity` | `tier4_planning_msgs::msg::VelocityLimit` | The topic is velocity limit | +| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficSignalArray` | The topic is status of traffic light | + +## Parameter + +### Core Parameters + +#### SignalDisplay + +| Name | Type | Default Value | Description | +| ------------------------ | ------ | -------------------- | --------------------------------- | +| `property_width_` | int | 128 | Width of the plotter window [px] | +| `property_height_` | int | 128 | Height of the plotter window [px] | +| `property_left_` | int | 128 | Left of the plotter window [px] | +| `property_top_` | int | 128 | Top of the plotter window [px] | +| `property_signal_color_` | QColor | QColor(25, 255, 240) | Turn Signal color | + +## Assumptions / Known limits + +TBD. + +## Usage + +1. Start `rviz2` and click `Add` button under the `Displays` panel. + + ![select_add](./assets/images/select_add.png) + +2. Under `By display type` tab, select `autoware_overlay_rviz_plugin/SignalDisplay` and press OK. + +3. Enter the names of the topics if necessary. + + ![select_topic_name](./assets/images/select_topic_name.png) diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/LICENSE b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/LICENSE new file mode 100644 index 0000000000000..cc04d393573f0 --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/LICENSE @@ -0,0 +1,93 @@ +Copyright 2011 The Quicksand Project Authors (https://github.com/andrew-paglinawan/QuicksandFamily), with Reserved Font Name “Quicksand”. + +This Font Software is licensed under the SIL Open Font License, Version 1.1. +This license is copied below, and is also available with a FAQ at: +http://scripts.sil.org/OFL + + +----------------------------------------------------------- +SIL OPEN FONT LICENSE Version 1.1 - 26 February 2007 +----------------------------------------------------------- + +PREAMBLE +The goals of the Open Font License (OFL) are to stimulate worldwide +development of collaborative font projects, to support the font creation +efforts of academic and linguistic communities, and to provide a free and +open framework in which fonts may be shared and improved in partnership +with others. + +The OFL allows the licensed fonts to be used, studied, modified and +redistributed freely as long as they are not sold by themselves. The +fonts, including any derivative works, can be bundled, embedded, +redistributed and/or sold with any software provided that any reserved +names are not used by derivative works. The fonts and derivatives, +however, cannot be released under any other type of license. The +requirement for fonts to remain under this license does not apply +to any document created using the fonts or their derivatives. + +DEFINITIONS +"Font Software" refers to the set of files released by the Copyright +Holder(s) under this license and clearly marked as such. This may +include source files, build scripts and documentation. + +"Reserved Font Name" refers to any names specified as such after the +copyright statement(s). + +"Original Version" refers to the collection of Font Software components as +distributed by the Copyright Holder(s). + +"Modified Version" refers to any derivative made by adding to, deleting, +or substituting -- in part or in whole -- any of the components of the +Original Version, by changing formats or by porting the Font Software to a +new environment. + +"Author" refers to any designer, engineer, programmer, technical +writer or other person who contributed to the Font Software. + +PERMISSION & CONDITIONS +Permission is hereby granted, free of charge, to any person obtaining +a copy of the Font Software, to use, study, copy, merge, embed, modify, +redistribute, and sell modified and unmodified copies of the Font +Software, subject to the following conditions: + +1) Neither the Font Software nor any of its individual components, +in Original or Modified Versions, may be sold by itself. + +2) Original or Modified Versions of the Font Software may be bundled, +redistributed and/or sold with any software, provided that each copy +contains the above copyright notice and this license. These can be +included either as stand-alone text files, human-readable headers or +in the appropriate machine-readable metadata fields within text or +binary files as long as those fields can be easily viewed by the user. + +3) No Modified Version of the Font Software may use the Reserved Font +Name(s) unless explicit written permission is granted by the corresponding +Copyright Holder. This restriction only applies to the primary font name as +presented to the users. + +4) The name(s) of the Copyright Holder(s) or the Author(s) of the Font +Software shall not be used to promote, endorse or advertise any +Modified Version, except to acknowledge the contribution(s) of the +Copyright Holder(s) and the Author(s) or with their explicit written +permission. + +5) The Font Software, modified or unmodified, in part or in whole, +must be distributed entirely under this license, and must not be +distributed under any other license. The requirement for fonts to +remain under this license does not apply to any document created +using the Font Software. + +TERMINATION +This license becomes null and void if any of the above conditions are +not met. + +DISCLAIMER +THE FONT SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, +EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO ANY WARRANTIES OF +MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT +OF COPYRIGHT, PATENT, TRADEMARK, OR OTHER RIGHT. IN NO EVENT SHALL THE +COPYRIGHT HOLDER BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, +INCLUDING ANY GENERAL, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL +DAMAGES, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +FROM, OUT OF THE USE OR INABILITY TO USE THE FONT SOFTWARE OR FROM +OTHER DEALINGS IN THE FONT SOFTWARE. diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Bold.ttf b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Bold.ttf new file mode 100644 index 0000000000000..07d5127c04b17 Binary files /dev/null and b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Bold.ttf differ diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Light.ttf b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Light.ttf new file mode 100644 index 0000000000000..800531084fa6c Binary files /dev/null and b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Light.ttf differ diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Medium.ttf b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Medium.ttf new file mode 100644 index 0000000000000..f4634cd7c3f16 Binary files /dev/null and b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Medium.ttf differ diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Regular.ttf b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Regular.ttf new file mode 100644 index 0000000000000..60323ed6abdf1 Binary files /dev/null and b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Regular.ttf differ diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-SemiBold.ttf b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-SemiBold.ttf new file mode 100644 index 0000000000000..52059c3a3da3f Binary files /dev/null and b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-SemiBold.ttf differ diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/arrow.png b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/arrow.png new file mode 100644 index 0000000000000..18de535ce4489 Binary files /dev/null and b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/arrow.png differ diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/select_add.png b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/select_add.png new file mode 100644 index 0000000000000..c5130b6092384 Binary files /dev/null and b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/select_add.png differ diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/select_topic_name.png b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/select_topic_name.png new file mode 100644 index 0000000000000..490552efc114f Binary files /dev/null and b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/select_topic_name.png differ diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/traffic.png b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/traffic.png new file mode 100644 index 0000000000000..79715b0573e1a Binary files /dev/null and b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/traffic.png differ diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/wheel.png b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/wheel.png new file mode 100644 index 0000000000000..07b1e33132e43 Binary files /dev/null and b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/wheel.png differ diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin-extras.cmake b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin-extras.cmake new file mode 100644 index 0000000000000..9d6f156555e57 --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin-extras.cmake @@ -0,0 +1,31 @@ +# Copyright (c) 2021, Open Source Robotics Foundation, Inc. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# find package Qt5 because otherwise using the rviz_default_plugins::rviz_default_plugins +# exported target will complain that the Qt5::Widgets target does not exist +find_package(Qt5 REQUIRED QUIET COMPONENTS Widgets) diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp new file mode 100644 index 0000000000000..3fe473d5d5123 --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp @@ -0,0 +1,49 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GEAR_DISPLAY_HPP_ +#define GEAR_DISPLAY_HPP_ +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include "autoware_auto_vehicle_msgs/msg/gear_report.hpp" + +#include +#include +#include + +namespace autoware_overlay_rviz_plugin +{ + +class GearDisplay +{ +public: + GearDisplay(); + void drawGearIndicator(QPainter & painter, const QRectF & backgroundRect); + void updateGearData(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg); + +private: + int current_gear_; // Internal variable to store current gear + QColor gray = QColor(194, 194, 194); +}; + +} // namespace autoware_overlay_rviz_plugin + +#endif // GEAR_DISPLAY_HPP_ diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/overlay_text_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/overlay_text_display.hpp new file mode 100644 index 0000000000000..ccea51b9a5349 --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/overlay_text_display.hpp @@ -0,0 +1,157 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// -*- mode: c++; -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Team Spatzenhirn + * Copyright (c) 2014, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ +#ifndef OVERLAY_TEXT_DISPLAY_HPP_ +#define OVERLAY_TEXT_DISPLAY_HPP_ + +#include "autoware_overlay_msgs/msg/overlay_text.hpp" +#ifndef Q_MOC_RUN +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#endif + +namespace autoware_overlay_rviz_plugin +{ +class OverlayTextDisplay +: public rviz_common::RosTopicDisplay +{ + Q_OBJECT +public: + OverlayTextDisplay(); + virtual ~OverlayTextDisplay(); + +protected: + autoware_overlay_rviz_plugin::OverlayObject::SharedPtr overlay_; + + int texture_width_; + int texture_height_; + + bool overtake_fg_color_properties_; + bool overtake_bg_color_properties_; + bool overtake_position_properties_; + bool align_bottom_; + bool invert_shadow_; + QColor bg_color_; + QColor fg_color_; + int text_size_; + int line_width_; + std::string text_; + QStringList font_families_; + std::string font_; + int horizontal_dist_; + int vertical_dist_; + HorizontalAlignment horizontal_alignment_; + VerticalAlignment vertical_alignment_; + + void onInitialize() override; + void onEnable() override; + void onDisable() override; + void update(float wall_dt, float ros_dt) override; + void reset() override; + + bool require_update_texture_; + // properties are raw pointers since they are owned by Qt + rviz_common::properties::BoolProperty * overtake_position_properties_property_; + rviz_common::properties::BoolProperty * overtake_fg_color_properties_property_; + rviz_common::properties::BoolProperty * overtake_bg_color_properties_property_; + rviz_common::properties::BoolProperty * align_bottom_property_; + rviz_common::properties::BoolProperty * invert_shadow_property_; + rviz_common::properties::IntProperty * hor_dist_property_; + rviz_common::properties::IntProperty * ver_dist_property_; + rviz_common::properties::EnumProperty * hor_alignment_property_; + rviz_common::properties::EnumProperty * ver_alignment_property_; + rviz_common::properties::IntProperty * width_property_; + rviz_common::properties::IntProperty * height_property_; + rviz_common::properties::IntProperty * text_size_property_; + rviz_common::properties::IntProperty * line_width_property_; + rviz_common::properties::ColorProperty * bg_color_property_; + rviz_common::properties::FloatProperty * bg_alpha_property_; + rviz_common::properties::ColorProperty * fg_color_property_; + rviz_common::properties::FloatProperty * fg_alpha_property_; + rviz_common::properties::EnumProperty * font_property_; + +protected Q_SLOTS: + void updateOvertakePositionProperties(); + void updateOvertakeFGColorProperties(); + void updateOvertakeBGColorProperties(); + void updateAlignBottom(); + void updateInvertShadow(); + void updateHorizontalDistance(); + void updateVerticalDistance(); + void updateHorizontalAlignment(); + void updateVerticalAlignment(); + void updateWidth(); + void updateHeight(); + void updateTextSize(); + void updateFGColor(); + void updateFGAlpha(); + void updateBGColor(); + void updateBGAlpha(); + void updateFont(); + void updateLineWidth(); + +private: + void processMessage(autoware_overlay_msgs::msg::OverlayText::ConstSharedPtr msg) override; +}; +} // namespace autoware_overlay_rviz_plugin + +#endif // OVERLAY_TEXT_DISPLAY_HPP_ diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/overlay_utils.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/overlay_utils.hpp new file mode 100644 index 0000000000000..8581628ef0bce --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/overlay_utils.hpp @@ -0,0 +1,141 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Team Spatzenhirn + * Copyright (c) 2014, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef OVERLAY_UTILS_HPP_ +#define OVERLAY_UTILS_HPP_ + +#include +#include + +#include "autoware_overlay_msgs/msg/overlay_text.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace autoware_overlay_rviz_plugin +{ +class OverlayObject; + +class ScopedPixelBuffer +{ +public: + explicit ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer); + virtual ~ScopedPixelBuffer(); + virtual Ogre::HardwarePixelBufferSharedPtr getPixelBuffer(); + virtual QImage getQImage(unsigned int width, unsigned int height); + virtual QImage getQImage(OverlayObject & overlay); + virtual QImage getQImage(unsigned int width, unsigned int height, QColor & bg_color); + virtual QImage getQImage(OverlayObject & overlay, QColor & bg_color); + +protected: + Ogre::HardwarePixelBufferSharedPtr pixel_buffer_; +}; + +enum class VerticalAlignment : uint8_t { + CENTER = autoware_overlay_msgs::msg::OverlayText::CENTER, + TOP = autoware_overlay_msgs::msg::OverlayText::TOP, + BOTTOM = autoware_overlay_msgs::msg::OverlayText::BOTTOM, +}; + +enum class HorizontalAlignment : uint8_t { + LEFT = autoware_overlay_msgs::msg::OverlayText::LEFT, + RIGHT = autoware_overlay_msgs::msg::OverlayText::RIGHT, + CENTER = autoware_overlay_msgs::msg::OverlayText::CENTER +}; + +/** + * Helper class for realizing an overlay object on top of the rviz 3D panel. + * + * This class is supposed to be instantiated in the onInitialize method of the + * rviz_common::Display class. + */ +class OverlayObject +{ +public: + using SharedPtr = std::shared_ptr; + + explicit OverlayObject(const std::string & name); + virtual ~OverlayObject(); + + virtual std::string getName() const; + virtual void hide(); + virtual void show(); + virtual bool isTextureReady() const; + virtual void updateTextureSize(unsigned int width, unsigned int height); + virtual ScopedPixelBuffer getBuffer(); + virtual void setPosition( + double hor_dist, double ver_dist, HorizontalAlignment hor_alignment = HorizontalAlignment::LEFT, + VerticalAlignment ver_alignment = VerticalAlignment::TOP); + virtual void setDimensions(double width, double height); + virtual bool isVisible() const; + virtual unsigned int getTextureWidth() const; + virtual unsigned int getTextureHeight() const; + +protected: + const std::string name_; + Ogre::Overlay * overlay_; + Ogre::PanelOverlayElement * panel_; + Ogre::MaterialPtr panel_material_; + Ogre::TexturePtr texture_; +}; +} // namespace autoware_overlay_rviz_plugin + +#endif // OVERLAY_UTILS_HPP_ diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp new file mode 100644 index 0000000000000..4fa39f2c5a903 --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp @@ -0,0 +1,125 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SIGNAL_DISPLAY_HPP_ +#define SIGNAL_DISPLAY_HPP_ +#ifndef Q_MOC_RUN +#include "gear_display.hpp" +#include "overlay_utils.hpp" +#include "speed_display.hpp" +#include "speed_limit_display.hpp" +#include "steering_wheel_display.hpp" +#include "traffic_display.hpp" +#include "turn_signals_display.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#endif + +namespace autoware_overlay_rviz_plugin +{ +class SignalDisplay : public rviz_common::Display +{ + Q_OBJECT +public: + SignalDisplay(); + ~SignalDisplay() override; + +protected: + void onInitialize() override; + void update(float wall_dt, float ros_dt) override; + void reset() override; + void onEnable() override; + void onDisable() override; + +private Q_SLOTS: + void updateOverlaySize(); + void updateSmallOverlaySize(); + void updateOverlayPosition(); + void updateOverlayColor(); + void topic_updated_gear(); + void topic_updated_steering(); + void topic_updated_speed(); + void topic_updated_speed_limit(); + void topic_updated_turn_signals(); + void topic_updated_hazard_lights(); + void topic_updated_traffic(); + +private: + std::mutex mutex_; + autoware_overlay_rviz_plugin::OverlayObject::SharedPtr overlay_; + rviz_common::properties::IntProperty * property_width_; + rviz_common::properties::IntProperty * property_height_; + rviz_common::properties::IntProperty * property_left_; + rviz_common::properties::IntProperty * property_top_; + rviz_common::properties::ColorProperty * property_signal_color_; + std::unique_ptr steering_topic_property_; + std::unique_ptr gear_topic_property_; + std::unique_ptr speed_topic_property_; + std::unique_ptr turn_signals_topic_property_; + std::unique_ptr hazard_lights_topic_property_; + std::unique_ptr traffic_topic_property_; + std::unique_ptr speed_limit_topic_property_; + + void drawHorizontalRoundedRectangle(QPainter & painter, const QRectF & backgroundRect); + void drawVerticalRoundedRectangle(QPainter & painter, const QRectF & backgroundRect); + void setupRosSubscriptions(); + + std::unique_ptr steering_wheel_display_; + std::unique_ptr gear_display_; + std::unique_ptr speed_display_; + std::unique_ptr turn_signals_display_; + std::unique_ptr traffic_display_; + std::unique_ptr speed_limit_display_; + + rclcpp::Subscription::SharedPtr gear_sub_; + rclcpp::Subscription::SharedPtr steering_sub_; + rclcpp::Subscription::SharedPtr speed_sub_; + rclcpp::Subscription::SharedPtr + turn_signals_sub_; + rclcpp::Subscription::SharedPtr + hazard_lights_sub_; + rclcpp::Subscription::SharedPtr traffic_sub_; + rclcpp::Subscription::SharedPtr speed_limit_sub_; + + std::mutex property_mutex_; + + void updateGearData(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg); + void updateSteeringData( + const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg); + void updateSpeedData(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); + void updateTurnSignalsData( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg); + void updateHazardLightsData( + const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); + void updateSpeedLimitData(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); + void updateTrafficLightData( + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); + void drawWidget(QImage & hud); +}; +} // namespace autoware_overlay_rviz_plugin + +#endif // SIGNAL_DISPLAY_HPP_ diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp new file mode 100644 index 0000000000000..0669028dba3b2 --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp @@ -0,0 +1,49 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SPEED_DISPLAY_HPP_ +#define SPEED_DISPLAY_HPP_ +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" + +#include +#include +#include + +namespace autoware_overlay_rviz_plugin +{ + +class SpeedDisplay +{ +public: + SpeedDisplay(); + void drawSpeedDisplay(QPainter & painter, const QRectF & backgroundRect); + void updateSpeedData(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); + +private: + float current_speed_; // Internal variable to store current speed + QColor gray = QColor(194, 194, 194); +}; + +} // namespace autoware_overlay_rviz_plugin + +#endif // SPEED_DISPLAY_HPP_ diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp new file mode 100644 index 0000000000000..b59f4acf380db --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp @@ -0,0 +1,52 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SPEED_LIMIT_DISPLAY_HPP_ +#define SPEED_LIMIT_DISPLAY_HPP_ +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" +#include + +#include +#include +#include + +namespace autoware_overlay_rviz_plugin +{ + +class SpeedLimitDisplay +{ +public: + SpeedLimitDisplay(); + void drawSpeedLimitIndicator(QPainter & painter, const QRectF & backgroundRect); + void updateSpeedLimitData(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); + void updateSpeedData(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); + +private: + float current_limit; // Internal variable to store current gear + float current_speed_; // Internal variable to store current speed + QColor gray = QColor(194, 194, 194); +}; + +} // namespace autoware_overlay_rviz_plugin + +#endif // SPEED_LIMIT_DISPLAY_HPP_ diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp new file mode 100644 index 0000000000000..121ee9a0a4d84 --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp @@ -0,0 +1,54 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef STEERING_WHEEL_DISPLAY_HPP_ +#define STEERING_WHEEL_DISPLAY_HPP_ +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" + +#include +#include +#include + +namespace autoware_overlay_rviz_plugin +{ + +class SteeringWheelDisplay +{ +public: + SteeringWheelDisplay(); + void drawSteeringWheel(QPainter & painter, const QRectF & backgroundRect); + void updateSteeringData( + const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg); + +private: + float steering_angle_ = 0.0f; + QColor gray = QColor(194, 194, 194); + + QImage wheelImage; + QImage scaledWheelImage; + QImage coloredImage(const QImage & source, const QColor & color); +}; + +} // namespace autoware_overlay_rviz_plugin + +#endif // STEERING_WHEEL_DISPLAY_HPP_ diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp new file mode 100644 index 0000000000000..20410a78edc44 --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp @@ -0,0 +1,59 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRAFFIC_DISPLAY_HPP_ +#define TRAFFIC_DISPLAY_HPP_ +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +namespace autoware_overlay_rviz_plugin +{ + +class TrafficDisplay +{ +public: + TrafficDisplay(); + void drawTrafficLightIndicator(QPainter & painter, const QRectF & backgroundRect); + void updateTrafficLightData( + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr & msg); + autoware_perception_msgs::msg::TrafficSignalArray current_traffic_; + +private: + QImage traffic_light_image_; + + const QColor tl_red_; + const QColor tl_yellow_; + const QColor tl_green_; + const QColor tl_gray_; + + QImage coloredImage(const QImage & source, const QColor & color); +}; + +} // namespace autoware_overlay_rviz_plugin + +#endif // TRAFFIC_DISPLAY_HPP_ diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/turn_signals_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/turn_signals_display.hpp new file mode 100644 index 0000000000000..87f141353d5b2 --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/turn_signals_display.hpp @@ -0,0 +1,63 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TURN_SIGNALS_DISPLAY_HPP_ +#define TURN_SIGNALS_DISPLAY_HPP_ +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include + +namespace autoware_overlay_rviz_plugin +{ + +class TurnSignalsDisplay +{ +public: + TurnSignalsDisplay(); + void drawArrows(QPainter & painter, const QRectF & backgroundRect, const QColor & color); + void updateTurnSignalsData( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg); + void updateHazardLightsData( + const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); + +private: + QImage arrowImage; + QColor gray = QColor(46, 46, 46); + + int current_turn_signal_; // Internal variable to store turn signal state + int current_hazard_lights_; // Internal variable to store hazard lights state + QImage coloredImage(const QImage & source, const QColor & color); + + std::chrono::steady_clock::time_point last_toggle_time_; + bool blink_on_ = false; + const std::chrono::milliseconds blink_interval_{500}; // Blink interval in milliseconds +}; + +} // namespace autoware_overlay_rviz_plugin + +#endif // TURN_SIGNALS_DISPLAY_HPP_ diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml new file mode 100644 index 0000000000000..da075b2648937 --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml @@ -0,0 +1,31 @@ + + + + autoware_overlay_rviz_plugin + 0.0.1 + + RViz2 plugin for 2D overlays in the 3D view. Mainly a port of the JSK overlay plugin + (https://github.com/jsk-ros-pkg/jsk_visualization). + + Khalil Selyan + + BSD-3-Clause + + autoware_auto_vehicle_msgs + autoware_overlay_msgs + autoware_perception_msgs + boost + rviz_common + rviz_ogre_vendor + rviz_rendering + tier4_planning_msgs + + ament_lint_auto + autoware_lint_common + + ament_cmake_auto + + + ament_cmake + + diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/plugins_description.xml b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/plugins_description.xml new file mode 100644 index 0000000000000..05c33f2fd10a5 --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/plugins_description.xml @@ -0,0 +1,5 @@ + + + Signal overlay plugin for the 3D view. + + diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp new file mode 100644 index 0000000000000..8f05ce6fdce1c --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp @@ -0,0 +1,99 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gear_display.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware_overlay_rviz_plugin +{ + +GearDisplay::GearDisplay() : current_gear_(0) +{ + std::string package_path = + ament_index_cpp::get_package_share_directory("autoware_overlay_rviz_plugin"); + std::string font_path = package_path + "/assets/font/Quicksand/static/Quicksand-Regular.ttf"; + std::string font_path2 = package_path + "/assets/font/Quicksand/static/Quicksand-Bold.ttf"; + int fontId = QFontDatabase::addApplicationFont( + font_path.c_str()); // returns -1 on failure (see docs for more info) + int fontId2 = QFontDatabase::addApplicationFont( + font_path2.c_str()); // returns -1 on failure (see docs for more info) + if (fontId == -1 || fontId2 == -1) { + std::cout << "Failed to load the Quicksand font."; + } +} + +void GearDisplay::updateGearData( + const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg) +{ + current_gear_ = msg->report; // Assuming msg->report contains the gear information +} + +void GearDisplay::drawGearIndicator(QPainter & painter, const QRectF & backgroundRect) +{ + // we deal with the different gears here + std::string gearString; + switch (current_gear_) { + case autoware_auto_vehicle_msgs::msg::GearReport::NEUTRAL: + gearString = "N"; + break; + case autoware_auto_vehicle_msgs::msg::GearReport::LOW: + case autoware_auto_vehicle_msgs::msg::GearReport::LOW_2: + gearString = "L"; + break; + case autoware_auto_vehicle_msgs::msg::GearReport::NONE: + case autoware_auto_vehicle_msgs::msg::GearReport::PARK: + gearString = "P"; + break; + case autoware_auto_vehicle_msgs::msg::GearReport::REVERSE: + case autoware_auto_vehicle_msgs::msg::GearReport::REVERSE_2: + gearString = "R"; + break; + // all the drive gears from DRIVE to DRIVE_16 + default: + gearString = "D"; + break; + } + + QFont gearFont("Quicksand", 16, QFont::Bold); + painter.setFont(gearFont); + QPen borderPen(gray); + borderPen.setWidth(4); + painter.setPen(borderPen); + + int gearBoxSize = 30; + int gearX = backgroundRect.left() + 30 + gearBoxSize; + int gearY = backgroundRect.height() - gearBoxSize - 20; + QRect gearRect(gearX, gearY, gearBoxSize, gearBoxSize); + painter.setBrush(QColor(0, 0, 0, 0)); + painter.drawRoundedRect(gearRect, 10, 10); + painter.drawText(gearRect, Qt::AlignCenter, QString::fromStdString(gearString)); +} + +} // namespace autoware_overlay_rviz_plugin diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/overlay_text_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/overlay_text_display.cpp new file mode 100644 index 0000000000000..b853e14a5858d --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/overlay_text_display.cpp @@ -0,0 +1,556 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// -*- mode: c++; -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Team Spatzenhirn + * Copyright (c) 2014, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "overlay_text_display.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +namespace autoware_overlay_rviz_plugin +{ +OverlayTextDisplay::OverlayTextDisplay() +: texture_width_(0), + texture_height_(0), + bg_color_(0, 0, 0, 0), + fg_color_(255, 255, 255, 255.0), + text_size_(14), + line_width_(2), + text_(""), + font_(""), + require_update_texture_(false) +{ + overtake_position_properties_property_ = new rviz_common::properties::BoolProperty( + "Overtake Position Properties", false, + "overtake position properties specified by message such as left, top and font", this, + SLOT(updateOvertakePositionProperties())); + overtake_fg_color_properties_property_ = new rviz_common::properties::BoolProperty( + "Overtake FG Color Properties", false, + "overtake color properties specified by message such as foreground color and alpha", this, + SLOT(updateOvertakeFGColorProperties())); + overtake_bg_color_properties_property_ = new rviz_common::properties::BoolProperty( + "Overtake BG Color Properties", false, + "overtake color properties specified by message such as background color and alpha", this, + SLOT(updateOvertakeBGColorProperties())); + align_bottom_property_ = new rviz_common::properties::BoolProperty( + "Align Bottom", false, "align text with the bottom of the overlay region", this, + SLOT(updateAlignBottom())); + invert_shadow_property_ = new rviz_common::properties::BoolProperty( + "Invert Shadow", false, "make shadow lighter than original text", this, + SLOT(updateInvertShadow())); + hor_dist_property_ = new rviz_common::properties::IntProperty( + "hor_dist", 0, "horizontal distance to anchor", this, SLOT(updateHorizontalDistance())); + ver_dist_property_ = new rviz_common::properties::IntProperty( + "ver_dist", 0, "vertical distance to anchor", this, SLOT(updateVerticalDistance())); + hor_alignment_property_ = new rviz_common::properties::EnumProperty( + "hor_alignment", "left", "horizontal alignment of the overlay", this, + SLOT(updateHorizontalAlignment())); + hor_alignment_property_->addOption("left", autoware_overlay_msgs::msg::OverlayText::LEFT); + hor_alignment_property_->addOption("center", autoware_overlay_msgs::msg::OverlayText::CENTER); + hor_alignment_property_->addOption("right", autoware_overlay_msgs::msg::OverlayText::RIGHT); + ver_alignment_property_ = new rviz_common::properties::EnumProperty( + "ver_alignment", "top", "vertical alignment of the overlay", this, + SLOT(updateVerticalAlignment())); + ver_alignment_property_->addOption("top", autoware_overlay_msgs::msg::OverlayText::TOP); + ver_alignment_property_->addOption("center", autoware_overlay_msgs::msg::OverlayText::CENTER); + ver_alignment_property_->addOption("bottom", autoware_overlay_msgs::msg::OverlayText::BOTTOM); + width_property_ = new rviz_common::properties::IntProperty( + "width", 128, "width position", this, SLOT(updateWidth())); + width_property_->setMin(0); + height_property_ = new rviz_common::properties::IntProperty( + "height", 128, "height position", this, SLOT(updateHeight())); + height_property_->setMin(0); + text_size_property_ = new rviz_common::properties::IntProperty( + "text size", 12, "text size", this, SLOT(updateTextSize())); + text_size_property_->setMin(0); + line_width_property_ = new rviz_common::properties::IntProperty( + "line width", 2, "line width", this, SLOT(updateLineWidth())); + line_width_property_->setMin(0); + fg_color_property_ = new rviz_common::properties::ColorProperty( + "Foreground Color", QColor(25, 255, 240), "Foreground Color", this, SLOT(updateFGColor())); + fg_alpha_property_ = new rviz_common::properties::FloatProperty( + "Foreground Alpha", 0.8, "Foreground Alpha", this, SLOT(updateFGAlpha())); + fg_alpha_property_->setMin(0.0); + fg_alpha_property_->setMax(1.0); + bg_color_property_ = new rviz_common::properties::ColorProperty( + "Background Color", QColor(0, 0, 0), "Background Color", this, SLOT(updateBGColor())); + bg_alpha_property_ = new rviz_common::properties::FloatProperty( + "Background Alpha", 0.8, "Background Alpha", this, SLOT(updateBGAlpha())); + bg_alpha_property_->setMin(0.0); + bg_alpha_property_->setMax(1.0); + + QFontDatabase database; + font_families_ = database.families(); + font_property_ = new rviz_common::properties::EnumProperty( + "font", "DejaVu Sans Mono", "font", this, SLOT(updateFont())); + for (ssize_t i = 0; i < font_families_.size(); i++) { + font_property_->addOption(font_families_[i], static_cast(i)); + } +} + +OverlayTextDisplay::~OverlayTextDisplay() +{ + onDisable(); +} + +void OverlayTextDisplay::onEnable() +{ + if (overlay_) { + overlay_->show(); + } + subscribe(); +} + +void OverlayTextDisplay::onDisable() +{ + if (overlay_) { + overlay_->hide(); + } + unsubscribe(); +} + +// only the first time +void OverlayTextDisplay::onInitialize() +{ + RTDClass::onInitialize(); + rviz_rendering::RenderSystem::get()->prepareOverlays(scene_manager_); + + onEnable(); + updateTopic(); + updateOvertakePositionProperties(); + updateOvertakeFGColorProperties(); + updateOvertakeBGColorProperties(); + updateAlignBottom(); + updateInvertShadow(); + updateHorizontalDistance(); + updateVerticalDistance(); + updateHorizontalAlignment(); + updateVerticalAlignment(); + updateWidth(); + updateHeight(); + updateTextSize(); + updateFGColor(); + updateFGAlpha(); + updateBGColor(); + updateBGAlpha(); + updateFont(); + updateLineWidth(); + require_update_texture_ = true; +} + +void OverlayTextDisplay::update(float /*wall_dt*/, float /*ros_dt*/) +{ + if (!require_update_texture_) { + return; + } + if (!isEnabled()) { + return; + } + if (!overlay_) { + return; + } + + overlay_->updateTextureSize(texture_width_, texture_height_); + { + autoware_overlay_rviz_plugin::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage Hud = buffer.getQImage(*overlay_, bg_color_); + QPainter painter(&Hud); + painter.setRenderHint(QPainter::Antialiasing, true); + painter.setPen(QPen(fg_color_, std::max(line_width_, 1), Qt::SolidLine)); + uint16_t w = overlay_->getTextureWidth(); + uint16_t h = overlay_->getTextureHeight(); + + // font + if (text_size_ != 0) { + // QFont font = painter.font(); + QFont font(font_.length() > 0 ? font_.c_str() : "Liberation Sans"); + font.setPointSize(text_size_); + font.setBold(true); + painter.setFont(font); + } + if (text_.length() > 0) { + QColor shadow_color; + if (invert_shadow_) + shadow_color = Qt::white; // fg_color_.lighter(); + else + shadow_color = Qt::black; // fg_color_.darker(); + shadow_color.setAlpha(fg_color_.alpha()); + + std::string color_wrapped_text = + (boost::format("%1%") % text_ % + fg_color_.red() % fg_color_.green() % fg_color_.blue() % fg_color_.alpha()) + .str(); + + // find a remove "color: XXX;" regex match to generate a proper shadow + std::regex color_tag_re("color:.+?;"); + std::string null_char(""); + std::string formatted_text_ = std::regex_replace(text_, color_tag_re, null_char); + std::string color_wrapped_shadow = + (boost::format("%1%") % + formatted_text_ % shadow_color.red() % shadow_color.green() % shadow_color.blue() % + shadow_color.alpha()) + .str(); + + QStaticText static_text( + boost::algorithm::replace_all_copy(color_wrapped_text, "\n", "
").c_str()); + static_text.setTextWidth(w); + + painter.setPen(QPen(shadow_color, std::max(line_width_, 1), Qt::SolidLine)); + QStaticText static_shadow( + boost::algorithm::replace_all_copy(color_wrapped_shadow, "\n", "
").c_str()); + static_shadow.setTextWidth(w); + + if (!align_bottom_) { + painter.drawStaticText(1, 1, static_shadow); + painter.drawStaticText(0, 0, static_text); + } else { + QStaticText only_wrapped_text(color_wrapped_text.c_str()); + QFontMetrics fm(painter.fontMetrics()); + QRect text_rect = fm.boundingRect( + 0, 0, w, h, Qt::TextWordWrap | Qt::AlignLeft | Qt::AlignTop, + only_wrapped_text.text().remove(QRegExp("<[^>]*>"))); + painter.drawStaticText(1, h - text_rect.height() + 1, static_shadow); + painter.drawStaticText(0, h - text_rect.height(), static_text); + } + } + painter.end(); + } + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); + require_update_texture_ = false; +} + +void OverlayTextDisplay::reset() +{ + RTDClass::reset(); + + if (overlay_) { + overlay_->hide(); + } +} + +void OverlayTextDisplay::processMessage(autoware_overlay_msgs::msg::OverlayText::ConstSharedPtr msg) +{ + if (!isEnabled()) { + return; + } + if (!overlay_) { + static int count = 0; + std::stringstream ss; + ss << "OverlayTextDisplayObject" << count++; + overlay_.reset(new autoware_overlay_rviz_plugin::OverlayObject(ss.str())); + overlay_->show(); + } + if (overlay_) { + if (msg->action == autoware_overlay_msgs::msg::OverlayText::DELETE) { + overlay_->hide(); + } else if (msg->action == autoware_overlay_msgs::msg::OverlayText::ADD) { + overlay_->show(); + } + } + + // store message for update method + text_ = msg->text; + + if (!overtake_position_properties_) { + texture_width_ = msg->width; + texture_height_ = msg->height; + text_size_ = msg->text_size; + horizontal_dist_ = msg->horizontal_distance; + vertical_dist_ = msg->vertical_distance; + + horizontal_alignment_ = HorizontalAlignment{msg->horizontal_alignment}; + vertical_alignment_ = VerticalAlignment{msg->vertical_alignment}; + } + if (!overtake_bg_color_properties_) + bg_color_ = QColor( + msg->bg_color.r * 255.0, msg->bg_color.g * 255.0, msg->bg_color.b * 255.0, + msg->bg_color.a * 255.0); + if (!overtake_fg_color_properties_) { + fg_color_ = QColor( + msg->fg_color.r * 255.0, msg->fg_color.g * 255.0, msg->fg_color.b * 255.0, + msg->fg_color.a * 255.0); + font_ = msg->font; + line_width_ = msg->line_width; + } + if (overlay_) { + overlay_->setPosition( + horizontal_dist_, vertical_dist_, horizontal_alignment_, vertical_alignment_); + } + require_update_texture_ = true; +} + +void OverlayTextDisplay::updateOvertakePositionProperties() +{ + if (!overtake_position_properties_ && overtake_position_properties_property_->getBool()) { + updateVerticalDistance(); + updateHorizontalDistance(); + updateVerticalAlignment(); + updateHorizontalAlignment(); + updateWidth(); + updateHeight(); + updateTextSize(); + require_update_texture_ = true; + } + + overtake_position_properties_ = overtake_position_properties_property_->getBool(); + if (overtake_position_properties_) { + hor_dist_property_->show(); + ver_dist_property_->show(); + hor_alignment_property_->show(); + ver_alignment_property_->show(); + width_property_->show(); + height_property_->show(); + text_size_property_->show(); + } else { + hor_dist_property_->hide(); + ver_dist_property_->hide(); + hor_alignment_property_->hide(); + ver_alignment_property_->hide(); + width_property_->hide(); + height_property_->hide(); + text_size_property_->hide(); + } +} + +void OverlayTextDisplay::updateOvertakeFGColorProperties() +{ + if (!overtake_fg_color_properties_ && overtake_fg_color_properties_property_->getBool()) { + // read all the parameters from properties + updateFGColor(); + updateFGAlpha(); + updateFont(); + updateLineWidth(); + require_update_texture_ = true; + } + overtake_fg_color_properties_ = overtake_fg_color_properties_property_->getBool(); + if (overtake_fg_color_properties_) { + fg_color_property_->show(); + fg_alpha_property_->show(); + line_width_property_->show(); + font_property_->show(); + } else { + fg_color_property_->hide(); + fg_alpha_property_->hide(); + line_width_property_->hide(); + font_property_->hide(); + } +} + +void OverlayTextDisplay::updateOvertakeBGColorProperties() +{ + if (!overtake_bg_color_properties_ && overtake_bg_color_properties_property_->getBool()) { + // read all the parameters from properties + updateBGColor(); + updateBGAlpha(); + require_update_texture_ = true; + } + overtake_bg_color_properties_ = overtake_bg_color_properties_property_->getBool(); + if (overtake_bg_color_properties_) { + bg_color_property_->show(); + bg_alpha_property_->show(); + } else { + bg_color_property_->hide(); + bg_alpha_property_->hide(); + } +} + +void OverlayTextDisplay::updateAlignBottom() +{ + if (align_bottom_ != align_bottom_property_->getBool()) { + require_update_texture_ = true; + } + align_bottom_ = align_bottom_property_->getBool(); +} + +void OverlayTextDisplay::updateInvertShadow() +{ + if (invert_shadow_ != invert_shadow_property_->getBool()) { + require_update_texture_ = true; + } + invert_shadow_ = invert_shadow_property_->getBool(); +} + +void OverlayTextDisplay::updateVerticalDistance() +{ + vertical_dist_ = ver_dist_property_->getInt(); + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateHorizontalDistance() +{ + horizontal_dist_ = hor_dist_property_->getInt(); + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateVerticalAlignment() +{ + vertical_alignment_ = + VerticalAlignment{static_cast(ver_alignment_property_->getOptionInt())}; + + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateHorizontalAlignment() +{ + horizontal_alignment_ = + HorizontalAlignment{static_cast(hor_alignment_property_->getOptionInt())}; + + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateWidth() +{ + texture_width_ = width_property_->getInt(); + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateHeight() +{ + texture_height_ = height_property_->getInt(); + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateTextSize() +{ + text_size_ = text_size_property_->getInt(); + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateBGColor() +{ + QColor c = bg_color_property_->getColor(); + bg_color_.setRed(c.red()); + bg_color_.setGreen(c.green()); + bg_color_.setBlue(c.blue()); + if (overtake_bg_color_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateBGAlpha() +{ + bg_color_.setAlpha(bg_alpha_property_->getFloat() * 255.0); + if (overtake_bg_color_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateFGColor() +{ + QColor c = fg_color_property_->getColor(); + fg_color_.setRed(c.red()); + fg_color_.setGreen(c.green()); + fg_color_.setBlue(c.blue()); + if (overtake_fg_color_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateFGAlpha() +{ + fg_color_.setAlpha(fg_alpha_property_->getFloat() * 255.0); + if (overtake_fg_color_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateFont() +{ + int font_index = font_property_->getOptionInt(); + if (font_index < font_families_.size()) { + font_ = font_families_[font_index].toStdString(); + } else { + RVIZ_COMMON_LOG_ERROR_STREAM("Unexpected error at selecting font index " << font_index); + return; + } + if (overtake_fg_color_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateLineWidth() +{ + line_width_ = line_width_property_->getInt(); + if (overtake_fg_color_properties_) { + require_update_texture_ = true; + } +} + +} // namespace autoware_overlay_rviz_plugin + +#include +PLUGINLIB_EXPORT_CLASS(autoware_overlay_rviz_plugin::OverlayTextDisplay, rviz_common::Display) diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/overlay_utils.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/overlay_utils.cpp new file mode 100644 index 0000000000000..d6f061e724369 --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/overlay_utils.cpp @@ -0,0 +1,267 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Team Spatzenhirn + * Copyright (c) 2014, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "overlay_utils.hpp" + +#include + +namespace autoware_overlay_rviz_plugin +{ +ScopedPixelBuffer::ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer) +: pixel_buffer_(pixel_buffer) +{ + pixel_buffer_->lock(Ogre::HardwareBuffer::HBL_NORMAL); +} + +ScopedPixelBuffer::~ScopedPixelBuffer() +{ + pixel_buffer_->unlock(); +} + +Ogre::HardwarePixelBufferSharedPtr ScopedPixelBuffer::getPixelBuffer() +{ + return pixel_buffer_; +} + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height) +{ + const Ogre::PixelBox & pixelBox = pixel_buffer_->getCurrentLock(); + Ogre::uint8 * pDest = static_cast(pixelBox.data); + memset(pDest, 0, width * height); + return QImage(pDest, width, height, QImage::Format_ARGB32); +} + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height, QColor & bg_color) +{ + QImage Hud = getQImage(width, height); + for (unsigned int i = 0; i < width; i++) { + for (unsigned int j = 0; j < height; j++) { + Hud.setPixel(i, j, bg_color.rgba()); + } + } + return Hud; +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight()); +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay, QColor & bg_color) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight(), bg_color); +} + +OverlayObject::OverlayObject(const std::string & name) : name_(name) +{ + std::string material_name = name_ + "Material"; + Ogre::OverlayManager * mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + overlay_ = mOverlayMgr->create(name_); + panel_ = static_cast( + mOverlayMgr->createOverlayElement("Panel", name_ + "Panel")); + panel_->setMetricsMode(Ogre::GMM_PIXELS); + + panel_material_ = Ogre::MaterialManager::getSingleton().create( + material_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + panel_->setMaterialName(panel_material_->getName()); + overlay_->add2D(panel_); +} + +OverlayObject::~OverlayObject() +{ + Ogre::OverlayManager * mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + if (mOverlayMgr) { + mOverlayMgr->destroyOverlayElement(panel_); + mOverlayMgr->destroy(overlay_); + } + if (panel_material_) { + panel_material_->unload(); + Ogre::MaterialManager::getSingleton().remove(panel_material_->getName()); + } +} + +std::string OverlayObject::getName() const +{ + return name_; +} + +void OverlayObject::hide() +{ + if (overlay_->isVisible()) { + overlay_->hide(); + } +} + +void OverlayObject::show() +{ + if (!overlay_->isVisible()) { + overlay_->show(); + } +} + +bool OverlayObject::isTextureReady() const +{ + return texture_ != nullptr; +} + +void OverlayObject::updateTextureSize(unsigned int width, unsigned int height) +{ + const std::string texture_name = name_ + "Texture"; + if (width == 0) { + RVIZ_COMMON_LOG_WARNING_STREAM("[OverlayObject] width=0 is specified as texture size"); + width = 1; + } + + if (height == 0) { + RVIZ_COMMON_LOG_WARNING_STREAM("[OverlayObject] height=0 is specified as texture size"); + height = 1; + } + + if (!isTextureReady() || ((width != texture_->getWidth()) || (height != texture_->getHeight()))) { + if (isTextureReady()) { + Ogre::TextureManager::getSingleton().remove(texture_name); + panel_material_->getTechnique(0)->getPass(0)->removeAllTextureUnitStates(); + } + + texture_ = Ogre::TextureManager::getSingleton().createManual( + texture_name, // name + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + Ogre::TEX_TYPE_2D, // type + width, height, // width & height of the render window + 0, // number of mipmaps + Ogre::PF_A8R8G8B8, // pixel format chosen to match a format Qt can use + Ogre::TU_DEFAULT // usage + ); + panel_material_->getTechnique(0)->getPass(0)->createTextureUnitState(texture_name); + + panel_material_->getTechnique(0)->getPass(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + } +} + +ScopedPixelBuffer OverlayObject::getBuffer() +{ + if (isTextureReady()) { + return ScopedPixelBuffer(texture_->getBuffer()); + } else { + return ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr()); + } +} + +void OverlayObject::setPosition( + double hor_dist, double ver_dist, HorizontalAlignment hor_alignment, + VerticalAlignment ver_alignment) +{ + // ogre position is always based on the top left corner of the panel, while our position input + // depends on the chosen alignment, i.e. if the horizontal alignment is right, increasing the + // horizontal dist moves the panel to the left (further away from the right border) + double left = 0; + double top = 0; + + switch (hor_alignment) { + case HorizontalAlignment::LEFT: + panel_->setHorizontalAlignment(Ogre::GuiHorizontalAlignment::GHA_LEFT); + left = hor_dist; + break; + case HorizontalAlignment::CENTER: + panel_->setHorizontalAlignment(Ogre::GuiHorizontalAlignment::GHA_CENTER); + left = hor_dist - panel_->getWidth() / 2; + break; + case HorizontalAlignment::RIGHT: + panel_->setHorizontalAlignment(Ogre::GuiHorizontalAlignment::GHA_RIGHT); + left = -hor_dist - panel_->getWidth(); + break; + } + + switch (ver_alignment) { + case VerticalAlignment::BOTTOM: + panel_->setVerticalAlignment(Ogre::GuiVerticalAlignment::GVA_BOTTOM); + top = -ver_dist - panel_->getHeight(); + break; + case VerticalAlignment::CENTER: + panel_->setVerticalAlignment(Ogre::GuiVerticalAlignment::GVA_CENTER); + top = ver_dist - panel_->getHeight() / 2; + break; + case VerticalAlignment::TOP: + panel_->setVerticalAlignment(Ogre::GuiVerticalAlignment::GVA_TOP); + top = ver_dist; + break; + } + + panel_->setPosition(left, top); +} + +void OverlayObject::setDimensions(double width, double height) +{ + panel_->setDimensions(width, height); +} + +bool OverlayObject::isVisible() const +{ + return overlay_->isVisible(); +} + +unsigned int OverlayObject::getTextureWidth() const +{ + if (isTextureReady()) { + return texture_->getWidth(); + } else { + return 0; + } +} + +unsigned int OverlayObject::getTextureHeight() const +{ + if (isTextureReady()) { + return texture_->getHeight(); + } else { + return 0; + } +} +} // namespace autoware_overlay_rviz_plugin diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp new file mode 100644 index 0000000000000..b2995327eb472 --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp @@ -0,0 +1,522 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "signal_display.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace autoware_overlay_rviz_plugin +{ + +SignalDisplay::SignalDisplay() +{ + property_width_ = new rviz_common::properties::IntProperty( + "Width", 517, "Width of the overlay", this, SLOT(updateOverlaySize())); + property_height_ = new rviz_common::properties::IntProperty( + "Height", 175, "Height of the overlay", this, SLOT(updateOverlaySize())); + property_left_ = new rviz_common::properties::IntProperty( + "Left", 10, "Left position of the overlay", this, SLOT(updateOverlayPosition())); + property_top_ = new rviz_common::properties::IntProperty( + "Top", 10, "Top position of the overlay", this, SLOT(updateOverlayPosition())); + property_signal_color_ = new rviz_common::properties::ColorProperty( + "Signal Color", QColor(QString("#00E678")), "Color of the signal arrows", this, + SLOT(updateOverlayColor())); + + // Initialize the component displays + steering_wheel_display_ = std::make_unique(); + gear_display_ = std::make_unique(); + speed_display_ = std::make_unique(); + turn_signals_display_ = std::make_unique(); + traffic_display_ = std::make_unique(); + speed_limit_display_ = std::make_unique(); +} + +void SignalDisplay::onInitialize() +{ + std::lock_guard lock(property_mutex_); + + rviz_common::Display::onInitialize(); + rviz_rendering::RenderSystem::get()->prepareOverlays(scene_manager_); + static int count = 0; + std::stringstream ss; + ss << "SignalDisplayObject" << count++; + overlay_.reset(new autoware_overlay_rviz_plugin::OverlayObject(ss.str())); + overlay_->show(); + updateOverlaySize(); + updateOverlayPosition(); + + auto rviz_ros_node = context_->getRosNodeAbstraction(); + + gear_topic_property_ = std::make_unique( + "Gear Topic Test", "/vehicle/status/gear_status", "autoware_auto_vehicle_msgs/msg/GearReport", + "Topic for Gear Data", this, SLOT(topic_updated_gear())); + gear_topic_property_->initialize(rviz_ros_node); + + turn_signals_topic_property_ = std::make_unique( + "Turn Signals Topic", "/vehicle/status/turn_indicators_status", + "autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport", "Topic for Turn Signals Data", this, + SLOT(topic_updated_turn_signals())); + turn_signals_topic_property_->initialize(rviz_ros_node); + + speed_topic_property_ = std::make_unique( + "Speed Topic", "/vehicle/status/velocity_status", + "autoware_auto_vehicle_msgs/msg/VelocityReport", "Topic for Speed Data", this, + SLOT(topic_updated_speed())); + speed_topic_property_->initialize(rviz_ros_node); + + steering_topic_property_ = std::make_unique( + "Steering Topic", "/vehicle/status/steering_status", + "autoware_auto_vehicle_msgs/msg/SteeringReport", "Topic for Steering Data", this, + SLOT(topic_updated_steering())); + steering_topic_property_->initialize(rviz_ros_node); + + hazard_lights_topic_property_ = std::make_unique( + "Hazard Lights Topic", "/vehicle/status/hazard_lights_status", + "autoware_auto_vehicle_msgs/msg/HazardLightsReport", "Topic for Hazard Lights Data", this, + SLOT(topic_updated_hazard_lights())); + hazard_lights_topic_property_->initialize(rviz_ros_node); + + speed_limit_topic_property_ = std::make_unique( + "Speed Limit Topic", "/planning/scenario_planning/current_max_velocity", + "tier4_planning_msgs/msg/VelocityLimit", "Topic for Speed Limit Data", this, + SLOT(topic_updated_speed_limit())); + speed_limit_topic_property_->initialize(rviz_ros_node); + + traffic_topic_property_ = std::make_unique( + "Traffic Topic", "/perception/traffic_light_recognition/traffic_signals", + "autoware_perception/msgs/msg/TrafficSignalArray", "Topic for Traffic Light Data", this, + SLOT(topic_updated_traffic())); + traffic_topic_property_->initialize(rviz_ros_node); +} + +void SignalDisplay::setupRosSubscriptions() +{ + // Don't create a node, just use the one from the parent class + auto rviz_node_ = context_->getRosNodeAbstraction().lock()->get_raw_node(); + + gear_sub_ = rviz_node_->create_subscription( + gear_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg) { + updateGearData(msg); + }); + + steering_sub_ = rviz_node_->create_subscription( + steering_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg) { + updateSteeringData(msg); + }); + + speed_sub_ = rviz_node_->create_subscription( + speed_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::VelocityReport::SharedPtr msg) { + updateSpeedData(msg); + }); + + turn_signals_sub_ = + rviz_node_->create_subscription( + turn_signals_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::SharedPtr msg) { + updateTurnSignalsData(msg); + }); + + hazard_lights_sub_ = + rviz_node_->create_subscription( + hazard_lights_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::HazardLightsReport::SharedPtr msg) { + updateHazardLightsData(msg); + }); + + traffic_sub_ = rviz_node_->create_subscription( + traffic_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_perception_msgs::msg::TrafficSignalArray::SharedPtr msg) { + updateTrafficLightData(msg); + }); + + speed_limit_sub_ = rviz_node_->create_subscription( + speed_limit_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) { + updateSpeedLimitData(msg); + }); +} + +SignalDisplay::~SignalDisplay() +{ + std::lock_guard lock(property_mutex_); + overlay_.reset(); + + gear_sub_.reset(); + steering_sub_.reset(); + speed_sub_.reset(); + turn_signals_sub_.reset(); + hazard_lights_sub_.reset(); + traffic_sub_.reset(); + + steering_wheel_display_.reset(); + gear_display_.reset(); + speed_display_.reset(); + turn_signals_display_.reset(); + traffic_display_.reset(); + speed_limit_display_.reset(); + + gear_topic_property_.reset(); + turn_signals_topic_property_.reset(); + speed_topic_property_.reset(); + steering_topic_property_.reset(); + hazard_lights_topic_property_.reset(); + traffic_topic_property_.reset(); +} + +void SignalDisplay::update(float /* wall_dt */, float /* ros_dt */) +{ + if (!overlay_) { + return; + } + autoware_overlay_rviz_plugin::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage hud = buffer.getQImage(*overlay_); + hud.fill(Qt::transparent); + drawWidget(hud); +} + +void SignalDisplay::onEnable() +{ + std::lock_guard lock(property_mutex_); + if (overlay_) { + overlay_->show(); + } + setupRosSubscriptions(); +} + +void SignalDisplay::onDisable() +{ + std::lock_guard lock(property_mutex_); + + gear_sub_.reset(); + steering_sub_.reset(); + speed_sub_.reset(); + turn_signals_sub_.reset(); + hazard_lights_sub_.reset(); + + if (overlay_) { + overlay_->hide(); + } +} + +void SignalDisplay::updateTrafficLightData( + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg) +{ + std::lock_guard lock(property_mutex_); + + if (traffic_display_) { + traffic_display_->updateTrafficLightData(msg); + } +} + +void SignalDisplay::updateSpeedLimitData( + const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) +{ + std::lock_guard lock(property_mutex_); + + if (speed_limit_display_) { + speed_limit_display_->updateSpeedLimitData(msg); + queueRender(); + } +} + +void SignalDisplay::updateHazardLightsData( + const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg) +{ + std::lock_guard lock(property_mutex_); + + if (turn_signals_display_) { + turn_signals_display_->updateHazardLightsData(msg); + queueRender(); + } +} + +void SignalDisplay::updateGearData( + const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg) +{ + std::lock_guard lock(property_mutex_); + + if (gear_display_) { + gear_display_->updateGearData(msg); + queueRender(); + } +} + +void SignalDisplay::updateSteeringData( + const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg) +{ + std::lock_guard lock(property_mutex_); + + if (steering_wheel_display_) { + steering_wheel_display_->updateSteeringData(msg); + queueRender(); + } +} + +void SignalDisplay::updateSpeedData( + const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) +{ + std::lock_guard lock(property_mutex_); + + if (speed_display_) { + speed_display_->updateSpeedData(msg); + speed_limit_display_->updateSpeedData(msg); + queueRender(); + } +} + +void SignalDisplay::updateTurnSignalsData( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg) +{ + std::lock_guard lock(property_mutex_); + + if (turn_signals_display_) { + turn_signals_display_->updateTurnSignalsData(msg); + queueRender(); + } +} + +void SignalDisplay::drawWidget(QImage & hud) +{ + std::lock_guard lock(property_mutex_); + + if (!overlay_->isVisible()) { + return; + } + + QPainter painter(&hud); + painter.setRenderHint(QPainter::Antialiasing, true); + + QRectF backgroundRect(0, 0, 322, hud.height()); + drawHorizontalRoundedRectangle(painter, backgroundRect); + + // Draw components + if (steering_wheel_display_) { + steering_wheel_display_->drawSteeringWheel(painter, backgroundRect); + } + if (gear_display_) { + gear_display_->drawGearIndicator(painter, backgroundRect); + } + if (speed_display_) { + speed_display_->drawSpeedDisplay(painter, backgroundRect); + } + if (turn_signals_display_) { + turn_signals_display_->drawArrows(painter, backgroundRect, property_signal_color_->getColor()); + } + + // a 27px space between the two halves of the HUD + + QRectF smallerBackgroundRect(340, 0, 190.0 / 2, hud.height()); + + drawVerticalRoundedRectangle(painter, smallerBackgroundRect); + + if (traffic_display_) { + traffic_display_->drawTrafficLightIndicator(painter, smallerBackgroundRect); + } + + if (speed_limit_display_) { + speed_limit_display_->drawSpeedLimitIndicator(painter, smallerBackgroundRect); + } + + painter.end(); +} + +void SignalDisplay::drawHorizontalRoundedRectangle( + QPainter & painter, const QRectF & backgroundRect) +{ + painter.setRenderHint(QPainter::Antialiasing, true); + QColor colorFromHSV; + colorFromHSV.setHsv(0, 0, 0); // Hue, Saturation, Value + colorFromHSV.setAlphaF(0.65); // Transparency + + painter.setBrush(colorFromHSV); + + painter.setPen(Qt::NoPen); + painter.drawRoundedRect( + backgroundRect, backgroundRect.height() / 2, backgroundRect.height() / 2); // Circular ends +} +void SignalDisplay::drawVerticalRoundedRectangle(QPainter & painter, const QRectF & backgroundRect) +{ + painter.setRenderHint(QPainter::Antialiasing, true); + QColor colorFromHSV; + colorFromHSV.setHsv(0, 0, 0); // Hue, Saturation, Value + colorFromHSV.setAlphaF(0.65); // Transparency + + painter.setBrush(colorFromHSV); + + painter.setPen(Qt::NoPen); + painter.drawRoundedRect( + backgroundRect, backgroundRect.width() / 2, backgroundRect.width() / 2); // Circular ends +} + +void SignalDisplay::reset() +{ + rviz_common::Display::reset(); + overlay_->hide(); +} + +void SignalDisplay::updateOverlaySize() +{ + std::lock_guard lock(mutex_); + overlay_->updateTextureSize(property_width_->getInt(), property_height_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); + queueRender(); +} + +void SignalDisplay::updateOverlayPosition() +{ + std::lock_guard lock(mutex_); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + queueRender(); +} + +void SignalDisplay::updateOverlayColor() +{ + std::lock_guard lock(mutex_); + queueRender(); +} + +void SignalDisplay::topic_updated_gear() +{ + // resubscribe to the topic + gear_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + gear_sub_ = + rviz_ros_node->get_raw_node()->create_subscription( + gear_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg) { + updateGearData(msg); + }); +} + +void SignalDisplay::topic_updated_steering() +{ + // resubscribe to the topic + steering_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + steering_sub_ = rviz_ros_node->get_raw_node() + ->create_subscription( + steering_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg) { + updateSteeringData(msg); + }); +} + +void SignalDisplay::topic_updated_speed() +{ + // resubscribe to the topic + speed_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + speed_sub_ = rviz_ros_node->get_raw_node() + ->create_subscription( + speed_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::VelocityReport::SharedPtr msg) { + updateSpeedData(msg); + }); +} + +void SignalDisplay::topic_updated_speed_limit() +{ + // resubscribe to the topic + speed_limit_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + speed_limit_sub_ = + rviz_ros_node->get_raw_node()->create_subscription( + speed_limit_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) { + updateSpeedLimitData(msg); + }); +} + +void SignalDisplay::topic_updated_turn_signals() +{ + // resubscribe to the topic + turn_signals_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + + turn_signals_sub_ = + rviz_ros_node->get_raw_node() + ->create_subscription( + turn_signals_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::SharedPtr msg) { + updateTurnSignalsData(msg); + }); +} + +void SignalDisplay::topic_updated_hazard_lights() +{ + // resubscribe to the topic + hazard_lights_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + + hazard_lights_sub_ = + rviz_ros_node->get_raw_node() + ->create_subscription( + hazard_lights_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::HazardLightsReport::SharedPtr msg) { + updateHazardLightsData(msg); + }); +} + +void SignalDisplay::topic_updated_traffic() +{ + // resubscribe to the topic + traffic_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + traffic_sub_ = + rviz_ros_node->get_raw_node() + ->create_subscription( + traffic_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_perception_msgs::msg::TrafficSignalArray::SharedPtr msg) { + updateTrafficLightData(msg); + }); +} + +} // namespace autoware_overlay_rviz_plugin + +#include +PLUGINLIB_EXPORT_CLASS(autoware_overlay_rviz_plugin::SignalDisplay, rviz_common::Display) diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp new file mode 100644 index 0000000000000..b1df14956d16c --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp @@ -0,0 +1,110 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "speed_display.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware_overlay_rviz_plugin +{ + +SpeedDisplay::SpeedDisplay() : current_speed_(0.0) +{ + std::string package_path = + ament_index_cpp::get_package_share_directory("autoware_overlay_rviz_plugin"); + std::string font_path = package_path + "/assets/font/Quicksand/static/Quicksand-Regular.ttf"; + std::string font_path2 = package_path + "/assets/font/Quicksand/static/Quicksand-Bold.ttf"; + int fontId = QFontDatabase::addApplicationFont( + font_path.c_str()); // returns -1 on failure (see docs for more info) + int fontId2 = QFontDatabase::addApplicationFont( + font_path2.c_str()); // returns -1 on failure (see docs for more info) + if (fontId == -1 || fontId2 == -1) { + std::cout << "Failed to load the Quicksand font."; + } +} + +void SpeedDisplay::updateSpeedData( + const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) +{ + try { + // Assuming msg->state.longitudinal_velocity_mps is the field you're interested in + float speed = msg->longitudinal_velocity; + // we received it as a m/s value, but we want to display it in km/h + current_speed_ = (speed * 3.6); + } catch (const std::exception & e) { + // Log the error + std::cerr << "Error in processMessage: " << e.what() << std::endl; + } +} + +// void SpeedDisplay::processMessage(const +// autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) +// { +// try +// { +// current_speed_ = std::round(msg->state.longitudinal_velocity_mps * 3.6); +// } +// catch (const std::exception &e) +// { +// std::cerr << "Error in processMessage: " << e.what() << std::endl; +// } +// } + +void SpeedDisplay::drawSpeedDisplay(QPainter & painter, const QRectF & backgroundRect) +{ + QFont referenceFont("Quicksand", 80, QFont::Bold); + painter.setFont(referenceFont); + QRect referenceRect = painter.fontMetrics().boundingRect("88"); + QPointF referencePos( + backgroundRect.width() / 2 - referenceRect.width() / 2 - 5, backgroundRect.height() / 2); + + QString speedNumber = QString::number(current_speed_, 'f', 0); + int fontSize = 60; + QFont speedFont("Quicksand", fontSize); + painter.setFont(speedFont); + + // Calculate the bounding box of the speed number + QRect speedNumberRect = painter.fontMetrics().boundingRect(speedNumber); + + // Center the speed number in the backgroundRect + QPointF speedPos( + backgroundRect.center().x() - speedNumberRect.width() / 2, backgroundRect.center().y()); + painter.setPen(gray); + painter.drawText(speedPos, speedNumber); + + QFont unitFont("Quicksand", 14); + painter.setFont(unitFont); + QString speedUnit = "km/h"; + QRect unitRect = painter.fontMetrics().boundingRect(speedUnit); + QPointF unitPos( + (backgroundRect.width() / 2 - unitRect.width() / 2), referencePos.y() + unitRect.height()); + painter.drawText(unitPos, speedUnit); +} + +} // namespace autoware_overlay_rviz_plugin diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp new file mode 100644 index 0000000000000..6913ef6a48ecd --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp @@ -0,0 +1,142 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "speed_limit_display.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware_overlay_rviz_plugin +{ + +SpeedLimitDisplay::SpeedLimitDisplay() : current_limit(0.0), current_speed_(0.0) +{ + std::string package_path = + ament_index_cpp::get_package_share_directory("autoware_overlay_rviz_plugin"); + std::string font_path = package_path + "/assets/font/Quicksand/static/Quicksand-Regular.ttf"; + std::string font_path2 = package_path + "/assets/font/Quicksand/static/Quicksand-Bold.ttf"; + int fontId = QFontDatabase::addApplicationFont( + font_path.c_str()); // returns -1 on failure (see docs for more info) + int fontId2 = QFontDatabase::addApplicationFont( + font_path2.c_str()); // returns -1 on failure (see docs for more info) + if (fontId == -1 || fontId2 == -1) { + std::cout << "Failed to load the Quicksand font."; + } +} + +void SpeedLimitDisplay::updateSpeedLimitData( + const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) +{ + current_limit = msg->max_velocity; +} + +void SpeedLimitDisplay::updateSpeedData( + const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) +{ + try { + float speed = msg->longitudinal_velocity; + current_speed_ = speed; + } catch (const std::exception & e) { + // Log the error + std::cerr << "Error in processMessage: " << e.what() << std::endl; + } +} + +void SpeedLimitDisplay::drawSpeedLimitIndicator(QPainter & painter, const QRectF & backgroundRect) +{ + // Enable Antialiasing for smoother drawing + painter.setRenderHint(QPainter::Antialiasing, true); + painter.setRenderHint(QPainter::SmoothPixmapTransform, true); + + const double color_s_min = 0.4; + const double color_s_max = 0.8; + QColor colorMin; + colorMin.setHsvF(0.0, color_s_min, 1.0); + QColor colorMax; + colorMax.setHsvF(0.0, color_s_max, 1.0); + + QColor borderColor = colorMin; + if (current_limit > 0.0) { + double speed_to_limit_ratio = current_speed_ / current_limit; + const double speed_to_limit_ratio_min = 0.6; + const double speed_to_limit_ratio_max = 0.9; + + if (speed_to_limit_ratio >= speed_to_limit_ratio_max) { + borderColor = colorMax; + } else if (speed_to_limit_ratio > speed_to_limit_ratio_min) { + double interpolation_factor = (speed_to_limit_ratio - speed_to_limit_ratio_min) / + (speed_to_limit_ratio_max - speed_to_limit_ratio_min); + // Interpolate between colorMin and colorMax + double saturation = color_s_min + (color_s_max - color_s_min) * interpolation_factor; + + borderColor.setHsvF(0.0, saturation, 1.0); + } + } + + // Define the area for the outer circle + QRectF outerCircleRect = backgroundRect; + outerCircleRect.setWidth(backgroundRect.width() - 30); + outerCircleRect.setHeight(backgroundRect.width() - 30); + outerCircleRect.moveTopLeft(QPointF(backgroundRect.left() + 15, backgroundRect.top() + 10)); + + // Now use borderColor for drawing + painter.setPen(QPen(borderColor, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); + painter.setBrush(borderColor); + // Draw the outer circle for the speed limit indicator + painter.drawEllipse(outerCircleRect); + + // Define the area for the inner circle + QRectF innerCircleRect = outerCircleRect; + innerCircleRect.setWidth(outerCircleRect.width() / 1.25); + innerCircleRect.setHeight(outerCircleRect.height() / 1.25); + innerCircleRect.moveCenter(outerCircleRect.center()); + + painter.setRenderHint(QPainter::Antialiasing, true); + QColor colorFromHSV; + colorFromHSV.setHsv(0, 0, 0); // Hue, Saturation, Value + + painter.setBrush(colorFromHSV); + painter.drawEllipse(innerCircleRect); + + int current_limit_int = std::round(current_limit * 3.6); + + // Define the text to be drawn + QString text = QString::number(current_limit_int); + + // Set the font and color for the text + QFont font = QFont("Quicksand", 24, QFont::Bold); + + painter.setFont(font); + // #C2C2C2 + painter.setPen(QPen(gray, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); + + // Draw the text in the center of the circle + painter.drawText(innerCircleRect, Qt::AlignCenter, text); +} + +} // namespace autoware_overlay_rviz_plugin diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp new file mode 100644 index 0000000000000..5882d7780dc92 --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp @@ -0,0 +1,124 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "steering_wheel_display.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware_overlay_rviz_plugin +{ + +SteeringWheelDisplay::SteeringWheelDisplay() +{ + // Load the Quicksand font + std::string package_path = + ament_index_cpp::get_package_share_directory("autoware_overlay_rviz_plugin"); + std::string font_path = package_path + "/assets/font/Quicksand/static/Quicksand-Regular.ttf"; + std::string font_path2 = package_path + "/assets/font/Quicksand/static/Quicksand-Bold.ttf"; + int fontId = QFontDatabase::addApplicationFont( + font_path.c_str()); // returns -1 on failure (see docs for more info) + int fontId2 = QFontDatabase::addApplicationFont( + font_path2.c_str()); // returns -1 on failure (see docs for more info) + if (fontId == -1 || fontId2 == -1) { + std::cout << "Failed to load the Quicksand font."; + } + + // Load the wheel image + std::string image_path = package_path + "/assets/images/wheel.png"; + wheelImage.load(image_path.c_str()); + scaledWheelImage = wheelImage.scaled(54, 54, Qt::KeepAspectRatio, Qt::SmoothTransformation); +} + +void SteeringWheelDisplay::updateSteeringData( + const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg) +{ + try { + // Assuming msg->steering_angle is the field you're interested in + float steeringAngle = msg->steering_tire_angle; + // we received it as a radian value, but we want to display it in degrees + steering_angle_ = + (steeringAngle * -180 / M_PI) * + 17; // 17 is the ratio between the steering wheel and the steering tire angle i assume + } catch (const std::exception & e) { + // Log the error + std::cerr << "Error in processMessage: " << e.what() << std::endl; + } +} + +void SteeringWheelDisplay::drawSteeringWheel(QPainter & painter, const QRectF & backgroundRect) +{ + // Enable Antialiasing for smoother drawing + painter.setRenderHint(QPainter::Antialiasing, true); + painter.setRenderHint(QPainter::SmoothPixmapTransform, true); + + QImage wheel = coloredImage(scaledWheelImage, gray); + + // Rotate the wheel + float steeringAngle = steering_angle_; // No need to round here + // Calculate the position + int wheelCenterX = backgroundRect.right() - wheel.width() - 17.5; + int wheelCenterY = backgroundRect.height() - wheel.height() + 15; + + // Rotate the wheel image + QTransform rotationTransform; + rotationTransform.translate(wheel.width() / 2.0, wheel.height() / 2.0); + rotationTransform.rotate(steeringAngle); + rotationTransform.translate(-wheel.width() / 2.0, -wheel.height() / 2.0); + + QImage rotatedWheel = wheel.transformed(rotationTransform, Qt::SmoothTransformation); + + QPointF drawPoint( + wheelCenterX - rotatedWheel.width() / 2, wheelCenterY - rotatedWheel.height() / 2); + + // Draw the rotated image + painter.drawImage(drawPoint.x(), drawPoint.y(), rotatedWheel); + + QString steeringAngleStringAfterModulo = QString::number(fmod(steeringAngle, 360), 'f', 0); + + // Draw the steering angle text + QFont steeringFont("Quicksand", 9, QFont::Bold); + painter.setFont(steeringFont); + painter.setPen(QColor(0, 0, 0, 255)); + QRect steeringRect( + wheelCenterX - wheelImage.width() / 2, wheelCenterY - wheelImage.height() / 2, + wheelImage.width(), wheelImage.height()); + painter.drawText(steeringRect, Qt::AlignCenter, steeringAngleStringAfterModulo + "°"); +} + +QImage SteeringWheelDisplay::coloredImage(const QImage & source, const QColor & color) +{ + QImage result = source; + QPainter p(&result); + p.setCompositionMode(QPainter::CompositionMode_SourceAtop); + p.fillRect(result.rect(), color); + p.end(); + return result; +} + +} // namespace autoware_overlay_rviz_plugin diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp new file mode 100644 index 0000000000000..4fea8f78b80da --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp @@ -0,0 +1,128 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "traffic_display.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware_overlay_rviz_plugin +{ + +TrafficDisplay::TrafficDisplay() +: tl_red_(QString("#cc3d3d")), + tl_yellow_(QString("#ccb43d")), + tl_green_(QString("#3dcc55")), + tl_gray_(QString("#4f4f4f")) +{ + // Load the traffic light image + std::string package_path = + ament_index_cpp::get_package_share_directory("autoware_overlay_rviz_plugin"); + std::string image_path = package_path + "/assets/images/traffic.png"; + traffic_light_image_.load(image_path.c_str()); +} + +void TrafficDisplay::updateTrafficLightData( + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr & msg) +{ + current_traffic_ = *msg; +} + +void TrafficDisplay::drawTrafficLightIndicator(QPainter & painter, const QRectF & backgroundRect) +{ + // Enable Antialiasing for smoother drawing + painter.setRenderHint(QPainter::Antialiasing, true); + painter.setRenderHint(QPainter::SmoothPixmapTransform, true); + + painter.setBrush(QBrush(tl_gray_, Qt::SolidPattern)); + painter.setPen(Qt::NoPen); + // Define the area for the circle (background) + QRectF circleRect = backgroundRect; + circleRect.setWidth(backgroundRect.width() - 30); + circleRect.setHeight(backgroundRect.width() - 30); + circleRect.moveTopRight(QPointF( + backgroundRect.left() + circleRect.width() + 15, + backgroundRect.top() + circleRect.height() + 30)); + painter.drawEllipse(circleRect); + + if (!current_traffic_.signals.empty()) { + switch (current_traffic_.signals[0].elements[0].color) { + case 1: + painter.setBrush(QBrush(tl_red_)); + painter.drawEllipse(circleRect); + break; + case 2: + painter.setBrush(QBrush(tl_yellow_)); + painter.drawEllipse(circleRect); + break; + case 3: + painter.setBrush(QBrush(tl_green_)); + painter.drawEllipse(circleRect); + break; + case 4: + painter.setBrush(tl_gray_); + painter.drawEllipse(circleRect); + break; + default: + painter.setBrush(tl_gray_); + painter.drawEllipse(circleRect); + break; + } + } + + // Scaling factor (e.g., 1.5 for 150% size) + float scaleFactor = 1.00; + + // Calculate the scaled size + QSize scaledSize = traffic_light_image_.size() * scaleFactor; + + // Ensure the scaled image is still within the circle bounds or adjust scaleFactor accordingly + + // Calculate the centered rectangle for the scaled image + QRectF scaledImageRect(0, 0, scaledSize.width(), scaledSize.height()); + scaledImageRect.moveCenter(circleRect.center()); + + // Scale the image to the new size + QImage scaledTrafficImage = + traffic_light_image_.scaled(scaledSize, Qt::KeepAspectRatio, Qt::SmoothTransformation); + + // Draw the scaled and centered image + painter.drawImage(scaledImageRect.topLeft(), scaledTrafficImage); +} + +QImage TrafficDisplay::coloredImage(const QImage & source, const QColor & color) +{ + QImage result = source; + QPainter p(&result); + p.setCompositionMode(QPainter::CompositionMode_SourceAtop); + p.fillRect(result.rect(), color); + p.end(); + return result; +} + +} // namespace autoware_overlay_rviz_plugin diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp new file mode 100644 index 0000000000000..b6d37062bf08b --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp @@ -0,0 +1,123 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "turn_signals_display.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware_overlay_rviz_plugin +{ + +TurnSignalsDisplay::TurnSignalsDisplay() : current_turn_signal_(0) +{ + last_toggle_time_ = std::chrono::steady_clock::now(); + + // Load the arrow image + std::string package_path = + ament_index_cpp::get_package_share_directory("autoware_overlay_rviz_plugin"); + std::string image_path = package_path + "/assets/images/arrow.png"; + arrowImage.load(image_path.c_str()); +} + +void TurnSignalsDisplay::updateTurnSignalsData( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg) +{ + try { + // Assuming msg->report is the field you're interested in + current_turn_signal_ = msg->report; + } catch (const std::exception & e) { + // Log the error + std::cerr << "Error in processMessage: " << e.what() << std::endl; + } +} + +void TurnSignalsDisplay::updateHazardLightsData( + const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg) +{ + try { + // Assuming msg->report is the field you're interested in + current_hazard_lights_ = msg->report; + } catch (const std::exception & e) { + // Log the error + std::cerr << "Error in processMessage: " << e.what() << std::endl; + } +} + +void TurnSignalsDisplay::drawArrows( + QPainter & painter, const QRectF & backgroundRect, const QColor & color) +{ + QImage scaledLeftArrow = arrowImage.scaled(64, 43, Qt::KeepAspectRatio, Qt::SmoothTransformation); + scaledLeftArrow = coloredImage(scaledLeftArrow, gray); + QImage scaledRightArrow = scaledLeftArrow.mirrored(true, false); + int arrowYPos = (backgroundRect.height() / 3 - scaledLeftArrow.height() / 2); + int leftArrowXPos = backgroundRect.width() / 4 - scaledLeftArrow.width(); // Adjust as needed + int rightArrowXPos = backgroundRect.width() * 3 / 4; // Adjust as needed + + bool leftActive = + (current_turn_signal_ == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT || + current_hazard_lights_ == autoware_auto_vehicle_msgs::msg::HazardLightsReport::ENABLE); + bool rightActive = + (current_turn_signal_ == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT || + current_hazard_lights_ == autoware_auto_vehicle_msgs::msg::HazardLightsReport::ENABLE); + + // Color the arrows based on the state of the turn signals and hazard lights by having them blink + // on and off + if (this->blink_on_) { + if (leftActive) { + scaledLeftArrow = coloredImage(scaledLeftArrow, color); + } + if (rightActive) { + scaledRightArrow = coloredImage(scaledRightArrow, color); + } + } + + // Draw the arrows + painter.drawImage(QPointF(leftArrowXPos, arrowYPos), scaledLeftArrow); + painter.drawImage(QPointF(rightArrowXPos, arrowYPos), scaledRightArrow); + + auto now = std::chrono::steady_clock::now(); + if ( + std::chrono::duration_cast(now - last_toggle_time_) >= + blink_interval_) { + blink_on_ = !blink_on_; // Toggle the blink state + last_toggle_time_ = now; + } +} + +QImage TurnSignalsDisplay::coloredImage(const QImage & source, const QColor & color) +{ + QImage result = source; + QPainter p(&result); + p.setCompositionMode(QPainter::CompositionMode_SourceAtop); + p.fillRect(result.rect(), color); + p.end(); + return result; +} + +} // namespace autoware_overlay_rviz_plugin diff --git a/common/component_interface_specs/CMakeLists.txt b/common/component_interface_specs/CMakeLists.txt index b519a0ddce86a..146f3688513cd 100644 --- a/common/component_interface_specs/CMakeLists.txt +++ b/common/component_interface_specs/CMakeLists.txt @@ -4,26 +4,8 @@ project(component_interface_specs) find_package(autoware_cmake REQUIRED) autoware_package() -include_directories( - include - SYSTEM - ${rclcpp_INCLUDE_DIRS} - ${rosidl_runtime_cpp_INCLUDE_DIRS} - ${rcl_INCLUDE_DIRS} - ${autoware_adapi_v1_msgs_INCLUDE_DIRS} - ${autoware_auto_planning_msgs_INCLUDE_DIRS} - ${autoware_planning_msgs_INCLUDE_DIRS} - ${autoware_auto_vehicle_msgs_INCLUDE_DIRS} - ${tier4_control_msgs_INCLUDE_DIRS} - ${nav_msgs_INCLUDE_DIRS} - ${tier4_system_msgs_INCLUDE_DIRS} - ${tier4_vehicle_msgs_INCLUDE_DIRS} - ${autoware_auto_perception_msgs_INCLUDE_DIRS} - ${tier4_map_msgs_INCLUDE_DIRS} -) - if(BUILD_TESTING) - ament_add_ros_isolated_gtest(test_component_interface_specs + ament_auto_add_gtest(gtest_${PROJECT_NAME} test/gtest_main.cpp test/test_planning.cpp test/test_control.cpp @@ -33,9 +15,6 @@ if(BUILD_TESTING) test/test_perception.cpp test/test_vehicle.cpp ) - target_include_directories(test_component_interface_specs - PRIVATE include - ) endif() ament_auto_package() diff --git a/common/component_interface_specs/include/component_interface_specs/planning.hpp b/common/component_interface_specs/include/component_interface_specs/planning.hpp index 567ea9d69f2a7..9efd8c871f68f 100644 --- a/common/component_interface_specs/include/component_interface_specs/planning.hpp +++ b/common/component_interface_specs/include/component_interface_specs/planning.hpp @@ -17,77 +17,48 @@ #include -#include -#include -#include -#include #include #include +#include +#include +#include +#include namespace planning_interface { -struct SetRoutePoints +struct SetLaneletRoute { - using Service = autoware_adapi_v1_msgs::srv::SetRoutePoints; - static constexpr char name[] = "/planning/mission_planning/set_route_points"; + using Service = tier4_planning_msgs::srv::SetLaneletRoute; + static constexpr char name[] = "/planning/mission_planning/route_selector/main/set_lanelet_route"; }; -struct SetRoute +struct SetWaypointRoute { - using Service = autoware_adapi_v1_msgs::srv::SetRoute; - static constexpr char name[] = "/planning/mission_planning/set_route"; -}; - -struct ChangeRoutePoints -{ - using Service = autoware_adapi_v1_msgs::srv::SetRoutePoints; - static constexpr char name[] = "/planning/mission_planning/change_route_points"; -}; - -struct ChangeRoute -{ - using Service = autoware_adapi_v1_msgs::srv::SetRoute; - static constexpr char name[] = "/planning/mission_planning/change_route"; + using Service = tier4_planning_msgs::srv::SetWaypointRoute; + static constexpr char name[] = + "/planning/mission_planning/route_selector/main/set_waypoint_route"; }; struct ClearRoute { - using Service = autoware_adapi_v1_msgs::srv::ClearRoute; - static constexpr char name[] = "/planning/mission_planning/clear_route"; + using Service = tier4_planning_msgs::srv::ClearRoute; + static constexpr char name[] = "/planning/mission_planning/route_selector/main/clear_route"; }; struct RouteState { - using Message = autoware_adapi_v1_msgs::msg::RouteState; - static constexpr char name[] = "/planning/mission_planning/route_state"; - static constexpr size_t depth = 1; - static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; -}; - -struct Route -{ - using Message = autoware_planning_msgs::msg::LaneletRoute; - static constexpr char name[] = "/planning/mission_planning/route"; - static constexpr size_t depth = 1; - static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; -}; - -struct NormalRoute -{ - using Message = autoware_planning_msgs::msg::LaneletRoute; - static constexpr char name[] = "/planning/mission_planning/normal_route"; + using Message = tier4_planning_msgs::msg::RouteState; + static constexpr char name[] = "/planning/mission_planning/route_selector/main/state"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; }; -struct MrmRoute +struct LaneletRoute { using Message = autoware_planning_msgs::msg::LaneletRoute; - static constexpr char name[] = "/planning/mission_planning/mrm_route"; + static constexpr char name[] = "/planning/mission_planning/route_selector/main/route"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; diff --git a/common/component_interface_specs/package.xml b/common/component_interface_specs/package.xml index 93f77c651869d..1ad5f410a814a 100644 --- a/common/component_interface_specs/package.xml +++ b/common/component_interface_specs/package.xml @@ -5,20 +5,12 @@ 0.0.0 The component_interface_specs package Takagi, Isamu - yabuta - Kah Hooi Tan Yukihiro Saito Apache License 2.0 ament_cmake_auto - ament_cmake_core - ament_cmake_export_dependencies - ament_cmake_test autoware_cmake - ament_cmake_core - ament_cmake_test - autoware_adapi_v1_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs @@ -30,9 +22,11 @@ rosidl_runtime_cpp tier4_control_msgs tier4_map_msgs + tier4_planning_msgs tier4_system_msgs tier4_vehicle_msgs - ament_cmake_ros + + ament_cmake_gtest ament_lint_auto autoware_lint_common diff --git a/common/component_interface_specs/test/test_planning.cpp b/common/component_interface_specs/test/test_planning.cpp index 8c504cb119854..c9cf353de5f34 100644 --- a/common/component_interface_specs/test/test_planning.cpp +++ b/common/component_interface_specs/test/test_planning.cpp @@ -27,26 +27,8 @@ TEST(planning, interface) } { - using planning_interface::Route; - Route route; - size_t depth = 1; - EXPECT_EQ(route.depth, depth); - EXPECT_EQ(route.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); - EXPECT_EQ(route.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); - } - - { - using planning_interface::NormalRoute; - NormalRoute route; - size_t depth = 1; - EXPECT_EQ(route.depth, depth); - EXPECT_EQ(route.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); - EXPECT_EQ(route.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); - } - - { - using planning_interface::MrmRoute; - MrmRoute route; + using planning_interface::LaneletRoute; + LaneletRoute route; size_t depth = 1; EXPECT_EQ(route.depth, depth); EXPECT_EQ(route.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); diff --git a/common/component_interface_tools/package.xml b/common/component_interface_tools/package.xml index ebf0684d0066c..cff1829473e86 100644 --- a/common/component_interface_tools/package.xml +++ b/common/component_interface_tools/package.xml @@ -5,8 +5,6 @@ 0.1.0 The component_interface_tools package Takagi, Isamu - yabuta - Kah Hooi Tan Apache License 2.0 ament_cmake_auto diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp index bc48e2a0294e0..97f46933f2fe6 100644 --- a/common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp +++ b/common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp @@ -52,7 +52,7 @@ struct NodeInterface {ServiceLog::CLIENT_RESPONSE, "client exit"}, {ServiceLog::ERROR_UNREADY, "client unready"}, {ServiceLog::ERROR_TIMEOUT, "client timeout"}}); - RCLCPP_INFO_STREAM(node->get_logger(), type_text.at(type) << ": " << name); + RCLCPP_DEBUG_STREAM(node->get_logger(), type_text.at(type) << ": " << name); ServiceLog msg; msg.stamp = node->now(); diff --git a/common/component_interface_utils/package.xml b/common/component_interface_utils/package.xml index dc82fda3f5c64..7a6a6d12880ad 100755 --- a/common/component_interface_utils/package.xml +++ b/common/component_interface_utils/package.xml @@ -5,8 +5,6 @@ 0.0.0 The component_interface_utils package Takagi, Isamu - yabuta - Kah Hooi Tan Yukihiro Saito Apache License 2.0 diff --git a/common/fake_test_node/test/test_fake_test_node.cpp b/common/fake_test_node/test/test_fake_test_node.cpp index d66d0fed3033c..e065f332b75e4 100644 --- a/common/fake_test_node/test/test_fake_test_node.cpp +++ b/common/fake_test_node/test/test_fake_test_node.cpp @@ -17,7 +17,7 @@ /// \copyright Copyright 2021 Apex.AI, Inc. /// All rights reserved. -#include +#include #include #include diff --git a/common/geography_utils/src/lanelet2_projector.cpp b/common/geography_utils/src/lanelet2_projector.cpp index 3e982ac2ccf4d..532606248cd1f 100644 --- a/common/geography_utils/src/lanelet2_projector.cpp +++ b/common/geography_utils/src/lanelet2_projector.cpp @@ -34,6 +34,7 @@ std::unique_ptr get_lanelet2_projector(const MapProjectorInf } else if (projector_info.projector_type == MapProjectorInfo::MGRS) { lanelet::projection::MGRSProjector projector{}; + projector.setMGRSCode(projector_info.mgrs_grid); return std::make_unique(projector); } else if (projector_info.projector_type == MapProjectorInfo::TRANSVERSE_MERCATOR) { 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/include/interpolation/spherical_linear_interpolation.hpp b/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp index 516b3ab09e8b7..8c1d49fb5f607 100644 --- a/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp +++ b/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp @@ -39,6 +39,10 @@ std::vector slerp( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys); + +geometry_msgs::msg::Quaternion lerpOrientation( + const geometry_msgs::msg::Quaternion & o_from, const geometry_msgs::msg::Quaternion & o_to, + const double ratio); } // namespace interpolation #endif // INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ diff --git a/common/interpolation/src/spherical_linear_interpolation.cpp b/common/interpolation/src/spherical_linear_interpolation.cpp index c3595d212f349..e44626498a80b 100644 --- a/common/interpolation/src/spherical_linear_interpolation.cpp +++ b/common/interpolation/src/spherical_linear_interpolation.cpp @@ -57,4 +57,15 @@ std::vector slerp( return query_values; } +geometry_msgs::msg::Quaternion lerpOrientation( + const geometry_msgs::msg::Quaternion & o_from, const geometry_msgs::msg::Quaternion & o_to, + const double ratio) +{ + tf2::Quaternion q_from, q_to; + tf2::fromMsg(o_from, q_from); + tf2::fromMsg(o_to, q_to); + + const auto q_interpolated = q_from.slerp(q_to, ratio); + return tf2::toMsg(q_interpolated); +} } // namespace interpolation 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/mission_planner_rviz_plugin/CMakeLists.txt b/common/mission_planner_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..2b06e6db3e70d --- /dev/null +++ b/common/mission_planner_rviz_plugin/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.14) +project(mission_planner_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/mrm_goal.cpp + src/route_selector_panel.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} +) + +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package() diff --git a/common/mission_planner_rviz_plugin/README.md b/common/mission_planner_rviz_plugin/README.md new file mode 100644 index 0000000000000..59d36a0a2f840 --- /dev/null +++ b/common/mission_planner_rviz_plugin/README.md @@ -0,0 +1,18 @@ +# mission_planner_rviz_plugin + +## MrmGoalTool + +This is a copy of `rviz_default_plugins::tools::GoalTool`. Used together with the RouteSelectorPanel to set the MRM route. +After adding the tool, change the topic name to `/rviz/route_selector/mrm/goal` from the topic property panel in rviz. + +## RouteSelectorPanel + +This panel shows the main and mrm route state in the route_selector and the route states in the mission_planner. +Additionally, it provides clear and set functions for each main route and mrm route. + +| Trigger | Action | +| -------------------------------------- | ------------------------------------------------------------------------ | +| main route clear button | call `/planning/mission_planning/route_selector/main/clear_route` | +| mrm route clear button | call `/planning/mission_planning/route_selector/mrm/clear_route` | +| `/rviz/route_selector/main/goal` topic | call `/planning/mission_planning/route_selector/main/set_waypoint_route` | +| `/rviz/route_selector/mrm/goal` topic | call `/planning/mission_planning/route_selector/mrm/set_waypoint_route` | diff --git a/common/mission_planner_rviz_plugin/package.xml b/common/mission_planner_rviz_plugin/package.xml new file mode 100644 index 0000000000000..e45cf2739f260 --- /dev/null +++ b/common/mission_planner_rviz_plugin/package.xml @@ -0,0 +1,29 @@ + + + + mission_planner_rviz_plugin + 0.0.0 + The mission_planner_rviz_plugin package + Takagi, Isamu + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + geometry_msgs + libqt5-core + libqt5-gui + libqt5-widgets + rclcpp + rviz_common + rviz_default_plugins + tier4_planning_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/common/mission_planner_rviz_plugin/plugins/plugin_description.xml b/common/mission_planner_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..c8285fcc604d7 --- /dev/null +++ b/common/mission_planner_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,8 @@ + + + MrmGoalTool + + + RouteSelectorPanel + + diff --git a/common/mission_planner_rviz_plugin/src/mrm_goal.cpp b/common/mission_planner_rviz_plugin/src/mrm_goal.cpp new file mode 100644 index 0000000000000..ef5529abfb4a7 --- /dev/null +++ b/common/mission_planner_rviz_plugin/src/mrm_goal.cpp @@ -0,0 +1,34 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "mrm_goal.hpp" + +namespace rviz_plugins +{ + +MrmGoalTool::MrmGoalTool() +{ + shortcut_key_ = 'm'; +} + +void MrmGoalTool::onInitialize() +{ + GoalTool::onInitialize(); + setName("MRM Goal Pose"); +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::MrmGoalTool, rviz_common::Tool) diff --git a/common/mission_planner_rviz_plugin/src/mrm_goal.hpp b/common/mission_planner_rviz_plugin/src/mrm_goal.hpp new file mode 100644 index 0000000000000..e16b0abf0bab3 --- /dev/null +++ b/common/mission_planner_rviz_plugin/src/mrm_goal.hpp @@ -0,0 +1,34 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MRM_GOAL_HPP_ +#define MRM_GOAL_HPP_ + +#include + +namespace rviz_plugins +{ + +class MrmGoalTool : public rviz_default_plugins::tools::GoalTool +{ + Q_OBJECT + +public: + MrmGoalTool(); + void onInitialize() override; +}; + +} // namespace rviz_plugins + +#endif // MRM_GOAL_HPP_ diff --git a/common/mission_planner_rviz_plugin/src/route_selector_panel.cpp b/common/mission_planner_rviz_plugin/src/route_selector_panel.cpp new file mode 100644 index 0000000000000..b4b376b1e76ec --- /dev/null +++ b/common/mission_planner_rviz_plugin/src/route_selector_panel.cpp @@ -0,0 +1,148 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "route_selector_panel.hpp" + +#include +#include + +#include +#include + +namespace rviz_plugins +{ + +QString to_string_state(const RouteState & state) +{ + // clang-format off + switch (state.state) { + case RouteState::UNKNOWN: return "unknown"; + case RouteState::INITIALIZING: return "initializing"; + case RouteState::UNSET: return "unset"; + case RouteState::ROUTING: return "routing"; + case RouteState::SET: return "set"; + case RouteState::REROUTING: return "rerouting"; + case RouteState::ARRIVED: return "arrived"; + case RouteState::ABORTED: return "aborted"; + case RouteState::INTERRUPTED: return "interrupted"; + default: return "-----"; + } + // clang-format on +} + +RouteSelectorPanel::RouteSelectorPanel(QWidget * parent) : rviz_common::Panel(parent) +{ + main_state_ = new QLabel("unknown"); + main_clear_ = new QPushButton("clear"); + mrm_state_ = new QLabel("unknown"); + mrm_clear_ = new QPushButton("clear"); + planner_state_ = new QLabel("unknown"); + + connect(main_clear_, &QPushButton::clicked, this, &RouteSelectorPanel::onMainClear); + connect(mrm_clear_, &QPushButton::clicked, this, &RouteSelectorPanel::onMrmClear); + + const auto layout = new QGridLayout(); + setLayout(layout); + layout->addWidget(new QLabel("main"), 0, 0); + layout->addWidget(main_state_, 0, 1); + layout->addWidget(main_clear_, 0, 2); + layout->addWidget(new QLabel("mrm"), 1, 0); + layout->addWidget(mrm_state_, 1, 1); + layout->addWidget(mrm_clear_, 1, 2); + layout->addWidget(new QLabel("planner"), 2, 0); + layout->addWidget(planner_state_, 2, 1); +} + +void RouteSelectorPanel::onInitialize() +{ + auto lock = getDisplayContext()->getRosNodeAbstraction().lock(); + auto node = lock->get_raw_node(); + + const auto durable_qos = rclcpp::QoS(1).transient_local(); + + sub_main_goal_ = node->create_subscription( + "/rviz/route_selector/main/goal", rclcpp::QoS(1), + std::bind(&RouteSelectorPanel::onMainGoal, this, std::placeholders::_1)); + sub_mrm_goal_ = node->create_subscription( + "/rviz/route_selector/mrm/goal", rclcpp::QoS(1), + std::bind(&RouteSelectorPanel::onMrmGoal, this, std::placeholders::_1)); + sub_main_state_ = node->create_subscription( + "/planning/mission_planning/route_selector/main/state", durable_qos, + std::bind(&RouteSelectorPanel::onMainState, this, std::placeholders::_1)); + sub_mrm_state_ = node->create_subscription( + "/planning/mission_planning/route_selector/mrm/state", durable_qos, + std::bind(&RouteSelectorPanel::onMrmState, this, std::placeholders::_1)); + sub_planner_state_ = node->create_subscription( + "/planning/mission_planning/state", durable_qos, + std::bind(&RouteSelectorPanel::onPlannerState, this, std::placeholders::_1)); + + cli_main_clear_ = + node->create_client("/planning/mission_planning/route_selector/main/clear_route"); + cli_mrm_clear_ = + node->create_client("/planning/mission_planning/route_selector/mrm/clear_route"); + cli_main_set_waypoint_ = node->create_client( + "/planning/mission_planning/route_selector/main/set_waypoint_route"); + cli_mrm_set_waypoint_ = node->create_client( + "/planning/mission_planning/route_selector/mrm/set_waypoint_route"); +} + +void RouteSelectorPanel::onMainGoal(const PoseStamped::ConstSharedPtr msg) +{ + const auto req = std::make_shared(); + req->header = msg->header; + req->goal_pose = msg->pose; + req->allow_modification = true; + cli_main_set_waypoint_->async_send_request(req); +} + +void RouteSelectorPanel::onMrmGoal(const PoseStamped::ConstSharedPtr msg) +{ + const auto req = std::make_shared(); + req->header = msg->header; + req->goal_pose = msg->pose; + req->allow_modification = true; + cli_mrm_set_waypoint_->async_send_request(req); +} + +void RouteSelectorPanel::onMainState(RouteState::ConstSharedPtr msg) +{ + main_state_->setText(to_string_state(*msg)); +} + +void RouteSelectorPanel::onMrmState(RouteState::ConstSharedPtr msg) +{ + mrm_state_->setText(to_string_state(*msg)); +} + +void RouteSelectorPanel::onPlannerState(RouteState::ConstSharedPtr msg) +{ + planner_state_->setText(to_string_state(*msg)); +} + +void RouteSelectorPanel::onMainClear() +{ + const auto req = std::make_shared(); + cli_main_clear_->async_send_request(req); +} + +void RouteSelectorPanel::onMrmClear() +{ + const auto req = std::make_shared(); + cli_mrm_clear_->async_send_request(req); +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::RouteSelectorPanel, rviz_common::Panel) diff --git a/common/mission_planner_rviz_plugin/src/route_selector_panel.hpp b/common/mission_planner_rviz_plugin/src/route_selector_panel.hpp new file mode 100644 index 0000000000000..99101730661e0 --- /dev/null +++ b/common/mission_planner_rviz_plugin/src/route_selector_panel.hpp @@ -0,0 +1,75 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROUTE_SELECTOR_PANEL_HPP_ +#define ROUTE_SELECTOR_PANEL_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace rviz_plugins +{ + +using geometry_msgs::msg::PoseStamped; +using tier4_planning_msgs::msg::RouteState; +using tier4_planning_msgs::srv::ClearRoute; +using tier4_planning_msgs::srv::SetWaypointRoute; + +class RouteSelectorPanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit RouteSelectorPanel(QWidget * parent = nullptr); + void onInitialize() override; + +private: + QLabel * main_state_; + QLabel * mrm_state_; + QLabel * planner_state_; + QPushButton * main_clear_; + QPushButton * mrm_clear_; + + rclcpp::Subscription::SharedPtr sub_main_goal_; + rclcpp::Subscription::SharedPtr sub_mrm_goal_; + void onMainGoal(const PoseStamped::ConstSharedPtr msg); + void onMrmGoal(const PoseStamped::ConstSharedPtr msg); + + rclcpp::Subscription::SharedPtr sub_main_state_; + rclcpp::Subscription::SharedPtr sub_mrm_state_; + rclcpp::Subscription::SharedPtr sub_planner_state_; + void onMainState(RouteState::ConstSharedPtr msg); + void onMrmState(RouteState::ConstSharedPtr msg); + void onPlannerState(RouteState::ConstSharedPtr msg); + + rclcpp::Client::SharedPtr cli_main_clear_; + rclcpp::Client::SharedPtr cli_mrm_clear_; + rclcpp::Client::SharedPtr cli_main_set_waypoint_; + rclcpp::Client::SharedPtr cli_mrm_set_waypoint_; + +private Q_SLOTS: + void onMainClear(); + void onMrmClear(); +}; + +} // namespace rviz_plugins + +#endif // ROUTE_SELECTOR_PANEL_HPP_ diff --git a/common/motion_utils/CMakeLists.txt b/common/motion_utils/CMakeLists.txt index 43c8158b2f98b..f42deaa7f8c41 100644 --- a/common/motion_utils/CMakeLists.txt +++ b/common/motion_utils/CMakeLists.txt @@ -14,7 +14,7 @@ ament_auto_add_library(motion_utils SHARED src/trajectory/trajectory.cpp src/trajectory/interpolation.cpp src/trajectory/path_with_lane_id.cpp - src/trajectory/tmp_conversion.cpp + src/trajectory/conversion.cpp src/vehicle/vehicle_state_checker.cpp ) 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/marker/virtual_wall_marker_creator.hpp b/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp index 95bbcd93e30df..be5a70ed7ffc9 100644 --- a/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp +++ b/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp @@ -20,9 +20,11 @@ #include #include +#include #include #include #include + namespace motion_utils { @@ -38,7 +40,7 @@ struct VirtualWall double longitudinal_offset{}; bool is_driving_forward{true}; }; -typedef std::vector VirtualWalls; +using VirtualWalls = std::vector; /// @brief class to manage the creation of virtual wall markers /// @details creates both ADD and DELETE markers @@ -55,8 +57,8 @@ class VirtualWallMarkerCreator const rclcpp::Time & now, const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, const bool is_driving_forward)>; - VirtualWalls virtual_walls; - std::unordered_map marker_count_per_namespace; + VirtualWalls virtual_walls_; + std::unordered_map marker_count_per_namespace_; /// @brief internal cleanup: clear the stored markers and remove unused namespace from the map void cleanup(); diff --git a/common/motion_utils/include/motion_utils/resample/resample.hpp b/common/motion_utils/include/motion_utils/resample/resample.hpp index 106ed32b18576..258386ca236a3 100644 --- a/common/motion_utils/include/motion_utils/resample/resample.hpp +++ b/common/motion_utils/include/motion_utils/resample/resample.hpp @@ -15,13 +15,10 @@ #ifndef MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ #define MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ -#include "autoware_auto_planning_msgs/msg/path.hpp" -#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_planning_msgs/msg/detail/path__struct.hpp" +#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" +#include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" -#include -#include -#include #include namespace motion_utils @@ -187,7 +184,8 @@ autoware_auto_planning_msgs::msg::Path resamplePath( autoware_auto_planning_msgs::msg::Path resamplePath( const autoware_auto_planning_msgs::msg::Path & input_path, const double resample_interval, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, - const bool use_zero_order_hold_for_v = true, const bool resample_input_path_stop_point = true); + const bool use_zero_order_hold_for_twist = true, + const bool resample_input_path_stop_point = true); /** * @brief A resampling function for a trajectory. Note that in a default setting, position xy are diff --git a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp b/common/motion_utils/include/motion_utils/resample/resample_utils.hpp index 1e4d0d633325e..8bb5f13e3fb78 100644 --- a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp +++ b/common/motion_utils/include/motion_utils/resample/resample_utils.hpp @@ -15,6 +15,8 @@ #ifndef MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ #define MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ +#include "tier4_autoware_utils/system/backtrace.hpp" + #include #include #include @@ -23,26 +25,21 @@ namespace resample_utils { -constexpr double CLOSE_S_THRESHOLD = 1e-6; +constexpr double close_s_threshold = 1e-6; + +#define log_error(message) std::cerr << "\033[31m " << message << " \033[0m" << std::endl; template bool validate_size(const T & points) { - if (points.size() < 2) { - return false; - } - return true; + return points.size() >= 2; } template bool validate_resampling_range(const T & points, const std::vector & resampling_intervals) { const double points_length = motion_utils::calcArcLength(points); - if (points_length < resampling_intervals.back()) { - return false; - } - - return true; + return points_length >= resampling_intervals.back(); } template @@ -52,7 +49,7 @@ bool validate_points_duplication(const T & points) const auto & curr_pt = tier4_autoware_utils::getPoint(points.at(i)); const auto & next_pt = tier4_autoware_utils::getPoint(points.at(i + 1)); const double ds = tier4_autoware_utils::calcDistance2d(curr_pt, next_pt); - if (ds < CLOSE_S_THRESHOLD) { + if (ds < close_s_threshold) { return false; } } @@ -65,22 +62,28 @@ bool validate_arguments(const T & input_points, const std::vector & resa { // Check size of the arguments if (!validate_size(input_points)) { - std::cerr << "The number of input points is less than 2" << std::endl; + log_error("[resample_utils] invalid argument: The number of input points is less than 2"); + tier4_autoware_utils::print_backtrace(); return false; - } else if (!validate_size(resampling_intervals)) { - std::cerr << "The number of resampling intervals is less than 2" << std::endl; + } + if (!validate_size(resampling_intervals)) { + log_error( + "[resample_utils] invalid argument: The number of resampling intervals is less than 2"); + tier4_autoware_utils::print_backtrace(); return false; } // Check resampling range if (!validate_resampling_range(input_points, resampling_intervals)) { - std::cerr << "resampling interval is longer than input points" << std::endl; + log_error("[resample_utils] invalid argument: resampling interval is longer than input points"); + tier4_autoware_utils::print_backtrace(); return false; } // Check duplication if (!validate_points_duplication(input_points)) { - std::cerr << "input points has some duplicated points" << std::endl; + log_error("[resample_utils] invalid argument: input points has some duplicated points"); + tier4_autoware_utils::print_backtrace(); return false; } @@ -92,20 +95,24 @@ bool validate_arguments(const T & input_points, const double resampling_interval { // Check size of the arguments if (!validate_size(input_points)) { - std::cerr << "The number of input points is less than 2" << std::endl; + log_error("[resample_utils] invalid argument: The number of input points is less than 2"); + tier4_autoware_utils::print_backtrace(); return false; } // check resampling interval if (resampling_interval < motion_utils::overlap_threshold) { - std::cerr << "resampling interval is less than " << motion_utils::overlap_threshold - << std::endl; + log_error( + "[resample_utils] invalid argument: resampling interval is less than " + + std::to_string(motion_utils::overlap_threshold)); + tier4_autoware_utils::print_backtrace(); return false; } // Check duplication if (!validate_points_duplication(input_points)) { - std::cerr << "input points has some duplicated points" << std::endl; + log_error("[resample_utils] invalid argument: input points has some duplicated points"); + tier4_autoware_utils::print_backtrace(); return false; } diff --git a/common/motion_utils/include/motion_utils/trajectory/conversion.hpp b/common/motion_utils/include/motion_utils/trajectory/conversion.hpp new file mode 100644 index 0000000000000..7d4be216e89fe --- /dev/null +++ b/common/motion_utils/include/motion_utils/trajectory/conversion.hpp @@ -0,0 +1,121 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ +#define MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ + +#include "autoware_auto_planning_msgs/msg/detail/path__struct.hpp" +#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" +#include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" +#include "autoware_auto_planning_msgs/msg/detail/trajectory_point__struct.hpp" +#include "std_msgs/msg/header.hpp" + +#include + +namespace motion_utils +{ +using TrajectoryPoints = std::vector; + +/** + * @brief Convert std::vector to + * autoware_auto_planning_msgs::msg::Trajectory. This function is temporarily added for porting to + * autoware_auto_msgs. We should consider whether to remove this function after the porting is done. + * @attention This function just clips + * std::vector up to the capacity of Trajectory. + * Therefore, the error handling out of this function is necessary if the size of the input greater + * than the capacity. + * @todo Decide how to handle the situation that we need to use the trajectory with the size of + * points larger than the capacity. (Tier IV) + */ +autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( + const std::vector & trajectory, + const std_msgs::msg::Header & header = std_msgs::msg::Header{}); + +/** + * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to + * std::vector. + */ +std::vector convertToTrajectoryPointArray( + const autoware_auto_planning_msgs::msg::Trajectory & trajectory); + +template +autoware_auto_planning_msgs::msg::Path convertToPath([[maybe_unused]] const T & input) +{ + static_assert(sizeof(T) == 0, "Only specializations of convertToPath can be used."); + throw std::logic_error("Only specializations of convertToPath can be used."); +} + +template <> +inline autoware_auto_planning_msgs::msg::Path convertToPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input) +{ + autoware_auto_planning_msgs::msg::Path output{}; + output.header = input.header; + output.left_bound = input.left_bound; + output.right_bound = input.right_bound; + output.points.resize(input.points.size()); + for (size_t i = 0; i < input.points.size(); ++i) { + output.points.at(i) = input.points.at(i).point; + } + return output; +} + +template +TrajectoryPoints convertToTrajectoryPoints([[maybe_unused]] const T & input) +{ + static_assert(sizeof(T) == 0, "Only specializations of convertToTrajectoryPoints can be used."); + throw std::logic_error("Only specializations of convertToTrajectoryPoints can be used."); +} + +template <> +inline TrajectoryPoints convertToTrajectoryPoints( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input) +{ + TrajectoryPoints output{}; + for (const auto & p : input.points) { + autoware_auto_planning_msgs::msg::TrajectoryPoint tp; + tp.pose = p.point.pose; + tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps; + // since path point doesn't have acc for now + tp.acceleration_mps2 = 0; + output.emplace_back(tp); + } + return output; +} + +template +autoware_auto_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( + [[maybe_unused]] const T & input) +{ + static_assert(sizeof(T) == 0, "Only specializations of convertToPathWithLaneId can be used."); + throw std::logic_error("Only specializations of convertToPathWithLaneId can be used."); +} + +template <> +inline autoware_auto_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( + const TrajectoryPoints & input) +{ + autoware_auto_planning_msgs::msg::PathWithLaneId output{}; + for (const auto & p : input) { + autoware_auto_planning_msgs::msg::PathPointWithLaneId pp; + pp.point.pose = p.pose; + pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps; + output.points.emplace_back(pp); + } + return output; +} + +} // namespace motion_utils + +#endif // MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ diff --git a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp index 41a9e9dd41a40..aec329f9f540e 100644 --- a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp @@ -16,17 +16,12 @@ #define MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/constants.hpp" -#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" - -#include +#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" +#include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" #include #include -#include -#include namespace motion_utils { 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..d0ed761b76d9a 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 @@ -15,15 +15,14 @@ #ifndef MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ #define MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ -#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" +#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.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/tmp_conversion.hpp b/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp deleted file mode 100644 index 6921c56e0199e..0000000000000 --- a/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp +++ /dev/null @@ -1,48 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ -#define MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ - -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" - -#include - -namespace motion_utils -{ -/** - * @brief Convert std::vector to - * autoware_auto_planning_msgs::msg::Trajectory. This function is temporarily added for porting to - * autoware_auto_msgs. We should consider whether to remove this function after the porting is done. - * @attention This function just clips - * std::vector up to the capacity of Trajectory. - * Therefore, the error handling out of this function is necessary if the size of the input greater - * than the capacity. - * @todo Decide how to handle the situation that we need to use the trajectory with the size of - * points larger than the capacity. (Tier IV) - */ -autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( - const std::vector & trajectory); - -/** - * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to - * std::vector. - */ -std::vector convertToTrajectoryPointArray( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory); - -} // namespace motion_utils - -#endif // MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index f4f9f45935347..b9136bc4002e3 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -26,15 +26,16 @@ #include #include -#include - #include #include +#include #include +#include #include #include namespace motion_utils { +#define log_error(message) std::cerr << "\033[31m " << message << " \033[0m" << std::endl; /** * @brief validate if points container is empty or not @@ -45,7 +46,7 @@ void validateNonEmpty(const T & points) { if (points.empty()) { tier4_autoware_utils::print_backtrace(); - throw std::invalid_argument("Points is empty."); + throw std::invalid_argument("[motion_utils] validateNonEmpty(): Points is empty."); } } @@ -83,7 +84,7 @@ void validateNonSharpAngle( constexpr double epsilon = 1e-3; if (std::cos(angle_threshold) < product / dist_1to2 / dist_3to2 + epsilon) { tier4_autoware_utils::print_backtrace(); - throw std::invalid_argument("Sharp angle."); + throw std::invalid_argument("[motion_utils] validateNonSharpAngle(): Too sharp angle."); } } @@ -93,10 +94,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 +107,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,31 +124,31 @@ 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())) { return true; - } else if (0.0 > tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) { + } + 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,13 +210,13 @@ removeOverlapPoints -boost::optional searchZeroVelocityIndex( +std::optional searchZeroVelocityIndex( const T & points_with_twist, const size_t src_idx, const size_t dst_idx) { try { validateNonEmpty(points_with_twist); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -229,7 +230,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,19 +243,19 @@ 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); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } 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 +267,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 +330,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()) @@ -337,7 +338,7 @@ boost::optional findNearestIndex( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -363,20 +364,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(), @@ -398,12 +403,17 @@ double calcLongitudinalOffsetToSegment( const bool throw_exception = false) { if (seg_idx >= points.size() - 1) { - const std::out_of_range e("Segment index is invalid."); + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + ": Failed to calculate longitudinal offset because the given segment index is out of the " + "points size."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { - throw e; + throw std::out_of_range(error_message); } - std::cerr << e.what() << std::endl; + log_error( + error_message + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } @@ -415,18 +425,22 @@ double calcLongitudinalOffsetToSegment( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return std::nan(""); } } if (seg_idx >= overlap_removed_points.size() - 1) { - const std::runtime_error e("Same points are given."); + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + ": Longitudinal offset calculation is not supported for the same points."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { - throw e; + throw std::runtime_error(error_message); } - std::cerr << e.what() << std::endl; + log_error( + error_message + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } @@ -507,7 +521,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 +529,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 +548,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(), @@ -572,18 +586,24 @@ double calcLateralOffset( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error( + std::string(e.what()) + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } } if (overlap_removed_points.size() == 1) { - const std::runtime_error e("Same points are given."); + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + ": Lateral offset calculation is not supported for the same points."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { - throw e; + throw std::runtime_error(error_message); } - std::cerr << e.what() << std::endl; + log_error( + error_message + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } @@ -634,18 +654,24 @@ double calcLateralOffset( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error( + std::string(e.what()) + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } } if (overlap_removed_points.size() == 1) { - const std::runtime_error e("Same points are given."); + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + ": Lateral offset calculation is not supported for the same points."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { - throw e; + throw std::runtime_error(error_message); } - std::cerr << e.what() << std::endl; + log_error( + error_message + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } @@ -681,7 +707,7 @@ double calcSignedArcLength(const T & points, const size_t src_idx, const size_t try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } @@ -724,7 +750,7 @@ std::vector calcSignedArcLengthPartialSum( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -776,7 +802,7 @@ double calcSignedArcLength( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } @@ -819,7 +845,7 @@ double calcSignedArcLength( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } @@ -858,7 +884,7 @@ double calcSignedArcLength( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } @@ -898,7 +924,7 @@ double calcArcLength(const T & points) try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } @@ -926,7 +952,10 @@ calcArcLength>( template std::vector calcCurvature(const T & points) { - std::vector curvature_vec(points.size()); + std::vector curvature_vec(points.size(), 0.0); + if (points.size() < 3) { + return curvature_vec; + } for (size_t i = 1; i < points.size() - 1; ++i) { const auto p1 = tier4_autoware_utils::getPoint(points.at(i - 1)); @@ -965,7 +994,7 @@ std::vector> calcCurvatureAndArcLength(const T & point { // Note that arclength is for the segment, not the sum. std::vector> curvature_arc_length_vec; - curvature_arc_length_vec.push_back(std::pair(0.0, 0.0)); + curvature_arc_length_vec.emplace_back(0.0, 0.0); for (size_t i = 1; i < points.size() - 1; ++i) { const auto p1 = tier4_autoware_utils::getPoint(points.at(i - 1)); const auto p2 = tier4_autoware_utils::getPoint(points.at(i)); @@ -973,9 +1002,9 @@ std::vector> calcCurvatureAndArcLength(const T & point const double curvature = tier4_autoware_utils::calcCurvature(p1, p2, p3); const double arc_length = tier4_autoware_utils::calcDistance2d(points.at(i - 1), points.at(i)) + tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); - curvature_arc_length_vec.push_back(std::pair(curvature, arc_length)); + curvature_arc_length_vec.emplace_back(curvature, arc_length); } - curvature_arc_length_vec.push_back(std::pair(0.0, 0.0)); + curvature_arc_length_vec.emplace_back(0.0, 0.0); return curvature_arc_length_vec; } @@ -998,26 +1027,26 @@ calcCurvatureAndArcLength -boost::optional calcDistanceToForwardStopPoint( +std::optional calcDistanceToForwardStopPoint( const T & points_with_twist, const size_t src_idx = 0) { try { validateNonEmpty(points_with_twist); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } const auto closest_stop_idx = searchZeroVelocityIndex(points_with_twist, src_idx, points_with_twist.size()); if (!closest_stop_idx) { - return 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,23 +1061,28 @@ calcDistanceToForwardStopPoint -boost::optional calcLongitudinalOffsetPoint( +std::optional calcLongitudinalOffsetPoint( const T & points, const size_t src_idx, const double offset, const bool throw_exception = false) { try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } if (points.size() - 1 < src_idx) { - const auto e = std::out_of_range("Invalid source index"); + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + " error: The given source index is out of the points size. Failed to calculate longitudinal " + "offset."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { - throw e; + throw std::out_of_range(error_message); } - std::cerr << e.what() << std::endl; + log_error( + error_message + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return {}; } @@ -1087,15 +1121,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,13 +1143,13 @@ calcLongitudinalOffsetPoint -boost::optional calcLongitudinalOffsetPoint( +std::optional calcLongitudinalOffsetPoint( const T & points, const geometry_msgs::msg::Point & src_point, const double offset) { try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error("Failed to calculate longitudinal offset: " + std::string(e.what())); return {}; } @@ -1132,15 +1166,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,28 +1190,32 @@ 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) { try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error("Failed to calculate longitudinal offset: " + std::string(e.what())); return {}; } if (points.size() - 1 < src_idx) { - const auto e = std::out_of_range("Invalid source index"); + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + " error: The given source index is out of the points size. Failed to calculate longitudinal " + "offset."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { - throw e; + throw std::out_of_range(error_message); } - std::cerr << e.what() << std::endl; + log_error(error_message); return {}; } if (points.size() == 1) { + log_error("Failed to calculate longitudinal offset: points size is one."); return {}; } @@ -1228,17 +1266,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,14 +1293,14 @@ 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) { try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -1275,17 +1313,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,14 +1339,14 @@ 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) { try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -1323,7 +1361,7 @@ boost::optional insertTargetPoint( try { validateNonSharpAngle(p_front, p_target, p_back); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -1339,7 +1377,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 +1390,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 +1399,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 +1415,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 +1442,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 +1464,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 +1497,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 +1528,7 @@ boost::optional insertTargetPoint( } if (!segment_idx) { - return boost::none; + return std::nullopt; } // Get Target Point @@ -1505,17 +1543,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 +1573,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 +1581,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 +1596,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 +1625,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 +1648,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 +1673,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 +1694,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 +1727,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 +1735,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 +1752,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 +1781,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 +1789,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 +1813,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 +1824,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 +1841,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, @@ -2038,9 +2076,8 @@ size_t findFirstNearestIndexWithSoftConstraints( if (squared_dist_threshold < squared_dist || yaw_threshold < std::abs(yaw)) { if (is_within_constraints) { break; - } else { - continue; } + continue; } if (min_squared_dist <= squared_dist) { @@ -2070,9 +2107,8 @@ size_t findFirstNearestIndexWithSoftConstraints( if (squared_dist_threshold < squared_dist) { if (is_within_constraints) { break; - } else { - continue; } + continue; } if (min_squared_dist <= squared_dist) { @@ -2183,7 +2219,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()) @@ -2191,7 +2227,7 @@ boost::optional calcDistanceToForwardStopPoint( try { validateNonEmpty(points_with_twist); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error("Failed to calculate stop distance" + std::string(e.what())); return {}; } @@ -2199,14 +2235,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 +2251,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(), @@ -2330,9 +2366,7 @@ T cropPoints( cropped_forward_points, target_pos, modified_target_seg_idx, backward_length); if (cropped_points.size() < 2) { - RCLCPP_ERROR( - rclcpp::get_logger("obstacle_avoidance_planner.trajectory_utils"), - ". Return original points since cropped_points size is less than 2."); + log_error("Return original points since cropped_points size is less than 2."); return points; } @@ -2377,18 +2411,22 @@ double calcYawDeviation( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } } if (overlap_removed_points.size() <= 1) { - const std::runtime_error e("points size is less than 2"); + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + " Given points size is less than 2. Failed to calculate yaw deviation."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { - throw e; + throw std::runtime_error(error_message); } - std::cerr << e.what() << std::endl; + log_error( + error_message + + " Return 0 since no_throw option is enabled. The maintainer must check the code."); return 0.0; } diff --git a/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp b/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp index 6a9bd01a5a9a0..82433d8e3c241 100644 --- a/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp +++ b/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp @@ -22,7 +22,6 @@ #include #include -#include namespace motion_utils { diff --git a/common/motion_utils/src/distance/distance.cpp b/common/motion_utils/src/distance/distance.cpp index 14ba02ab38703..3ac37bbc7733b 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) { @@ -262,7 +262,8 @@ boost::optional calcDecelDistWithJerkAndAccConstraints( if (t_during_min_acc > epsilon) { return calcDecelDistPlanType1( current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec, t_during_min_acc); - } else if (is_decel_needed || current_acc > epsilon) { + } + if (is_decel_needed || current_acc > epsilon) { return calcDecelDistPlanType2(current_vel, target_vel, current_acc, jerk_acc, jerk_dec); } diff --git a/common/motion_utils/src/marker/marker_helper.cpp b/common/motion_utils/src/marker/marker_helper.cpp index 50def253b6d16..6e9c45adc241b 100644 --- a/common/motion_utils/src/marker/marker_helper.cpp +++ b/common/motion_utils/src/marker/marker_helper.cpp @@ -14,14 +14,12 @@ #include "motion_utils/marker/marker_helper.hpp" -#include "motion_utils/resample/resample_utils.hpp" #include "tier4_autoware_utils/ros/marker_helper.hpp" -#include +#include -#include +#include -using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createDeletedDefaultMarker; using tier4_autoware_utils::createMarkerColor; diff --git a/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp b/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp index c22aaa0d5e87c..34aae096ffa9e 100644 --- a/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp +++ b/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp @@ -21,24 +21,24 @@ namespace motion_utils void VirtualWallMarkerCreator::cleanup() { - for (auto it = marker_count_per_namespace.begin(); it != marker_count_per_namespace.end();) { + for (auto it = marker_count_per_namespace_.begin(); it != marker_count_per_namespace_.end();) { const auto & marker_count = it->second; const auto is_unused_namespace = marker_count.previous == 0 && marker_count.current == 0; if (is_unused_namespace) - it = marker_count_per_namespace.erase(it); + it = marker_count_per_namespace_.erase(it); else ++it; } - virtual_walls.clear(); + virtual_walls_.clear(); } void VirtualWallMarkerCreator::add_virtual_wall(const VirtualWall & virtual_wall) { - virtual_walls.push_back(virtual_wall); + virtual_walls_.push_back(virtual_wall); } void VirtualWallMarkerCreator::add_virtual_walls(const VirtualWalls & walls) { - virtual_walls.insert(virtual_walls.end(), walls.begin(), walls.end()); + virtual_walls_.insert(virtual_walls_.end(), walls.begin(), walls.end()); } visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers( @@ -46,13 +46,13 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers( { visualization_msgs::msg::MarkerArray marker_array; // update marker counts - for (auto & [ns, count] : marker_count_per_namespace) { + for (auto & [ns, count] : marker_count_per_namespace_) { count.previous = count.current; count.current = 0UL; } // convert to markers create_wall_function create_fn; - for (const auto & virtual_wall : virtual_walls) { + for (const auto & virtual_wall : virtual_walls_) { switch (virtual_wall.style) { case stop: create_fn = motion_utils::createStopVirtualWallMarker; @@ -68,15 +68,16 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers( virtual_wall.pose, virtual_wall.text, now, 0, virtual_wall.longitudinal_offset, virtual_wall.ns, virtual_wall.is_driving_forward); for (auto & marker : markers.markers) { - marker.id = marker_count_per_namespace[marker.ns].current++; + marker.id = static_cast(marker_count_per_namespace_[marker.ns].current++); marker_array.markers.push_back(marker); } } // create delete markers visualization_msgs::msg::Marker marker; marker.action = visualization_msgs::msg::Marker::DELETE; - for (const auto & [ns, count] : marker_count_per_namespace) { - for (marker.id = count.current; marker.id < static_cast(count.previous); ++marker.id) { + for (const auto & [ns, count] : marker_count_per_namespace_) { + for (marker.id = static_cast(count.current); marker.id < static_cast(count.previous); + ++marker.id) { marker.ns = ns; marker_array.markers.push_back(marker); } diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index 6bada921804d4..834d07a87ea09 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -14,15 +14,12 @@ #include "motion_utils/resample/resample.hpp" -#include "interpolation/interpolation_utils.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/zero_order_hold.hpp" #include "motion_utils/resample/resample_utils.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/geometry/pose_deviation.hpp" -#include "tier4_autoware_utils/math/constants.hpp" namespace motion_utils { @@ -489,7 +486,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath( autoware_auto_planning_msgs::msg::Path resampled_path; resampled_path.header = input_path.header; resampled_path.left_bound = input_path.left_bound; - resampled_path.right_bound = resampled_path.right_bound; + resampled_path.right_bound = input_path.right_bound; resampled_path.points.resize(interpolated_pose.size()); for (size_t i = 0; i < resampled_path.points.size(); ++i) { autoware_auto_planning_msgs::msg::PathPoint path_point; diff --git a/common/motion_utils/src/trajectory/conversion.cpp b/common/motion_utils/src/trajectory/conversion.cpp new file mode 100644 index 0000000000000..f198003d84091 --- /dev/null +++ b/common/motion_utils/src/trajectory/conversion.cpp @@ -0,0 +1,54 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "motion_utils/trajectory/conversion.hpp" + +#include + +namespace motion_utils +{ +/** + * @brief Convert std::vector to + * autoware_auto_planning_msgs::msg::Trajectory. This function is temporarily added for porting to + * autoware_auto_msgs. We should consider whether to remove this function after the porting is done. + * @attention This function just clips + * std::vector up to the capacity of Trajectory. + * Therefore, the error handling out of this function is necessary if the size of the input greater + * than the capacity. + * @todo Decide how to handle the situation that we need to use the trajectory with the size of + * points larger than the capacity. (Tier IV) + */ +autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( + const std::vector & trajectory, + const std_msgs::msg::Header & header) +{ + autoware_auto_planning_msgs::msg::Trajectory output{}; + output.header = header; + for (const auto & pt : trajectory) output.points.push_back(pt); + return output; +} + +/** + * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to + * std::vector. + */ +std::vector convertToTrajectoryPointArray( + const autoware_auto_planning_msgs::msg::Trajectory & trajectory) +{ + std::vector output(trajectory.points.size()); + std::copy(trajectory.points.begin(), trajectory.points.end(), output.begin()); + return output; +} + +} // namespace motion_utils diff --git a/common/motion_utils/src/trajectory/interpolation.cpp b/common/motion_utils/src/trajectory/interpolation.cpp index 4077202aba8a4..6ee3e665f746a 100644 --- a/common/motion_utils/src/trajectory/interpolation.cpp +++ b/common/motion_utils/src/trajectory/interpolation.cpp @@ -15,8 +15,6 @@ #include "motion_utils/trajectory/interpolation.hpp" #include "interpolation/linear_interpolation.hpp" -#include "interpolation/zero_order_hold.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" #include "motion_utils/trajectory/trajectory.hpp" using autoware_auto_planning_msgs::msg::PathPointWithLaneId; @@ -34,7 +32,8 @@ TrajectoryPoint calcInterpolatedPoint( TrajectoryPoint interpolated_point{}; interpolated_point.pose = target_pose; return interpolated_point; - } else if (trajectory.points.size() == 1) { + } + if (trajectory.points.size() == 1) { return trajectory.points.front(); } @@ -101,7 +100,8 @@ PathPointWithLaneId calcInterpolatedPoint( PathPointWithLaneId interpolated_point{}; interpolated_point.point.pose = target_pose; return interpolated_point; - } else if (path.points.size() == 1) { + } + if (path.points.size() == 1) { return path.points.front(); } 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 deleted file mode 100644 index 5c907a5926046..0000000000000 --- a/common/motion_utils/src/trajectory/tmp_conversion.cpp +++ /dev/null @@ -1,60 +0,0 @@ -// Copyright 2023 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "motion_utils/trajectory/tmp_conversion.hpp" - -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/geometry/pose_deviation.hpp" - -#include - -namespace motion_utils -{ -/** - * @brief Convert std::vector to - * autoware_auto_planning_msgs::msg::Trajectory. This function is temporarily added for porting to - * autoware_auto_msgs. We should consider whether to remove this function after the porting is done. - * @attention This function just clips - * std::vector up to the capacity of Trajectory. - * Therefore, the error handling out of this function is necessary if the size of the input greater - * than the capacity. - * @todo Decide how to handle the situation that we need to use the trajectory with the size of - * points larger than the capacity. (Tier IV) - */ -autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( - const std::vector & trajectory) -{ - autoware_auto_planning_msgs::msg::Trajectory output{}; - for (const auto & pt : trajectory) { - output.points.push_back(pt); - if (output.points.size() >= output.CAPACITY) { - break; - } - } - return output; -} - -/** - * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to - * std::vector. - */ -std::vector convertToTrajectoryPointArray( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory) -{ - std::vector output(trajectory.points.size()); - std::copy(trajectory.points.begin(), trajectory.points.end(), output.begin()); - return output; -} - -} // namespace motion_utils diff --git a/common/motion_utils/src/trajectory/trajectory.cpp b/common/motion_utils/src/trajectory/trajectory.cpp 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_interpolation.cpp b/common/motion_utils/test/src/trajectory/test_interpolation.cpp index 80df1fabf562d..5794ed61e9e44 100644 --- a/common/motion_utils/test/src/trajectory/test_interpolation.cpp +++ b/common/motion_utils/test/src/trajectory/test_interpolation.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/interpolation.hpp" -#include "motion_utils/trajectory/tmp_conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" 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..eb6a06041e65d 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" @@ -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) { @@ -1894,8 +1883,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_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle"), std::string::npos); + EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Behind of end point) @@ -1907,8 +1896,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_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle"), std::string::npos); + EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Huge lateral offset) @@ -1920,8 +1909,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_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle"), std::string::npos); + 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) @@ -2321,8 +2310,8 @@ TEST(trajectory, insertTargetPoint_Length) const auto p_target = createPoint(4.0, 10.0, 0.0); 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_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); + 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); } } @@ -4327,8 +4317,8 @@ 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_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); + EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Behind of end point) @@ -4340,8 +4330,8 @@ 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_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); + EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Huge lateral offset) @@ -4353,8 +4343,8 @@ 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_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); + 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/object_recognition_utils/package.xml b/common/object_recognition_utils/package.xml index 2f2472515ebad..796e276c376d6 100644 --- a/common/object_recognition_utils/package.xml +++ b/common/object_recognition_utils/package.xml @@ -6,8 +6,8 @@ The object_recognition_utils package Takayuki Murooka Satoshi Tanaka - Yusuke Muramatsu Shunsuke Miura + Yoshi Ri Apache License 2.0 ament_cmake_auto diff --git a/common/path_distance_calculator/package.xml b/common/path_distance_calculator/package.xml index b5e770a0ea624..e796bbd0d20f7 100644 --- a/common/path_distance_calculator/package.xml +++ b/common/path_distance_calculator/package.xml @@ -5,8 +5,6 @@ 0.0.0 The path_distance_calculator package Takagi, Isamu - yabuta - Kah Hooi Tan Apache License 2.0 ament_cmake diff --git a/common/perception_utils/package.xml b/common/perception_utils/package.xml index 5304d481737e5..9d5fbf40a4e89 100644 --- a/common/perception_utils/package.xml +++ b/common/perception_utils/package.xml @@ -5,8 +5,8 @@ 0.1.0 The perception_utils package Satoshi Tanaka - Yusuke Muramatsu Shunsuke Miura + Yoshi Ri Apache License 2.0 ament_cmake_auto diff --git a/common/tensorrt_common/include/tensorrt_common/logger.hpp b/common/tensorrt_common/include/tensorrt_common/logger.hpp index 98fe18794998d..355efe5167885 100644 --- a/common/tensorrt_common/include/tensorrt_common/logger.hpp +++ b/common/tensorrt_common/include/tensorrt_common/logger.hpp @@ -19,6 +19,7 @@ #include "NvInferRuntimeCommon.h" +#include #include #include #include @@ -26,6 +27,7 @@ #include #include #include +#include namespace tensorrt_common { @@ -200,7 +202,15 @@ class Logger : public nvinfer1::ILogger // NOLINT public: // Logger(Severity severity = Severity::kWARNING) // Logger(Severity severity = Severity::kVERBOSE) - explicit Logger(Severity severity = Severity::kINFO) : mReportableSeverity(severity) {} + explicit Logger(Severity severity = Severity::kINFO) + : mReportableSeverity(severity), mVerbose(true), mThrottleStopFlag(false) + { + } + + explicit Logger(const bool verbose, Severity severity = Severity::kINFO) + : mReportableSeverity(severity), mVerbose(verbose), mThrottleStopFlag(false) + { + } //! //! \enum TestResult @@ -234,7 +244,44 @@ class Logger : public nvinfer1::ILogger // NOLINT //! void log(Severity severity, const char * msg) noexcept override { - LogStreamConsumer(mReportableSeverity, severity) << "[TRT] " << std::string(msg) << std::endl; + if (mVerbose) { + LogStreamConsumer(mReportableSeverity, severity) << "[TRT] " << std::string(msg) << std::endl; + } + } + + /** + * @brief Logging with throttle. + * + * @example + * Logger logger(); + * auto log_thread = logger.log_throttle(nvinfer1::ILogger::Severity::kINFO, "SOME MSG", 1); + * // some operation + * logger.stop_throttle(log_thread); + * + * @param severity + * @param msg + * @param duration + * @return std::thread + * + */ + std::thread log_throttle(Severity severity, const char * msg, const int duration) noexcept + { + mThrottleStopFlag.store(false); + auto log_func = [this](Severity s, const char * m, const int d) { + while (!mThrottleStopFlag.load()) { + this->log(s, m); + std::this_thread::sleep_for(std::chrono::seconds(d)); + } + }; + + std::thread log_thread(log_func, severity, msg, duration); + return log_thread; + } + + void stop_throttle(std::thread & log_thread) noexcept + { + mThrottleStopFlag.store(true); + log_thread.join(); } //! @@ -430,6 +477,8 @@ class Logger : public nvinfer1::ILogger // NOLINT } Severity mReportableSeverity; + bool mVerbose; + std::atomic mThrottleStopFlag; }; namespace diff --git a/common/tensorrt_common/package.xml b/common/tensorrt_common/package.xml index 7d3995f93f7fe..f5a3896b55881 100644 --- a/common/tensorrt_common/package.xml +++ b/common/tensorrt_common/package.xml @@ -6,7 +6,6 @@ Taichi Higashide Daisuke Nishimatsu - Daisuke Nishimatsu Dan Umeda Manato Hirabayashi diff --git a/common/tensorrt_common/src/tensorrt_common.cpp b/common/tensorrt_common/src/tensorrt_common.cpp index f1e4835d2ba2d..2b218cd3e49f2 100644 --- a/common/tensorrt_common/src/tensorrt_common.cpp +++ b/common/tensorrt_common/src/tensorrt_common.cpp @@ -186,7 +186,11 @@ void TrtCommon::setup() } else { std::cout << "Building... " << cache_engine_path << std::endl; logger_.log(nvinfer1::ILogger::Severity::kINFO, "Start build engine"); + auto log_thread = logger_.log_throttle( + nvinfer1::ILogger::Severity::kINFO, + "Applying optimizations and building TRT CUDA engine. Please wait for a few minutes...", 5); buildEngineFromOnnx(model_file_path_, cache_engine_path); + logger_.stop_throttle(log_thread); logger_.log(nvinfer1::ILogger::Severity::kINFO, "End build engine"); } engine_path = cache_engine_path; diff --git a/common/tier4_adapi_rviz_plugin/README.md b/common/tier4_adapi_rviz_plugin/README.md index 41ef7c6b6243b..4056130776178 100644 --- a/common/tier4_adapi_rviz_plugin/README.md +++ b/common/tier4_adapi_rviz_plugin/README.md @@ -9,3 +9,21 @@ Enable or disable of allow_goal_modification option can be set with the check bo Push the mode button in the waypoint to enter waypoint mode. In this mode, the pose is added to waypoints. Press the apply button to set the route using the saved waypoints (the last one is a goal). Reset the saved waypoints with the reset button. + +## Material Design Icons + +This project uses [Material Design Icons](https://developers.google.com/fonts/docs/material_symbols) by Google. These icons are used under the terms of the Apache License, Version 2.0. + +Material Design Icons are a collection of symbols provided by Google that are used to enhance the user interface of applications, websites, and other digital products. + +### License + +The Material Design Icons are licensed under the Apache License, Version 2.0. You may obtain a copy of the License at: + + + +Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. + +### Acknowledgments + +We would like to express our gratitude to Google for making these icons available to the community, helping developers and designers enhance the visual appeal and user experience of their projects. diff --git a/common/tier4_adapi_rviz_plugin/icons/classes/RouteTool.png b/common/tier4_adapi_rviz_plugin/icons/classes/RouteTool.png index 6a67573717ae1..fad4466a99326 100644 Binary files a/common/tier4_adapi_rviz_plugin/icons/classes/RouteTool.png and b/common/tier4_adapi_rviz_plugin/icons/classes/RouteTool.png differ diff --git a/common/tier4_adapi_rviz_plugin/src/route_panel.cpp b/common/tier4_adapi_rviz_plugin/src/route_panel.cpp index 3e99761fd9fb3..5bdee028bf4b3 100644 --- a/common/tier4_adapi_rviz_plugin/src/route_panel.cpp +++ b/common/tier4_adapi_rviz_plugin/src/route_panel.cpp @@ -14,11 +14,13 @@ #include "route_panel.hpp" +#include #include #include #include #include +#include namespace tier4_adapi_rviz_plugins { @@ -28,6 +30,11 @@ RoutePanel::RoutePanel(QWidget * parent) : rviz_common::Panel(parent) waypoints_mode_ = new QPushButton("mode"); waypoints_reset_ = new QPushButton("reset"); waypoints_apply_ = new QPushButton("apply"); + adapi_clear_ = new QPushButton("clear"); + adapi_set_ = new QPushButton("set"); + adapi_change_ = new QPushButton("change"); + adapi_response_ = new QLabel("the response will be displayed here"); + adapi_auto_clear_ = new QCheckBox("auto clear"); allow_goal_modification_ = new QCheckBox("allow goal modification"); waypoints_mode_->setCheckable(true); @@ -37,6 +44,22 @@ RoutePanel::RoutePanel(QWidget * parent) : rviz_common::Panel(parent) connect(waypoints_reset_, &QPushButton::clicked, this, &RoutePanel::onWaypointsReset); connect(waypoints_apply_, &QPushButton::clicked, this, &RoutePanel::onWaypointsApply); + const auto buttons = new QButtonGroup(this); + buttons->addButton(adapi_set_); + buttons->addButton(adapi_change_); + buttons->setExclusive(true); + adapi_set_->setCheckable(true); + adapi_change_->setCheckable(true); + adapi_response_->setAlignment(Qt::AlignCenter); + + connect(adapi_clear_, &QPushButton::clicked, this, &RoutePanel::clearRoute); + connect(adapi_set_, &QPushButton::clicked, [this] { adapi_mode_ = AdapiMode::Set; }); + connect(adapi_change_, &QPushButton::clicked, [this] { adapi_mode_ = AdapiMode::Change; }); + + adapi_auto_clear_->setChecked(true); + adapi_set_->setChecked(true); + adapi_mode_ = AdapiMode::Set; + const auto layout = new QVBoxLayout(); setLayout(layout); @@ -52,6 +75,19 @@ RoutePanel::RoutePanel(QWidget * parent) : rviz_common::Panel(parent) waypoints_group_ = group; } + // adapi group + { + const auto group = new QGroupBox("adapi"); + const auto local = new QGridLayout(); + local->addWidget(adapi_clear_, 0, 0); + local->addWidget(adapi_set_, 0, 1); + local->addWidget(adapi_change_, 0, 2); + local->addWidget(adapi_auto_clear_, 1, 0); + local->addWidget(adapi_response_, 1, 1, 1, 2); + group->setLayout(local); + layout->addWidget(group); + } + // options group { const auto group = new QGroupBox("options"); @@ -73,25 +109,14 @@ void RoutePanel::onInitialize() const auto adaptor = component_interface_utils::NodeAdaptor(node.get()); adaptor.init_cli(cli_clear_); - adaptor.init_cli(cli_route_); -} - -void RoutePanel::setRoute(const PoseStamped & pose) -{ - const auto req = std::make_shared(); - cli_clear_->async_send_request(req, [this, pose](auto) { - const auto req = std::make_shared(); - req->header = pose.header; - req->goal = pose.pose; - req->option.allow_goal_modification = allow_goal_modification_->isChecked(); - cli_route_->async_send_request(req); - }); + adaptor.init_cli(cli_set_); + adaptor.init_cli(cli_change_); } void RoutePanel::onPose(const PoseStamped::ConstSharedPtr msg) { if (!waypoints_mode_->isChecked()) { - setRoute(*msg); + requestRoute(*msg); } else { waypoints_.push_back(*msg); waypoints_group_->setTitle(QString("waypoints (count: %1)").arg(waypoints_.size())); @@ -120,8 +145,7 @@ void RoutePanel::onWaypointsApply() { if (waypoints_.empty()) return; - const auto req = std::make_shared(); - cli_clear_->async_send_request(req, [this](auto) { + const auto call = [this] { const auto req = std::make_shared(); req->header = waypoints_.back().header; req->goal = waypoints_.back().pose; @@ -129,9 +153,66 @@ void RoutePanel::onWaypointsApply() req->waypoints.push_back(waypoints_[i].pose); } req->option.allow_goal_modification = allow_goal_modification_->isChecked(); - cli_route_->async_send_request(req); + asyncSendRequest(req); onWaypointsReset(); - }); + }; + + if (adapi_mode_ == AdapiMode::Set && adapi_auto_clear_->isChecked()) { + const auto req = std::make_shared(); + cli_clear_->async_send_request(req, [call](auto) { call(); }); + } else { + call(); + } +} + +void RoutePanel::requestRoute(const PoseStamped & pose) +{ + const auto call = [this, pose] { + const auto req = std::make_shared(); + req->header = pose.header; + req->goal = pose.pose; + req->option.allow_goal_modification = allow_goal_modification_->isChecked(); + asyncSendRequest(req); + }; + + if (adapi_mode_ == AdapiMode::Set && adapi_auto_clear_->isChecked()) { + const auto req = std::make_shared(); + cli_clear_->async_send_request(req, [call](auto) { call(); }); + } else { + call(); + } +} + +void RoutePanel::clearRoute() +{ + const auto callback = [this](auto future) { + const auto status = future.get()->status; + std::string text = status.success ? "OK" : "Error"; + text += "[" + std::to_string(status.code) + "]"; + text += " " + status.message; + adapi_response_->setText(QString::fromStdString(text)); + }; + + const auto req = std::make_shared(); + cli_clear_->async_send_request(req, callback); +} + +void RoutePanel::asyncSendRequest(SetRoutePoints::Service::Request::SharedPtr req) +{ + const auto callback = [this](auto future) { + const auto status = future.get()->status; + std::string text = status.success ? "OK" : "Error"; + text += "[" + std::to_string(status.code) + "]"; + text += " " + status.message; + adapi_response_->setText(QString::fromStdString(text)); + }; + + if (adapi_mode_ == AdapiMode::Set) { + cli_set_->async_send_request(req, callback); + } + if (adapi_mode_ == AdapiMode::Change) { + cli_change_->async_send_request(req, callback); + } } } // namespace tier4_adapi_rviz_plugins diff --git a/common/tier4_adapi_rviz_plugin/src/route_panel.hpp b/common/tier4_adapi_rviz_plugin/src/route_panel.hpp index f7c953d65dd2d..47855d243a564 100644 --- a/common/tier4_adapi_rviz_plugin/src/route_panel.hpp +++ b/common/tier4_adapi_rviz_plugin/src/route_panel.hpp @@ -15,8 +15,10 @@ #ifndef ROUTE_PANEL_HPP_ #define ROUTE_PANEL_HPP_ +#include #include #include +#include #include #include #include @@ -35,6 +37,7 @@ class RoutePanel : public rviz_common::Panel Q_OBJECT using ClearRoute = autoware_ad_api::routing::ClearRoute; using SetRoutePoints = autoware_ad_api::routing::SetRoutePoints; + using ChangeRoutePoints = autoware_ad_api::routing::ChangeRoutePoints; using PoseStamped = geometry_msgs::msg::PoseStamped; public: @@ -45,6 +48,11 @@ class RoutePanel : public rviz_common::Panel QPushButton * waypoints_mode_; QPushButton * waypoints_reset_; QPushButton * waypoints_apply_; + QPushButton * adapi_clear_; + QPushButton * adapi_set_; + QPushButton * adapi_change_; + QLabel * adapi_response_; + QCheckBox * adapi_auto_clear_; QGroupBox * waypoints_group_; QCheckBox * allow_goal_modification_; @@ -52,11 +60,17 @@ class RoutePanel : public rviz_common::Panel std::vector waypoints_; void onPose(const PoseStamped::ConstSharedPtr msg); + enum AdapiMode { Set, Change }; + AdapiMode adapi_mode_; + component_interface_utils::Client::SharedPtr cli_clear_; - component_interface_utils::Client::SharedPtr cli_route_; - void setRoute(const PoseStamped & pose); + component_interface_utils::Client::SharedPtr cli_set_; + component_interface_utils::Client::SharedPtr cli_change_; + void requestRoute(const PoseStamped & pose); + void asyncSendRequest(SetRoutePoints::Service::Request::SharedPtr req); private slots: + void clearRoute(); void onWaypointsMode(bool clicked); void onWaypointsReset(); void onWaypointsApply(); diff --git a/common/tier4_api_utils/include/tier4_api_utils/rclcpp/client.hpp b/common/tier4_api_utils/include/tier4_api_utils/rclcpp/client.hpp index aedbe4b34ea22..16ff5a011536c 100644 --- a/common/tier4_api_utils/include/tier4_api_utils/rclcpp/client.hpp +++ b/common/tier4_api_utils/include/tier4_api_utils/rclcpp/client.hpp @@ -41,20 +41,20 @@ class Client const typename ServiceT::Request::SharedPtr & request, const std::chrono::nanoseconds & timeout = std::chrono::seconds(2)) { - RCLCPP_INFO(logger_, "client request"); + RCLCPP_DEBUG(logger_, "client request"); if (!client_->service_is_ready()) { - RCLCPP_INFO(logger_, "client available"); + RCLCPP_DEBUG(logger_, "client available"); return {response_error("Internal service is not available."), nullptr}; } auto future = client_->async_send_request(request); if (future.wait_for(timeout) != std::future_status::ready) { - RCLCPP_INFO(logger_, "client timeout"); + RCLCPP_DEBUG(logger_, "client timeout"); return {response_error("Internal service has timed out."), nullptr}; } - RCLCPP_INFO(logger_, "client response"); + RCLCPP_DEBUG(logger_, "client response"); return {response_success(), future.get()}; } diff --git a/common/tier4_api_utils/package.xml b/common/tier4_api_utils/package.xml index 091f512480150..d05b810caf904 100644 --- a/common/tier4_api_utils/package.xml +++ b/common/tier4_api_utils/package.xml @@ -5,8 +5,6 @@ 0.0.0 The tier4_api_utils package Takagi, Isamu - yabuta - Kah Hooi Tan Apache License 2.0 ament_cmake_auto diff --git a/common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt b/common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt index c6d4e30061626..cdc57743a6cb1 100644 --- a/common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt +++ b/common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt @@ -11,6 +11,7 @@ set(CMAKE_INCLUDE_CURRENT_DIR ON) add_definitions(-DQT_NO_KEYWORDS) ament_auto_add_library(${PROJECT_NAME} SHARED + src/automatic_checkpoint_append_tool.cpp src/automatic_goal_sender.cpp src/automatic_goal_append_tool.cpp src/automatic_goal_panel.cpp diff --git a/common/tier4_automatic_goal_rviz_plugin/README.md b/common/tier4_automatic_goal_rviz_plugin/README.md index 48fb577375714..6fd626d5a7642 100644 --- a/common/tier4_automatic_goal_rviz_plugin/README.md +++ b/common/tier4_automatic_goal_rviz_plugin/README.md @@ -78,3 +78,21 @@ In this situation, check the terminal output for more information. - Often it is enough to try again. - Sometimes a clearing of the current route is required before retrying. + +## Material Design Icons + +This project uses [Material Design Icons](https://developers.google.com/fonts/docs/material_symbols) by Google. These icons are used under the terms of the Apache License, Version 2.0. + +Material Design Icons are a collection of symbols provided by Google that are used to enhance the user interface of applications, websites, and other digital products. + +### License + +The Material Design Icons are licensed under the Apache License, Version 2.0. You may obtain a copy of the License at: + + + +Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. + +### Acknowledgments + +We would like to express our gratitude to Google for making these icons available to the community, helping developers and designers enhance the visual appeal and user experience of their projects. diff --git a/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticCheckpointTool.png b/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticCheckpointTool.png new file mode 100644 index 0000000000000..4f5b4961f2500 Binary files /dev/null and b/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticCheckpointTool.png differ diff --git a/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalTool.png b/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalTool.png index 6a67573717ae1..58d12f95ebfd6 100644 Binary files a/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalTool.png and b/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalTool.png differ diff --git a/common/tier4_automatic_goal_rviz_plugin/package.xml b/common/tier4_automatic_goal_rviz_plugin/package.xml index deda078ace22e..fb5331379acb6 100644 --- a/common/tier4_automatic_goal_rviz_plugin/package.xml +++ b/common/tier4_automatic_goal_rviz_plugin/package.xml @@ -6,6 +6,8 @@ The autoware automatic goal rviz plugin package Shumpei Wakabayashi Dawid Moszyński + Kyoichi Sugahara + Satoshi Ota Apache License 2.0 Dawid Moszyński @@ -22,6 +24,7 @@ rviz_default_plugins tf2 tf2_geometry_msgs + tier4_autoware_utils visualization_msgs yaml-cpp diff --git a/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml b/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml index e7d5224550868..e9d77491941ed 100644 --- a/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml @@ -9,4 +9,9 @@ base_class_type="rviz_common::Tool"> AutowareAutomaticGoalTool + + AutowareAutomaticCheckpointTool + diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.cpp new file mode 100644 index 0000000000000..4efa6fedbaabd --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.cpp @@ -0,0 +1,122 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "automatic_checkpoint_append_tool.hpp" + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include + +#include + +namespace rviz_plugins +{ +AutowareAutomaticCheckpointTool::AutowareAutomaticCheckpointTool() +{ + shortcut_key_ = 'c'; + + pose_topic_property_ = new rviz_common::properties::StringProperty( + "Pose Topic", "~/automatic_goal/checkpoint", "The topic on which to publish automatic_goal.", + getPropertyContainer(), SLOT(updateTopic()), this); + std_dev_x_ = new rviz_common::properties::FloatProperty( + "X std deviation", 0.5, "X standard deviation for checkpoint pose [m]", getPropertyContainer()); + std_dev_y_ = new rviz_common::properties::FloatProperty( + "Y std deviation", 0.5, "Y standard deviation for checkpoint pose [m]", getPropertyContainer()); + std_dev_theta_ = new rviz_common::properties::FloatProperty( + "Theta std deviation", M_PI / 12.0, "Theta standard deviation for checkpoint pose [rad]", + getPropertyContainer()); + position_z_ = new rviz_common::properties::FloatProperty( + "Z position", 0.0, "Z position for checkpoint pose [m]", getPropertyContainer()); + std_dev_x_->setMin(0); + std_dev_y_->setMin(0); + std_dev_theta_->setMin(0); + position_z_->setMin(0); +} + +void AutowareAutomaticCheckpointTool::onInitialize() +{ + PoseTool::onInitialize(); + setName("2D AppendCheckpoint"); + updateTopic(); +} + +void AutowareAutomaticCheckpointTool::updateTopic() +{ + rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + pose_pub_ = raw_node->create_publisher( + pose_topic_property_->getStdString(), 1); + clock_ = raw_node->get_clock(); +} + +void AutowareAutomaticCheckpointTool::onPoseSet(double x, double y, double theta) +{ + // pose + std::string fixed_frame = context_->getFixedFrame().toStdString(); + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = fixed_frame; + pose.header.stamp = clock_->now(); + pose.pose.position.x = x; + pose.pose.position.y = y; + pose.pose.position.z = position_z_->getFloat(); + + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, theta); + pose.pose.orientation = tf2::toMsg(quat); + RCLCPP_INFO( + rclcpp::get_logger("AutowareAutomaticCheckpointTool"), + "Setting pose: %.3f %.3f %.3f %.3f [frame=%s]", x, y, position_z_->getFloat(), theta, + fixed_frame.c_str()); + pose_pub_->publish(pose); +} + +} // end namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareAutomaticCheckpointTool, rviz_common::Tool) diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.hpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.hpp new file mode 100644 index 0000000000000..4ea3fa4bd009a --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.hpp @@ -0,0 +1,91 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef AUTOMATIC_CHECKPOINT_APPEND_TOOL_HPP_ +#define AUTOMATIC_CHECKPOINT_APPEND_TOOL_HPP_ + +#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 +#include +#include +#include +#include +#include +#include +#endif + +#include + +namespace rviz_plugins +{ +class AutowareAutomaticCheckpointTool : public rviz_default_plugins::tools::PoseTool +{ + Q_OBJECT + +public: + AutowareAutomaticCheckpointTool(); + void onInitialize() override; + +protected: + void onPoseSet(double x, double y, double theta) override; + +private Q_SLOTS: + void updateTopic(); + +private: // NOLINT for Qt + rclcpp::Clock::SharedPtr clock_{nullptr}; + rclcpp::Publisher::SharedPtr pose_pub_{nullptr}; + + rviz_common::properties::StringProperty * pose_topic_property_{nullptr}; + rviz_common::properties::FloatProperty * std_dev_x_{nullptr}; + rviz_common::properties::FloatProperty * std_dev_y_{nullptr}; + rviz_common::properties::FloatProperty * std_dev_theta_{nullptr}; + rviz_common::properties::FloatProperty * position_z_{nullptr}; +}; + +} // namespace rviz_plugins + +#endif // AUTOMATIC_CHECKPOINT_APPEND_TOOL_HPP_ diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp index bee390bfe29ed..6b8d7765a989a 100644 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp @@ -16,6 +16,8 @@ #include "automatic_goal_panel.hpp" +#include + namespace rviz_plugins { AutowareAutomaticGoalPanel::AutowareAutomaticGoalPanel(QWidget * parent) @@ -139,6 +141,9 @@ void AutowareAutomaticGoalPanel::onInitialize() sub_append_goal_ = raw_node_->create_subscription( "~/automatic_goal/goal", 5, std::bind(&AutowareAutomaticGoalPanel::onAppendGoal, this, std::placeholders::_1)); + sub_append_checkpoint_ = raw_node_->create_subscription( + "~/automatic_goal/checkpoint", 5, + std::bind(&AutowareAutomaticGoalPanel::onAppendCheckpoint, this, std::placeholders::_1)); initCommunication(raw_node_.get()); } @@ -244,12 +249,28 @@ void AutowareAutomaticGoalPanel::onClickSaveListToFile() void AutowareAutomaticGoalPanel::onAppendGoal(const PoseStamped::ConstSharedPtr pose) { if (state_ == State::EDITING) { - goals_list_.push_back(pose); + goals_list_.emplace_back(pose); updateGoalsList(); updateGUI(); } } +void AutowareAutomaticGoalPanel::onAppendCheckpoint(const PoseStamped::ConstSharedPtr pose) +{ + if (goals_list_widget_ptr_->selectedItems().count() != 1) { + showMessageBox("Select a goal in GoalsList before set checkpoint"); + return; + } + + current_goal_ = goals_list_widget_ptr_->currentRow(); + if (current_goal_ >= goals_list_.size()) { + return; + } + + goals_list_.at(current_goal_).checkpoint_pose_ptrs.push_back(pose); + publishMarkers(); +} + // Override void AutowareAutomaticGoalPanel::onCallResult() { @@ -418,47 +439,71 @@ void AutowareAutomaticGoalPanel::updateGoalIcon(const unsigned goal_index, const void AutowareAutomaticGoalPanel::publishMarkers() { + using tier4_autoware_utils::createDefaultMarker; + using tier4_autoware_utils::createMarkerColor; + using tier4_autoware_utils::createMarkerScale; + MarkerArray text_array; MarkerArray arrow_array; // Clear existing - visualization_msgs::msg::Marker marker; - marker.header.stamp = rclcpp::Time(); - marker.ns = "names"; - marker.id = 0; - marker.action = visualization_msgs::msg::Marker::DELETEALL; - text_array.markers.push_back(marker); - marker.ns = "poses"; - arrow_array.markers.push_back(marker); - pub_marker_->publish(text_array); - pub_marker_->publish(arrow_array); + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "names", 0L, Marker::CUBE, + createMarkerScale(1.0, 1.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + marker.action = visualization_msgs::msg::Marker::DELETEALL; + text_array.markers.push_back(marker); + pub_marker_->publish(text_array); + } + + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "poses", 0L, Marker::CUBE, + createMarkerScale(1.0, 1.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + arrow_array.markers.push_back(marker); + pub_marker_->publish(arrow_array); + } + text_array.markers.clear(); arrow_array.markers.clear(); - // Publish current - for (unsigned i = 0; i < goals_list_.size(); i++) { - visualization_msgs::msg::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = rclcpp::Time(); - marker.ns = "names"; - marker.id = static_cast(i); - marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + + const auto push_arrow_marker = [&](const auto & pose, const auto & color, const size_t id) { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "poses", id, Marker::ARROW, + createMarkerScale(1.6, 0.5, 0.5), color); marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose = goals_list_[i]->pose; + marker.pose = pose; marker.lifetime = rclcpp::Duration(0, 0); - marker.scale.x = 1.6; - marker.scale.y = 1.6; - marker.scale.z = 1.6; - marker.color.r = 1.0; - marker.color.g = 1.0; - marker.color.b = 1.0; - marker.color.a = 1.0; marker.frame_locked = false; - marker.text = "G" + std::to_string(i); - text_array.markers.push_back(marker); - marker.ns = "poses"; - marker.scale.y = 0.2; - marker.scale.z = 0.2; - marker.type = visualization_msgs::msg::Marker::ARROW; arrow_array.markers.push_back(marker); + }; + + const auto push_text_marker = [&](const auto & pose, const auto & text, const size_t id) { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "names", id, Marker::TEXT_VIEW_FACING, + createMarkerScale(1.6, 1.6, 1.6), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose = pose; + marker.lifetime = rclcpp::Duration(0, 0); + marker.frame_locked = false; + marker.text = text; + text_array.markers.push_back(marker); + }; + + // Publish current + size_t id = 0; + for (size_t i = 0; i < goals_list_.size(); ++i) { + { + const auto pose = goals_list_.at(i).goal_pose_ptr->pose; + push_arrow_marker(pose, createMarkerColor(0.0, 1.0, 0.0, 0.999), id++); + push_text_marker(pose, "Goal:" + std::to_string(i), id++); + } + + for (size_t j = 0; j < goals_list_.at(i).checkpoint_pose_ptrs.size(); ++j) { + const auto pose = goals_list_.at(i).checkpoint_pose_ptrs.at(j)->pose; + push_arrow_marker(pose, createMarkerColor(1.0, 1.0, 0.0, 0.999), id++); + push_text_marker( + pose, "Checkpoint:" + std::to_string(i) + "[Goal:" + std::to_string(j) + "]", id++); + } } pub_marker_->publish(text_array); pub_marker_->publish(arrow_array); @@ -469,13 +514,13 @@ void AutowareAutomaticGoalPanel::saveGoalsList(const std::string & file_path) { YAML::Node node; for (unsigned i = 0; i < goals_list_.size(); ++i) { - node[i]["position_x"] = goals_list_[i]->pose.position.x; - node[i]["position_y"] = goals_list_[i]->pose.position.y; - node[i]["position_z"] = goals_list_[i]->pose.position.z; - node[i]["orientation_x"] = goals_list_[i]->pose.orientation.x; - node[i]["orientation_y"] = goals_list_[i]->pose.orientation.y; - node[i]["orientation_z"] = goals_list_[i]->pose.orientation.z; - node[i]["orientation_w"] = goals_list_[i]->pose.orientation.w; + node[i]["position_x"] = goals_list_[i].goal_pose_ptr->pose.position.x; + node[i]["position_y"] = goals_list_[i].goal_pose_ptr->pose.position.y; + node[i]["position_z"] = goals_list_[i].goal_pose_ptr->pose.position.z; + node[i]["orientation_x"] = goals_list_[i].goal_pose_ptr->pose.orientation.x; + node[i]["orientation_y"] = goals_list_[i].goal_pose_ptr->pose.orientation.y; + node[i]["orientation_z"] = goals_list_[i].goal_pose_ptr->pose.orientation.z; + node[i]["orientation_w"] = goals_list_[i].goal_pose_ptr->pose.orientation.w; } std::ofstream file_out(file_path); file_out << node; diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp index 0ec9ca530f074..72a5bd4fb80fe 100644 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp @@ -61,7 +61,9 @@ class AutowareAutomaticGoalPanel : public rviz_common::Panel, public automatic_goal::AutowareAutomaticGoalSender { using State = automatic_goal::AutomaticGoalState; + using Pose = geometry_msgs::msg::Pose; using PoseStamped = geometry_msgs::msg::PoseStamped; + using Marker = visualization_msgs::msg::Marker; using MarkerArray = visualization_msgs::msg::MarkerArray; using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState; using ChangeOperationMode = autoware_adapi_v1_msgs::srv::ChangeOperationMode; @@ -96,6 +98,7 @@ public Q_SLOTS: // NOLINT for Qt // Inputs void onAppendGoal(const PoseStamped::ConstSharedPtr pose); + void onAppendCheckpoint(const PoseStamped::ConstSharedPtr pose); // Visual updates void updateGUI(); @@ -116,6 +119,7 @@ public Q_SLOTS: // NOLINT for Qt // Pub/Sub rclcpp::Publisher::SharedPtr pub_marker_{nullptr}; rclcpp::Subscription::SharedPtr sub_append_goal_{nullptr}; + rclcpp::Subscription::SharedPtr sub_append_checkpoint_{nullptr}; // Containers rclcpp::Node::SharedPtr raw_node_{nullptr}; diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp index cc8d46e2f60c6..3ca368a3bd1a4 100644 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp @@ -108,9 +108,9 @@ void AutowareAutomaticGoalSender::updateGoalsList() std::stringstream ss; ss << std::fixed << std::setprecision(2); tf2::Quaternion tf2_quat; - tf2::convert(goal->pose.orientation, tf2_quat); - ss << "G" << i << " (" << goal->pose.position.x << ", "; - ss << goal->pose.position.y << ", " << tf2::getYaw(tf2_quat) << ")"; + tf2::convert(goal.goal_pose_ptr->pose.orientation, tf2_quat); + ss << "G" << i << " (" << goal.goal_pose_ptr->pose.position.x << ", "; + ss << goal.goal_pose_ptr->pose.position.y << ", " << tf2::getYaw(tf2_quat) << ")"; goals_achieved_.insert({i++, std::make_pair(ss.str(), 0)}); } onGoalListUpdated(); @@ -178,7 +178,7 @@ void AutowareAutomaticGoalSender::loadGoalsList(const std::string & file_path) pose->pose.orientation.y = goal["orientation_y"].as(); pose->pose.orientation.z = goal["orientation_z"].as(); pose->pose.orientation.w = goal["orientation_w"].as(); - goals_list_.push_back(pose); + goals_list_.emplace_back(pose); } resetAchievedGoals(); updateGoalsList(); diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp index aacdada352811..88b7b5e7dac20 100644 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp @@ -80,8 +80,11 @@ class AutowareAutomaticGoalSender : public rclcpp::Node } auto req = std::make_shared(); - req->header = goals_list_.at(goal_index)->header; - req->goal = goals_list_.at(goal_index)->pose; + req->header = goals_list_.at(goal_index).goal_pose_ptr->header; + req->goal = goals_list_.at(goal_index).goal_pose_ptr->pose; + for (const auto & checkpoint : goals_list_.at(goal_index).checkpoint_pose_ptrs) { + req->waypoints.push_back(checkpoint->pose); + } client->async_send_request( req, [this](typename rclcpp::Client::SharedFuture result) { if (result.get()->status.code != 0) state_ = State::ERROR; @@ -120,6 +123,13 @@ class AutowareAutomaticGoalSender : public rclcpp::Node } } + struct Route + { + explicit Route(const PoseStamped::ConstSharedPtr & goal) : goal_pose_ptr{goal} {} + PoseStamped::ConstSharedPtr goal_pose_ptr{}; + std::vector checkpoint_pose_ptrs{}; + }; + // Update void updateGoalsList(); virtual void updateAutoExecutionTimerTick(); @@ -155,7 +165,7 @@ class AutowareAutomaticGoalSender : public rclcpp::Node // Containers unsigned current_goal_{0}; State state_{State::INITIALIZING}; - std::vector goals_list_{}; + std::vector goals_list_{}; std::map> goals_achieved_{}; std::string goals_achieved_file_path_{}; diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp index 04bcfbacbbc17..d986d0f23fc85 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #define EIGEN_MPL2_ONLY #include @@ -98,5 +99,6 @@ BOOST_GEOMETRY_REGISTER_POINT_2D( // NOLIN tier4_autoware_utils::Point2d, double, cs::cartesian, x(), y()) // NOLINT BOOST_GEOMETRY_REGISTER_POINT_3D( // NOLINT tier4_autoware_utils::Point3d, double, cs::cartesian, x(), y(), z()) // NOLINT +BOOST_GEOMETRY_REGISTER_RING(tier4_autoware_utils::LinearRing2d) // NOLINT #endif // TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_ 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_camera_view_rviz_plugin/README.md b/common/tier4_camera_view_rviz_plugin/README.md index 99480d5a92e1c..3297b578dd609 100644 --- a/common/tier4_camera_view_rviz_plugin/README.md +++ b/common/tier4_camera_view_rviz_plugin/README.md @@ -7,3 +7,21 @@ Add the `tier4_camera_view_rviz_plugin/ThirdPersonViewTool` tool to the RViz. Pu ## BirdEyeView Tool Add the `tier4_camera_view_rviz_plugin/BirdEyeViewTool` tool to the RViz. Push the button, the camera will turn to the BEV view, the target frame is consistent with the latest frame. Short cut key 'r'. + +## Material Design Icons + +This project uses [Material Design Icons](https://developers.google.com/fonts/docs/material_symbols) by Google. These icons are used under the terms of the Apache License, Version 2.0. + +Material Design Icons are a collection of symbols provided by Google that are used to enhance the user interface of applications, websites, and other digital products. + +### License + +The Material Design Icons are licensed under the Apache License, Version 2.0. You may obtain a copy of the License at: + + + +Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. + +### Acknowledgments + +We would like to express our gratitude to Google for making these icons available to the community, helping developers and designers enhance the visual appeal and user experience of their projects. diff --git a/common/tier4_camera_view_rviz_plugin/icons/classes/BirdEyeViewTool.png b/common/tier4_camera_view_rviz_plugin/icons/classes/BirdEyeViewTool.png index 6a67573717ae1..ae5a41b241e7b 100644 Binary files a/common/tier4_camera_view_rviz_plugin/icons/classes/BirdEyeViewTool.png and b/common/tier4_camera_view_rviz_plugin/icons/classes/BirdEyeViewTool.png differ diff --git a/common/tier4_camera_view_rviz_plugin/icons/classes/ThirdPersonViewTool.png b/common/tier4_camera_view_rviz_plugin/icons/classes/ThirdPersonViewTool.png index 6a67573717ae1..93cb3f411bc35 100644 Binary files a/common/tier4_camera_view_rviz_plugin/icons/classes/ThirdPersonViewTool.png and b/common/tier4_camera_view_rviz_plugin/icons/classes/ThirdPersonViewTool.png differ diff --git a/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp b/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp index 9610bf1e2b7f1..fdd270fcce2ef 100644 --- a/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp +++ b/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp @@ -263,9 +263,9 @@ void ManualController::onClickEnableButton() { auto req = std::make_shared(); req->engage = true; - RCLCPP_INFO(raw_node_->get_logger(), "client request"); + RCLCPP_DEBUG(raw_node_->get_logger(), "client request"); if (!client_engage_->service_is_ready()) { - RCLCPP_INFO(raw_node_->get_logger(), "client is unavailable"); + RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable"); return; } client_engage_->async_send_request( diff --git a/common/tier4_datetime_rviz_plugin/package.xml b/common/tier4_datetime_rviz_plugin/package.xml index 6dc0c09b32a3b..82e1e82c61ba2 100644 --- a/common/tier4_datetime_rviz_plugin/package.xml +++ b/common/tier4_datetime_rviz_plugin/package.xml @@ -5,8 +5,6 @@ 0.0.0 The tier4_datetime_rviz_plugin package Takagi, Isamu - yabuta - Kah Hooi Tan Apache License 2.0 ament_cmake_auto diff --git a/common/tier4_debug_rviz_plugin/CMakeLists.txt b/common/tier4_debug_rviz_plugin/CMakeLists.txt index 05cf35b93ef9f..02e65ad759ed6 100644 --- a/common/tier4_debug_rviz_plugin/CMakeLists.txt +++ b/common/tier4_debug_rviz_plugin/CMakeLists.txt @@ -12,7 +12,9 @@ add_definitions(-DQT_NO_KEYWORDS) ament_auto_add_library(tier4_debug_rviz_plugin SHARED include/tier4_debug_rviz_plugin/float32_multi_array_stamped_pie_chart.hpp include/tier4_debug_rviz_plugin/jsk_overlay_utils.hpp + include/tier4_debug_rviz_plugin/string_stamped.hpp src/float32_multi_array_stamped_pie_chart.cpp + src/string_stamped.cpp src/jsk_overlay_utils.cpp ) diff --git a/common/tier4_debug_rviz_plugin/icons/classes/StringStampedOverlayDisplay.png b/common/tier4_debug_rviz_plugin/icons/classes/StringStampedOverlayDisplay.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/tier4_debug_rviz_plugin/icons/classes/StringStampedOverlayDisplay.png differ diff --git a/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/string_stamped.hpp b/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/string_stamped.hpp new file mode 100644 index 0000000000000..0960875d7eee2 --- /dev/null +++ b/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/string_stamped.hpp @@ -0,0 +1,107 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef TIER4_DEBUG_RVIZ_PLUGIN__STRING_STAMPED_HPP_ +#define TIER4_DEBUG_RVIZ_PLUGIN__STRING_STAMPED_HPP_ + +#include +#include + +#ifndef Q_MOC_RUN +#include "tier4_debug_rviz_plugin/jsk_overlay_utils.hpp" + +#include +#include +#include +#include + +#endif + +#include + +namespace rviz_plugins +{ +class StringStampedOverlayDisplay +: public rviz_common::RosTopicDisplay + +{ + Q_OBJECT + +public: + StringStampedOverlayDisplay(); + ~StringStampedOverlayDisplay() override; + + void onInitialize() override; + void onEnable() override; + void onDisable() override; + +private Q_SLOTS: + void updateVisualization(); + +protected: + void update(float wall_dt, float ros_dt) override; + void processMessage(const tier4_debug_msgs::msg::StringStamped::ConstSharedPtr msg_ptr) override; + jsk_rviz_plugins::OverlayObject::Ptr overlay_; + rviz_common::properties::ColorProperty * property_text_color_; + rviz_common::properties::IntProperty * property_left_; + rviz_common::properties::IntProperty * property_top_; + rviz_common::properties::IntProperty * property_value_height_offset_; + rviz_common::properties::FloatProperty * property_value_scale_; + rviz_common::properties::IntProperty * property_font_size_; + rviz_common::properties::IntProperty * property_max_letter_num_; + // QImage hud_; + +private: + static constexpr int line_width_ = 2; + static constexpr int hand_width_ = 4; + + std::mutex mutex_; + tier4_debug_msgs::msg::StringStamped::ConstSharedPtr last_msg_ptr_; +}; +} // namespace rviz_plugins + +#endif // TIER4_DEBUG_RVIZ_PLUGIN__STRING_STAMPED_HPP_ diff --git a/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml b/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml index c1158465e1cf1..e18900afc8d84 100644 --- a/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml @@ -4,4 +4,9 @@ base_class_type="rviz_common::Display"> Display drivable area of tier4_debug_msgs::msg::Float32MultiArrayStamped + + Display drivable area of tier4_debug_msgs::msg::StringStamped + diff --git a/common/tier4_debug_rviz_plugin/src/string_stamped.cpp b/common/tier4_debug_rviz_plugin/src/string_stamped.cpp new file mode 100644 index 0000000000000..538dc0cbe7069 --- /dev/null +++ b/common/tier4_debug_rviz_plugin/src/string_stamped.cpp @@ -0,0 +1,198 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "tier4_debug_rviz_plugin/string_stamped.hpp" + +#include "tier4_debug_rviz_plugin/jsk_overlay_utils.hpp" + +#include +#include + +#include + +#include +#include +#include +#include + +namespace rviz_plugins +{ +StringStampedOverlayDisplay::StringStampedOverlayDisplay() +{ + const Screen * screen_info = DefaultScreenOfDisplay(XOpenDisplay(NULL)); + + constexpr float hight_4k = 2160.0; + const float scale = static_cast(screen_info->height) / hight_4k; + const auto left = static_cast(std::round(1024 * scale)); + const auto top = static_cast(std::round(128 * scale)); + + property_text_color_ = new rviz_common::properties::ColorProperty( + "Text Color", QColor(25, 255, 240), "text color", this, SLOT(updateVisualization()), this); + property_left_ = new rviz_common::properties::IntProperty( + "Left", left, "Left of the plotter window", this, SLOT(updateVisualization()), this); + property_left_->setMin(0); + property_top_ = new rviz_common::properties::IntProperty( + "Top", top, "Top of the plotter window", this, SLOT(updateVisualization())); + property_top_->setMin(0); + + property_value_height_offset_ = new rviz_common::properties::IntProperty( + "Value height offset", 0, "Height offset of the plotter window", this, + SLOT(updateVisualization())); + property_font_size_ = new rviz_common::properties::IntProperty( + "Font Size", 15, "Font Size", this, SLOT(updateVisualization()), this); + property_font_size_->setMin(1); + property_max_letter_num_ = new rviz_common::properties::IntProperty( + "Max Letter Num", 100, "Max Letter Num", this, SLOT(updateVisualization()), this); + property_max_letter_num_->setMin(10); +} + +StringStampedOverlayDisplay::~StringStampedOverlayDisplay() +{ + if (initialized()) { + overlay_->hide(); + } +} + +void StringStampedOverlayDisplay::onInitialize() +{ + RTDClass::onInitialize(); + + static int count = 0; + rviz_common::UniformStringStream ss; + ss << "StringOverlayDisplayObject" << count++; + auto logger = context_->getRosNodeAbstraction().lock()->get_raw_node()->get_logger(); + overlay_.reset(new jsk_rviz_plugins::OverlayObject(scene_manager_, logger, ss.str())); + + overlay_->show(); + + const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); + overlay_->updateTextureSize(texture_size, texture_size); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +void StringStampedOverlayDisplay::onEnable() +{ + subscribe(); + overlay_->show(); +} + +void StringStampedOverlayDisplay::onDisable() +{ + unsubscribe(); + reset(); + overlay_->hide(); +} + +void StringStampedOverlayDisplay::update(float wall_dt, float ros_dt) +{ + (void)wall_dt; + (void)ros_dt; + + std::lock_guard message_lock(mutex_); + if (!last_msg_ptr_) { + return; + } + + // Display + QColor background_color; + background_color.setAlpha(0); + jsk_rviz_plugins::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage hud = buffer.getQImage(*overlay_); + hud.fill(background_color); + + QPainter painter(&hud); + painter.setRenderHint(QPainter::Antialiasing, true); + + const int w = overlay_->getTextureWidth() - line_width_; + const int h = overlay_->getTextureHeight() - line_width_; + + // text + QColor text_color(property_text_color_->getColor()); + text_color.setAlpha(255); + painter.setPen(QPen(text_color, static_cast(2), Qt::SolidLine)); + QFont font = painter.font(); + font.setPixelSize(property_font_size_->getInt()); + font.setBold(true); + painter.setFont(font); + + // same as above, but align on right side + painter.drawText( + 0, std::min(property_value_height_offset_->getInt(), h - 1), w, + std::max(h - property_value_height_offset_->getInt(), 1), Qt::AlignLeft | Qt::AlignTop, + last_msg_ptr_->data.c_str()); + painter.end(); + updateVisualization(); +} + +void StringStampedOverlayDisplay::processMessage( + const tier4_debug_msgs::msg::StringStamped::ConstSharedPtr msg_ptr) +{ + if (!isEnabled()) { + return; + } + + { + std::lock_guard message_lock(mutex_); + last_msg_ptr_ = msg_ptr; + } + + queueRender(); +} + +void StringStampedOverlayDisplay::updateVisualization() +{ + const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); + overlay_->updateTextureSize(texture_size, texture_size); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::StringStampedOverlayDisplay, rviz_common::Display) diff --git a/common/tier4_debug_tools/CMakeLists.txt b/common/tier4_debug_tools/CMakeLists.txt deleted file mode 100644 index 609b873725429..0000000000000 --- a/common/tier4_debug_tools/CMakeLists.txt +++ /dev/null @@ -1,33 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(tier4_debug_tools) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Eigen3 REQUIRED) - -include_directories( - SYSTEM - ${EIGEN3_INCLUDE_DIR} -) - -ament_auto_add_library(lateral_error_publisher SHARED - src/lateral_error_publisher.cpp -) - -rclcpp_components_register_node(lateral_error_publisher - PLUGIN "LateralErrorPublisher" - EXECUTABLE lateral_error_publisher_node -) - -ament_auto_package( - INSTALL_TO_SHARE - config - launch -) - -install(PROGRAMS scripts/stop_reason2pose.py scripts/pose2tf.py scripts/tf2pose.py - scripts/case_converter.py scripts/self_pose_listener.py - scripts/stop_reason2tf DESTINATION lib/${PROJECT_NAME}) - -install(FILES DESTINATION share/${PROJECT_NAME}) diff --git a/common/tier4_debug_tools/README.md b/common/tier4_debug_tools/README.md deleted file mode 100644 index 606172d1d57f3..0000000000000 --- a/common/tier4_debug_tools/README.md +++ /dev/null @@ -1,132 +0,0 @@ -# tier4_debug_tools - -This package provides useful features for debugging Autoware. - -## Usage - -### tf2pose - -This tool converts any `tf` to `pose` topic. -With this tool, for example, you can plot `x` values of `tf` in `rqt_multiplot`. - -```sh -ros2 run tier4_debug_tools tf2pose {tf_from} {tf_to} {hz} -``` - -Example: - -```sh -$ ros2 run tier4_debug_tools tf2pose base_link ndt_base_link 100 - -$ ros2 topic echo /tf2pose/pose -n1 -header: - seq: 13 - stamp: - secs: 1605168366 - nsecs: 549174070 - frame_id: "base_link" -pose: - position: - x: 0.0387684271191 - y: -0.00320360406477 - z: 0.000276674520819 - orientation: - x: 0.000335221893885 - y: 0.000122020672186 - z: -0.00539673212896 - w: 0.999985368502 ---- -``` - -### pose2tf - -This tool converts any `pose` topic to `tf`. - -```sh -ros2 run tier4_debug_tools pose2tf {pose_topic_name} {tf_name} -``` - -Example: - -```sh -$ ros2 run tier4_debug_tools pose2tf /localization/pose_estimator/pose ndt_pose - -$ ros2 run tf tf_echo ndt_pose ndt_base_link 100 -At time 1605168365.449 -- Translation: [0.000, 0.000, 0.000] -- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000] - in RPY (radian) [0.000, -0.000, 0.000] - in RPY (degree) [0.000, -0.000, 0.000] -``` - -### stop_reason2pose - -This tool extracts `pose` from `stop_reasons`. -Topics without numbers such as `/stop_reason2pose/pose/detection_area` are the nearest stop_reasons, and topics with numbers are individual stop_reasons that are roughly matched with previous ones. - -```sh -ros2 run tier4_debug_tools stop_reason2pose {stop_reason_topic_name} -``` - -Example: - -```sh -$ ros2 run tier4_debug_tools stop_reason2pose /planning/scenario_planning/status/stop_reasons - -$ ros2 topic list | ag stop_reason2pose -/stop_reason2pose/pose/detection_area -/stop_reason2pose/pose/detection_area_1 -/stop_reason2pose/pose/obstacle_stop -/stop_reason2pose/pose/obstacle_stop_1 - -$ ros2 topic echo /stop_reason2pose/pose/detection_area -n1 -header: - seq: 1 - stamp: - secs: 1605168355 - nsecs: 821713 - frame_id: "map" -pose: - position: - x: 60608.8433457 - y: 43886.2410876 - z: 44.9078212441 - orientation: - x: 0.0 - y: 0.0 - z: -0.190261378408 - w: 0.981733470901 ---- -``` - -### stop_reason2tf - -This is an all-in-one script that uses `tf2pose`, `pose2tf`, and `stop_reason2pose`. -With this tool, you can view the relative position from base_link to the nearest stop_reason. - -```sh -ros2 run tier4_debug_tools stop_reason2tf {stop_reason_name} -``` - -Example: - -```sh -$ ros2 run tier4_debug_tools stop_reason2tf obstacle_stop -At time 1605168359.501 -- Translation: [0.291, -0.095, 0.266] -- Rotation: in Quaternion [0.007, 0.011, -0.005, 1.000] - in RPY (radian) [0.014, 0.023, -0.010] - in RPY (degree) [0.825, 1.305, -0.573] -``` - -### lateral_error_publisher - -This node calculate the control error and localization error in the trajectory normal direction as shown in the figure below. - -![lateral_error_publisher_overview](./media/lateral_error_publisher.svg) - -Set the reference trajectory, vehicle pose and ground truth pose in the launch file. - -```sh -ros2 launch tier4_debug_tools lateral_error_publisher.launch.xml -``` diff --git a/common/tier4_debug_tools/config/lateral_error_publisher.param.yaml b/common/tier4_debug_tools/config/lateral_error_publisher.param.yaml deleted file mode 100644 index ecde03de7c658..0000000000000 --- a/common/tier4_debug_tools/config/lateral_error_publisher.param.yaml +++ /dev/null @@ -1,3 +0,0 @@ -/**: - ros__parameters: - yaw_threshold_to_search_closest: 0.785398 # yaw threshold to search closest index [rad] diff --git a/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp b/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp deleted file mode 100644 index 962fee8a370a4..0000000000000 --- a/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp +++ /dev/null @@ -1,74 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef TIER4_DEBUG_TOOLS__LATERAL_ERROR_PUBLISHER_HPP_ -#define TIER4_DEBUG_TOOLS__LATERAL_ERROR_PUBLISHER_HPP_ - -#define EIGEN_MPL2_ONLY - -#include -#include -#include -#include - -#include -#include -#include - -class LateralErrorPublisher : public rclcpp::Node -{ -public: - explicit LateralErrorPublisher(const rclcpp::NodeOptions & node_options); - -private: - /* Parameters */ - double yaw_threshold_to_search_closest_; - - /* States */ - autoware_auto_planning_msgs::msg::Trajectory::SharedPtr - current_trajectory_ptr_; //!< @brief reference trajectory - geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr - current_vehicle_pose_ptr_; //!< @brief current EKF pose - geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr - current_ground_truth_pose_ptr_; //!< @brief current GNSS pose - - /* Publishers and Subscribers */ - rclcpp::Subscription::SharedPtr - sub_trajectory_; //!< @brief subscription for reference trajectory - rclcpp::Subscription::SharedPtr - sub_vehicle_pose_; //!< @brief subscription for vehicle pose - rclcpp::Subscription::SharedPtr - sub_ground_truth_pose_; //!< @brief subscription for gnss pose - rclcpp::Publisher::SharedPtr - pub_control_lateral_error_; //!< @brief publisher for control lateral error - rclcpp::Publisher::SharedPtr - pub_localization_lateral_error_; //!< @brief publisher for localization lateral error - rclcpp::Publisher::SharedPtr - pub_lateral_error_; //!< @brief publisher for lateral error (control + localization) - - /** - * @brief set current_trajectory_ with received message - */ - void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr); - /** - * @brief set current_vehicle_pose_ with received message - */ - void onVehiclePose(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - /** - * @brief set current_ground_truth_pose_ and calculate lateral error - */ - void onGroundTruthPose(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); -}; - -#endif // TIER4_DEBUG_TOOLS__LATERAL_ERROR_PUBLISHER_HPP_ diff --git a/common/tier4_debug_tools/launch/lateral_error_publisher.launch.xml b/common/tier4_debug_tools/launch/lateral_error_publisher.launch.xml deleted file mode 100644 index cd1188326fb86..0000000000000 --- a/common/tier4_debug_tools/launch/lateral_error_publisher.launch.xml +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/common/tier4_debug_tools/media/lateral_error_publisher.svg b/common/tier4_debug_tools/media/lateral_error_publisher.svg deleted file mode 100644 index 9d44bc2621bcd..0000000000000 --- a/common/tier4_debug_tools/media/lateral_error_publisher.svg +++ /dev/null @@ -1,183 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
- ① -
-
-
- - ① - - - - - - - - - - - -
-
-
- Closest trajectory point -
-
-
- - Closest trajectory poi... - - - - - -
-
-
- Next trajectory point -
-
-
- - Next trajectory point - - - - - -
-
-
- Trajectory -
-
-
- - Trajectory - - - - - - -
-
-
- Ground truth pose -
-
-
- - Ground truth pose - - - - - - -
-
-
- Vehicle pose -
-
-
- - Vehicle pose - - - - - -
-
-
- ② -
-
-
- - ② - - - - - - -
-
-
- ① Control lateral error -
- ② Localization lateral error -
- ①+② Lateral error -
-
-
- - ① Control lateral error... - - - - - - - Viewer does not support full SVG 1.1 - - - diff --git a/common/tier4_debug_tools/package.xml b/common/tier4_debug_tools/package.xml deleted file mode 100644 index 8f03a3b55cf26..0000000000000 --- a/common/tier4_debug_tools/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - tier4_debug_tools - 0.1.0 - The tier4_debug_tools package - Ryohsuke Mitsudome - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - autoware_auto_planning_msgs - geometry_msgs - motion_utils - rclcpp - rclcpp_components - tf2_ros - tier4_autoware_utils - tier4_debug_msgs - - launch_ros - python3-rtree - rclpy - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/common/tier4_debug_tools/scripts/case_converter.py b/common/tier4_debug_tools/scripts/case_converter.py deleted file mode 100755 index 2fcc3eebac62e..0000000000000 --- a/common/tier4_debug_tools/scripts/case_converter.py +++ /dev/null @@ -1,25 +0,0 @@ -#! /usr/bin/env python3 - -# Copyright 2020 Tier IV, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import re - - -def pascal2snake(s): - return re.sub(r"([a-z])([A-Z])", r"\1_\2", s).lower() - - -def snake2pascal(s): - return "".join([w.capitalize() for w in s.split("_")]) diff --git a/common/tier4_debug_tools/scripts/pose2tf.py b/common/tier4_debug_tools/scripts/pose2tf.py deleted file mode 100755 index c954e370d4069..0000000000000 --- a/common/tier4_debug_tools/scripts/pose2tf.py +++ /dev/null @@ -1,78 +0,0 @@ -#! /usr/bin/env python3 - -# Copyright 2020 Tier IV, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -import argparse -import sys - -from geometry_msgs.msg import PoseStamped -from geometry_msgs.msg import TransformStamped -import rclpy -from rclpy.node import Node -import tf2_ros - - -class Pose2TfNode(Node): - def __init__(self, options): - super().__init__("pose2tf") - self._options = options - self._tf_broadcaster = tf2_ros.TransformBroadcaster(self) - self._sub_pose = self.create_subscription( - PoseStamped, self._options.topic_name, self._on_pose, 1 - ) - - def _on_pose(self, msg): - try: - tfs = Pose2TfNode.create_transform(self, msg) - transforms = [] - transforms.append(tfs) - self._tf_broadcaster.sendTransform(transforms) - except tf2_ros.TransformException as e: - print(e) - - @staticmethod - def create_transform(self, msg): - tfs = TransformStamped() - tfs.header.frame_id = msg.header.frame_id - tfs.header.stamp = msg.header.stamp - tfs.child_frame_id = self._options.tf_name - tfs.transform.translation.x = msg.pose.position.x - tfs.transform.translation.y = msg.pose.position.y - tfs.transform.translation.z = msg.pose.position.z - tfs.transform.rotation.x = msg.pose.orientation.x - tfs.transform.rotation.y = msg.pose.orientation.y - tfs.transform.rotation.z = msg.pose.orientation.z - tfs.transform.rotation.w = msg.pose.orientation.w - return tfs - - -def main(args): - print("{}".format(args)) - rclpy.init() - - parser = argparse.ArgumentParser() - parser.add_argument("topic_name", type=str) - parser.add_argument("tf_name", type=str) - ns = parser.parse_args(args) - - pose2tf_node = Pose2TfNode(ns) - rclpy.spin(pose2tf_node) - pose2tf_node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main(sys.argv[1:]) diff --git a/common/tier4_debug_tools/scripts/self_pose_listener.py b/common/tier4_debug_tools/scripts/self_pose_listener.py deleted file mode 100755 index 0087bfbbbdf1e..0000000000000 --- a/common/tier4_debug_tools/scripts/self_pose_listener.py +++ /dev/null @@ -1,56 +0,0 @@ -#! /usr/bin/env python3 - -# Copyright 2020 Tier IV, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from geometry_msgs.msg import PoseStamped -import rclpy -from rclpy.node import Node -from tf2_ros import LookupException -from tf2_ros.buffer import Buffer -from tf2_ros.transform_listener import TransformListener - - -class SelfPoseListener(Node): - def __init__(self): - super().__init__("self_pose_listener") - self.tf_buffer = Buffer() - self._tf_listener = TransformListener(self.tf_buffer, self) - self.self_pose = PoseStamped() - - def get_current_pose(self): - try: - tf = self.tf_buffer.lookup_transform("map", "base_link", rclpy.time.Time()) - tf_time = self.tf_buffer.get_latest_common_time("map", "base_link") - self.self_pose = SelfPoseListener.create_pose(tf_time, "map", tf) - except LookupException as e: - self.get_logger().warn("Required transformation not found: `{}`".format(str(e))) - return None - - @staticmethod - def create_pose(time, frame_id, tf): - pose = PoseStamped() - - pose.header.stamp = time.to_msg() - pose.header.frame_id = frame_id - - pose.pose.position.x = tf.transform.translation.x - pose.pose.position.y = tf.transform.translation.y - pose.pose.position.z = tf.transform.translation.z - pose.pose.orientation.x = tf.transform.rotation.x - pose.pose.orientation.y = tf.transform.rotation.y - pose.pose.orientation.z = tf.transform.rotation.z - pose.pose.orientation.w = tf.transform.rotation.w - - return pose diff --git a/common/tier4_debug_tools/scripts/stop_reason2pose.py b/common/tier4_debug_tools/scripts/stop_reason2pose.py deleted file mode 100755 index 433f4e75042f8..0000000000000 --- a/common/tier4_debug_tools/scripts/stop_reason2pose.py +++ /dev/null @@ -1,167 +0,0 @@ -#! /usr/bin/env python3 - -# Copyright 2020 Tier IV, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import argparse -import math -import sys - -from case_converter import pascal2snake -from geometry_msgs.msg import PoseStamped -import numpy as np -import rclpy -from rclpy.node import Node -from rtree import index -from self_pose_listener import SelfPoseListener -from tier4_planning_msgs.msg import StopReasonArray - - -class StopReason2PoseNode(Node): - def __init__(self, options): - super().__init__("stop_reason2pose_node") - self._options = options - self._sub_pose = self.create_subscription( - StopReasonArray, self._options.topic_name, self._on_stop_reasons, 1 - ) - self._pub_pose_map = {} - self._idx_map = {} - self._pose_map = {} - self._self_pose_listener = SelfPoseListener() - self.timer = self.create_timer((1.0 / 100), self._self_pose_listener.get_current_pose) - - def _on_stop_reasons(self, msg): - for stop_reason in msg.stop_reasons: - snake_case_stop_reason = pascal2snake(stop_reason.reason) - - if len(stop_reason.stop_factors) == 0: - self.get_logger().warn("stop_factor is null") - return - - for stop_factor in stop_reason.stop_factors: - pose = PoseStamped() - pose.header = msg.header - pose.pose = stop_factor.stop_pose - - # Get nearest pose - th_dist = 1.0 - nearest_pose_id = self._get_nearest_pose_id( - snake_case_stop_reason, pose.pose, th_dist - ) - if nearest_pose_id: - self._update_pose(snake_case_stop_reason, pose.pose, nearest_pose_id) - pose_id = nearest_pose_id - else: - pose_id = self._register_pose(snake_case_stop_reason, pose.pose) - - pose_topic_name = "{snake_case_stop_reason}_{pose_id}".format(**locals()) - topic_ns = "/tier4_debug_tools/stop_reason2pose/" - if pose_topic_name not in self._pub_pose_map: - self._pub_pose_map[pose_topic_name] = self.create_publisher( - PoseStamped, topic_ns + pose_topic_name, 1 - ) - self._pub_pose_map[pose_topic_name].publish(pose) - - # Publish nearest stop_reason without number - nearest_pose = PoseStamped() - nearest_pose.header = msg.header - nearest_pose.pose = self._get_nearest_pose_in_array( - stop_reason, self._self_pose_listener.self_pose - ) - - if nearest_pose.pose: - if snake_case_stop_reason not in self._pub_pose_map: - topic_ns = "/tier4_debug_tools/stop_reason2pose/" - self._pub_pose_map[snake_case_stop_reason] = self.create_publisher( - PoseStamped, topic_ns + snake_case_stop_reason, 1 - ) - self._pub_pose_map[snake_case_stop_reason].publish(nearest_pose) - - def _get_nearest_pose_in_array(self, stop_reason, self_pose): - poses = [stop_factor.stop_pose for stop_factor in stop_reason.stop_factors] - if not poses: - return None - - distances = [StopReason2PoseNode.calc_distance2d(p, self_pose.pose) for p in poses] - nearest_idx = np.argmin(distances) - - return poses[nearest_idx] - - def _find_nearest_pose_id(self, name, pose): - if name not in self._idx_map: - self._idx_map[name] = index.Index() - - return self._idx_map[name].nearest(StopReason2PoseNode.pose2boundingbox(pose), 1) - - def _get_nearest_pose_id(self, name, pose, th_dist): - nearest_pose_ids = list(self._find_nearest_pose_id(name, pose)) - if not nearest_pose_ids: - return None - - nearest_pose_id = nearest_pose_ids[0] - nearest_pose = self._get_pose(name, nearest_pose_id) - if not nearest_pose: - return None - - dist = StopReason2PoseNode.calc_distance2d(pose, nearest_pose) - if dist > th_dist: - return None - - return nearest_pose_id - - def _get_pose(self, name, pose_id): - if name not in self._pose_map: - return None - - return self._pose_map[name][pose_id] - - def _update_pose(self, name, pose, pose_id): - self._pose_map[name][id] = pose - self._idx_map[name].insert(pose_id, StopReason2PoseNode.pose2boundingbox(pose)) - - def _register_pose(self, name, pose): - if name not in self._pose_map: - self._pose_map[name] = {} - - pose_id = len(self._pose_map[name]) + 1 - self._pose_map[name][pose_id] = pose - self._idx_map[name].insert(pose_id, StopReason2PoseNode.pose2boundingbox(pose)) - return pose_id - - @staticmethod - def calc_distance2d(pose1, pose2): - p1 = pose1.position - p2 = pose2.position - return math.hypot(p1.x - p2.x, p1.y - p2.y) - - @staticmethod - def pose2boundingbox(pose): - return [pose.position.x, pose.position.y, pose.position.x, pose.position.y] - - -def main(args): - rclpy.init() - - parser = argparse.ArgumentParser() - parser.add_argument("topic_name", type=str) - ns = parser.parse_args(args) - - stop_reason2pose_node = StopReason2PoseNode(ns) - rclpy.spin(stop_reason2pose_node) - stop_reason2pose_node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main(sys.argv[1:]) diff --git a/common/tier4_debug_tools/scripts/stop_reason2tf b/common/tier4_debug_tools/scripts/stop_reason2tf deleted file mode 100755 index 60d5956130869..0000000000000 --- a/common/tier4_debug_tools/scripts/stop_reason2tf +++ /dev/null @@ -1,15 +0,0 @@ -#!/usr/bin/env bash - -stop_reason_name="$1" - -if [ -z "${stop_reason_name}" ]; then - echo "Please input stop_reason_name as the 1st argument." - exit 1 -fi - -ros2 run tier4_debug_tools stop_reason2pose.py /planning/scenario_planning/status/stop_reasons >/dev/null 2>&1 & -ros2 run tier4_debug_tools pose2tf.py /tier4_debug_tools/stop_reason2pose/"${stop_reason_name}" "${stop_reason_name}" >/dev/null 2>&1 & -ros2 run tier4_debug_tools tf2pose.py "${stop_reason_name}" base_link 100 >/dev/null 2>&1 & -ros2 run tf2_ros tf2_echo "${stop_reason_name}" base_link 100 2>/dev/null - -wait diff --git a/common/tier4_debug_tools/scripts/tf2pose.py b/common/tier4_debug_tools/scripts/tf2pose.py deleted file mode 100755 index a4422fdd8015b..0000000000000 --- a/common/tier4_debug_tools/scripts/tf2pose.py +++ /dev/null @@ -1,83 +0,0 @@ -#! /usr/bin/env python3 - -# Copyright 2020 Tier IV, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import argparse -import sys - -from geometry_msgs.msg import PoseStamped -import rclpy -from rclpy.node import Node -from tf2_ros import LookupException -from tf2_ros.buffer import Buffer -from tf2_ros.transform_listener import TransformListener - - -class Tf2PoseNode(Node): - def __init__(self, options): - super().__init__("tf2pose") - - self._options = options - self.tf_buffer = Buffer() - self.tf_listener = TransformListener(self.tf_buffer, self) - self._pub_pose = self.create_publisher(PoseStamped, "/tier4_debug_tools/tf2pose/pose", 1) - self.timer = self.create_timer((1.0 / self._options.hz), self._on_timer) - - def _on_timer(self): - try: - tf = self.tf_buffer.lookup_transform( - self._options.tf_from, self._options.tf_to, rclpy.time.Time() - ) - time = self.tf_buffer.get_latest_common_time(self._options.tf_from, self._options.tf_to) - pose = Tf2PoseNode.create_pose(time, self._options.tf_from, tf) - self._pub_pose.publish(pose) - except LookupException as e: - print(e) - - @staticmethod - def create_pose(time, frame_id, tf): - pose = PoseStamped() - - pose.header.stamp = time.to_msg() - pose.header.frame_id = frame_id - - pose.pose.position.x = tf.transform.translation.x - pose.pose.position.y = tf.transform.translation.y - pose.pose.position.z = tf.transform.translation.z - pose.pose.orientation.x = tf.transform.rotation.x - pose.pose.orientation.y = tf.transform.rotation.y - pose.pose.orientation.z = tf.transform.rotation.z - pose.pose.orientation.w = tf.transform.rotation.w - - return pose - - -def main(args): - rclpy.init() - - parser = argparse.ArgumentParser() - parser.add_argument("tf_from", type=str) - parser.add_argument("tf_to", type=str) - parser.add_argument("hz", type=int, default=10) - ns = parser.parse_args(args) - - tf2pose_node = Tf2PoseNode(ns) - rclpy.spin(tf2pose_node) - tf2pose_node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main(sys.argv[1:]) diff --git a/common/tier4_debug_tools/src/lateral_error_publisher.cpp b/common/tier4_debug_tools/src/lateral_error_publisher.cpp deleted file mode 100644 index 18c97a47aa55a..0000000000000 --- a/common/tier4_debug_tools/src/lateral_error_publisher.cpp +++ /dev/null @@ -1,151 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "tier4_debug_tools/lateral_error_publisher.hpp" - -#include - -LateralErrorPublisher::LateralErrorPublisher(const rclcpp::NodeOptions & node_options) -: Node("lateral_error_publisher", node_options) -{ - using std::placeholders::_1; - - /* Parameters */ - yaw_threshold_to_search_closest_ = - declare_parameter("yaw_threshold_to_search_closest", M_PI / 4.0); - - /* Publishers and Subscribers */ - sub_trajectory_ = create_subscription( - "~/input/reference_trajectory", rclcpp::QoS{1}, - std::bind(&LateralErrorPublisher::onTrajectory, this, _1)); - sub_vehicle_pose_ = create_subscription( - "~/input/vehicle_pose_with_covariance", rclcpp::QoS{1}, - std::bind(&LateralErrorPublisher::onVehiclePose, this, _1)); - sub_ground_truth_pose_ = create_subscription( - "~/input/ground_truth_pose_with_covariance", rclcpp::QoS{1}, - std::bind(&LateralErrorPublisher::onGroundTruthPose, this, _1)); - pub_control_lateral_error_ = - create_publisher("~/control_lateral_error", 1); - pub_localization_lateral_error_ = - create_publisher("~/localization_lateral_error", 1); - pub_lateral_error_ = - create_publisher("~/lateral_error", 1); -} - -void LateralErrorPublisher::onTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) -{ - current_trajectory_ptr_ = msg; -} - -void LateralErrorPublisher::onVehiclePose( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) -{ - current_vehicle_pose_ptr_ = msg; -} - -void LateralErrorPublisher::onGroundTruthPose( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) -{ - current_ground_truth_pose_ptr_ = msg; - - // Guard - if (current_trajectory_ptr_ == nullptr || current_vehicle_pose_ptr_ == nullptr) { - RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 1000 /* ms */, - "Reference trajectory or EKF pose is not received"); - return; - } - if (current_trajectory_ptr_->points.size() < 2) { - RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 1000 /* ms */, "Reference trajectory is too short"); - return; - } - - // Search closest trajectory point with vehicle pose - const auto closest_index = motion_utils::findNearestIndex( - current_trajectory_ptr_->points, current_vehicle_pose_ptr_->pose.pose, - std::numeric_limits::max(), yaw_threshold_to_search_closest_); - if (!closest_index) { - RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 1000 /* ms */, "Failed to search closest index"); - return; - } - - // Calculate the normal vector in the reference trajectory direction - size_t base_index = 0; - size_t next_index = 0; - if (*closest_index == current_trajectory_ptr_->points.size() - 1) { - base_index = *closest_index - 1; - next_index = *closest_index; - } else { - base_index = *closest_index; - next_index = *closest_index + 1; - } - - const auto & base_pose = current_trajectory_ptr_->points.at(base_index).pose; - const auto & next_pose = current_trajectory_ptr_->points.at(next_index).pose; - const double dx = next_pose.position.x - base_pose.position.x; - const double dy = next_pose.position.y - base_pose.position.y; - const Eigen::Vector2d trajectory_direction(dx, dy); - RCLCPP_DEBUG(this->get_logger(), "trajectory direction: (%f, %f)", dx, dy); - - const auto rotation = Eigen::Rotation2Dd(M_PI_2); - const Eigen::Vector2d normal_direction = rotation * trajectory_direction; - RCLCPP_DEBUG( - this->get_logger(), "normal direction: (%f, %f)", normal_direction(0), normal_direction(1)); - const Eigen::Vector2d unit_normal_direction = normal_direction.normalized(); - RCLCPP_DEBUG( - this->get_logger(), "unit normal direction: (%f, %f)", unit_normal_direction(0), - unit_normal_direction(1)); - - // Calculate control lateral error - const auto & closest_pose = current_trajectory_ptr_->points.at(*closest_index).pose; - const auto & vehicle_pose = current_vehicle_pose_ptr_->pose.pose; - const Eigen::Vector2d closest_to_vehicle( - vehicle_pose.position.x - closest_pose.position.x, - vehicle_pose.position.y - closest_pose.position.y); - const auto control_lateral_error = closest_to_vehicle.dot(unit_normal_direction); - RCLCPP_DEBUG(this->get_logger(), "control_lateral_error: %f", control_lateral_error); - - // Calculate localization lateral error - const auto ground_truth_pose = current_ground_truth_pose_ptr_->pose.pose; - const Eigen::Vector2d vehicle_to_ground_truth( - ground_truth_pose.position.x - vehicle_pose.position.x, - ground_truth_pose.position.y - vehicle_pose.position.y); - const auto localization_lateral_error = vehicle_to_ground_truth.dot(unit_normal_direction); - RCLCPP_DEBUG(this->get_logger(), "localization_lateral_error: %f", localization_lateral_error); - - const auto lateral_error = control_lateral_error + localization_lateral_error; - RCLCPP_DEBUG(this->get_logger(), "localization_error: %f", lateral_error); - - // Publish lateral errors - tier4_debug_msgs::msg::Float32Stamped control_msg; - control_msg.stamp = this->now(); - control_msg.data = static_cast(control_lateral_error); - pub_control_lateral_error_->publish(control_msg); - - tier4_debug_msgs::msg::Float32Stamped localization_msg; - localization_msg.stamp = this->now(); - localization_msg.data = static_cast(localization_lateral_error); - pub_localization_lateral_error_->publish(localization_msg); - - tier4_debug_msgs::msg::Float32Stamped sum_msg; - sum_msg.stamp = this->now(); - sum_msg.data = static_cast(lateral_error); - pub_lateral_error_->publish(sum_msg); -} - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(LateralErrorPublisher) diff --git a/common/tier4_localization_rviz_plugin/package.xml b/common/tier4_localization_rviz_plugin/package.xml index e103db19f1d43..f2482b2d45a3b 100644 --- a/common/tier4_localization_rviz_plugin/package.xml +++ b/common/tier4_localization_rviz_plugin/package.xml @@ -5,8 +5,6 @@ 0.1.0 The tier4_localization_rviz_plugin package Takagi, Isamu - yabuta - Kah Hooi Tan Takamasa Horibe Apache License 2.0 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..a298967a31af9 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 @@ -38,9 +38,21 @@ Planning: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance - behavior_path_planner_lane_change: + behavior_path_planner_goal_planner: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner - logger_name: lane_change + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.goal_planner + + behavior_path_planner_start_planner: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.start_planner + + behavior_path_avoidance_by_lane_change: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: lane_change.AVOIDANCE_BY_LANE_CHANGE + + behavior_path_lane_change: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: lane_change.NORMAL behavior_velocity_planner: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner @@ -85,3 +97,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_perception_rviz_plugin/README.md b/common/tier4_perception_rviz_plugin/README.md index 7ce9ba5114ef9..6ec6a1a0e28a1 100644 --- a/common/tier4_perception_rviz_plugin/README.md +++ b/common/tier4_perception_rviz_plugin/README.md @@ -112,3 +112,21 @@ You can interactively manipulate the object. | ADD | Shift + Click Right Button | | MOVE | Hold down Right Button + Drug and Drop | | DELETE | Alt + Click Right Button | + +## Material Design Icons + +This project uses [Material Design Icons](https://developers.google.com/fonts/docs/material_symbols) by Google. These icons are used under the terms of the Apache License, Version 2.0. + +Material Design Icons are a collection of symbols provided by Google that are used to enhance the user interface of applications, websites, and other digital products. + +### License + +The Material Design Icons are licensed under the Apache License, Version 2.0. You may obtain a copy of the License at: + + + +Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. + +### Acknowledgments + +We would like to express our gratitude to Google for making these icons available to the community, helping developers and designers enhance the visual appeal and user experience of their projects. diff --git a/common/tier4_perception_rviz_plugin/icons/classes/BikeInitialPoseTool.png b/common/tier4_perception_rviz_plugin/icons/classes/BikeInitialPoseTool.png index 6a67573717ae1..b36ea7c84774b 100644 Binary files a/common/tier4_perception_rviz_plugin/icons/classes/BikeInitialPoseTool.png and b/common/tier4_perception_rviz_plugin/icons/classes/BikeInitialPoseTool.png differ diff --git a/common/tier4_perception_rviz_plugin/icons/classes/BusInitialPoseTool.png b/common/tier4_perception_rviz_plugin/icons/classes/BusInitialPoseTool.png index 6a67573717ae1..d6ef8fb25c063 100644 Binary files a/common/tier4_perception_rviz_plugin/icons/classes/BusInitialPoseTool.png and b/common/tier4_perception_rviz_plugin/icons/classes/BusInitialPoseTool.png differ diff --git a/common/tier4_perception_rviz_plugin/icons/classes/CarInitialPoseTool.png b/common/tier4_perception_rviz_plugin/icons/classes/CarInitialPoseTool.png index 6a67573717ae1..2959a2c639c3e 100644 Binary files a/common/tier4_perception_rviz_plugin/icons/classes/CarInitialPoseTool.png and b/common/tier4_perception_rviz_plugin/icons/classes/CarInitialPoseTool.png differ diff --git a/common/tier4_perception_rviz_plugin/icons/classes/DeleteAllObjectsTool.png b/common/tier4_perception_rviz_plugin/icons/classes/DeleteAllObjectsTool.png index 6a67573717ae1..b1991030c4bb0 100644 Binary files a/common/tier4_perception_rviz_plugin/icons/classes/DeleteAllObjectsTool.png and b/common/tier4_perception_rviz_plugin/icons/classes/DeleteAllObjectsTool.png differ diff --git a/common/tier4_perception_rviz_plugin/icons/classes/PedestrianInitialPoseTool.png b/common/tier4_perception_rviz_plugin/icons/classes/PedestrianInitialPoseTool.png index 6a67573717ae1..2ae4aa47e3c74 100644 Binary files a/common/tier4_perception_rviz_plugin/icons/classes/PedestrianInitialPoseTool.png and b/common/tier4_perception_rviz_plugin/icons/classes/PedestrianInitialPoseTool.png differ diff --git a/common/tier4_perception_rviz_plugin/icons/classes/UnknownInitialPoseTool.png b/common/tier4_perception_rviz_plugin/icons/classes/UnknownInitialPoseTool.png index 6a67573717ae1..138457d72e0ac 100644 Binary files a/common/tier4_perception_rviz_plugin/icons/classes/UnknownInitialPoseTool.png and b/common/tier4_perception_rviz_plugin/icons/classes/UnknownInitialPoseTool.png differ diff --git a/common/tier4_planning_rviz_plugin/CMakeLists.txt b/common/tier4_planning_rviz_plugin/CMakeLists.txt index 5c17af5df22d1..69b61ba506b32 100644 --- a/common/tier4_planning_rviz_plugin/CMakeLists.txt +++ b/common/tier4_planning_rviz_plugin/CMakeLists.txt @@ -18,13 +18,13 @@ include_directories( ament_auto_add_library(tier4_planning_rviz_plugin SHARED # path point - include/path/display_base.hpp - include/path/display.hpp + include/tier4_planning_rviz_plugin/path/display_base.hpp + include/tier4_planning_rviz_plugin/path/display.hpp src/path/display.cpp # footprint - include/pose_with_uuid_stamped/display.hpp + include/tier4_planning_rviz_plugin/pose_with_uuid_stamped/display.hpp src/pose_with_uuid_stamped/display.cpp - include/mission_checkpoint/mission_checkpoint.hpp + include/tier4_planning_rviz_plugin/mission_checkpoint/mission_checkpoint.hpp src/mission_checkpoint/mission_checkpoint.cpp src/tools/jsk_overlay_utils.cpp src/tools/max_velocity.cpp diff --git a/common/tier4_planning_rviz_plugin/README.md b/common/tier4_planning_rviz_plugin/README.md index 8b9154422eae6..ef9df25f2b657 100644 --- a/common/tier4_planning_rviz_plugin/README.md +++ b/common/tier4_planning_rviz_plugin/README.md @@ -125,3 +125,21 @@ This plugin displays the path, trajectory, and maximum speed. ![select_planning_plugin](./images/select_planning_plugin.png) 3. Enter the name of the topic where you want to view the path or trajectory. ![select_topic_name](./images/select_topic_name.png) + +## Material Design Icons + +This project uses [Material Design Icons](https://developers.google.com/fonts/docs/material_symbols) by Google. These icons are used under the terms of the Apache License, Version 2.0. + +Material Design Icons are a collection of symbols provided by Google that are used to enhance the user interface of applications, websites, and other digital products. + +### License + +The Material Design Icons are licensed under the Apache License, Version 2.0. You may obtain a copy of the License at: + + + +Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. + +### Acknowledgments + +We would like to express our gratitude to Google for making these icons available to the community, helping developers and designers enhance the visual appeal and user experience of their projects. diff --git a/common/tier4_planning_rviz_plugin/icons/classes/MissionCheckpointTool.png b/common/tier4_planning_rviz_plugin/icons/classes/MissionCheckpointTool.png index 6a67573717ae1..4f5b4961f2500 100644 Binary files a/common/tier4_planning_rviz_plugin/icons/classes/MissionCheckpointTool.png and b/common/tier4_planning_rviz_plugin/icons/classes/MissionCheckpointTool.png differ diff --git a/common/tier4_planning_rviz_plugin/include/path/display.hpp b/common/tier4_planning_rviz_plugin/include/path/display.hpp deleted file mode 100644 index 7d635f0cedcad..0000000000000 --- a/common/tier4_planning_rviz_plugin/include/path/display.hpp +++ /dev/null @@ -1,231 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef PATH__DISPLAY_HPP_ -#define PATH__DISPLAY_HPP_ - -#include - -#include -#include -#include - -#include -#include -#include - -namespace -{ -bool validateBoundFloats(const std::vector & bound) -{ - for (auto && point : bound) { - if (!rviz_common::validateFloats(point)) { - return false; - } - } - return true; -} - -template -bool validateBoundFloats(const typename T::ConstSharedPtr & msg_ptr) -{ - return validateBoundFloats(msg_ptr->left_bound) && validateBoundFloats(msg_ptr->right_bound); -} - -void visualizeBound( - const std::vector & bound, const Ogre::ColourValue & color, - const float width, Ogre::ManualObject * bound_object) -{ - if (bound.size() < 2) { - return; - } - - bound_object->estimateVertexCount(bound.size() * 2); - bound_object->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_STRIP); - - // calculate normal vector of bound and widths depending on the normal vector - std::vector normal_vector_angles; - std::vector adaptive_widths; - for (size_t i = 0; i < bound.size(); ++i) { - const auto [normal_vector_angle, adaptive_width] = [&]() -> std::pair { - if (i == 0) { - return std::make_pair( - tier4_autoware_utils::calcAzimuthAngle(bound.at(i), bound.at(i + 1)) + M_PI_2, width); - } - if (i == bound.size() - 1) { - return std::make_pair( - tier4_autoware_utils::calcAzimuthAngle(bound.at(i - 1), bound.at(i)) + M_PI_2, width); - } - const auto & prev_p = bound.at(i - 1); - const auto & curr_p = bound.at(i); - const auto & next_p = bound.at(i + 1); - - const float curr_to_prev_angle = tier4_autoware_utils::calcAzimuthAngle(curr_p, prev_p); - const float curr_to_next_angle = tier4_autoware_utils::calcAzimuthAngle(curr_p, next_p); - const float normal_vector_angle = (curr_to_prev_angle + curr_to_next_angle) / 2.0; - - const float diff_angle = - tier4_autoware_utils::normalizeRadian(normal_vector_angle - curr_to_next_angle); - if (diff_angle == 0.0) { - return std::make_pair(normal_vector_angle, width); - } - - return std::make_pair(normal_vector_angle, width / std::sin(diff_angle)); - }(); - - normal_vector_angles.push_back(normal_vector_angle); - adaptive_widths.push_back(adaptive_width); - } - - // calculate triangle - for (size_t i = 0; i < bound.size(); ++i) { - const float normal_vector_angle = normal_vector_angles.at(i); - const float adaptive_width = adaptive_widths.at(i); - - const auto x_offset = static_cast(adaptive_width * 0.5 * std::cos(normal_vector_angle)); - const auto y_offset = static_cast(adaptive_width * 0.5 * std::sin(normal_vector_angle)); - auto target_lp = bound.at(i); - target_lp.x = target_lp.x + x_offset; - target_lp.y = target_lp.y + y_offset; - target_lp.z = target_lp.z; - bound_object->position(target_lp.x, target_lp.y, target_lp.z); - bound_object->colour(color); - auto target_rp = bound.at(i); - target_rp.x = target_rp.x - x_offset; - target_rp.y = target_rp.y - y_offset; - target_rp.z = target_rp.z; - bound_object->position(target_rp.x, target_rp.y, target_rp.z); - bound_object->colour(color); - } - bound_object->end(); -} -} // namespace - -namespace rviz_plugins -{ -template -class AutowarePathWithDrivableAreaDisplay : public AutowarePathBaseDisplay -{ -public: - AutowarePathWithDrivableAreaDisplay() - : property_drivable_area_view_{"View Drivable Area", true, "", this}, - property_drivable_area_alpha_{"Alpha", 0.999, "", &property_drivable_area_view_}, - property_drivable_area_color_{"Color", QColor(0, 148, 205), "", &property_drivable_area_view_}, - property_drivable_area_width_{"Width", 0.3f, "", &property_drivable_area_view_} - { - property_drivable_area_alpha_.setMin(0.0); - property_drivable_area_alpha_.setMax(1.0); - property_drivable_area_width_.setMin(0.001); - } - - ~AutowarePathWithDrivableAreaDisplay() - { - if (this->initialized()) { - this->scene_manager_->destroyManualObject(left_bound_object_); - this->scene_manager_->destroyManualObject(right_bound_object_); - } - } - void onInitialize() override - { - AutowarePathBaseDisplay::onInitialize(); - left_bound_object_ = this->scene_manager_->createManualObject(); - right_bound_object_ = this->scene_manager_->createManualObject(); - left_bound_object_->setDynamic(true); - right_bound_object_->setDynamic(true); - this->scene_node_->attachObject(left_bound_object_); - this->scene_node_->attachObject(right_bound_object_); - } - - void reset() override - { - AutowarePathBaseDisplay::reset(); - left_bound_object_->clear(); - right_bound_object_->clear(); - } - -protected: - void visualizeDrivableArea(const typename T::ConstSharedPtr msg_ptr) override - { - left_bound_object_->clear(); - right_bound_object_->clear(); - - if (!validateBoundFloats(msg_ptr)) { - this->setStatus( - rviz_common::properties::StatusProperty::Error, "Topic", - "Message drivable area contained invalid floating point values (nans or infs)"); - return; - } - - if (property_drivable_area_view_.getBool()) { - Ogre::ColourValue color = - rviz_common::properties::qtToOgre(property_drivable_area_color_.getColor()); - color.a = property_drivable_area_alpha_.getFloat(); - const auto line_width = property_drivable_area_width_.getFloat(); - visualizeBound(msg_ptr->left_bound, color, line_width, left_bound_object_); - visualizeBound(msg_ptr->right_bound, color, line_width, right_bound_object_); - } - } - -private: - Ogre::ManualObject * left_bound_object_{nullptr}; - Ogre::ManualObject * right_bound_object_{nullptr}; - - rviz_common::properties::BoolProperty property_drivable_area_view_; - rviz_common::properties::FloatProperty property_drivable_area_alpha_; - rviz_common::properties::ColorProperty property_drivable_area_color_; - rviz_common::properties::FloatProperty property_drivable_area_width_; -}; - -class AutowarePathWithLaneIdDisplay -: public AutowarePathWithDrivableAreaDisplay -{ - Q_OBJECT -public: - AutowarePathWithLaneIdDisplay(); - ~AutowarePathWithLaneIdDisplay(); - -private: - void preProcessMessageDetail() override; - void preVisualizePathFootprintDetail( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) override; - void visualizePathFootprintDetail( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, - const size_t p_idx) override; - - rviz_common::properties::BoolProperty property_lane_id_view_; - rviz_common::properties::FloatProperty property_lane_id_scale_; - - using LaneIdObject = - std::pair, std::unique_ptr>; - std::vector lane_id_obj_ptrs_; -}; - -class AutowarePathDisplay -: public AutowarePathWithDrivableAreaDisplay -{ - Q_OBJECT -private: - void preProcessMessageDetail() override; -}; - -class AutowareTrajectoryDisplay -: public AutowarePathBaseDisplay -{ - Q_OBJECT -private: - void preProcessMessageDetail() override; -}; -} // namespace rviz_plugins - -#endif // PATH__DISPLAY_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/pose_with_uuid_stamped/display.hpp b/common/tier4_planning_rviz_plugin/include/pose_with_uuid_stamped/display.hpp deleted file mode 100644 index f1fe3c30cd457..0000000000000 --- a/common/tier4_planning_rviz_plugin/include/pose_with_uuid_stamped/display.hpp +++ /dev/null @@ -1,84 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef POSE_WITH_UUID_STAMPED__DISPLAY_HPP_ -#define POSE_WITH_UUID_STAMPED__DISPLAY_HPP_ - -#include - -#include - -#include -#include -#include - -namespace rviz_rendering -{ -class Axes; -class MovableText; -} // namespace rviz_rendering -namespace rviz_common::properties -{ -class FloatProperty; -class TfFrameProperty; -} // namespace rviz_common::properties - -namespace rviz_plugins -{ -class AutowarePoseWithUuidStampedDisplay -: public rviz_common::MessageFilterDisplay -{ - Q_OBJECT - -public: - AutowarePoseWithUuidStampedDisplay(); - ~AutowarePoseWithUuidStampedDisplay() override; - AutowarePoseWithUuidStampedDisplay(const AutowarePoseWithUuidStampedDisplay &) = delete; - AutowarePoseWithUuidStampedDisplay(const AutowarePoseWithUuidStampedDisplay &&) = delete; - AutowarePoseWithUuidStampedDisplay & operator=(const AutowarePoseWithUuidStampedDisplay &) = - delete; - AutowarePoseWithUuidStampedDisplay & operator=(const AutowarePoseWithUuidStampedDisplay &&) = - delete; - -protected: - void onInitialize() override; - void onEnable() override; - void onDisable() override; - -private Q_SLOTS: - void updateVisualization(); - -private: - void subscribe() override; - void unsubscribe() override; - void processMessage( - const autoware_planning_msgs::msg::PoseWithUuidStamped::ConstSharedPtr meg_ptr) override; - - std::unique_ptr axes_; - std::unique_ptr uuid_node_; - rviz_rendering::MovableText * uuid_; - - rviz_common::properties::FloatProperty * length_property_; - rviz_common::properties::FloatProperty * radius_property_; - rviz_common::properties::TfFrameProperty * frame_property_; - - rviz_common::properties::BoolProperty * uuid_text_view_property_; - rviz_common::properties::FloatProperty * uuid_text_scale_property_; - - autoware_planning_msgs::msg::PoseWithUuidStamped::ConstSharedPtr last_msg_ptr_; -}; - -} // namespace rviz_plugins - -#endif // POSE_WITH_UUID_STAMPED__DISPLAY_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/mission_checkpoint/mission_checkpoint.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/mission_checkpoint/mission_checkpoint.hpp similarity index 93% rename from common/tier4_planning_rviz_plugin/include/mission_checkpoint/mission_checkpoint.hpp rename to common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/mission_checkpoint/mission_checkpoint.hpp index ea15788d789fe..964c418a3df1f 100644 --- a/common/tier4_planning_rviz_plugin/include/mission_checkpoint/mission_checkpoint.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/mission_checkpoint/mission_checkpoint.hpp @@ -45,8 +45,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef MISSION_CHECKPOINT__MISSION_CHECKPOINT_HPP_ -#define MISSION_CHECKPOINT__MISSION_CHECKPOINT_HPP_ +#ifndef TIER4_PLANNING_RVIZ_PLUGIN__MISSION_CHECKPOINT__MISSION_CHECKPOINT_HPP_ +#define TIER4_PLANNING_RVIZ_PLUGIN__MISSION_CHECKPOINT__MISSION_CHECKPOINT_HPP_ #ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 #include @@ -88,4 +88,4 @@ private Q_SLOTS: } // namespace rviz_plugins -#endif // MISSION_CHECKPOINT__MISSION_CHECKPOINT_HPP_ +#endif // TIER4_PLANNING_RVIZ_PLUGIN__MISSION_CHECKPOINT__MISSION_CHECKPOINT_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp new file mode 100644 index 0000000000000..66b4a31f0993f --- /dev/null +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp @@ -0,0 +1,231 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TIER4_PLANNING_RVIZ_PLUGIN__PATH__DISPLAY_HPP_ +#define TIER4_PLANNING_RVIZ_PLUGIN__PATH__DISPLAY_HPP_ + +#include "tier4_planning_rviz_plugin/path/display_base.hpp" + +#include +#include +#include + +#include +#include +#include + +namespace +{ +bool validateBoundFloats(const std::vector & bound) +{ + for (auto && point : bound) { + if (!rviz_common::validateFloats(point)) { + return false; + } + } + return true; +} + +template +bool validateBoundFloats(const typename T::ConstSharedPtr & msg_ptr) +{ + return validateBoundFloats(msg_ptr->left_bound) && validateBoundFloats(msg_ptr->right_bound); +} + +void visualizeBound( + const std::vector & bound, const Ogre::ColourValue & color, + const float width, Ogre::ManualObject * bound_object) +{ + if (bound.size() < 2) { + return; + } + + bound_object->estimateVertexCount(bound.size() * 2); + bound_object->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_STRIP); + + // calculate normal vector of bound and widths depending on the normal vector + std::vector normal_vector_angles; + std::vector adaptive_widths; + for (size_t i = 0; i < bound.size(); ++i) { + const auto [normal_vector_angle, adaptive_width] = [&]() -> std::pair { + if (i == 0) { + return std::make_pair( + tier4_autoware_utils::calcAzimuthAngle(bound.at(i), bound.at(i + 1)) + M_PI_2, width); + } + if (i == bound.size() - 1) { + return std::make_pair( + tier4_autoware_utils::calcAzimuthAngle(bound.at(i - 1), bound.at(i)) + M_PI_2, width); + } + const auto & prev_p = bound.at(i - 1); + const auto & curr_p = bound.at(i); + const auto & next_p = bound.at(i + 1); + + const float curr_to_prev_angle = tier4_autoware_utils::calcAzimuthAngle(curr_p, prev_p); + const float curr_to_next_angle = tier4_autoware_utils::calcAzimuthAngle(curr_p, next_p); + const float normal_vector_angle = (curr_to_prev_angle + curr_to_next_angle) / 2.0; + + const float diff_angle = + tier4_autoware_utils::normalizeRadian(normal_vector_angle - curr_to_next_angle); + if (diff_angle == 0.0) { + return std::make_pair(normal_vector_angle, width); + } + + return std::make_pair(normal_vector_angle, width / std::sin(diff_angle)); + }(); + + normal_vector_angles.push_back(normal_vector_angle); + adaptive_widths.push_back(adaptive_width); + } + + // calculate triangle + for (size_t i = 0; i < bound.size(); ++i) { + const float normal_vector_angle = normal_vector_angles.at(i); + const float adaptive_width = adaptive_widths.at(i); + + const auto x_offset = static_cast(adaptive_width * 0.5 * std::cos(normal_vector_angle)); + const auto y_offset = static_cast(adaptive_width * 0.5 * std::sin(normal_vector_angle)); + auto target_lp = bound.at(i); + target_lp.x = target_lp.x + x_offset; + target_lp.y = target_lp.y + y_offset; + target_lp.z = target_lp.z; + bound_object->position(target_lp.x, target_lp.y, target_lp.z); + bound_object->colour(color); + auto target_rp = bound.at(i); + target_rp.x = target_rp.x - x_offset; + target_rp.y = target_rp.y - y_offset; + target_rp.z = target_rp.z; + bound_object->position(target_rp.x, target_rp.y, target_rp.z); + bound_object->colour(color); + } + bound_object->end(); +} +} // namespace + +namespace rviz_plugins +{ +template +class AutowarePathWithDrivableAreaDisplay : public AutowarePathBaseDisplay +{ +public: + AutowarePathWithDrivableAreaDisplay() + : property_drivable_area_view_{"View Drivable Area", true, "", this}, + property_drivable_area_alpha_{"Alpha", 0.999, "", &property_drivable_area_view_}, + property_drivable_area_color_{"Color", QColor(0, 148, 205), "", &property_drivable_area_view_}, + property_drivable_area_width_{"Width", 0.3f, "", &property_drivable_area_view_} + { + property_drivable_area_alpha_.setMin(0.0); + property_drivable_area_alpha_.setMax(1.0); + property_drivable_area_width_.setMin(0.001); + } + + ~AutowarePathWithDrivableAreaDisplay() + { + if (this->initialized()) { + this->scene_manager_->destroyManualObject(left_bound_object_); + this->scene_manager_->destroyManualObject(right_bound_object_); + } + } + void onInitialize() override + { + AutowarePathBaseDisplay::onInitialize(); + left_bound_object_ = this->scene_manager_->createManualObject(); + right_bound_object_ = this->scene_manager_->createManualObject(); + left_bound_object_->setDynamic(true); + right_bound_object_->setDynamic(true); + this->scene_node_->attachObject(left_bound_object_); + this->scene_node_->attachObject(right_bound_object_); + } + + void reset() override + { + AutowarePathBaseDisplay::reset(); + left_bound_object_->clear(); + right_bound_object_->clear(); + } + +protected: + void visualizeDrivableArea(const typename T::ConstSharedPtr msg_ptr) override + { + left_bound_object_->clear(); + right_bound_object_->clear(); + + if (!validateBoundFloats(msg_ptr)) { + this->setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", + "Message drivable area contained invalid floating point values (nans or infs)"); + return; + } + + if (property_drivable_area_view_.getBool()) { + Ogre::ColourValue color = + rviz_common::properties::qtToOgre(property_drivable_area_color_.getColor()); + color.a = property_drivable_area_alpha_.getFloat(); + const auto line_width = property_drivable_area_width_.getFloat(); + visualizeBound(msg_ptr->left_bound, color, line_width, left_bound_object_); + visualizeBound(msg_ptr->right_bound, color, line_width, right_bound_object_); + } + } + +private: + Ogre::ManualObject * left_bound_object_{nullptr}; + Ogre::ManualObject * right_bound_object_{nullptr}; + + rviz_common::properties::BoolProperty property_drivable_area_view_; + rviz_common::properties::FloatProperty property_drivable_area_alpha_; + rviz_common::properties::ColorProperty property_drivable_area_color_; + rviz_common::properties::FloatProperty property_drivable_area_width_; +}; + +class AutowarePathWithLaneIdDisplay +: public AutowarePathWithDrivableAreaDisplay +{ + Q_OBJECT +public: + AutowarePathWithLaneIdDisplay(); + ~AutowarePathWithLaneIdDisplay(); + +private: + void preProcessMessageDetail() override; + void preVisualizePathFootprintDetail( + const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) override; + void visualizePathFootprintDetail( + const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, + const size_t p_idx) override; + + rviz_common::properties::BoolProperty property_lane_id_view_; + rviz_common::properties::FloatProperty property_lane_id_scale_; + + using LaneIdObject = + std::pair, std::unique_ptr>; + std::vector lane_id_obj_ptrs_; +}; + +class AutowarePathDisplay +: public AutowarePathWithDrivableAreaDisplay +{ + Q_OBJECT +private: + void preProcessMessageDetail() override; +}; + +class AutowareTrajectoryDisplay +: public AutowarePathBaseDisplay +{ + Q_OBJECT +private: + void preProcessMessageDetail() override; +}; +} // namespace rviz_plugins + +#endif // TIER4_PLANNING_RVIZ_PLUGIN__PATH__DISPLAY_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp similarity index 99% rename from common/tier4_planning_rviz_plugin/include/path/display_base.hpp rename to common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp index 74c2a60df3f32..4a59006e3bb96 100644 --- a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH__DISPLAY_BASE_HPP_ -#define PATH__DISPLAY_BASE_HPP_ +#ifndef TIER4_PLANNING_RVIZ_PLUGIN__PATH__DISPLAY_BASE_HPP_ +#define TIER4_PLANNING_RVIZ_PLUGIN__PATH__DISPLAY_BASE_HPP_ #include #include @@ -40,9 +40,10 @@ #include #define EIGEN_MPL2_ONLY +#include "tier4_planning_rviz_plugin/utils.hpp" + #include #include -#include namespace { @@ -654,4 +655,4 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay }; } // namespace rviz_plugins -#endif // PATH__DISPLAY_BASE_HPP_ +#endif // TIER4_PLANNING_RVIZ_PLUGIN__PATH__DISPLAY_BASE_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/pose_with_uuid_stamped/display.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/pose_with_uuid_stamped/display.hpp new file mode 100644 index 0000000000000..285a6902ccc1a --- /dev/null +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/pose_with_uuid_stamped/display.hpp @@ -0,0 +1,84 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TIER4_PLANNING_RVIZ_PLUGIN__POSE_WITH_UUID_STAMPED__DISPLAY_HPP_ +#define TIER4_PLANNING_RVIZ_PLUGIN__POSE_WITH_UUID_STAMPED__DISPLAY_HPP_ + +#include + +#include + +#include +#include +#include + +namespace rviz_rendering +{ +class Axes; +class MovableText; +} // namespace rviz_rendering +namespace rviz_common::properties +{ +class FloatProperty; +class TfFrameProperty; +} // namespace rviz_common::properties + +namespace rviz_plugins +{ +class AutowarePoseWithUuidStampedDisplay +: public rviz_common::MessageFilterDisplay +{ + Q_OBJECT + +public: + AutowarePoseWithUuidStampedDisplay(); + ~AutowarePoseWithUuidStampedDisplay() override; + AutowarePoseWithUuidStampedDisplay(const AutowarePoseWithUuidStampedDisplay &) = delete; + AutowarePoseWithUuidStampedDisplay(const AutowarePoseWithUuidStampedDisplay &&) = delete; + AutowarePoseWithUuidStampedDisplay & operator=(const AutowarePoseWithUuidStampedDisplay &) = + delete; + AutowarePoseWithUuidStampedDisplay & operator=(const AutowarePoseWithUuidStampedDisplay &&) = + delete; + +protected: + void onInitialize() override; + void onEnable() override; + void onDisable() override; + +private Q_SLOTS: + void updateVisualization(); + +private: + void subscribe() override; + void unsubscribe() override; + void processMessage( + const autoware_planning_msgs::msg::PoseWithUuidStamped::ConstSharedPtr meg_ptr) override; + + std::unique_ptr axes_; + std::unique_ptr uuid_node_; + rviz_rendering::MovableText * uuid_; + + rviz_common::properties::FloatProperty * length_property_; + rviz_common::properties::FloatProperty * radius_property_; + rviz_common::properties::TfFrameProperty * frame_property_; + + rviz_common::properties::BoolProperty * uuid_text_view_property_; + rviz_common::properties::FloatProperty * uuid_text_scale_property_; + + autoware_planning_msgs::msg::PoseWithUuidStamped::ConstSharedPtr last_msg_ptr_; +}; + +} // namespace rviz_plugins + +#endif // TIER4_PLANNING_RVIZ_PLUGIN__POSE_WITH_UUID_STAMPED__DISPLAY_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/utils.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/utils.hpp new file mode 100644 index 0000000000000..94943a25f6a64 --- /dev/null +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/utils.hpp @@ -0,0 +1,64 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TIER4_PLANNING_RVIZ_PLUGIN__UTILS_HPP_ +#define TIER4_PLANNING_RVIZ_PLUGIN__UTILS_HPP_ + +#include +#include + +#include + +namespace rviz_plugins +{ +template +bool isDrivingForward(const T points_with_twist, size_t target_idx) +{ + constexpr double epsilon = 1e-6; + + // 1. check velocity + const double target_velocity = + tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.at(target_idx)); + if (epsilon < target_velocity) { + return true; + } else if (target_velocity < -epsilon) { + return false; + } + + // 2. check points size is enough + if (points_with_twist.size() < 2) { + return true; + } + + // 3. check points direction + const bool is_last_point = target_idx == points_with_twist.size() - 1; + const size_t first_idx = is_last_point ? target_idx - 1 : target_idx; + const size_t second_idx = is_last_point ? target_idx : target_idx + 1; + + const auto first_pose = tier4_autoware_utils::getPose(points_with_twist.at(first_idx)); + const auto second_pose = tier4_autoware_utils::getPose(points_with_twist.at(second_idx)); + const double first_traj_yaw = tf2::getYaw(first_pose.orientation); + const double driving_direction_yaw = + tier4_autoware_utils::calcAzimuthAngle(first_pose.position, second_pose.position); + if ( + std::abs(tier4_autoware_utils::normalizeRadian(first_traj_yaw - driving_direction_yaw)) < + M_PI_2) { + return true; + } + + return false; +} +} // namespace rviz_plugins + +#endif // TIER4_PLANNING_RVIZ_PLUGIN__UTILS_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/utils.hpp b/common/tier4_planning_rviz_plugin/include/utils.hpp deleted file mode 100644 index 76f9da0685908..0000000000000 --- a/common/tier4_planning_rviz_plugin/include/utils.hpp +++ /dev/null @@ -1,64 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef UTILS_HPP_ -#define UTILS_HPP_ - -#include -#include - -#include - -namespace rviz_plugins -{ -template -bool isDrivingForward(const T points_with_twist, size_t target_idx) -{ - constexpr double epsilon = 1e-6; - - // 1. check velocity - const double target_velocity = - tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.at(target_idx)); - if (epsilon < target_velocity) { - return true; - } else if (target_velocity < -epsilon) { - return false; - } - - // 2. check points size is enough - if (points_with_twist.size() < 2) { - return true; - } - - // 3. check points direction - const bool is_last_point = target_idx == points_with_twist.size() - 1; - const size_t first_idx = is_last_point ? target_idx - 1 : target_idx; - const size_t second_idx = is_last_point ? target_idx : target_idx + 1; - - const auto first_pose = tier4_autoware_utils::getPose(points_with_twist.at(first_idx)); - const auto second_pose = tier4_autoware_utils::getPose(points_with_twist.at(second_idx)); - const double first_traj_yaw = tf2::getYaw(first_pose.orientation); - const double driving_direction_yaw = - tier4_autoware_utils::calcAzimuthAngle(first_pose.position, second_pose.position); - if ( - std::abs(tier4_autoware_utils::normalizeRadian(first_traj_yaw - driving_direction_yaw)) < - M_PI_2) { - return true; - } - - return false; -} -} // namespace rviz_plugins - -#endif // UTILS_HPP_ diff --git a/common/tier4_planning_rviz_plugin/src/mission_checkpoint/mission_checkpoint.cpp b/common/tier4_planning_rviz_plugin/src/mission_checkpoint/mission_checkpoint.cpp index 6be2cfc426870..2ba5a309baa5c 100644 --- a/common/tier4_planning_rviz_plugin/src/mission_checkpoint/mission_checkpoint.cpp +++ b/common/tier4_planning_rviz_plugin/src/mission_checkpoint/mission_checkpoint.cpp @@ -45,7 +45,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include +#include "tier4_planning_rviz_plugin/mission_checkpoint/mission_checkpoint.hpp" #ifdef ROS_DISTRO_GALACTIC #include diff --git a/common/tier4_planning_rviz_plugin/src/path/display.cpp b/common/tier4_planning_rviz_plugin/src/path/display.cpp index 5bda4cdafd7e3..916201669e9e3 100644 --- a/common/tier4_planning_rviz_plugin/src/path/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/path/display.cpp @@ -12,7 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "tier4_planning_rviz_plugin/path/display.hpp" + #include namespace rviz_plugins diff --git a/common/tier4_planning_rviz_plugin/src/pose_with_uuid_stamped/display.cpp b/common/tier4_planning_rviz_plugin/src/pose_with_uuid_stamped/display.cpp index 8df04e04b2963..b525608a65625 100644 --- a/common/tier4_planning_rviz_plugin/src/pose_with_uuid_stamped/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/pose_with_uuid_stamped/display.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rviz_common/properties/tf_frame_property.hpp" +#include "tier4_planning_rviz_plugin/pose_with_uuid_stamped/display.hpp" -#include #include +#include #include #include #include diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp index eba3e4eacd275..8f67a90215bd1 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp @@ -171,15 +171,15 @@ public Q_SLOTS: // NOLINT for Qt { auto req = std::make_shared(); - RCLCPP_INFO(raw_node_->get_logger(), "client request"); + RCLCPP_DEBUG(raw_node_->get_logger(), "client request"); if (!client->service_is_ready()) { - RCLCPP_INFO(raw_node_->get_logger(), "client is unavailable"); + RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable"); return; } client->async_send_request(req, [this](typename rclcpp::Client::SharedFuture result) { - RCLCPP_INFO( + RCLCPP_DEBUG( raw_node_->get_logger(), "Status: %d, %s", result.get()->status.code, result.get()->status.message.c_str()); }); 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..6ea142ed66a1b 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); } @@ -256,9 +174,6 @@ void VelocitySteeringFactorsPanel::onSteeringFactors(const SteeringFactorArray:: case SteeringFactor::APPROACHING: label->setText("APPROACHING"); break; - case SteeringFactor::TRYING: - label->setText("TRYING"); - break; case SteeringFactor::TURNING: label->setText("TURNING"); break; 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/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.cpp b/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.cpp index cf5960cac281f..e014307942bab 100644 --- a/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.cpp +++ b/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.cpp @@ -60,7 +60,12 @@ void TargetObjectTypePanel::setParameters() modules_ = { "avoidance", "avoidance_by_lane_change", + "dynamic_avoidance", "lane_change", + "start_planner", + "goal_planner", + "crosswalk", + "surround_obstacle_checker", "obstacle_cruise (inside)", "obstacle_cruise (outside)", "obstacle_stop", @@ -89,15 +94,8 @@ void TargetObjectTypePanel::setParameters() ParamNameEnableObject param_name; param_name.node = "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner"; - param_name.ns = "avoidance.target_object"; - param_name.name.emplace("car", "car.is_target"); - param_name.name.emplace("truck", "truck.is_target"); - param_name.name.emplace("bus", "bus.is_target"); - param_name.name.emplace("trailer", "trailer.is_target"); - param_name.name.emplace("unknown", "unknown.is_target"); - param_name.name.emplace("bicycle", "bicycle.is_target"); - param_name.name.emplace("motorcycle", "motorcycle.is_target"); - param_name.name.emplace("pedestrian", "pedestrian.is_target"); + param_name.ns = "avoidance.target_filtering.target_type"; + param_name.name = default_param_name.name; param_names_.emplace(module, param_name); } @@ -107,15 +105,8 @@ void TargetObjectTypePanel::setParameters() ParamNameEnableObject param_name; param_name.node = "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner"; - param_name.ns = "avoidance_by_lane_change.target_object"; - param_name.name.emplace("car", "car.is_target"); - param_name.name.emplace("truck", "truck.is_target"); - param_name.name.emplace("bus", "bus.is_target"); - param_name.name.emplace("trailer", "trailer.is_target"); - param_name.name.emplace("unknown", "unknown.is_target"); - param_name.name.emplace("bicycle", "bicycle.is_target"); - param_name.name.emplace("motorcycle", "motorcycle.is_target"); - param_name.name.emplace("pedestrian", "pedestrian.is_target"); + param_name.ns = "avoidance_by_lane_change.target_filtering.target_type"; + param_name.name = default_param_name.name; param_names_.emplace(module, param_name); } @@ -130,6 +121,64 @@ void TargetObjectTypePanel::setParameters() param_names_.emplace(module, param_name); } + // start_planner + { + const auto module = "start_planner"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner"; + param_name.ns = "start_planner.path_safety_check.target_filtering.object_types_to_check"; + param_name.name.emplace("car", "check_car"); + param_name.name.emplace("truck", "check_truck"); + param_name.name.emplace("bus", "check_bus"); + param_name.name.emplace("trailer", "check_trailer"); + param_name.name.emplace("unknown", "check_unknown"); + param_name.name.emplace("bicycle", "check_bicycle"); + param_name.name.emplace("motorcycle", "check_motorcycle"); + param_name.name.emplace("pedestrian", "check_pedestrian"); + param_names_.emplace(module, param_name); + } + + // goal_planner + { + const auto module = "goal_planner"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner"; + param_name.ns = "goal_planner.path_safety_check.target_filtering.object_types_to_check"; + param_name.name.emplace("car", "check_car"); + param_name.name.emplace("truck", "check_truck"); + param_name.name.emplace("bus", "check_bus"); + param_name.name.emplace("trailer", "check_trailer"); + param_name.name.emplace("unknown", "check_unknown"); + param_name.name.emplace("bicycle", "check_bicycle"); + param_name.name.emplace("motorcycle", "check_motorcycle"); + param_name.name.emplace("pedestrian", "check_pedestrian"); + param_names_.emplace(module, param_name); + } + + // dynamic_avoidance + { + const auto module = "dynamic_avoidance"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner"; + param_name.ns = "dynamic_avoidance.target_object"; + param_name.name = default_param_name.name; + param_names_.emplace(module, param_name); + } + + // crosswalk + { + const auto module = "crosswalk"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner"; + param_name.ns = "crosswalk.object_filtering.target_object"; + param_name.name = default_param_name.name; + param_names_.emplace(module, param_name); + } + // obstacle cruise (inside) { const auto module = "obstacle_cruise (inside)"; @@ -152,6 +201,24 @@ void TargetObjectTypePanel::setParameters() param_names_.emplace(module, param_name); } + // surround_obstacle_check + { + const auto module = "surround_obstacle_checker"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker"; + param_name.ns = ""; + param_name.name.emplace("car", "car.enable_check"); + param_name.name.emplace("truck", "truck.enable_check"); + param_name.name.emplace("bus", "bus.enable_check"); + param_name.name.emplace("trailer", "trailer.enable_check"); + param_name.name.emplace("unknown", "unknown.enable_check"); + param_name.name.emplace("bicycle", "bicycle.enable_check"); + param_name.name.emplace("motorcycle", "motorcycle.enable_check"); + param_name.name.emplace("pedestrian", "pedestrian.enable_check"); + param_names_.emplace(module, param_name); + } + // obstacle stop { const auto module = "obstacle_stop"; @@ -225,7 +292,8 @@ void TargetObjectTypePanel::updateMatrix() continue; } - std::string param_name = module_params.ns + "." + module_params.name.at(target); + std::string param_name = + (module_params.ns.empty() ? "" : module_params.ns + ".") + module_params.name.at(target); auto parameter_result = parameters_client->get_parameters({param_name}); if (!parameter_result.empty()) { diff --git a/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp b/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp index 081eab28e8833..9c3acd4f45fa4 100644 --- a/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp +++ b/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp @@ -15,10 +15,13 @@ #ifndef TRAFFIC_LIGHT_UTILS__TRAFFIC_LIGHT_UTILS_HPP_ #define TRAFFIC_LIGHT_UTILS__TRAFFIC_LIGHT_UTILS_HPP_ +#include "autoware_perception_msgs/msg/traffic_signal.hpp" +#include "autoware_perception_msgs/msg/traffic_signal_element.hpp" +#include "tier4_perception_msgs/msg/traffic_light.hpp" #include "tier4_perception_msgs/msg/traffic_light_element.hpp" #include "tier4_perception_msgs/msg/traffic_light_roi.hpp" -#include "tier4_perception_msgs/msg/traffic_signal.hpp" +#include #include #include #include @@ -32,9 +35,50 @@ bool isRoiValid( void setRoiInvalid(tier4_perception_msgs::msg::TrafficLightRoi & roi); -bool isSignalUnknown(const tier4_perception_msgs::msg::TrafficSignal & signal); +bool isSignalUnknown(const tier4_perception_msgs::msg::TrafficLight & signal); -void setSignalUnknown(tier4_perception_msgs::msg::TrafficSignal & signal, float confidence = -1); +void setSignalUnknown(tier4_perception_msgs::msg::TrafficLight & signal, float confidence = -1); + +/** + * @brief Checks if a traffic light state includes a circle-shaped light with the specified color. + * + * Iterates through the traffic light elements to find a circle-shaped light that matches the given + * color. + * + * @param tl_state The traffic light state to check. + * @param lamp_color The color to look for in the traffic light's circle-shaped lamps. + * @return True if a circle-shaped light with the specified color is found, false otherwise. + */ +bool hasTrafficLightCircleColor( + const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_color); + +/** + * @brief Checks if a traffic light state includes a light with the specified shape. + * + * Searches through the traffic light elements to find a light that matches the given shape. + * + * @param tl_state The traffic light state to check. + * @param shape The shape to look for in the traffic light's lights. + * @return True if a light with the specified shape is found, false otherwise. + */ +bool hasTrafficLightShape( + const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_shape); + +/** + * @brief Determines if a traffic signal indicates a stop for the given lanelet. + * + * Evaluates the current state of the traffic light, considering if it's green or unknown, + * which would not necessitate a stop. Then, it checks the turn direction attribute of the lanelet + * against the traffic light's arrow shapes to determine whether a vehicle must stop or if it can + * proceed based on allowed turn directions. + * + * @param lanelet The lanelet to check for a stop signal at its traffic light. + * @param tl_state The current state of the traffic light associated with the lanelet. + * @return True if the traffic signal indicates a stop is required, false otherwise. + */ +bool isTrafficSignalStop( + const lanelet::ConstLanelet & lanelet, + const autoware_perception_msgs::msg::TrafficSignal & tl_state); tf2::Vector3 getTrafficLightTopLeft(const lanelet::ConstLineString3d & traffic_light); diff --git a/common/traffic_light_utils/package.xml b/common/traffic_light_utils/package.xml index de05355eafd66..7adb856c3c447 100644 --- a/common/traffic_light_utils/package.xml +++ b/common/traffic_light_utils/package.xml @@ -4,8 +4,9 @@ traffic_light_utils 0.1.0 The traffic_light_utils package - Mingyu Li + Kotaro Uetake Shunsuke Miura + Satoshi Ota Apache License 2.0 ament_cmake_auto @@ -15,6 +16,7 @@ ament_lint_auto autoware_lint_common + autoware_perception_msgs lanelet2_extension tier4_perception_msgs diff --git a/common/traffic_light_utils/src/traffic_light_utils.cpp b/common/traffic_light_utils/src/traffic_light_utils.cpp index 5a68223afb5ff..0bd3d85a9b71f 100644 --- a/common/traffic_light_utils/src/traffic_light_utils.cpp +++ b/common/traffic_light_utils/src/traffic_light_utils.cpp @@ -33,24 +33,81 @@ void setRoiInvalid(tier4_perception_msgs::msg::TrafficLightRoi & roi) roi.roi.height = roi.roi.width = 0; } -bool isSignalUnknown(const tier4_perception_msgs::msg::TrafficSignal & signal) +bool isSignalUnknown(const tier4_perception_msgs::msg::TrafficLight & signal) { return signal.elements.size() == 1 && signal.elements[0].shape == tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN && signal.elements[0].color == tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; } -void setSignalUnknown(tier4_perception_msgs::msg::TrafficSignal & signal, float confidence) +void setSignalUnknown(tier4_perception_msgs::msg::TrafficLight & signal, float confidence) { signal.elements.resize(1); signal.elements[0].shape = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; signal.elements[0].color = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; // the default value is -1, which means to not set confidence - if (confidence >= 0) { + if (confidence > 0) { signal.elements[0].confidence = confidence; } } +bool hasTrafficLightCircleColor( + const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_color) +{ + const auto it_lamp = + std::find_if(tl_state.elements.begin(), tl_state.elements.end(), [&lamp_color](const auto & x) { + return x.shape == autoware_perception_msgs::msg::TrafficSignalElement::CIRCLE && + x.color == lamp_color; + }); + + return it_lamp != tl_state.elements.end(); +} + +bool hasTrafficLightShape( + const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_shape) +{ + const auto it_lamp = std::find_if( + tl_state.elements.begin(), tl_state.elements.end(), + [&lamp_shape](const auto & x) { return x.shape == lamp_shape; }); + + return it_lamp != tl_state.elements.end(); +} + +bool isTrafficSignalStop( + const lanelet::ConstLanelet & lanelet, + const autoware_perception_msgs::msg::TrafficSignal & tl_state) +{ + if (hasTrafficLightCircleColor( + tl_state, autoware_perception_msgs::msg::TrafficSignalElement::GREEN)) { + return false; + } + + const std::string turn_direction = lanelet.attributeOr("turn_direction", "else"); + + if (turn_direction == "else") { + return true; + } + if ( + turn_direction == "right" && + hasTrafficLightShape( + tl_state, autoware_perception_msgs::msg::TrafficSignalElement::RIGHT_ARROW)) { + return false; + } + if ( + turn_direction == "left" && + hasTrafficLightShape( + tl_state, autoware_perception_msgs::msg::TrafficSignalElement::LEFT_ARROW)) { + return false; + } + if ( + turn_direction == "straight" && + hasTrafficLightShape(tl_state, autoware_perception_msgs::msg::TrafficSignalElement::UP_ARROW)) { + return false; + } + + return true; +} + tf2::Vector3 getTrafficLightTopLeft(const lanelet::ConstLineString3d & traffic_light) { const auto & tl_bl = traffic_light.front(); diff --git a/common/traffic_light_utils/test/test_traffic_light_utils.cpp b/common/traffic_light_utils/test/test_traffic_light_utils.cpp index bd777265b4195..a80510693c487 100644 --- a/common/traffic_light_utils/test/test_traffic_light_utils.cpp +++ b/common/traffic_light_utils/test/test_traffic_light_utils.cpp @@ -48,7 +48,7 @@ TEST(setRoiInvalid, set_roi_size) TEST(isSignalUnknown, signal_element) { - tier4_perception_msgs::msg::TrafficSignal test_signal; + tier4_perception_msgs::msg::TrafficLight test_signal; tier4_perception_msgs::msg::TrafficLightElement element; element.color = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; element.shape = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; @@ -60,7 +60,7 @@ TEST(isSignalUnknown, signal_element) TEST(setSignalUnknown, set_signal_element) { - tier4_perception_msgs::msg::TrafficSignal test_signal; + tier4_perception_msgs::msg::TrafficLight test_signal; tier4_perception_msgs::msg::TrafficLightElement element; element.color = tier4_perception_msgs::msg::TrafficLightElement::RED; element.shape = tier4_perception_msgs::msg::TrafficLightElement::CROSS; 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/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp index 27ec775110109..fb1973d777138 100644 --- a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp +++ b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp @@ -41,6 +41,7 @@ #include #include +#include #include #include @@ -140,10 +141,9 @@ class AEB : public rclcpp::Node bool hasCollision( const double current_v, const Path & ego_path, const std::vector & objects); - void generateEgoPath( - const double curr_v, const double curr_w, Path & path, std::vector & polygons); - void generateEgoPath( - const Trajectory & predicted_traj, Path & path, std::vector & polygons); + Path generateEgoPath(const double curr_v, const double curr_w, std::vector & polygons); + std::optional generateEgoPath( + const Trajectory & predicted_traj, std::vector & polygons); void createObjectData( const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, std::vector & objects); diff --git a/control/autonomous_emergency_braking/package.xml b/control/autonomous_emergency_braking/package.xml index d45078064da3c..1ac255c21921b 100644 --- a/control/autonomous_emergency_braking/package.xml +++ b/control/autonomous_emergency_braking/package.xml @@ -6,6 +6,7 @@ Autonomous Emergency Braking package as a ROS 2 node Takamasa Horibe Tomoya Kimura + Mamoru Sobue Apache License 2.0 diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index e88a5ee833612..2b5553971a8d5 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -309,7 +309,6 @@ bool AEB::checkCollision(MarkerArray & debug_markers) // step3. create ego path based on sensor data bool has_collision_ego = false; if (use_imu_path_) { - Path ego_path; std::vector ego_polys; const double current_w = angular_velocity_ptr_->z; constexpr double color_r = 0.0 / 256.0; @@ -317,8 +316,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers) constexpr double color_b = 205.0 / 256.0; constexpr double color_a = 0.999; const auto current_time = get_clock()->now(); - generateEgoPath(current_v, current_w, ego_path, ego_polys); - + const auto ego_path = generateEgoPath(current_v, current_w, ego_polys); std::vector objects; createObjectData(ego_path, ego_polys, current_time, objects); has_collision_ego = hasCollision(current_v, ego_path, objects); @@ -332,7 +330,6 @@ bool AEB::checkCollision(MarkerArray & debug_markers) // step4. transform predicted trajectory from control module bool has_collision_predicted = false; if (use_predicted_trajectory_) { - Path predicted_path; std::vector predicted_polys; const auto predicted_traj_ptr = predicted_traj_ptr_; constexpr double color_r = 0.0; @@ -340,15 +337,18 @@ bool AEB::checkCollision(MarkerArray & debug_markers) constexpr double color_b = 0.0; constexpr double color_a = 0.999; const auto current_time = predicted_traj_ptr->header.stamp; - generateEgoPath(*predicted_traj_ptr, predicted_path, predicted_polys); - std::vector objects; - createObjectData(predicted_path, predicted_polys, current_time, objects); - has_collision_predicted = hasCollision(current_v, predicted_path, objects); - - std::string ns = "predicted"; - addMarker( - current_time, predicted_path, predicted_polys, objects, color_r, color_g, color_b, color_a, - ns, debug_markers); + const auto predicted_path_opt = generateEgoPath(*predicted_traj_ptr, predicted_polys); + if (predicted_path_opt) { + const auto & predicted_path = predicted_path_opt.value(); + std::vector objects; + createObjectData(predicted_path, predicted_polys, current_time, objects); + has_collision_predicted = hasCollision(current_v, predicted_path, objects); + + std::string ns = "predicted"; + addMarker( + current_time, predicted_path, predicted_polys, objects, color_r, color_g, color_b, color_a, + ns, debug_markers); + } } return has_collision_ego || has_collision_predicted; @@ -358,31 +358,36 @@ bool AEB::hasCollision( const double current_v, const Path & ego_path, const std::vector & objects) { // calculate RSS - const auto current_p = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); + const auto current_p = tier4_autoware_utils::createPoint( + ego_path[0].position.x, ego_path[0].position.y, ego_path[0].position.z); const double & t = t_response_; for (const auto & obj : objects) { const double & obj_v = obj.velocity; const double rss_dist = current_v * t + (current_v * current_v) / (2 * std::fabs(a_ego_min_)) - obj_v * obj_v / (2 * std::fabs(a_obj_min_)) + longitudinal_offset_; - const double dist_ego_to_object = - motion_utils::calcSignedArcLength(ego_path, current_p, obj.position) - - vehicle_info_.max_longitudinal_offset_m; - if (dist_ego_to_object < rss_dist) { - // collision happens - ObjectData collision_data = obj; - collision_data.rss = rss_dist; - collision_data.distance_to_object = dist_ego_to_object; - collision_data_keeper_.update(collision_data); - return true; + + // check the object is front the ego or not + if ((obj.position.x - ego_path[0].position.x) > 0) { + const double dist_ego_to_object = + motion_utils::calcSignedArcLength(ego_path, current_p, obj.position) - + vehicle_info_.max_longitudinal_offset_m; + if (dist_ego_to_object < rss_dist) { + // collision happens + ObjectData collision_data = obj; + collision_data.rss = rss_dist; + collision_data.distance_to_object = dist_ego_to_object; + collision_data_keeper_.update(collision_data); + return true; + } } } - return false; } -void AEB::generateEgoPath( - const double curr_v, const double curr_w, Path & path, std::vector & polygons) +Path AEB::generateEgoPath( + const double curr_v, const double curr_w, std::vector & polygons) { + Path path; double curr_x = 0.0; double curr_y = 0.0; double curr_yaw = 0.0; @@ -393,7 +398,7 @@ void AEB::generateEgoPath( if (curr_v < 0.1) { // if current velocity is too small, assume it stops at the same point - return; + return path; } constexpr double epsilon = 1e-6; @@ -431,14 +436,14 @@ void AEB::generateEgoPath( for (size_t i = 0; i < path.size() - 1; ++i) { polygons.at(i) = createPolygon(path.at(i), path.at(i + 1), vehicle_info_, expand_width_); } + return path; } -void AEB::generateEgoPath( - const Trajectory & predicted_traj, Path & path, - std::vector & polygons) +std::optional AEB::generateEgoPath( + const Trajectory & predicted_traj, std::vector & polygons) { if (predicted_traj.points.empty()) { - return; + return std::nullopt; } geometry_msgs::msg::TransformStamped transform_stamped{}; @@ -448,10 +453,11 @@ void AEB::generateEgoPath( rclcpp::Duration::from_seconds(0.5)); } catch (tf2::TransformException & ex) { RCLCPP_ERROR_STREAM(get_logger(), "[AEB] Failed to look up transform from base_link to map"); - return; + return std::nullopt; } // create path + Path path; path.resize(predicted_traj.points.size()); for (size_t i = 0; i < predicted_traj.points.size(); ++i) { geometry_msgs::msg::Pose map_pose; @@ -467,6 +473,7 @@ void AEB::generateEgoPath( for (size_t i = 0; i < path.size() - 1; ++i) { polygons.at(i) = createPolygon(path.at(i), path.at(i + 1), vehicle_info_, expand_width_); } + return path; } void AEB::createObjectData( 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/control_validator/config/control_validator.param.yaml b/control/control_validator/config/control_validator.param.yaml index 7bbe6a466799b..12709b18b7932 100644 --- a/control/control_validator/config/control_validator.param.yaml +++ b/control/control_validator/config/control_validator.param.yaml @@ -1,8 +1,6 @@ /**: ros__parameters: - publish_diag: true # if true, diagnostic msg is published - # If the number of consecutive invalid trajectory exceeds this threshold, the Diag will be set to ERROR. # (For example, threshold = 1 means, even if the trajectory is invalid, Diag will not be ERROR if # the next trajectory is valid.) diff --git a/control/control_validator/include/control_validator/control_validator.hpp b/control/control_validator/include/control_validator/control_validator.hpp index 48b7eba7412a2..eba9bf700ba08 100644 --- a/control/control_validator/include/control_validator/control_validator.hpp +++ b/control/control_validator/include/control_validator/control_validator.hpp @@ -75,7 +75,6 @@ class ControlValidator : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_markers_; // system parameters - bool publish_diag_ = true; int diag_error_count_threshold_ = 0; bool display_on_terminal_ = true; diff --git a/control/control_validator/include/control_validator/utils.hpp b/control/control_validator/include/control_validator/utils.hpp index 39ec316dbe437..8033ac9442960 100644 --- a/control/control_validator/include/control_validator/utils.hpp +++ b/control/control_validator/include/control_validator/utils.hpp @@ -15,7 +15,7 @@ #ifndef CONTROL_VALIDATOR__UTILS_HPP_ #define CONTROL_VALIDATOR__UTILS_HPP_ -#include +#include #include #include diff --git a/control/control_validator/package.xml b/control/control_validator/package.xml index c46c2d237b605..faf254708ddbb 100644 --- a/control/control_validator/package.xml +++ b/control/control_validator/package.xml @@ -7,6 +7,9 @@ Kyoichi Sugahara Takamasa Horibe Makoto Kurihara + Mamoru Sobue + Takayuki Murooka + Apache License 2.0 Kyoichi Sugahara diff --git a/control/control_validator/src/control_validator.cpp b/control/control_validator/src/control_validator.cpp index ffce38f009a41..5282e31fef898 100644 --- a/control/control_validator/src/control_validator.cpp +++ b/control/control_validator/src/control_validator.cpp @@ -46,14 +46,11 @@ ControlValidator::ControlValidator(const rclcpp::NodeOptions & options) setupParameters(); - if (publish_diag_) { - setupDiag(); - } + setupDiag(); } void ControlValidator::setupParameters() { - publish_diag_ = declare_parameter("publish_diag"); diag_error_count_threshold_ = declare_parameter("diag_error_count_threshold"); display_on_terminal_ = declare_parameter("display_on_terminal"); @@ -121,6 +118,11 @@ bool ControlValidator::isDataReady() void ControlValidator::onReferenceTrajectory(const Trajectory::ConstSharedPtr msg) { + if (msg->points.size() < 2) { + RCLCPP_ERROR(get_logger(), "planning trajectory size is invalid (%lu)", msg->points.size()); + return; + } + current_reference_trajectory_ = msg; return; diff --git a/control/control_validator/src/utils.cpp b/control/control_validator/src/utils.cpp index fa8261e9c3f95..fab8b9f6f888b 100644 --- a/control/control_validator/src/utils.cpp +++ b/control/control_validator/src/utils.cpp @@ -104,10 +104,10 @@ Trajectory alignTrajectoryWithReferenceTrajectory( // If first point of predicted_trajectory is in front of start of trajectory, erase points which // are in front of trajectory start point and insert pNew along the predicted_trajectory - // predicted_trajectory: 　　　　p1-----p2-----p3----//------pN + // predicted_trajectory: p1-----p2-----p3----//------pN // trajectory: t1--------//------tN // ↓ - // predicted_trajectory: 　　　　 pNew--p3----//------pN + // predicted_trajectory: pNew--p3----//------pN // trajectory: t1--------//------tN auto predicted_trajectory_point_removed = removeFrontTrajectoryPoint( trajectory_points, modified_trajectory_points, predicted_trajectory_points); @@ -119,7 +119,7 @@ Trajectory alignTrajectoryWithReferenceTrajectory( // If last point of predicted_trajectory is behind of end of trajectory, erase points which are // behind trajectory last point and insert pNew along the predicted_trajectory - // predicted_trajectory: 　　　　p1-----//------pN-2-----pN-1-----pN + // predicted_trajectory: p1-----//------pN-2-----pN-1-----pN // trajectory: t1-----//-----tN-1--tN // ↓ // predicted_trajectory: p1-----//------pN-2-pNew diff --git a/control/joy_controller/README.md b/control/joy_controller/README.md index 4518de37b7034..4674ebdadb51d 100644 --- a/control/joy_controller/README.md +++ b/control/joy_controller/README.md @@ -4,6 +4,21 @@ `joy_controller` is the package to convert a joy msg to autoware commands (e.g. steering wheel, shift, turn signal, engage) for a vehicle. +## Usage + +### ROS 2 launch + +```bash +# With default config (ds4) +ros2 launch joy_controller joy_controller.launch.xml + +# Default config but select from the existing parameter files +ros2 launch joy_controller joy_controller_param_selection.launch.xml joy_type:=ds4 # or g29, p65, xbox + +# Override the param file +ros2 launch joy_controller joy_controller.launch.xml config_file:=/path/to/your/param.yaml +``` + ## Input / Output ### Input topics @@ -37,6 +52,7 @@ | `steering_angle_velocity` | double | steering angle velocity for operation | | `accel_sensitivity` | double | sensitivity to calculate acceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) | | `brake_sensitivity` | double | sensitivity to calculate deceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) | +| `raw_control` | bool | skip input odometry if true | | `velocity_gain` | double | ratio to calculate velocity by acceleration | | `max_forward_velocity` | double | absolute max velocity to go forward | | `max_backward_velocity` | double | absolute max velocity to go backward | @@ -85,3 +101,25 @@ | Autoware Disengage | ○ | | Vehicle Engage | △ | | Vehicle Disengage | △ | + +## XBOX Joystick Key Map + +| Action | Button | +| -------------------- | --------------------- | +| Acceleration | RT | +| Brake | LT | +| Steering | Left Stick Left Right | +| Shift up | Cursor Up | +| Shift down | Cursor Down | +| Shift Drive | Cursor Left | +| Shift Reverse | Cursor Right | +| Turn Signal Left | LB | +| Turn Signal Right | RB | +| Clear Turn Signal | A | +| Gate Mode | B | +| Emergency Stop | View | +| Clear Emergency Stop | Menu | +| Autoware Engage | X | +| Autoware Disengage | Y | +| Vehicle Engage | Left Stick Button | +| Vehicle Disengage | Right Stick Button | diff --git a/control/joy_controller/config/joy_controller.param.yaml b/control/joy_controller/config/joy_controller.param.yaml deleted file mode 100644 index 8d48948a2d133..0000000000000 --- a/control/joy_controller/config/joy_controller.param.yaml +++ /dev/null @@ -1,15 +0,0 @@ -/**: - ros__parameters: - joy_type: DS4 - update_rate: 10.0 - accel_ratio: 3.0 - brake_ratio: 5.0 - steer_ratio: 0.5 - steering_angle_velocity: 0.1 - accel_sensitivity: 1.0 - brake_sensitivity: 1.0 - control_command: - velocity_gain: 3.0 - max_forward_velocity: 20.0 - max_backward_velocity: 3.0 - backward_accel_ratio: 1.0 diff --git a/control/joy_controller/config/joy_controller_ds4.param.yaml b/control/joy_controller/config/joy_controller_ds4.param.yaml new file mode 100644 index 0000000000000..73a5d028985c5 --- /dev/null +++ b/control/joy_controller/config/joy_controller_ds4.param.yaml @@ -0,0 +1,16 @@ +/**: + ros__parameters: + joy_type: DS4 + update_rate: 10.0 + accel_ratio: 3.0 + brake_ratio: 5.0 + steer_ratio: 0.5 + steering_angle_velocity: 0.1 + accel_sensitivity: 1.0 + brake_sensitivity: 1.0 + control_command: + raw_control: false + velocity_gain: 3.0 + max_forward_velocity: 20.0 + max_backward_velocity: 3.0 + backward_accel_ratio: 1.0 diff --git a/control/joy_controller/config/joy_controller_g29.param.yaml b/control/joy_controller/config/joy_controller_g29.param.yaml new file mode 100644 index 0000000000000..c28a6c01d1326 --- /dev/null +++ b/control/joy_controller/config/joy_controller_g29.param.yaml @@ -0,0 +1,16 @@ +/**: + ros__parameters: + joy_type: G29 + update_rate: 10.0 + accel_ratio: 3.0 + brake_ratio: 5.0 + steer_ratio: 0.5 + steering_angle_velocity: 0.1 + accel_sensitivity: 1.0 + brake_sensitivity: 1.0 + control_command: + raw_control: false + velocity_gain: 3.0 + max_forward_velocity: 20.0 + max_backward_velocity: 3.0 + backward_accel_ratio: 1.0 diff --git a/control/joy_controller/config/joy_controller_p65.param.yaml b/control/joy_controller/config/joy_controller_p65.param.yaml new file mode 100644 index 0000000000000..4b13348f95fb6 --- /dev/null +++ b/control/joy_controller/config/joy_controller_p65.param.yaml @@ -0,0 +1,16 @@ +/**: + ros__parameters: + joy_type: P65 + update_rate: 10.0 + accel_ratio: 3.0 + brake_ratio: 5.0 + steer_ratio: 0.5 + steering_angle_velocity: 0.1 + accel_sensitivity: 1.0 + brake_sensitivity: 1.0 + control_command: + raw_control: false + velocity_gain: 3.0 + max_forward_velocity: 20.0 + max_backward_velocity: 3.0 + backward_accel_ratio: 1.0 diff --git a/control/joy_controller/config/joy_controller_xbox.param.yaml b/control/joy_controller/config/joy_controller_xbox.param.yaml new file mode 100644 index 0000000000000..7b45c2c164454 --- /dev/null +++ b/control/joy_controller/config/joy_controller_xbox.param.yaml @@ -0,0 +1,16 @@ +/**: + ros__parameters: + joy_type: XBOX + update_rate: 10.0 + accel_ratio: 3.0 + brake_ratio: 5.0 + steer_ratio: 0.5 + steering_angle_velocity: 0.1 + accel_sensitivity: 1.0 + brake_sensitivity: 1.0 + control_command: + raw_control: false + velocity_gain: 3.0 + max_forward_velocity: 20.0 + max_backward_velocity: 3.0 + backward_accel_ratio: 1.0 diff --git a/control/joy_controller/include/joy_controller/joy_controller.hpp b/control/joy_controller/include/joy_controller/joy_controller.hpp index c6de42afc38d9..22064f9cefaad 100644 --- a/control/joy_controller/include/joy_controller/joy_controller.hpp +++ b/control/joy_controller/include/joy_controller/joy_controller.hpp @@ -59,6 +59,7 @@ class AutowareJoyControllerNode : public rclcpp::Node double brake_sensitivity_; // ControlCommand Parameter + bool raw_control_; double velocity_gain_; double max_forward_velocity_; double max_backward_velocity_; diff --git a/control/joy_controller/include/joy_controller/joy_converter/xbox_joy_converter.hpp b/control/joy_controller/include/joy_controller/joy_converter/xbox_joy_converter.hpp new file mode 100644 index 0000000000000..a009ee1e12975 --- /dev/null +++ b/control/joy_controller/include/joy_controller/joy_converter/xbox_joy_converter.hpp @@ -0,0 +1,81 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ +#define JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ + +#include "joy_controller/joy_converter/joy_converter_base.hpp" + +#include + +namespace joy_controller +{ +class XBOXJoyConverter : public JoyConverterBase +{ +public: + explicit XBOXJoyConverter(const sensor_msgs::msg::Joy & j) : j_(j) {} + + float accel() const { return std::max(0.0f, -((RT() - 1.0f) / 2.0f)); } + + float brake() const { return std::max(0.0f, -((LT() - 1.0f) / 2.0f)); } + + float steer() const { return LStickLeftRight(); } + + bool shift_up() const { return CursorUpDown() == 1.0f; } + bool shift_down() const { return CursorUpDown() == -1.0f; } + bool shift_drive() const { return CursorLeftRight() == 1.0f; } + bool shift_reverse() const { return CursorLeftRight() == -1.0f; } + + bool turn_signal_left() const { return LB(); } + bool turn_signal_right() const { return RB(); } + bool clear_turn_signal() const { return A(); } + + bool gate_mode() const { return B(); } + + bool emergency_stop() const { return ChangeView(); } + bool clear_emergency_stop() const { return Menu(); } + + bool autoware_engage() const { return X(); } + bool autoware_disengage() const { return Y(); } + + bool vehicle_engage() const { return LStickButton(); } + bool vehicle_disengage() const { return RStickButton(); } + +private: + float LStickLeftRight() const { return j_.axes.at(0); } + float LStickUpDown() const { return j_.axes.at(1); } + float RStickLeftRight() const { return j_.axes.at(2); } + float RStickUpDown() const { return j_.axes.at(3); } + float RT() const { return j_.axes.at(4); } + float LT() const { return j_.axes.at(5); } + float CursorLeftRight() const { return j_.axes.at(6); } + float CursorUpDown() const { return j_.axes.at(7); } + + bool A() const { return j_.buttons.at(0); } + bool B() const { return j_.buttons.at(1); } + bool X() const { return j_.buttons.at(3); } + bool Y() const { return j_.buttons.at(4); } + bool LB() const { return j_.buttons.at(6); } + bool RB() const { return j_.buttons.at(7); } + bool Menu() const { return j_.buttons.at(11); } + bool LStickButton() const { return j_.buttons.at(13); } + bool RStickButton() const { return j_.buttons.at(14); } + bool ChangeView() const { return j_.buttons.at(15); } + bool Xbox() const { return j_.buttons.at(16); } + + const sensor_msgs::msg::Joy j_; +}; +} // namespace joy_controller + +#endif // JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ diff --git a/control/joy_controller/launch/joy_controller.launch.xml b/control/joy_controller/launch/joy_controller.launch.xml index f719d8bd78cb5..5236077680d0d 100644 --- a/control/joy_controller/launch/joy_controller.launch.xml +++ b/control/joy_controller/launch/joy_controller.launch.xml @@ -1,5 +1,4 @@ - @@ -13,7 +12,7 @@ - + @@ -30,8 +29,7 @@ - - + diff --git a/control/joy_controller/launch/joy_controller_param_selection.launch.xml b/control/joy_controller/launch/joy_controller_param_selection.launch.xml new file mode 100644 index 0000000000000..d8e3d0bfe8b25 --- /dev/null +++ b/control/joy_controller/launch/joy_controller_param_selection.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/control/joy_controller/schema/joy_controller.schema.json b/control/joy_controller/schema/joy_controller.schema.json index c67c575a6bd41..d4c6351324935 100644 --- a/control/joy_controller/schema/joy_controller.schema.json +++ b/control/joy_controller/schema/joy_controller.schema.json @@ -10,7 +10,7 @@ "type": "string", "description": "Joy controller type", "default": "DS4", - "enum": ["P65", "DS4", "G29"] + "enum": ["P65", "DS4", "G29", "XBOX"] }, "update_rate": { "type": "number", @@ -55,6 +55,11 @@ "control_command": { "type": "object", "properties": { + "raw_control": { + "type": "boolean", + "description": "Whether to skip input odometry", + "default": false + }, "velocity_gain": { "type": "number", "description": "Ratio to calculate velocity by acceleration", @@ -79,6 +84,7 @@ } }, "required": [ + "raw_control", "velocity_gain", "max_forward_velocity", "max_backward_velocity", diff --git a/control/joy_controller/src/joy_controller/joy_controller_node.cpp b/control/joy_controller/src/joy_controller/joy_controller_node.cpp index 897986a7a41cf..5eec438032410 100644 --- a/control/joy_controller/src/joy_controller/joy_controller_node.cpp +++ b/control/joy_controller/src/joy_controller/joy_controller_node.cpp @@ -16,6 +16,7 @@ #include "joy_controller/joy_converter/ds4_joy_converter.hpp" #include "joy_controller/joy_converter/g29_joy_converter.hpp" #include "joy_controller/joy_converter/p65_joy_converter.hpp" +#include "joy_controller/joy_converter/xbox_joy_converter.hpp" #include @@ -154,6 +155,8 @@ void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPt joy_ = std::make_shared(*msg); } else if (joy_type_ == "DS4") { joy_ = std::make_shared(*msg); + } else if (joy_type_ == "XBOX") { + joy_ = std::make_shared(*msg); } else { joy_ = std::make_shared(*msg); } @@ -217,7 +220,7 @@ bool AutowareJoyControllerNode::isDataReady() } // Twist - { + if (!raw_control_) { if (!twist_) { RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), std::chrono::milliseconds(5000).count(), @@ -458,6 +461,7 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & steering_angle_velocity_ = declare_parameter("steering_angle_velocity"); accel_sensitivity_ = declare_parameter("accel_sensitivity"); brake_sensitivity_ = declare_parameter("brake_sensitivity"); + raw_control_ = declare_parameter("control_command.raw_control"); velocity_gain_ = declare_parameter("control_command.velocity_gain"); max_forward_velocity_ = declare_parameter("control_command.max_forward_velocity"); max_backward_velocity_ = declare_parameter("control_command.max_backward_velocity"); @@ -477,10 +481,14 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & sub_joy_ = this->create_subscription( "input/joy", 1, std::bind(&AutowareJoyControllerNode::onJoy, this, std::placeholders::_1), subscriber_option); - sub_odom_ = this->create_subscription( - "input/odometry", 1, - std::bind(&AutowareJoyControllerNode::onOdometry, this, std::placeholders::_1), - subscriber_option); + if (!raw_control_) { + sub_odom_ = this->create_subscription( + "input/odometry", 1, + std::bind(&AutowareJoyControllerNode::onOdometry, this, std::placeholders::_1), + subscriber_option); + } else { + twist_ = std::make_shared(); + } // Publisher pub_control_command_ = diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index dc55576640133..be2411cd3268b 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -29,14 +29,20 @@ #include #include +#include +#include #include -#include #include +#include +#include +#include +#include #include #include #include +#include #include namespace lane_departure_checker @@ -112,6 +118,19 @@ class LaneDepartureChecker bool checkPathWillLeaveLane( const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const; + std::vector> getLaneletsFromPath( + const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const; + + std::optional getFusedLaneletPolygonForPath( + const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const; + + bool checkPathWillLeaveLane( + const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const; + + PathWithLaneId cropPointsOutsideOfLanes( + const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path, + const size_t end_index); + static bool isOutOfLane( const lanelet::ConstLanelets & candidate_lanelets, const LinearRing2d & vehicle_footprint); diff --git a/control/lane_departure_checker/schema/lane_departure_checker.json b/control/lane_departure_checker/schema/lane_departure_checker.json new file mode 100644 index 0000000000000..c7f39fbf7ef8a --- /dev/null +++ b/control/lane_departure_checker/schema/lane_departure_checker.json @@ -0,0 +1,79 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for lane_departure_checker", + "type": "object", + "definitions": { + "lane_departure_checker": { + "type": "object", + "properties": { + "footprint_margin_scale": { + "type": "number", + "default": 1.0, + "description": "Coefficient for expanding footprint margin. Multiplied by 1 standard deviation." + }, + "resample_interval": { + "type": "number", + "default": 0.3, + "description": "Minimum Euclidean distance between points when resample trajectory.[m]." + }, + "max_deceleration": { + "type": "number", + "default": 2.8, + "description": "Maximum deceleration when calculating braking distance." + }, + "delay_time": { + "type": "number", + "default": 1.3, + "description": "Delay time which took to actuate brake when calculating braking distance. [second]." + }, + "max_lateral_deviation": { + "type": "number", + "default": 2.0, + "description": "Maximum lateral deviation in vehicle coordinate. [m]." + }, + "max_longitudinal_deviation": { + "type": "number", + "default": 2.0, + "description": "Maximum longitudinal deviation in vehicle coordinate. [m]." + }, + "max_yaw_deviation_deg": { + "type": "number", + "default": 60.0, + "description": "Maximum ego yaw deviation from trajectory. [deg]." + }, + "ego_nearest_dist_threshold": { + "type": "number" + }, + "ego_nearest_yaw_threshold": { + "type": "number" + }, + "min_braking_distance": { + "type": "number" + } + }, + "required": [ + "footprint_margin_scale", + "resample_interval", + "max_deceleration", + "max_lateral_deviation", + "max_longitudinal_deviation", + "max_yaw_deviation_deg", + "ego_nearest_dist_threshold", + "ego_nearest_yaw_threshold", + "min_braking_distance" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/lane_departure_checker" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index 5bd0fcace9909..811e1652fcb4a 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -34,6 +34,7 @@ using motion_utils::calcArcLength; using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::MultiPoint2d; +using tier4_autoware_utils::MultiPolygon2d; using tier4_autoware_utils::Point2d; namespace @@ -92,6 +93,7 @@ lanelet::ConstLanelets getCandidateLanelets( return candidate_lanelets; } + } // namespace namespace lane_departure_checker @@ -298,6 +300,92 @@ bool LaneDepartureChecker::willLeaveLane( return false; } +std::vector> LaneDepartureChecker::getLaneletsFromPath( + const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const +{ + // Get Footprint Hull basic polygon + std::vector vehicle_footprints = createVehicleFootprints(path); + LinearRing2d footprint_hull = createHullFromFootprints(vehicle_footprints); + auto to_basic_polygon = [](const LinearRing2d & footprint_hull) -> lanelet::BasicPolygon2d { + lanelet::BasicPolygon2d basic_polygon; + for (const auto & point : footprint_hull) { + Eigen::Vector2d p(point.x(), point.y()); + basic_polygon.push_back(p); + } + return basic_polygon; + }; + lanelet::BasicPolygon2d footprint_hull_basic_polygon = to_basic_polygon(footprint_hull); + + // Find all lanelets that intersect the footprint hull + return lanelet::geometry::findWithin2d( + lanelet_map_ptr->laneletLayer, footprint_hull_basic_polygon, 0.0); +} + +std::optional LaneDepartureChecker::getFusedLaneletPolygonForPath( + const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const +{ + const auto lanelets_distance_pair = getLaneletsFromPath(lanelet_map_ptr, path); + // Fuse lanelets into a single BasicPolygon2d + auto fused_lanelets = [&lanelets_distance_pair]() -> std::optional { + if (lanelets_distance_pair.empty()) return std::nullopt; + if (lanelets_distance_pair.size() == 1) + return lanelets_distance_pair.at(0).second.polygon2d().basicPolygon(); + + lanelet::BasicPolygon2d merged_polygon = + lanelets_distance_pair.at(0).second.polygon2d().basicPolygon(); + for (size_t i = 1; i < lanelets_distance_pair.size(); ++i) { + const auto & route_lanelet = lanelets_distance_pair.at(i).second; + const auto & poly = route_lanelet.polygon2d().basicPolygon(); + + std::vector lanelet_union_temp; + boost::geometry::union_(poly, merged_polygon, lanelet_union_temp); + + // Update merged_polygon by accumulating all merged results + merged_polygon.clear(); + for (const auto & temp_poly : lanelet_union_temp) { + merged_polygon.insert(merged_polygon.end(), temp_poly.begin(), temp_poly.end()); + } + } + if (merged_polygon.empty()) return std::nullopt; + return merged_polygon; + }(); + + return fused_lanelets; +} + +bool LaneDepartureChecker::checkPathWillLeaveLane( + const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const +{ + // check if the footprint is not fully contained within the fused lanelets polygon + const std::vector vehicle_footprints = createVehicleFootprints(path); + const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path); + if (!fused_lanelets_polygon) return true; + return !std::all_of( + vehicle_footprints.begin(), vehicle_footprints.end(), + [&fused_lanelets_polygon](const auto & footprint) { + return boost::geometry::within(footprint, fused_lanelets_polygon.value()); + }); +} + +PathWithLaneId LaneDepartureChecker::cropPointsOutsideOfLanes( + const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path, const size_t end_index) +{ + PathWithLaneId temp_path; + const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path); + if (path.points.empty() || !fused_lanelets_polygon) return temp_path; + const auto vehicle_footprints = createVehicleFootprints(path); + size_t idx = 0; + std::for_each(vehicle_footprints.begin(), vehicle_footprints.end(), [&](const auto & footprint) { + if (idx > end_index || boost::geometry::within(footprint, fused_lanelets_polygon.value())) { + temp_path.points.push_back(path.points.at(idx)); + } + ++idx; + }); + PathWithLaneId cropped_path = path; + cropped_path.points = temp_path.points; + return cropped_path; +} + bool LaneDepartureChecker::isOutOfLane( const lanelet::ConstLanelets & candidate_lanelets, const LinearRing2d & vehicle_footprint) { @@ -364,4 +452,5 @@ bool LaneDepartureChecker::willCrossBoundary( } return false; } + } // namespace lane_departure_checker 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/include/mpc_lateral_controller/mpc.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp index 186481e92cdc4..2e83c5ab847c4 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -221,6 +221,7 @@ class MPC double m_min_prediction_length = 5.0; // Minimum prediction distance. + rclcpp::Publisher::SharedPtr m_debug_frenet_predicted_trajectory_pub; /** * @brief Get variables for MPC calculation. * @param trajectory The reference trajectory. @@ -333,6 +334,19 @@ class MPC const MPCMatrix & m, const MatrixXd & x0, const MatrixXd & Uex, const double u_filtered, const float current_steer, const double predict_dt) const; + /** + * @brief calculate predicted trajectory + * @param mpc_matrix The MPC matrix used in the mpc problem. + * @param x0 initial state used in the mpc problem. + * @param Uex optimized input. + * @param mpc_resampled_ref_traj reference trajectory resampled in the mpc time-step + * @param dt delta time used in the mpc problem. + * @return predicted path + */ + Trajectory calculatePredictedTrajectory( + const MPCMatrix & mpc_matrix, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & mpc_resampled_ref_traj, const double dt) const; + /** * @brief Check if the MPC matrix has any invalid values. * @param m The MPC matrix to check. @@ -352,18 +366,6 @@ class MPC : m_param.nominal_weight; } - /** - * @brief Calculate the predicted trajectory for the ego vehicle based on the MPC result. - * @param mpc_resampled_reference_trajectory The resampled reference trajectory. - * @param mpc_matrix The MPC matrix used for optimization. - * @param x0_delayed The delayed initial state vector. - * @param Uex The optimized input vector. - * @return The predicted trajectory. - */ - Trajectory calcPredictedTrajectory( - const MPCTrajectory & mpc_resampled_reference_trajectory, const MPCMatrix & mpc_matrix, - const VectorXd & x0_delayed, const VectorXd & Uex) const; - /** * @brief Generate diagnostic data for debugging purposes. * @param reference_trajectory The reference trajectory. @@ -424,8 +426,10 @@ class MPC double ego_nearest_dist_threshold = 3.0; // Threshold for nearest index search based on distance. double ego_nearest_yaw_threshold = M_PI_2; // Threshold for nearest index search based on yaw. + bool m_debug_publish_predicted_trajectory = false; // Flag to publish debug predicted trajectory + //!< Constructor. - MPC() = default; + explicit MPC(rclcpp::Node & node); /** * @brief Calculate control command using the MPC algorithm. diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp index 441101abfc243..92b01247105c6 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp @@ -91,7 +91,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase // trajectory buffer for detecting new trajectory std::deque m_trajectory_buffer; - MPC m_mpc; // MPC object for trajectory following. + std::unique_ptr m_mpc; // MPC object for trajectory following. // Check is mpc output converged bool m_is_mpc_history_filled{false}; diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp index f77ef72608edd..7e1c7ebdf1348 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp @@ -85,6 +85,12 @@ class MPCTrajectory */ size_t size() const; + /** + * @brief get trajectory point at index i + * @return trajectory point at index i + */ + MPCTrajectoryPoint at(const size_t i) const; + /** * @return true if the components sizes are all 0 or are inconsistent */ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp index 036e6a32a9b83..9062ff1ea34e3 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp @@ -92,6 +92,13 @@ Trajectory convertToAutowareTrajectory(const MPCTrajectory & input); */ void calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory, std::vector & arc_length); +/** + * @brief calculate the arc length of the given trajectory + * @param [in] trajectory trajectory for which to calculate the arc length + * @return total arc length + */ +double calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory); + /** * @brief resample the given trajectory with the given fixed interval * @param [in] input trajectory to resample @@ -194,6 +201,14 @@ double calcStopDistance(const Trajectory & current_trajectory, const int origin) void extendTrajectoryInYawDirection( const double yaw, const double interval, const bool is_forward_shift, MPCTrajectory & traj); +/** + * @brief clip trajectory size by length + * @param [in] trajectory original trajectory + * @param [in] length clip length + * @return clipped trajectory + */ +MPCTrajectory clipTrajectoryByLength(const MPCTrajectory & trajectory, const double length); + /** * @brief Updates the value of a parameter with the given name. * @tparam T The type of the parameter value. diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp index 29cab73826f31..488a3c7ab8be2 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp @@ -103,6 +103,16 @@ class DynamicsBicycleModel : public VehicleModelInterface std::string modelName() override { return "dynamics"; }; + MPCTrajectory calculatePredictedTrajectoryInWorldCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const override; + + MPCTrajectory calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const override; + private: double m_lf; //!< @brief length from center of mass to front wheel [m] double m_lr; //!< @brief length from center of mass to rear wheel [m] diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp index fb113d5e30df7..e2f66e95ab0e3 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp @@ -91,6 +91,16 @@ class KinematicsBicycleModel : public VehicleModelInterface std::string modelName() override { return "kinematics"; }; + MPCTrajectory calculatePredictedTrajectoryInWorldCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const override; + + MPCTrajectory calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const override; + private: double m_steer_lim; //!< @brief steering angle limit [rad] double m_steer_tau; //!< @brief steering time constant for 1d-model [s] diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp index 93d84caa89992..b503ad8eb1d13 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp @@ -87,6 +87,16 @@ class KinematicsBicycleModelNoDelay : public VehicleModelInterface std::string modelName() override { return "kinematics_no_delay"; }; + MPCTrajectory calculatePredictedTrajectoryInWorldCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const override; + + MPCTrajectory calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const override; + private: double m_steer_lim; //!< @brief steering angle limit [rad] }; diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp index 808ec1b7def2c..68806ff5a00dc 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp @@ -15,6 +15,8 @@ #ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ #define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#include "mpc_lateral_controller/mpc_trajectory.hpp" + #include #include @@ -55,25 +57,25 @@ class VehicleModelInterface * @brief get state x dimension * @return state dimension */ - int getDimX(); + int getDimX() const; /** * @brief get input u dimension * @return input dimension */ - int getDimU(); + int getDimU() const; /** * @brief get output y dimension * @return output dimension */ - int getDimY(); + int getDimY() const; /** * @brief get wheelbase of the vehicle * @return wheelbase value [m] */ - double getWheelbase(); + double getWheelbase() const; /** * @brief set velocity @@ -109,6 +111,42 @@ class VehicleModelInterface * @brief returns model name e.g. kinematics, dynamics */ virtual std::string modelName() = 0; + + /** + * @brief Calculate the predicted trajectory for the ego vehicle based on the MPC result in world + * coordinate + * @param a_d The MPC A matrix used for optimization. + * @param b_d The MPC B matrix used for optimization. + * @param c_d The MPC C matrix used for optimization. + * @param w_d The MPC W matrix used for optimization. + * @param x0 initial state vector. + * @param Uex The optimized input vector. + * @param reference_trajectory The resampled reference trajectory. + * @param dt delta time used in the optimization + * @return The predicted trajectory. + */ + virtual MPCTrajectory calculatePredictedTrajectoryInWorldCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const = 0; + + /** + * @brief Calculate the predicted trajectory for the ego vehicle based on the MPC result in Frenet + * Coordinate + * @param a_d The MPC A matrix used for optimization. + * @param b_d The MPC B matrix used for optimization. + * @param c_d The MPC C matrix used for optimization. + * @param w_d The MPC W matrix used for optimization. + * @param x0 initial state vector. + * @param Uex The optimized input vector. + * @param reference_trajectory The resampled reference trajectory. + * @param dt delta time used in the optimization + * @return The predicted trajectory. + */ + virtual MPCTrajectory calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const = 0; }; } // namespace autoware::motion::control::mpc_lateral_controller #endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ diff --git a/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml b/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml index 59d3f759c9c2f..182458d532c8a 100644 --- a/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml +++ b/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml @@ -83,3 +83,5 @@ mass_rr: 600.0 cf: 155494.663 cr: 155494.663 + + debug_publish_predicted_trajectory: false # publish debug predicted trajectory in Frenet coordinate diff --git a/control/mpc_lateral_controller/src/lowpass_filter.cpp b/control/mpc_lateral_controller/src/lowpass_filter.cpp index 07a6e2c6b0d21..8fbf3488c5a2e 100644 --- a/control/mpc_lateral_controller/src/lowpass_filter.cpp +++ b/control/mpc_lateral_controller/src/lowpass_filter.cpp @@ -120,10 +120,8 @@ bool filt_vector(const int num, std::vector & u) double tmp = 0.0; int num_tmp = 0; double count = 0; - if (i - num < 0) { - num_tmp = i; - } else if (i + num > static_cast(u.size()) - 1) { - num_tmp = static_cast(u.size()) - i - 1; + if ((i - num < 0) || (i + num > static_cast(u.size()) - 1)) { + num_tmp = std::min(i, static_cast(u.size()) - i - 1); } else { num_tmp = num; } diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 6bd885b78beb7..8fa95f4b3b87b 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -17,6 +17,7 @@ #include "interpolation/linear_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "mpc_lateral_controller/mpc_utils.hpp" +#include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" #include @@ -28,6 +29,12 @@ using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::normalizeRadian; using tier4_autoware_utils::rad2deg; +MPC::MPC(rclcpp::Node & node) +{ + m_debug_frenet_predicted_trajectory_pub = node.create_publisher( + "~/debug/predicted_trajectory_in_frenet_coordinate", rclcpp::QoS(1)); +} + bool MPC::calculateMPC( const SteeringReport & current_steer, const Odometry & current_kinematics, AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_trajectory, @@ -97,9 +104,9 @@ bool MPC::calculateMPC( m_raw_steer_cmd_pprev = m_raw_steer_cmd_prev; m_raw_steer_cmd_prev = Uex(0); - // calculate predicted trajectory + /* calculate predicted trajectory */ predicted_trajectory = - calcPredictedTrajectory(mpc_resampled_ref_trajectory, mpc_matrix, x0_delayed, Uex); + calculatePredictedTrajectory(mpc_matrix, x0, Uex, mpc_resampled_ref_trajectory, prediction_dt); // prepare diagnostic message diagnostic = @@ -108,30 +115,6 @@ bool MPC::calculateMPC( return true; } -Trajectory MPC::calcPredictedTrajectory( - const MPCTrajectory & mpc_resampled_ref_trajectory, const MPCMatrix & mpc_matrix, - const VectorXd & x0_delayed, const VectorXd & Uex) const -{ - const VectorXd Xex = mpc_matrix.Aex * x0_delayed + mpc_matrix.Bex * Uex + mpc_matrix.Wex; - MPCTrajectory mpc_predicted_traj; - const auto & traj = mpc_resampled_ref_trajectory; - for (int i = 0; i < m_param.prediction_horizon; ++i) { - const int DIM_X = m_vehicle_model_ptr->getDimX(); - const double lat_error = Xex(i * DIM_X); - const double yaw_error = Xex(i * DIM_X + 1); - const double x = traj.x.at(i) - std::sin(traj.yaw.at(i)) * lat_error; - const double y = traj.y.at(i) + std::cos(traj.yaw.at(i)) * lat_error; - const double z = traj.z.at(i); - const double yaw = traj.yaw.at(i) + yaw_error; - const double vx = traj.vx.at(i); - const double k = traj.k.at(i); - const double smooth_k = traj.smooth_k.at(i); - const double relative_time = traj.relative_time.at(i); - mpc_predicted_traj.push_back(x, y, z, yaw, vx, k, smooth_k, relative_time); - } - return MPCUtils::convertToAutowareTrajectory(mpc_predicted_traj); -} - Float32MultiArrayStamped MPC::generateDiagData( const MPCTrajectory & reference_trajectory, const MPCData & mpc_data, const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex, @@ -204,7 +187,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 @@ -794,6 +777,35 @@ VectorXd MPC::calcSteerRateLimitOnTrajectory( return steer_rate_limits; } +Trajectory MPC::calculatePredictedTrajectory( + const MPCMatrix & mpc_matrix, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const +{ + const auto predicted_mpc_trajectory = + m_vehicle_model_ptr->calculatePredictedTrajectoryInWorldCoordinate( + mpc_matrix.Aex, mpc_matrix.Bex, mpc_matrix.Cex, mpc_matrix.Wex, x0, Uex, reference_trajectory, + dt); + + // do not over the reference trajectory + const auto predicted_length = MPCUtils::calcMPCTrajectoryArcLength(reference_trajectory); + const auto clipped_trajectory = + MPCUtils::clipTrajectoryByLength(predicted_mpc_trajectory, predicted_length); + + const auto predicted_trajectory = MPCUtils::convertToAutowareTrajectory(clipped_trajectory); + + // Publish trajectory in relative coordinate for debug purpose. + if (m_debug_publish_predicted_trajectory) { + const auto frenet = m_vehicle_model_ptr->calculatePredictedTrajectoryInFrenetCoordinate( + mpc_matrix.Aex, mpc_matrix.Bex, mpc_matrix.Cex, mpc_matrix.Wex, x0, Uex, reference_trajectory, + dt); + const auto frenet_clipped = MPCUtils::convertToAutowareTrajectory( + MPCUtils::clipTrajectoryByLength(frenet, predicted_length)); + m_debug_frenet_predicted_trajectory_pub->publish(frenet_clipped); + } + + return predicted_trajectory; +} + bool MPC::isValid(const MPCMatrix & m) const { if ( diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp index 3d19104112a71..7eca1481ba921 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -42,7 +42,9 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) const auto dp_bool = [&](const std::string & s) { return node.declare_parameter(s); }; const auto dp_double = [&](const std::string & s) { return node.declare_parameter(s); }; - m_mpc.m_ctrl_period = node.get_parameter("ctrl_period").as_double(); + m_mpc = std::make_unique(node); + + m_mpc->m_ctrl_period = node.get_parameter("ctrl_period").as_double(); auto & p_filt = m_trajectory_filtering_param; p_filt.enable_path_smoothing = dp_bool("enable_path_smoothing"); @@ -52,10 +54,10 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) p_filt.traj_resample_dist = dp_double("traj_resample_dist"); p_filt.extend_trajectory_for_end_yaw_control = dp_bool("extend_trajectory_for_end_yaw_control"); - m_mpc.m_admissible_position_error = dp_double("admissible_position_error"); - m_mpc.m_admissible_yaw_error_rad = dp_double("admissible_yaw_error_rad"); - m_mpc.m_use_steer_prediction = dp_bool("use_steer_prediction"); - m_mpc.m_param.steer_tau = dp_double("vehicle_model_steer_tau"); + m_mpc->m_admissible_position_error = dp_double("admissible_position_error"); + m_mpc->m_admissible_yaw_error_rad = dp_double("admissible_yaw_error_rad"); + m_mpc->m_use_steer_prediction = dp_bool("use_steer_prediction"); + m_mpc->m_param.steer_tau = dp_double("vehicle_model_steer_tau"); /* stop state parameters */ m_stop_state_entry_ego_speed = dp_double("stop_state_entry_ego_speed"); @@ -70,7 +72,7 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); const double wheelbase = vehicle_info.wheel_base_m; constexpr double deg2rad = static_cast(M_PI) / 180.0; - m_mpc.m_steer_lim = vehicle_info.max_steer_angle_rad; + m_mpc->m_steer_lim = vehicle_info.max_steer_angle_rad; // steer rate limit depending on curvature const auto steer_rate_lim_dps_list_by_curvature = @@ -78,7 +80,7 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) const auto curvature_list_for_steer_rate_lim = node.declare_parameter>("curvature_list_for_steer_rate_lim"); for (size_t i = 0; i < steer_rate_lim_dps_list_by_curvature.size(); ++i) { - m_mpc.m_steer_rate_lim_map_by_curvature.emplace_back( + m_mpc->m_steer_rate_lim_map_by_curvature.emplace_back( curvature_list_for_steer_rate_lim.at(i), steer_rate_lim_dps_list_by_curvature.at(i) * deg2rad); } @@ -89,26 +91,26 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) const auto velocity_list_for_steer_rate_lim = node.declare_parameter>("velocity_list_for_steer_rate_lim"); for (size_t i = 0; i < steer_rate_lim_dps_list_by_velocity.size(); ++i) { - m_mpc.m_steer_rate_lim_map_by_velocity.emplace_back( + m_mpc->m_steer_rate_lim_map_by_velocity.emplace_back( velocity_list_for_steer_rate_lim.at(i), steer_rate_lim_dps_list_by_velocity.at(i) * deg2rad); } /* vehicle model setup */ auto vehicle_model_ptr = - createVehicleModel(wheelbase, m_mpc.m_steer_lim, m_mpc.m_param.steer_tau, node); - m_mpc.setVehicleModel(vehicle_model_ptr); + createVehicleModel(wheelbase, m_mpc->m_steer_lim, m_mpc->m_param.steer_tau, node); + m_mpc->setVehicleModel(vehicle_model_ptr); /* QP solver setup */ - m_mpc.setVehicleModel(vehicle_model_ptr); + m_mpc->setVehicleModel(vehicle_model_ptr); auto qpsolver_ptr = createQPSolverInterface(node); - m_mpc.setQPSolver(qpsolver_ptr); + m_mpc->setQPSolver(qpsolver_ptr); /* delay compensation */ { const double delay_tmp = dp_double("input_delay"); - const double delay_step = std::round(delay_tmp / m_mpc.m_ctrl_period); - m_mpc.m_param.input_delay = delay_step * m_mpc.m_ctrl_period; - m_mpc.m_input_buffer = std::deque(static_cast(delay_step), 0.0); + const double delay_step = std::round(delay_tmp / m_mpc->m_ctrl_period); + m_mpc->m_param.input_delay = delay_step * m_mpc->m_ctrl_period; + m_mpc->m_input_buffer = std::deque(static_cast(delay_step), 0.0); } /* steering offset compensation */ @@ -120,7 +122,7 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) { const double steering_lpf_cutoff_hz = dp_double("steering_lpf_cutoff_hz"); const double error_deriv_lpf_cutoff_hz = dp_double("error_deriv_lpf_cutoff_hz"); - m_mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); + m_mpc->initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); } // ego nearest index search @@ -129,8 +131,10 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) }; m_ego_nearest_dist_threshold = check_and_get_param("ego_nearest_dist_threshold"); m_ego_nearest_yaw_threshold = check_and_get_param("ego_nearest_yaw_threshold"); - m_mpc.ego_nearest_dist_threshold = m_ego_nearest_dist_threshold; - m_mpc.ego_nearest_yaw_threshold = m_ego_nearest_yaw_threshold; + m_mpc->ego_nearest_dist_threshold = m_ego_nearest_dist_threshold; + m_mpc->ego_nearest_yaw_threshold = m_ego_nearest_yaw_threshold; + + m_mpc->m_debug_publish_predicted_trajectory = dp_bool("debug_publish_predicted_trajectory"); m_pub_predicted_traj = node.create_publisher("~/output/predicted_trajectory", 1); m_pub_debug_values = @@ -144,10 +148,10 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) m_set_param_res = node.add_on_set_parameters_callback(std::bind(&MpcLateralController::paramCallback, this, _1)); - m_mpc.initializeSteeringPredictor(); + m_mpc->initializeSteeringPredictor(); - m_mpc.setLogger(logger_); - m_mpc.setClock(clock_); + m_mpc->setLogger(logger_); + m_mpc->setClock(clock_); } MpcLateralController::~MpcLateralController() @@ -244,7 +248,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( m_is_ctrl_cmd_prev_initialized = true; } - const bool is_mpc_solved = m_mpc.calculateMPC( + const bool is_mpc_solved = m_mpc->calculateMPC( m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values); // reset previous MPC result @@ -253,7 +257,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( // After the recovery, the previous value of the optimization may deviate greatly from // the actual steer angle, and it may make the optimization result unstable. if (!is_mpc_solved) { - m_mpc.resetPrevResult(m_current_steering); + m_mpc->resetPrevResult(m_current_steering); } else { setSteeringToHistory(ctrl_cmd); } @@ -284,11 +288,11 @@ trajectory_follower::LateralOutput MpcLateralController::run( if (isStoppedState()) { // Reset input buffer - for (auto & value : m_mpc.m_input_buffer) { + for (auto & value : m_mpc->m_input_buffer) { value = m_ctrl_cmd_prev.steering_tire_angle; } // Use previous command value as previous raw steer command - m_mpc.m_raw_steer_cmd_prev = m_ctrl_cmd_prev.steering_tire_angle; + m_mpc->m_raw_steer_cmd_prev = m_ctrl_cmd_prev.steering_tire_angle; return createLateralOutput(m_ctrl_cmd_prev, false); } @@ -323,15 +327,15 @@ bool MpcLateralController::isReady(const trajectory_follower::InputData & input_ m_current_kinematic_state = input_data.current_odometry; m_current_steering = input_data.current_steering; - if (!m_mpc.hasVehicleModel()) { + if (!m_mpc->hasVehicleModel()) { info_throttle("MPC does not have a vehicle model"); return false; } - if (!m_mpc.hasQPSolver()) { + if (!m_mpc->hasQPSolver()) { info_throttle("MPC does not have a QP solver"); return false; } - if (m_mpc.m_reference_trajectory.empty()) { + if (m_mpc->m_reference_trajectory.empty()) { info_throttle("trajectory size is zero."); return false; } @@ -354,7 +358,7 @@ void MpcLateralController::setTrajectory( return; } - m_mpc.setReferenceTrajectory(msg, m_trajectory_filtering_param, current_kinematics); + m_mpc->setReferenceTrajectory(msg, m_trajectory_filtering_param, current_kinematics); // update trajectory buffer to check the trajectory shape change. m_trajectory_buffer.push_back(m_current_trajectory); @@ -504,12 +508,12 @@ bool MpcLateralController::isMpcConverged() void MpcLateralController::declareMPCparameters(rclcpp::Node & node) { - m_mpc.m_param.prediction_horizon = node.declare_parameter("mpc_prediction_horizon"); - m_mpc.m_param.prediction_dt = node.declare_parameter("mpc_prediction_dt"); + m_mpc->m_param.prediction_horizon = node.declare_parameter("mpc_prediction_horizon"); + m_mpc->m_param.prediction_dt = node.declare_parameter("mpc_prediction_dt"); const auto dp = [&](const auto & param) { return node.declare_parameter(param); }; - auto & nw = m_mpc.m_param.nominal_weight; + auto & nw = m_mpc->m_param.nominal_weight; nw.lat_error = dp("mpc_weight_lat_error"); nw.heading_error = dp("mpc_weight_heading_error"); nw.heading_error_squared_vel = dp("mpc_weight_heading_error_squared_vel"); @@ -521,7 +525,7 @@ void MpcLateralController::declareMPCparameters(rclcpp::Node & node) nw.terminal_lat_error = dp("mpc_weight_terminal_lat_error"); nw.terminal_heading_error = dp("mpc_weight_terminal_heading_error"); - auto & lcw = m_mpc.m_param.low_curvature_weight; + auto & lcw = m_mpc->m_param.low_curvature_weight; lcw.lat_error = dp("mpc_low_curvature_weight_lat_error"); lcw.heading_error = dp("mpc_low_curvature_weight_heading_error"); lcw.heading_error_squared_vel = dp("mpc_low_curvature_weight_heading_error_squared_vel"); @@ -530,12 +534,12 @@ void MpcLateralController::declareMPCparameters(rclcpp::Node & node) lcw.lat_jerk = dp("mpc_low_curvature_weight_lat_jerk"); lcw.steer_rate = dp("mpc_low_curvature_weight_steer_rate"); lcw.steer_acc = dp("mpc_low_curvature_weight_steer_acc"); - m_mpc.m_param.low_curvature_thresh_curvature = dp("mpc_low_curvature_thresh_curvature"); + m_mpc->m_param.low_curvature_thresh_curvature = dp("mpc_low_curvature_thresh_curvature"); - m_mpc.m_param.zero_ff_steer_deg = dp("mpc_zero_ff_steer_deg"); - m_mpc.m_param.acceleration_limit = dp("mpc_acceleration_limit"); - m_mpc.m_param.velocity_time_constant = dp("mpc_velocity_time_constant"); - m_mpc.m_param.min_prediction_length = dp("mpc_min_prediction_length"); + m_mpc->m_param.zero_ff_steer_deg = dp("mpc_zero_ff_steer_deg"); + m_mpc->m_param.acceleration_limit = dp("mpc_acceleration_limit"); + m_mpc->m_param.velocity_time_constant = dp("mpc_velocity_time_constant"); + m_mpc->m_param.min_prediction_length = dp("mpc_min_prediction_length"); } rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback( @@ -546,7 +550,7 @@ rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback( result.reason = "success"; // strong exception safety wrt MPCParam - MPCParam param = m_mpc.m_param; + MPCParam param = m_mpc->m_param; auto & nw = param.nominal_weight; auto & lcw = param.low_curvature_weight; @@ -587,15 +591,15 @@ rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback( // initialize input buffer update_param(parameters, "input_delay", param.input_delay); - const double delay_step = std::round(param.input_delay / m_mpc.m_ctrl_period); - const double delay = delay_step * m_mpc.m_ctrl_period; + const double delay_step = std::round(param.input_delay / m_mpc->m_ctrl_period); + const double delay = delay_step * m_mpc->m_ctrl_period; if (param.input_delay != delay) { param.input_delay = delay; - m_mpc.m_input_buffer = std::deque(static_cast(delay_step), 0.0); + m_mpc->m_input_buffer = std::deque(static_cast(delay_step), 0.0); } // transaction succeeds, now assign values - m_mpc.m_param = param; + m_mpc->m_param = param; } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { result.successful = false; result.reason = e.what(); diff --git a/control/mpc_lateral_controller/src/mpc_trajectory.cpp b/control/mpc_lateral_controller/src/mpc_trajectory.cpp index 3c799722494b9..b7fed942375e8 100644 --- a/control/mpc_lateral_controller/src/mpc_trajectory.cpp +++ b/control/mpc_lateral_controller/src/mpc_trajectory.cpp @@ -58,6 +58,22 @@ MPCTrajectoryPoint MPCTrajectory::back() return p; } +MPCTrajectoryPoint MPCTrajectory::at(const size_t i) const +{ + MPCTrajectoryPoint p; + + p.x = x.at(i); + p.y = y.at(i); + p.z = z.at(i); + p.yaw = yaw.at(i); + p.vx = vx.at(i); + p.k = k.at(i); + p.smooth_k = smooth_k.at(i); + p.relative_time = relative_time.at(i); + + return p; +} + void MPCTrajectory::clear() { x.clear(); diff --git a/control/mpc_lateral_controller/src/mpc_utils.cpp b/control/mpc_lateral_controller/src/mpc_utils.cpp index d3fc5fba0b014..01a81d9015b47 100644 --- a/control/mpc_lateral_controller/src/mpc_utils.cpp +++ b/control/mpc_lateral_controller/src/mpc_utils.cpp @@ -89,6 +89,15 @@ void calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory, std::vector resampleMPCTrajectoryByDistance( const MPCTrajectory & input, const double resample_interval_dist, const size_t nearest_seg_idx, const double ego_offset_to_segment) @@ -458,5 +467,22 @@ void extendTrajectoryInYawDirection( } } +MPCTrajectory clipTrajectoryByLength(const MPCTrajectory & trajectory, const double length) +{ + MPCTrajectory clipped_trajectory; + clipped_trajectory.push_back(trajectory.at(0)); + + double current_length = 0.0; + for (size_t i = 1; i < trajectory.size(); ++i) { + current_length += calcDistance3d(trajectory, i, i - 1); + if (current_length > length) { + break; + } + clipped_trajectory.push_back(trajectory.at(i)); + } + + return clipped_trajectory; +} + } // namespace MPCUtils } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp index 576907e79da4b..2467b1f0c2311 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp @@ -86,4 +86,48 @@ void DynamicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd & u_ref) m_lr * m_mass / (2 * m_cf * m_wheelbase) - m_lf * m_mass / (2 * m_cr * m_wheelbase); u_ref(0, 0) = m_wheelbase * m_curvature + Kv * vel * vel * m_curvature; } + +MPCTrajectory DynamicsBicycleModel::calculatePredictedTrajectoryInWorldCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, + [[maybe_unused]] const Eigen::MatrixXd & c_d, const Eigen::MatrixXd & w_d, + const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, [[maybe_unused]] const double dt) const +{ + RCLCPP_ERROR( + rclcpp::get_logger("control.trajectory_follower.lateral_controller"), + "Predicted trajectory calculation in world coordinate is not supported in dynamic model. " + "Calculate in the Frenet coordinate instead."); + return calculatePredictedTrajectoryInFrenetCoordinate( + a_d, b_d, c_d, w_d, x0, Uex, reference_trajectory, dt); +} + +MPCTrajectory DynamicsBicycleModel::calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, + [[maybe_unused]] const Eigen::MatrixXd & c_d, const Eigen::MatrixXd & w_d, + const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, [[maybe_unused]] const double dt) const +{ + // state = [e, de, th, dth] + // e : lateral error + // de : derivative of lateral error + // th : heading angle error + // dth : derivative of heading angle error + // steer : steering angle (input) + + Eigen::VectorXd Xex = a_d * x0 + b_d * Uex + w_d; + MPCTrajectory mpc_predicted_trajectory; + const auto DIM_X = getDimX(); + const auto & t = reference_trajectory; + + for (size_t i = 0; i < reference_trajectory.size(); ++i) { + const auto lateral_error = Xex(i * DIM_X); // model dependent + const auto yaw_error = Xex(i * DIM_X + 2); // model dependent + const auto x = t.x.at(i) - std::sin(t.yaw.at(i)) * lateral_error; + const auto y = t.y.at(i) + std::cos(t.yaw.at(i)) * lateral_error; + const auto yaw = t.yaw.at(i) + yaw_error; + mpc_predicted_trajectory.push_back( + x, y, t.z.at(i), yaw, t.vx.at(i), t.k.at(i), t.smooth_k.at(i), t.relative_time.at(i)); + } + return mpc_predicted_trajectory; +} } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp index 35bef667125c5..cd52dd4f79314 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp @@ -68,4 +68,85 @@ void KinematicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd & u_ref) { u_ref(0, 0) = std::atan(m_wheelbase * m_curvature); } + +MPCTrajectory KinematicsBicycleModel::calculatePredictedTrajectoryInWorldCoordinate( + [[maybe_unused]] const Eigen::MatrixXd & a_d, [[maybe_unused]] const Eigen::MatrixXd & b_d, + [[maybe_unused]] const Eigen::MatrixXd & c_d, [[maybe_unused]] const Eigen::MatrixXd & w_d, + const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const +{ + // Calculate predicted state in world coordinate since there is modeling errors in Frenet + // Relative coordinate x = [lat_err, yaw_err, steer] + // World coordinate x = [x, y, yaw, steer] + + const auto & t = reference_trajectory; + + // create initial state in the world coordinate + Eigen::VectorXd state_w = [&]() { + Eigen::VectorXd state = Eigen::VectorXd::Zero(4); + const auto lateral_error_0 = x0(0); + const auto yaw_error_0 = x0(1); + state(0, 0) = t.x.at(0) - std::sin(t.yaw.at(0)) * lateral_error_0; // world-x + state(1, 0) = t.y.at(0) + std::cos(t.yaw.at(0)) * lateral_error_0; // world-y + state(2, 0) = t.yaw.at(0) + yaw_error_0; // world-yaw + state(3, 0) = x0(2); // steering + return state; + }(); + + // update state in the world coordinate + const auto updateState = [&]( + const Eigen::VectorXd & state_w, const Eigen::MatrixXd & input, + const double dt, const double velocity) { + const auto yaw = state_w(2); + const auto steer = state_w(3); + const auto desired_steer = input(0); + + Eigen::VectorXd dstate = Eigen::VectorXd::Zero(4); + dstate(0) = velocity * std::cos(yaw); + dstate(1) = velocity * std::sin(yaw); + dstate(2) = velocity * std::tan(steer) / m_wheelbase; + dstate(3) = -(steer - desired_steer) / m_steer_tau; + + // Note: don't do "return state_w + dstate * dt", which does not work due to the lazy evaluation + // in Eigen. + const Eigen::VectorXd next_state = state_w + dstate * dt; + return next_state; + }; + + MPCTrajectory mpc_predicted_trajectory; + const auto DIM_U = getDimU(); + + for (size_t i = 0; i < reference_trajectory.size(); ++i) { + state_w = updateState(state_w, Uex.block(i * DIM_U, 0, DIM_U, 1), dt, t.vx.at(i)); + mpc_predicted_trajectory.push_back( + state_w(0), state_w(1), t.z.at(i), state_w(2), t.vx.at(i), t.k.at(i), t.smooth_k.at(i), + t.relative_time.at(i)); + } + return mpc_predicted_trajectory; +} + +MPCTrajectory KinematicsBicycleModel::calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, + [[maybe_unused]] const Eigen::MatrixXd & c_d, const Eigen::MatrixXd & w_d, + const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, [[maybe_unused]] const double dt) const +{ + // Relative coordinate x = [lat_err, yaw_err, steer] + + Eigen::VectorXd Xex = a_d * x0 + b_d * Uex + w_d; + MPCTrajectory mpc_predicted_trajectory; + const auto DIM_X = getDimX(); + const auto & t = reference_trajectory; + + for (size_t i = 0; i < reference_trajectory.size(); ++i) { + const auto lateral_error = Xex(i * DIM_X); // model dependent + const auto yaw_error = Xex(i * DIM_X + 1); // model dependent + const auto x = t.x.at(i) - std::sin(t.yaw.at(i)) * lateral_error; + const auto y = t.y.at(i) + std::cos(t.yaw.at(i)) * lateral_error; + const auto yaw = t.yaw.at(i) + yaw_error; + mpc_predicted_trajectory.push_back( + x, y, t.z.at(i), yaw, t.vx.at(i), t.k.at(i), t.smooth_k.at(i), t.relative_time.at(i)); + } + return mpc_predicted_trajectory; +} } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp index 1536beba9cd00..8f4510510aca9 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp @@ -59,4 +59,82 @@ void KinematicsBicycleModelNoDelay::calculateReferenceInput(Eigen::MatrixXd & u_ { u_ref(0, 0) = std::atan(m_wheelbase * m_curvature); } + +MPCTrajectory KinematicsBicycleModelNoDelay::calculatePredictedTrajectoryInWorldCoordinate( + [[maybe_unused]] const Eigen::MatrixXd & a_d, [[maybe_unused]] const Eigen::MatrixXd & b_d, + [[maybe_unused]] const Eigen::MatrixXd & c_d, [[maybe_unused]] const Eigen::MatrixXd & w_d, + const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const +{ + // Calculate predicted state in world coordinate since there is modeling errors in Frenet + // Relative coordinate x = [lat_err, yaw_err] + // World coordinate x = [x, y, yaw] + + const auto & t = reference_trajectory; + + // create initial state in the world coordinate + Eigen::VectorXd state_w = [&]() { + Eigen::VectorXd state = Eigen::VectorXd::Zero(3); + const auto lateral_error_0 = x0(0); + const auto yaw_error_0 = x0(1); + state(0, 0) = t.x.at(0) - std::sin(t.yaw.at(0)) * lateral_error_0; // world-x + state(1, 0) = t.y.at(0) + std::cos(t.yaw.at(0)) * lateral_error_0; // world-y + state(2, 0) = t.yaw.at(0) + yaw_error_0; // world-yaw + return state; + }(); + + // update state in the world coordinate + const auto updateState = [&]( + const Eigen::VectorXd & state_w, const Eigen::MatrixXd & input, + const double dt, const double velocity) { + const auto yaw = state_w(2); + const auto steer = input(0); + + Eigen::VectorXd dstate = Eigen::VectorXd::Zero(3); + dstate(0) = velocity * std::cos(yaw); + dstate(1) = velocity * std::sin(yaw); + dstate(2) = velocity * std::tan(steer) / m_wheelbase; + + // Note: don't do "return state_w + dstate * dt", which does not work due to the lazy evaluation + // in Eigen. + const Eigen::VectorXd next_state = state_w + dstate * dt; + return next_state; + }; + + MPCTrajectory mpc_predicted_trajectory; + const auto DIM_U = getDimU(); + for (size_t i = 0; i < t.size(); ++i) { + state_w = updateState(state_w, Uex.block(i * DIM_U, 0, DIM_U, 1), dt, t.vx.at(i)); + mpc_predicted_trajectory.push_back( + state_w(0), state_w(1), t.z.at(i), state_w(2), t.vx.at(i), t.k.at(i), t.smooth_k.at(i), + t.relative_time.at(i)); + } + + return mpc_predicted_trajectory; +} + +MPCTrajectory KinematicsBicycleModelNoDelay::calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, + [[maybe_unused]] const Eigen::MatrixXd & c_d, const Eigen::MatrixXd & w_d, + const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, [[maybe_unused]] const double dt) const +{ + // Relative coordinate x = [lat_err, yaw_err] + + Eigen::VectorXd Xex = a_d * x0 + b_d * Uex + w_d; + MPCTrajectory mpc_predicted_trajectory; + const auto DIM_X = getDimX(); + const auto & t = reference_trajectory; + + for (size_t i = 0; i < reference_trajectory.size(); ++i) { + const auto lateral_error = Xex(i * DIM_X); // model dependent + const auto yaw_error = Xex(i * DIM_X + 1); // model dependent + const auto x = t.x.at(i) - std::sin(t.yaw.at(i)) * lateral_error; + const auto y = t.y.at(i) + std::cos(t.yaw.at(i)) * lateral_error; + const auto yaw = t.yaw.at(i) + yaw_error; + mpc_predicted_trajectory.push_back( + x, y, t.z.at(i), yaw, t.vx.at(i), t.k.at(i), t.smooth_k.at(i), t.relative_time.at(i)); + } + return mpc_predicted_trajectory; +} } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp index 97e3f2742f546..f54a5e325bead 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp @@ -20,19 +20,19 @@ VehicleModelInterface::VehicleModelInterface(int dim_x, int dim_u, int dim_y, do : m_dim_x(dim_x), m_dim_u(dim_u), m_dim_y(dim_y), m_wheelbase(wheelbase) { } -int VehicleModelInterface::getDimX() +int VehicleModelInterface::getDimX() const { return m_dim_x; } -int VehicleModelInterface::getDimU() +int VehicleModelInterface::getDimU() const { return m_dim_u; } -int VehicleModelInterface::getDimY() +int VehicleModelInterface::getDimY() const { return m_dim_y; } -double VehicleModelInterface::getWheelbase() +double VehicleModelInterface::getWheelbase() const { return m_wheelbase; } diff --git a/control/mpc_lateral_controller/test/test_lowpass_filter.cpp b/control/mpc_lateral_controller/test/test_lowpass_filter.cpp index 2cd641c031265..c68513586847b 100644 --- a/control/mpc_lateral_controller/test/test_lowpass_filter.cpp +++ b/control/mpc_lateral_controller/test/test_lowpass_filter.cpp @@ -61,6 +61,30 @@ TEST(TestLowpassFilter, MoveAverageFilter) EXPECT_EQ(filtered_vec[4], 23.0 / 3); EXPECT_EQ(filtered_vec[5], original_vec[5]); } + { + const int window_size = 3; + const std::vector original_vec = {1.0, 1.0, 1.0, 1.0}; + std::vector filtered_vec = original_vec; + EXPECT_TRUE(MoveAverageFilter::filt_vector(window_size, filtered_vec)); + ASSERT_EQ(filtered_vec.size(), original_vec.size()); + EXPECT_EQ(filtered_vec[0], original_vec[0]); + EXPECT_EQ(filtered_vec[1], 1.0); + EXPECT_EQ(filtered_vec[2], 1.0); + EXPECT_EQ(filtered_vec[3], original_vec[3]); + } + { + const int window_size = 4; + const std::vector original_vec = {1.0, 3.0, 4.0, 6.0, 7.0, 10.0}; + std::vector filtered_vec = original_vec; + EXPECT_TRUE(MoveAverageFilter::filt_vector(window_size, filtered_vec)); + ASSERT_EQ(filtered_vec.size(), original_vec.size()); + EXPECT_EQ(filtered_vec[0], original_vec[0]); + EXPECT_EQ(filtered_vec[1], 8.0 / 3); + EXPECT_EQ(filtered_vec[2], 21.0 / 5); + EXPECT_EQ(filtered_vec[3], 30.0 / 5); + EXPECT_EQ(filtered_vec[4], 23.0 / 3); + EXPECT_EQ(filtered_vec[5], original_vec[5]); + } } TEST(TestLowpassFilter, Butterworth2dFilter) { diff --git a/control/mpc_lateral_controller/test/test_mpc.cpp b/control/mpc_lateral_controller/test/test_mpc.cpp index dade035daf26c..ba145b5dd146c 100644 --- a/control/mpc_lateral_controller/test/test_mpc.cpp +++ b/control/mpc_lateral_controller/test/test_mpc.cpp @@ -19,6 +19,7 @@ #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" +#include "rclcpp/rclcpp.hpp" #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" @@ -48,6 +49,22 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; using tier4_debug_msgs::msg::Float32MultiArrayStamped; +TrajectoryPoint makePoint(const double x, const double y, const float vx) +{ + TrajectoryPoint p; + p.pose.position.x = x; + p.pose.position.y = y; + p.longitudinal_velocity_mps = vx; + return p; +} + +nav_msgs::msg::Odometry makeOdometry(const geometry_msgs::msg::Pose & pose, const double velocity) +{ + nav_msgs::msg::Odometry odometry; + odometry.pose.pose = pose; + odometry.twist.twist.linear.x = velocity; + return odometry; +} class MPCTest : public ::testing::Test { protected: @@ -86,16 +103,7 @@ class MPCTest : public ::testing::Test TrajectoryFilteringParam trajectory_param; - TrajectoryPoint makePoint(const double x, const double y, const float vx) - { - TrajectoryPoint p; - p.pose.position.x = x; - p.pose.position.y = y; - p.longitudinal_velocity_mps = vx; - return p; - } - - void SetUp() override + void initParam() { param.prediction_horizon = 50; param.prediction_dt = 0.1; @@ -168,259 +176,268 @@ class MPCTest : public ::testing::Test mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); } - nav_msgs::msg::Odometry makeOdometry(const geometry_msgs::msg::Pose & pose, const double velocity) + void SetUp() override { - nav_msgs::msg::Odometry odometry; - odometry.pose.pose = pose; - odometry.twist.twist.linear.x = velocity; - return odometry; + rclcpp::init(0, nullptr); + initParam(); } + + void TearDown() override { rclcpp::shutdown(); } }; // class MPCTest /* cppcheck-suppress syntaxError */ TEST_F(MPCTest, InitializeAndCalculate) { - MPC mpc; - EXPECT_FALSE(mpc.hasVehicleModel()); - EXPECT_FALSE(mpc.hasQPSolver()); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); + EXPECT_FALSE(mpc->hasVehicleModel()); + EXPECT_FALSE(mpc->hasQPSolver()); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Init parameters and reference trajectory - initializeMPC(mpc); + initializeMPC(*mpc); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, InitializeAndCalculateRightTurn) { - MPC mpc; - EXPECT_FALSE(mpc.hasVehicleModel()); - EXPECT_FALSE(mpc.hasQPSolver()); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); + EXPECT_FALSE(mpc->hasVehicleModel()); + EXPECT_FALSE(mpc->hasQPSolver()); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Init parameters and reference trajectory - initializeMPC(mpc); + initializeMPC(*mpc); const auto current_kinematics = makeOdometry(dummy_right_turn_trajectory.points.front().pose, 0.0); - mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); + mpc->setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_LT(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_LT(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, OsqpCalculate) { - MPC mpc; - initializeMPC(mpc); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); + initializeMPC(*mpc); const auto current_kinematics = makeOdometry(dummy_straight_trajectory.points.front().pose, 0.0); - mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); + mpc->setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(logger); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - EXPECT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + EXPECT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, OsqpCalculateRightTurn) { - MPC mpc; - initializeMPC(mpc); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); + initializeMPC(*mpc); const auto current_kinematics = makeOdometry(dummy_right_turn_trajectory.points.front().pose, 0.0); - mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); + mpc->setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(logger); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_LT(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_LT(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, KinematicsNoDelayCalculate) { - MPC mpc; - initializeMPC(mpc); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); + initializeMPC(*mpc); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Init filters - mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); + mpc->initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); // Init trajectory const auto current_kinematics = makeOdometry(dummy_straight_trajectory.points.front().pose, 0.0); - mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); + mpc->setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn) { - MPC mpc; - initializeMPC(mpc); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); + initializeMPC(*mpc); const auto current_kinematics = makeOdometry(dummy_right_turn_trajectory.points.front().pose, 0.0); - mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); + mpc->setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Init filters - mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); + mpc->initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_LT(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_LT(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, DynamicCalculate) { - MPC mpc; - initializeMPC(mpc); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); + initializeMPC(*mpc); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, mass_fl, mass_fr, mass_rl, mass_rr, cf, cr); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, MultiSolveWithBuffer) { - MPC mpc; + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr); + mpc->setVehicleModel(vehicle_model_ptr); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); + mpc->setQPSolver(qpsolver_ptr); // Init parameters and reference trajectory - initializeMPC(mpc); + initializeMPC(*mpc); - mpc.m_input_buffer = {0.0, 0.0, 0.0}; + mpc->m_input_buffer = {0.0, 0.0, 0.0}; // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); - EXPECT_EQ(mpc.m_input_buffer.size(), size_t(3)); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); - EXPECT_EQ(mpc.m_input_buffer.size(), size_t(3)); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); - EXPECT_EQ(mpc.m_input_buffer.size(), size_t(3)); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); - EXPECT_EQ(mpc.m_input_buffer.size(), size_t(3)); + EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3)); } TEST_F(MPCTest, FailureCases) { - MPC mpc; + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr); + mpc->setVehicleModel(vehicle_model_ptr); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); + mpc->setQPSolver(qpsolver_ptr); // Init parameters and reference trajectory - initializeMPC(mpc); + initializeMPC(*mpc); // Calculate MPC with a pose too far from the trajectory Pose pose_far; @@ -430,10 +447,10 @@ TEST_F(MPCTest, FailureCases) Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_far, default_velocity); - EXPECT_FALSE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + EXPECT_FALSE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); // Calculate MPC with a fast velocity to make the prediction go further than the reference path - EXPECT_FALSE(mpc.calculateMPC( + EXPECT_FALSE(mpc->calculateMPC( neutral_steer, makeOdometry(pose_far, default_velocity + 10.0), ctrl_cmd, pred_traj, diag)); } } // namespace autoware::motion::control::mpc_lateral_controller 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..57eab2d87f18e 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) @@ -234,6 +241,10 @@ If the ego is still running, strong acceleration (`strong_stop_acc`) to stop rig ### STOPPED Parameter +The `STOPPED` state assumes that the vehicle is completely stopped with the brakes fully applied. +Therefore, `stopped_acc` should be set to a value that allows the vehicle to apply the strongest possible brake. +If `stopped_acc` is not sufficiently low, there is a possibility of sliding down on steep slopes. + | Name | Type | Description | Default value | | :----------- | :----- | :------------------------------------------- | :------------ | | stopped_vel | double | target velocity in STOPPED state [m/s] | 0.0 | diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp index 9c01f7bc26c4b..ac4fec8dacb7d 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -16,6 +16,8 @@ #define PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ #include "interpolation/linear_interpolation.hpp" +#include "interpolation/spherical_linear_interpolation.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "tf2/utils.h" @@ -28,6 +30,7 @@ #include #include +#include namespace autoware::motion::control::pid_longitudinal_controller { @@ -60,11 +63,11 @@ double getPitchByPose(const Quaternion & quaternion); * @brief calculate pitch angle from trajectory on map * NOTE: there is currently no z information so this always returns 0.0 * @param [in] trajectory input trajectory - * @param [in] closest_idx nearest index to current vehicle position + * @param [in] start_idx nearest index to current vehicle position * @param [in] wheel_base length of wheel base */ double getPitchByTraj( - const Trajectory & trajectory, const size_t closest_idx, const double wheel_base); + const Trajectory & trajectory, const size_t start_idx, const double wheel_base); /** * @brief calculate vehicle pose after time delay by moving the vehicle at current velocity and @@ -74,21 +77,13 @@ Pose calcPoseAfterTimeDelay( const Pose & current_pose, const double delay_time, const double current_vel, const double current_acc); -/** - * @brief apply linear interpolation to orientation - * @param [in] o_from first orientation - * @param [in] o_to second orientation - * @param [in] ratio ratio between o_from and o_to for interpolation - */ -Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio); - /** * @brief apply linear interpolation to trajectory point that is nearest to a certain point * @param [in] points trajectory points * @param [in] point Interpolated point is nearest to this point. */ template -TrajectoryPoint lerpTrajectoryPoint( +std::pair lerpTrajectoryPoint( const T & points, const Pose & pose, const double max_dist, const double max_yaw) { TrajectoryPoint interpolated_point; @@ -107,7 +102,9 @@ TrajectoryPoint lerpTrajectoryPoint( points.at(i).pose.position.x, points.at(i + 1).pose.position.x, interpolate_ratio); interpolated_point.pose.position.y = interpolation::lerp( points.at(i).pose.position.y, points.at(i + 1).pose.position.y, interpolate_ratio); - interpolated_point.pose.orientation = lerpOrientation( + interpolated_point.pose.position.z = interpolation::lerp( + points.at(i).pose.position.z, points.at(i + 1).pose.position.z, interpolate_ratio); + interpolated_point.pose.orientation = interpolation::lerpOrientation( points.at(i).pose.orientation, points.at(i + 1).pose.orientation, interpolate_ratio); interpolated_point.longitudinal_velocity_mps = interpolation::lerp( points.at(i).longitudinal_velocity_mps, points.at(i + 1).longitudinal_velocity_mps, @@ -120,7 +117,7 @@ TrajectoryPoint lerpTrajectoryPoint( points.at(i).heading_rate_rps, points.at(i + 1).heading_rate_rps, interpolate_ratio); } - return interpolated_point; + return std::make_pair(interpolated_point, seg_idx); } /** @@ -144,6 +141,17 @@ double applyDiffLimitFilter( double applyDiffLimitFilter( const double input_val, const double prev_val, const double dt, const double max_val, const double min_val); + +/** + * @brief calculate the projected pose after distance from the current index + * @param [in] src_idx current index + * @param [in] distance distance to project + * @param [in] trajectory reference trajectory + */ +geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance( + const size_t src_idx, const double distance, + const autoware_auto_planning_msgs::msg::Trajectory & trajectory); + } // namespace longitudinal_utils } // namespace autoware::motion::control::pid_longitudinal_controller 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..8df7cb3b9b3f8 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 @@ -65,13 +66,25 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro double vel{0.0}; double acc{0.0}; }; - + struct StateAfterDelay + { + StateAfterDelay(const double velocity, const double acceleration, const double distance) + : vel(velocity), acc(acceleration), running_distance(distance) + { + } + double vel{0.0}; + double acc{0.0}; + double running_distance{0.0}; + }; enum class Shift { Forward = 0, Reverse }; struct ControlData { bool is_far_from_trajectory{false}; + autoware_auto_planning_msgs::msg::Trajectory interpolated_traj{}; size_t nearest_idx{0}; // nearest_idx = 0 when nearest_idx is not found with findNearestIdx + size_t target_idx{0}; + StateAfterDelay state_after_delay{0.0, 0.0, 0.0}; Motion current_motion{}; Shift shift{Shift::Forward}; // shift is used only to calculate the sign of pitch compensation double stop_dist{0.0}; // signed distance that is positive when car is before the stopline @@ -97,6 +110,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 +160,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; @@ -179,7 +196,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro double m_min_jerk; // slope compensation - bool m_use_traj_for_pitch; + enum class SlopeSource { RAW_PITCH = 0, TRAJECTORY_PITCH, TRAJECTORY_ADAPTIVE }; + SlopeSource m_slope_source{SlopeSource::RAW_PITCH}; + double m_adaptive_trajectory_velocity_th; std::shared_ptr m_lpf_pitch{nullptr}; double m_max_pitch_rad; double m_min_pitch_rad; @@ -271,11 +290,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro /** * @brief calculate control command based on the current control state - * @param [in] current_pose current ego pose * @param [in] control_data control data */ - Motion calcCtrlCmd( - const geometry_msgs::msg::Pose & current_pose, const ControlData & control_data); + Motion calcCtrlCmd(const ControlData & control_data); /** * @brief publish control command @@ -304,9 +321,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro /** * @brief calculate direction (forward or backward) that vehicle moves - * @param [in] nearest_idx nearest index on trajectory to vehicle + * @param [in] control_data data for control calculation */ - enum Shift getCurrentShift(const size_t nearest_idx) const; + enum Shift getCurrentShift(const ControlData & control_data) const; /** * @brief filter acceleration command with limitation of acceleration and jerk, and slope @@ -336,8 +353,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro * @param [in] motion delay compensated target motion */ Motion keepBrakeBeforeStop( - const autoware_auto_planning_msgs::msg::Trajectory & traj, const Motion & target_motion, - const size_t nearest_idx) const; + const ControlData & control_data, const Motion & target_motion, const size_t nearest_idx) const; /** * @brief interpolate trajectory point that is nearest to vehicle @@ -345,7 +361,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro * @param [in] point vehicle position * @param [in] nearest_idx index of the trajectory point nearest to the vehicle position */ - autoware_auto_planning_msgs::msg::TrajectoryPoint calcInterpolatedTargetValue( + std::pair + calcInterpolatedTrajPointAndSegment( const autoware_auto_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose) const; @@ -354,18 +371,14 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro * @param [in] current_motion current velocity and acceleration of the vehicle * @param [in] delay_compensation_time predicted time delay */ - double predictedVelocityInTargetPoint( + StateAfterDelay predictedStateAfterDelay( const Motion current_motion, const double delay_compensation_time) const; /** * @brief calculate velocity feedback with feed forward and pid controller - * @param [in] target_motion reference velocity and acceleration. This acceleration will be used - * as feed forward. - * @param [in] dt time step to use - * @param [in] current_vel current velocity of the vehicle + * @param [in] control_data data for control calculation */ - double applyVelocityFeedback( - const Motion target_motion, const double dt, const double current_vel, const Shift & shift); + double applyVelocityFeedback(const ControlData & control_data); /** * @brief update variables for debugging about pitch @@ -378,12 +391,11 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro /** * @brief update variables for velocity and acceleration * @param [in] ctrl_cmd latest calculated control command - * @param [in] current_pose current pose of the vehicle * @param [in] control_data data for control calculation */ - void updateDebugVelAcc( - const Motion & ctrl_cmd, const geometry_msgs::msg::Pose & current_pose, - const ControlData & control_data); + void updateDebugVelAcc(const ControlData & control_data); + + double getTimeUnderControl(); }; } // namespace autoware::motion::control::pid_longitudinal_controller diff --git a/control/pid_longitudinal_controller/package.xml b/control/pid_longitudinal_controller/package.xml index 0c2da18f9185c..959aca689816a 100644 --- a/control/pid_longitudinal_controller/package.xml +++ b/control/pid_longitudinal_controller/package.xml @@ -7,6 +7,7 @@ Takamasa Horibe Takayuki Murooka + Mamoru Sobue Apache 2.0 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..e95428096c17c 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 @@ -67,8 +69,9 @@ max_jerk: 2.0 min_jerk: -5.0 - # pitch - use_trajectory_for_pitch_calculation: false + # slope compensation lpf_pitch_gain: 0.95 + slope_source: "raw_pitch" # raw_pitch, trajectory_pitch or trajectory_adaptive + adaptive_trajectory_velocity_th: 1.0 max_pitch_rad: 0.1 min_pitch_rad: -0.1 diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 9791a1f0b5e3e..28cd6e1824859 100644 --- a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -84,7 +84,7 @@ double getPitchByPose(const Quaternion & quaternion_msg) } double getPitchByTraj( - const Trajectory & trajectory, const size_t nearest_idx, const double wheel_base) + const Trajectory & trajectory, const size_t start_idx, const double wheel_base) { // cannot calculate pitch if (trajectory.points.size() <= 1) { @@ -92,17 +92,17 @@ double getPitchByTraj( } const auto [prev_idx, next_idx] = [&]() { - for (size_t i = nearest_idx + 1; i < trajectory.points.size(); ++i) { - const double dist = tier4_autoware_utils::calcDistance2d( - trajectory.points.at(nearest_idx), trajectory.points.at(i)); + for (size_t i = start_idx + 1; i < trajectory.points.size(); ++i) { + const double dist = tier4_autoware_utils::calcDistance3d( + trajectory.points.at(start_idx), trajectory.points.at(i)); if (dist > wheel_base) { // calculate pitch from trajectory between rear wheel (nearest) and front center (i) - return std::make_pair(nearest_idx, i); + return std::make_pair(start_idx, i); } } // NOTE: The ego pose is close to the goal. return std::make_pair( - std::min(nearest_idx, trajectory.points.size() - 2), trajectory.points.size() - 1); + std::min(start_idx, trajectory.points.size() - 2), trajectory.points.size() - 1); }(); return tier4_autoware_utils::calcElevationAngle( @@ -141,16 +141,6 @@ double lerp(const double v_from, const double v_to, const double ratio) return v_from + (v_to - v_from) * ratio; } -Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio) -{ - tf2::Quaternion q_from, q_to; - tf2::fromMsg(o_from, q_from); - tf2::fromMsg(o_to, q_to); - - const auto q_interpolated = q_from.slerp(q_to, ratio); - return tf2::toMsg(q_interpolated); -} - double applyDiffLimitFilter( const double input_val, const double prev_val, const double dt, const double max_val, const double min_val) @@ -168,5 +158,33 @@ double applyDiffLimitFilter( const double min_val = -max_val; return applyDiffLimitFilter(input_val, prev_val, dt, max_val, min_val); } + +geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance( + const size_t src_idx, const double distance, + const autoware_auto_planning_msgs::msg::Trajectory & trajectory) +{ + double remain_dist = distance; + geometry_msgs::msg::Pose p = trajectory.points.back().pose; + for (size_t i = src_idx; i < trajectory.points.size() - 1; ++i) { + const double dist = tier4_autoware_utils::calcDistance3d( + trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose); + if (remain_dist < dist) { + if (remain_dist <= 0.0) { + return trajectory.points.at(i).pose; + } + double ratio = remain_dist / dist; + const auto p0 = trajectory.points.at(i).pose; + const auto p1 = trajectory.points.at(i + 1).pose; + p = trajectory.points.at(i).pose; + p.position.x = interpolation::lerp(p0.position.x, p1.position.x, ratio); + p.position.y = interpolation::lerp(p0.position.y, p1.position.y, ratio); + p.position.z = interpolation::lerp(p0.position.z, p1.position.z, ratio); + p.orientation = interpolation::lerpOrientation(p0.orientation, p1.orientation, ratio); + break; + } + remain_dist -= dist; + } + return p; +} } // namespace longitudinal_utils } // namespace autoware::motion::control::pid_longitudinal_controller diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index d13e628f2e1d4..aab2ecf8f081e 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] @@ -163,12 +168,27 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) m_min_jerk = node.declare_parameter("min_jerk"); // [m/s^3] // parameters for slope compensation - m_use_traj_for_pitch = node.declare_parameter("use_trajectory_for_pitch_calculation"); + m_adaptive_trajectory_velocity_th = + node.declare_parameter("adaptive_trajectory_velocity_th"); // [m/s^2] const double lpf_pitch_gain{node.declare_parameter("lpf_pitch_gain")}; m_lpf_pitch = std::make_shared(0.0, lpf_pitch_gain); m_max_pitch_rad = node.declare_parameter("max_pitch_rad"); // [rad] m_min_pitch_rad = node.declare_parameter("min_pitch_rad"); // [rad] + // check slope source is proper + const std::string slope_source = node.declare_parameter( + "slope_source"); // raw_pitch, trajectory_pitch or trajectory_adaptive + if (slope_source == "raw_pitch") { + m_slope_source = SlopeSource::RAW_PITCH; + } else if (slope_source == "trajectory_pitch") { + m_slope_source = SlopeSource::TRAJECTORY_PITCH; + } else if (slope_source == "trajectory_adaptive") { + m_slope_source = SlopeSource::TRAJECTORY_ADAPTIVE; + } else { + RCLCPP_ERROR(logger_, "Slope source is not valid. Using raw_pitch option as default"); + m_slope_source = SlopeSource::RAW_PITCH; + } + // ego nearest index search m_ego_nearest_dist_threshold = node.has_parameter("ego_nearest_dist_threshold") @@ -284,6 +304,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 @@ -349,6 +370,7 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac // slope compensation update_param("max_pitch_rad", m_max_pitch_rad); update_param("min_pitch_rad", m_min_pitch_rad); + update_param("adaptive_trajectory_velocity_th", m_adaptive_trajectory_velocity_th); rcl_interfaces::msg::SetParametersResult result; result.successful = true; @@ -395,7 +417,7 @@ trajectory_follower::LongitudinalOutput PidLongitudinalController::run( updateControlState(control_data); // calculate control command - const Motion ctrl_cmd = calcCtrlCmd(current_pose, control_data); + const Motion ctrl_cmd = calcCtrlCmd(control_data); // publish control command const auto cmd_msg = createCtrlCmdMsg(ctrl_cmd, control_data.current_motion.vel); @@ -422,19 +444,29 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData // current velocity and acceleration control_data.current_motion.vel = m_current_kinematic_state.twist.twist.linear.x; control_data.current_motion.acc = m_current_accel.accel.accel.linear.x; + control_data.interpolated_traj = m_trajectory; - // nearest idx - const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - m_trajectory.points, current_pose, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); - const auto & nearest_point = m_trajectory.points.at(nearest_idx).pose; + // calculate the interpolated point and segment + const auto current_interpolated_pose = + calcInterpolatedTrajPointAndSegment(control_data.interpolated_traj, current_pose); + + // Insert the interpolated point + control_data.interpolated_traj.points.insert( + control_data.interpolated_traj.points.begin() + current_interpolated_pose.second + 1, + current_interpolated_pose.first); + control_data.nearest_idx = current_interpolated_pose.second + 1; + control_data.target_idx = control_data.nearest_idx; + const auto nearest_point = current_interpolated_pose.first; + auto target_point = current_interpolated_pose.first; // check if the deviation is worth emergency m_diagnostic_data.trans_deviation = - tier4_autoware_utils::calcDistance2d(nearest_point, current_pose); + tier4_autoware_utils::calcDistance2d(current_interpolated_pose.first, current_pose); const bool is_dist_deviation_large = m_state_transition_params.emergency_state_traj_trans_dev < m_diagnostic_data.trans_deviation; m_diagnostic_data.rot_deviation = std::abs(tier4_autoware_utils::normalizeRadian( - tf2::getYaw(nearest_point.orientation) - tf2::getYaw(current_pose.orientation))); + tf2::getYaw(current_interpolated_pose.first.pose.orientation) - + tf2::getYaw(current_pose.orientation))); const bool is_yaw_deviation_large = m_state_transition_params.emergency_state_traj_rot_dev < m_diagnostic_data.rot_deviation; @@ -443,10 +475,52 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData control_data.is_far_from_trajectory = true; return control_data; } - control_data.nearest_idx = nearest_idx; + + // Delay compensation - Calculate the distance we got, predicted velocity and predicted + // acceleration after delay + control_data.state_after_delay = + predictedStateAfterDelay(control_data.current_motion, m_delay_compensation_time); + + // calculate the target motion for delay compensation + constexpr double min_running_dist = 0.01; + if (control_data.state_after_delay.running_distance > min_running_dist) { + const auto target_pose = longitudinal_utils::findTrajectoryPoseAfterDistance( + control_data.nearest_idx, control_data.state_after_delay.running_distance, + control_data.interpolated_traj); + const auto target_interpolated_point = + calcInterpolatedTrajPointAndSegment(control_data.interpolated_traj, target_pose); + control_data.target_idx = target_interpolated_point.second + 1; + control_data.interpolated_traj.points.insert( + control_data.interpolated_traj.points.begin() + control_data.target_idx, + target_interpolated_point.first); + target_point = target_interpolated_point.first; + } + + // ========================================================================================== + // NOTE: due to removeOverlapPoints(), the obtained control_data.target_idx and + // control_data.nearest_idx may become invalid if the number of points decreased. + // current API does not provide the way to check duplication beforehand and this function + // does not tell how many/which index points were removed, so there is no way + // to tell if our `control_data.target_idx` point still exists or removed. + // ========================================================================================== + // Remove overlapped points after inserting the interpolated points + control_data.interpolated_traj.points = + motion_utils::removeOverlapPoints(control_data.interpolated_traj.points); + control_data.nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + control_data.interpolated_traj.points, nearest_point.pose, m_ego_nearest_dist_threshold, + m_ego_nearest_yaw_threshold); + control_data.target_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + control_data.interpolated_traj.points, target_point.pose, m_ego_nearest_dist_threshold, + m_ego_nearest_yaw_threshold); + + // send debug values + m_debug_values.setValues(DebugValues::TYPE::PREDICTED_VEL, control_data.state_after_delay.vel); + m_debug_values.setValues( + DebugValues::TYPE::TARGET_VEL, + control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps); // shift - control_data.shift = getCurrentShift(control_data.nearest_idx); + control_data.shift = getCurrentShift(control_data); if (control_data.shift != m_prev_shift) { m_pid_vel.reset(); } @@ -454,15 +528,34 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData // distance to stopline control_data.stop_dist = longitudinal_utils::calcStopDistance( - current_pose, m_trajectory, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); + control_data.interpolated_traj.points.at(control_data.nearest_idx).pose, + control_data.interpolated_traj, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); // pitch // NOTE: getPitchByTraj() calculates the pitch angle as defined in // ../media/slope_definition.drawio.svg while getPitchByPose() is not, so `raw_pitch` is reversed const double raw_pitch = (-1.0) * longitudinal_utils::getPitchByPose(current_pose.orientation); - const double traj_pitch = - longitudinal_utils::getPitchByTraj(m_trajectory, control_data.nearest_idx, m_wheel_base); - control_data.slope_angle = m_use_traj_for_pitch ? traj_pitch : m_lpf_pitch->filter(raw_pitch); + const double traj_pitch = longitudinal_utils::getPitchByTraj( + control_data.interpolated_traj, control_data.target_idx, m_wheel_base); + + if (m_slope_source == SlopeSource::RAW_PITCH) { + control_data.slope_angle = m_lpf_pitch->filter(raw_pitch); + } else if (m_slope_source == SlopeSource::TRAJECTORY_PITCH) { + control_data.slope_angle = traj_pitch; + } else if (m_slope_source == SlopeSource::TRAJECTORY_ADAPTIVE) { + // if velocity is high, use target idx for slope, otherwise, use raw_pitch + if (control_data.current_motion.vel > m_adaptive_trajectory_velocity_th) { + control_data.slope_angle = traj_pitch; + m_lpf_pitch->filter(raw_pitch); + } else { + control_data.slope_angle = m_lpf_pitch->filter(raw_pitch); + } + } else { + RCLCPP_ERROR_THROTTLE( + logger_, *clock_, 3000, "Slope source is not valid. Using raw_pitch option as default"); + control_data.slope_angle = m_lpf_pitch->filter(raw_pitch); + } + updatePitchDebugValues(control_data.slope_angle, traj_pitch, raw_pitch); return control_data; @@ -533,8 +626,12 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d ? (clock_->now() - *m_last_running_time).seconds() > p.stopped_state_entry_duration_time : false; - const double current_vel_cmd = - std::fabs(m_trajectory.points.at(control_data.nearest_idx).longitudinal_velocity_mps); + // ========================================================================================== + // NOTE: due to removeOverlapPoints() in getControlData() m_trajectory and + // control_data.interpolated_traj have different size. + // ========================================================================================== + const double current_vel_cmd = std::fabs( + control_data.interpolated_traj.points.at(control_data.nearest_idx).longitudinal_velocity_mps); const bool emergency_condition = m_enable_overshoot_emergency && stop_dist < -p.emergency_state_overshoot_stop_dist && current_vel_cmd < vel_epsilon; @@ -549,13 +646,16 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d return; }; - const auto info_throttle = [this](const auto & s) { - RCLCPP_INFO_SKIPFIRST_THROTTLE(logger_, *clock_, 5000, "%s", s); - }; + const auto debug_msg_once = [this](const auto & s) { RCLCPP_DEBUG_ONCE(logger_, "%s", s); }; 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) { @@ -572,8 +672,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d if (m_enable_smooth_stop) { if (stopping_condition) { // predictions after input time delay - const double pred_vel_in_target = - predictedVelocityInTargetPoint(control_data.current_motion, m_delay_compensation_time); + const double pred_vel_in_target = control_data.state_after_delay.vel; const double pred_stop_dist = control_data.stop_dist - 0.5 * (pred_vel_in_target + current_vel) * m_delay_compensation_time; @@ -612,10 +711,10 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d if (m_control_state == ControlState::STOPPED) { // -- debug print -- if (has_nonzero_target_vel && !departure_condition_from_stopped) { - info_throttle("target speed > 0, but departure condition is not met. Keep STOPPED."); + debug_msg_once("target speed > 0, but departure condition is not met. Keep STOPPED."); } if (has_nonzero_target_vel && keep_stopped_condition) { - info_throttle("target speed > 0, but keep stop condition is met. Keep STOPPED."); + debug_msg_once("target speed > 0, but keep stop condition is met. Keep STOPPED."); } // --------------- @@ -635,10 +734,11 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d // in EMERGENCY state if (m_control_state == ControlState::EMERGENCY) { + if (stopped_condition) { + return changeState(ControlState::STOPPED); + } + if (!emergency_condition) { - if (stopped_condition) { - return changeState(ControlState::STOPPED); - } if (!is_under_control) { // NOTE: On manual driving, no need stopping to exit the emergency. return changeState(ControlState::DRIVE); @@ -652,41 +752,32 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d } PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( - const geometry_msgs::msg::Pose & current_pose, const ControlData & control_data) + const ControlData & control_data) { - const size_t nearest_idx = control_data.nearest_idx; - const double current_vel = control_data.current_motion.vel; - const double current_acc = control_data.current_motion.acc; + const size_t target_idx = control_data.target_idx; // velocity and acceleration command - Motion raw_ctrl_cmd{}; - Motion target_motion{}; + Motion raw_ctrl_cmd{ + control_data.interpolated_traj.points.at(target_idx).longitudinal_velocity_mps, + control_data.interpolated_traj.points.at(target_idx).acceleration_mps2}; + if (m_control_state == ControlState::DRIVE) { - const auto target_pose = longitudinal_utils::calcPoseAfterTimeDelay( - current_pose, m_delay_compensation_time, current_vel, current_acc); - const auto target_interpolated_point = calcInterpolatedTargetValue(m_trajectory, target_pose); - target_motion = Motion{ - target_interpolated_point.longitudinal_velocity_mps, - target_interpolated_point.acceleration_mps2}; - - target_motion = keepBrakeBeforeStop(m_trajectory, target_motion, nearest_idx); - - const double pred_vel_in_target = - predictedVelocityInTargetPoint(control_data.current_motion, m_delay_compensation_time); - m_debug_values.setValues(DebugValues::TYPE::PREDICTED_VEL, pred_vel_in_target); - - raw_ctrl_cmd.vel = target_motion.vel; - raw_ctrl_cmd.acc = - applyVelocityFeedback(target_motion, control_data.dt, pred_vel_in_target, control_data.shift); + raw_ctrl_cmd.vel = + control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps; + raw_ctrl_cmd.acc = applyVelocityFeedback(control_data); + raw_ctrl_cmd = keepBrakeBeforeStop(control_data, raw_ctrl_cmd, target_idx); + RCLCPP_DEBUG( logger_, "[feedback control] vel: %3.3f, acc: %3.3f, dt: %3.3f, v_curr: %3.3f, v_ref: %3.3f " "feedback_ctrl_cmd.ac: %3.3f", - raw_ctrl_cmd.vel, raw_ctrl_cmd.acc, control_data.dt, current_vel, target_motion.vel, + raw_ctrl_cmd.vel, raw_ctrl_cmd.acc, control_data.dt, control_data.current_motion.vel, + control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps, raw_ctrl_cmd.acc); } else if (m_control_state == ControlState::STOPPING) { raw_ctrl_cmd.acc = m_smooth_stop.calculate( - control_data.stop_dist, current_vel, current_acc, m_vel_hist, m_delay_compensation_time); + control_data.stop_dist, control_data.current_motion.vel, control_data.current_motion.acc, + m_vel_hist, m_delay_compensation_time); raw_ctrl_cmd.vel = m_stopped_state_params.vel; RCLCPP_DEBUG( @@ -712,7 +803,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( const Motion filtered_ctrl_cmd{raw_ctrl_cmd.vel, filtered_acc_cmd}; // update debug visualization - updateDebugVelAcc(target_motion, current_pose, control_data); + updateDebugVelAcc(control_data); return filtered_ctrl_cmd; } @@ -729,7 +820,8 @@ autoware_auto_control_msgs::msg::LongitudinalCommand PidLongitudinalController:: // store current velocity history m_vel_hist.push_back({clock_->now(), current_vel}); - while (m_vel_hist.size() > static_cast(0.5 / m_longitudinal_ctrl_period)) { + while (m_vel_hist.size() > + static_cast(m_delay_compensation_time / m_longitudinal_ctrl_period)) { m_vel_hist.erase(m_vel_hist.begin()); } @@ -781,11 +873,12 @@ double PidLongitudinalController::getDt() } enum PidLongitudinalController::Shift PidLongitudinalController::getCurrentShift( - const size_t nearest_idx) const + const ControlData & control_data) const { constexpr double epsilon = 1e-5; - const double target_vel = m_trajectory.points.at(nearest_idx).longitudinal_velocity_mps; + const double target_vel = + control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps; if (target_vel > epsilon) { return Shift::Forward; @@ -805,6 +898,7 @@ double PidLongitudinalController::calcFilteredAcc( // store ctrl cmd without slope filter storeAccelCmd(acc_max_filtered); + // apply slope compensation const double acc_slope_filtered = applySlopeCompensation(acc_max_filtered, control_data.slope_angle, control_data.shift); m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_SLOPE_APPLIED, acc_slope_filtered); @@ -856,15 +950,15 @@ double PidLongitudinalController::applySlopeCompensation( } PidLongitudinalController::Motion PidLongitudinalController::keepBrakeBeforeStop( - const autoware_auto_planning_msgs::msg::Trajectory & traj, const Motion & target_motion, - const size_t nearest_idx) const + const ControlData & control_data, const Motion & target_motion, const size_t nearest_idx) const { Motion output_motion = target_motion; if (m_enable_brake_keeping_before_stop == false) { return output_motion; } - // const auto stop_idx = motion_utils::searchZeroVelocityIndex(traj.points); + const auto traj = control_data.interpolated_traj; + const auto stop_idx = motion_utils::searchZeroVelocityIndex(traj.points); if (!stop_idx) { return output_motion; @@ -889,13 +983,13 @@ PidLongitudinalController::Motion PidLongitudinalController::keepBrakeBeforeStop return output_motion; } -autoware_auto_planning_msgs::msg::TrajectoryPoint -PidLongitudinalController::calcInterpolatedTargetValue( +std::pair +PidLongitudinalController::calcInterpolatedTrajPointAndSegment( const autoware_auto_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose) const { if (traj.points.size() == 1) { - return traj.points.at(0); + return std::make_pair(traj.points.at(0), 0); } // apply linear interpolation @@ -903,69 +997,87 @@ PidLongitudinalController::calcInterpolatedTargetValue( traj.points, pose, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); } -double PidLongitudinalController::predictedVelocityInTargetPoint( +PidLongitudinalController::StateAfterDelay PidLongitudinalController::predictedStateAfterDelay( const Motion current_motion, const double delay_compensation_time) const { const double current_vel = current_motion.vel; const double current_acc = current_motion.acc; - - if (std::fabs(current_vel) < 1e-01) { // when velocity is low, no prediction - return current_vel; - } - - const double current_vel_abs = std::fabs(current_vel); - if (m_ctrl_cmd_vec.size() == 0) { - const double pred_vel = current_vel + current_acc * delay_compensation_time; + double running_distance = 0.0; + double pred_vel = current_vel; + double pred_acc = current_acc; + + if (m_ctrl_cmd_vec.empty() || m_current_operation_mode.mode != OperationModeState::AUTONOMOUS) { + // check time to stop + const double time_to_stop = -current_vel / current_acc; + const double delay_time_calculation = + time_to_stop > 0.0 && time_to_stop < delay_compensation_time ? time_to_stop + : delay_compensation_time; + // simple linear prediction + pred_vel = current_vel + current_acc * delay_time_calculation; + running_distance = std::abs( + delay_time_calculation * current_vel + + 0.5 * current_acc * delay_time_calculation * delay_time_calculation); // avoid to change sign of current_vel and pred_vel - return pred_vel > 0 ? std::copysign(pred_vel, current_vel) : 0.0; + return StateAfterDelay{pred_vel, pred_acc, running_distance}; } - double pred_vel = current_vel_abs; - - const auto past_delay_time = - clock_->now() - rclcpp::Duration::from_seconds(delay_compensation_time); for (std::size_t i = 0; i < m_ctrl_cmd_vec.size(); ++i) { - if ((clock_->now() - m_ctrl_cmd_vec.at(i).stamp).seconds() < m_delay_compensation_time) { - if (i == 0) { - // size of m_ctrl_cmd_vec is less than m_delay_compensation_time - pred_vel = current_vel_abs + - static_cast(m_ctrl_cmd_vec.at(i).acceleration) * delay_compensation_time; - return pred_vel > 0 ? std::copysign(pred_vel, current_vel) : 0.0; - } + if ((clock_->now() - m_ctrl_cmd_vec.at(i).stamp).seconds() < delay_compensation_time) { // add velocity to accel * dt - const double acc = m_ctrl_cmd_vec.at(i - 1).acceleration; - const auto curr_time_i = rclcpp::Time(m_ctrl_cmd_vec.at(i).stamp); - const double time_to_next_acc = std::min( - (curr_time_i - rclcpp::Time(m_ctrl_cmd_vec.at(i - 1).stamp)).seconds(), - (curr_time_i - past_delay_time).seconds()); - pred_vel += acc * time_to_next_acc; + const double time_to_next_acc = + (i == m_ctrl_cmd_vec.size() - 1) + ? std::min( + (clock_->now() - m_ctrl_cmd_vec.back().stamp).seconds(), delay_compensation_time) + : std::min( + (rclcpp::Time(m_ctrl_cmd_vec.at(i + 1).stamp) - + rclcpp::Time(m_ctrl_cmd_vec.at(i).stamp)) + .seconds(), + delay_compensation_time); + const double acc = m_ctrl_cmd_vec.at(i).acceleration; + // because acc_cmd is positive when vehicle is running backward + pred_acc = std::copysignf(1.0, static_cast(pred_vel)) * acc; + running_distance += std::abs( + std::abs(pred_vel) * time_to_next_acc + 0.5 * acc * time_to_next_acc * time_to_next_acc); + pred_vel += pred_vel < 0.0 ? (-acc * time_to_next_acc) : (acc * time_to_next_acc); + if (pred_vel / current_vel < 0.0) { + // sign of velocity is changed + pred_vel = 0.0; + break; + } } } - const double last_acc = m_ctrl_cmd_vec.at(m_ctrl_cmd_vec.size() - 1).acceleration; - const double time_to_current = - (clock_->now() - m_ctrl_cmd_vec.at(m_ctrl_cmd_vec.size() - 1).stamp).seconds(); - pred_vel += last_acc * time_to_current; - - // avoid to change sign of current_vel and pred_vel - return pred_vel > 0 ? std::copysign(pred_vel, current_vel) : 0.0; + return StateAfterDelay{pred_vel, pred_acc, running_distance}; } -double PidLongitudinalController::applyVelocityFeedback( - const Motion target_motion, const double dt, const double current_vel, const Shift & shift) +double PidLongitudinalController::applyVelocityFeedback(const ControlData & control_data) { // NOTE: Acceleration command is always positive even if the ego drives backward. - const double vel_sign = (shift == Shift::Forward) ? 1.0 : (shift == Shift::Reverse ? -1.0 : 0.0); + const double vel_sign = (control_data.shift == Shift::Forward) + ? 1.0 + : (control_data.shift == Shift::Reverse ? -1.0 : 0.0); + const double current_vel = control_data.current_motion.vel; + const auto target_motion = Motion{ + control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps, + control_data.interpolated_traj.points.at(control_data.target_idx).acceleration_mps2}; 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); const double pid_acc = - m_pid_vel.calculate(error_vel_filtered, dt, enable_integration, pid_contributions); + m_pid_vel.calculate(error_vel_filtered, control_data.dt, enable_integration, pid_contributions); // Feedforward scaling: // This is for the coordinate conversion where feedforward is applied, from Time to Arclength. @@ -976,7 +1088,8 @@ double PidLongitudinalController::applyVelocityFeedback( constexpr double ff_scale_min = 0.5; // for safety const double ff_scale = std::clamp( std::abs(current_vel) / std::max(std::abs(target_motion.vel), 0.1), ff_scale_min, ff_scale_max); - const double ff_acc = target_motion.acc * ff_scale; + const double ff_acc = + control_data.interpolated_traj.points.at(control_data.target_idx).acceleration_mps2 * ff_scale; const double feedback_acc = ff_acc + pid_acc; @@ -1003,21 +1116,25 @@ void PidLongitudinalController::updatePitchDebugValues( m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_TRAJ_DEG, traj_pitch * to_degrees); } -void PidLongitudinalController::updateDebugVelAcc( - const Motion & target_motion, const geometry_msgs::msg::Pose & current_pose, - const ControlData & control_data) +void PidLongitudinalController::updateDebugVelAcc(const ControlData & control_data) { - const double current_vel = control_data.current_motion.vel; - - const auto interpolated_point = calcInterpolatedTargetValue(m_trajectory, current_pose); - - m_debug_values.setValues(DebugValues::TYPE::CURRENT_VEL, current_vel); - m_debug_values.setValues(DebugValues::TYPE::TARGET_VEL, target_motion.vel); - m_debug_values.setValues(DebugValues::TYPE::TARGET_ACC, target_motion.acc); + m_debug_values.setValues(DebugValues::TYPE::CURRENT_VEL, control_data.current_motion.vel); + m_debug_values.setValues( + DebugValues::TYPE::TARGET_VEL, + control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps); m_debug_values.setValues( - DebugValues::TYPE::NEAREST_VEL, interpolated_point.longitudinal_velocity_mps); - m_debug_values.setValues(DebugValues::TYPE::NEAREST_ACC, interpolated_point.acceleration_mps2); - m_debug_values.setValues(DebugValues::TYPE::ERROR_VEL, target_motion.vel - current_vel); + DebugValues::TYPE::TARGET_ACC, + control_data.interpolated_traj.points.at(control_data.target_idx).acceleration_mps2); + m_debug_values.setValues( + DebugValues::TYPE::NEAREST_VEL, + control_data.interpolated_traj.points.at(control_data.nearest_idx).longitudinal_velocity_mps); + m_debug_values.setValues( + DebugValues::TYPE::NEAREST_ACC, + control_data.interpolated_traj.points.at(control_data.nearest_idx).acceleration_mps2); + m_debug_values.setValues( + DebugValues::TYPE::ERROR_VEL, + control_data.interpolated_traj.points.at(control_data.nearest_idx).longitudinal_velocity_mps - + control_data.current_motion.vel); } void PidLongitudinalController::setupDiagnosticUpdater() @@ -1059,4 +1176,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/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp index 5c7698180f82b..02bcce8c91c4b 100644 --- a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include "gtest/gtest.h" +#include "interpolation/spherical_linear_interpolation.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "pid_longitudinal_controller/longitudinal_controller_utils.hpp" #include "tf2/LinearMath/Quaternion.h" @@ -130,32 +132,30 @@ TEST(TestLongitudinalControllerUtils, getPitchByTraj) point.pose.position.z = 0.0; traj.points.push_back(point); // non stopping trajectory: stop distance = trajectory length - point.pose.position.x = 1.0; + point.pose.position.x = 0.6; point.pose.position.y = 0.0; - point.pose.position.z = 1.0; + point.pose.position.z = 0.8; traj.points.push_back(point); - point.pose.position.x = 2.0; + point.pose.position.x = 1.2; point.pose.position.y = 0.0; point.pose.position.z = 0.0; traj.points.push_back(point); - point.pose.position.x = 3.0; + point.pose.position.x = 1.8; point.pose.position.y = 0.0; - point.pose.position.z = 0.5; + point.pose.position.z = 0.8; traj.points.push_back(point); size_t closest_idx = 0; EXPECT_DOUBLE_EQ( - std::abs(longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base)), M_PI_4); + longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base), std::atan2(0.8, 0.6)); closest_idx = 1; EXPECT_DOUBLE_EQ( - std::abs(longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base)), M_PI_4); + longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base), std::atan2(-0.8, 0.6)); closest_idx = 2; EXPECT_DOUBLE_EQ( - std::abs(longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base)), - std::atan2(0.5, 1)); + longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base), std::atan2(0.8, 0.6)); closest_idx = 3; EXPECT_DOUBLE_EQ( - std::abs(longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base)), - std::atan2(0.5, 1)); + longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base), std::atan2(0.8, 0.6)); } TEST(TestLongitudinalControllerUtils, calcPoseAfterTimeDelay) @@ -303,7 +303,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) o_to.setRPY(M_PI_4, M_PI_4, M_PI_4); ratio = 0.0; - result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, 0.0); @@ -311,7 +311,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) EXPECT_DOUBLE_EQ(yaw, 0.0); ratio = 1.0; - result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, M_PI_4); @@ -320,7 +320,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) ratio = 0.5; o_to.setRPY(M_PI_4, 0.0, 0.0); - result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, M_PI_4 / 2); @@ -328,7 +328,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) EXPECT_DOUBLE_EQ(yaw, 0.0); o_to.setRPY(0.0, M_PI_4, 0.0); - result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, 0.0); @@ -336,7 +336,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) EXPECT_DOUBLE_EQ(yaw, 0.0); o_to.setRPY(0.0, 0.0, M_PI_4); - result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, 0.0); @@ -353,95 +353,113 @@ TEST(TestLongitudinalControllerUtils, lerpTrajectoryPoint) TrajectoryPoint p; p.pose.position.x = 0.0; p.pose.position.y = 0.0; + p.pose.position.z = 0.0; p.longitudinal_velocity_mps = 10.0; p.acceleration_mps2 = 10.0; points.push_back(p); p.pose.position.x = 1.0; p.pose.position.y = 0.0; + p.pose.position.z = 0.0; p.longitudinal_velocity_mps = 20.0; p.acceleration_mps2 = 20.0; points.push_back(p); p.pose.position.x = 1.0; p.pose.position.y = 1.0; + p.pose.position.z = 1.0; p.longitudinal_velocity_mps = 30.0; p.acceleration_mps2 = 30.0; points.push_back(p); p.pose.position.x = 2.0; p.pose.position.y = 1.0; + p.pose.position.z = 2.0; p.longitudinal_velocity_mps = 40.0; p.acceleration_mps2 = 40.0; points.push_back(p); - TrajectoryPoint result; Pose pose; double max_dist = 3.0; double max_yaw = 0.7; // Points on the trajectory gives back the original trajectory points values pose.position.x = 0.0; pose.position.y = 0.0; - result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); - EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); - EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err); - EXPECT_NEAR(result.longitudinal_velocity_mps, 10.0, abs_err); - EXPECT_NEAR(result.acceleration_mps2, 10.0, abs_err); + pose.position.z = 0.0; + + auto result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); + EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.first.pose.position.y, pose.position.y, abs_err); + EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err); + EXPECT_NEAR(result.first.longitudinal_velocity_mps, 10.0, abs_err); + EXPECT_NEAR(result.first.acceleration_mps2, 10.0, abs_err); pose.position.x = 1.0; pose.position.y = 0.0; + pose.position.z = 0.0; result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); - EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); - EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err); - EXPECT_NEAR(result.longitudinal_velocity_mps, 20.0, abs_err); - EXPECT_NEAR(result.acceleration_mps2, 20.0, abs_err); + EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.first.pose.position.y, pose.position.y, abs_err); + EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err); + EXPECT_NEAR(result.first.longitudinal_velocity_mps, 20.0, abs_err); + EXPECT_NEAR(result.first.acceleration_mps2, 20.0, abs_err); pose.position.x = 1.0; pose.position.y = 1.0; + pose.position.z = 1.0; result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); - EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); - EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err); - EXPECT_NEAR(result.longitudinal_velocity_mps, 30.0, abs_err); - EXPECT_NEAR(result.acceleration_mps2, 30.0, abs_err); + EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.first.pose.position.y, pose.position.y, abs_err); + EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err); + EXPECT_NEAR(result.first.longitudinal_velocity_mps, 30.0, abs_err); + EXPECT_NEAR(result.first.acceleration_mps2, 30.0, abs_err); pose.position.x = 2.0; pose.position.y = 1.0; + pose.position.z = 2.0; result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); - EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); - EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err); - EXPECT_NEAR(result.longitudinal_velocity_mps, 40.0, abs_err); - EXPECT_NEAR(result.acceleration_mps2, 40.0, abs_err); + EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.first.pose.position.y, pose.position.y, abs_err); + EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err); + EXPECT_NEAR(result.first.longitudinal_velocity_mps, 40.0, abs_err); + EXPECT_NEAR(result.first.acceleration_mps2, 40.0, abs_err); // Interpolate between trajectory points pose.position.x = 0.5; pose.position.y = 0.0; + pose.position.z = 0.0; result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); - EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); - EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err); - EXPECT_NEAR(result.longitudinal_velocity_mps, 15.0, abs_err); - EXPECT_NEAR(result.acceleration_mps2, 15.0, abs_err); + EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.first.pose.position.y, pose.position.y, abs_err); + EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err); + EXPECT_NEAR(result.first.longitudinal_velocity_mps, 15.0, abs_err); + EXPECT_NEAR(result.first.acceleration_mps2, 15.0, abs_err); pose.position.x = 0.75; pose.position.y = 0.0; + pose.position.z = 0.0; result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); - EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); - EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err); - EXPECT_NEAR(result.longitudinal_velocity_mps, 17.5, abs_err); - EXPECT_NEAR(result.acceleration_mps2, 17.5, abs_err); + EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.first.pose.position.y, pose.position.y, abs_err); + EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err); + EXPECT_NEAR(result.first.longitudinal_velocity_mps, 17.5, abs_err); + EXPECT_NEAR(result.first.acceleration_mps2, 17.5, abs_err); // Interpolate away from the trajectory (interpolated point is projected) pose.position.x = 0.5; pose.position.y = -1.0; result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); - EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); - EXPECT_NEAR(result.pose.position.y, 0.0, abs_err); - EXPECT_NEAR(result.longitudinal_velocity_mps, 15.0, abs_err); - EXPECT_NEAR(result.acceleration_mps2, 15.0, abs_err); + EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.first.pose.position.y, 0.0, abs_err); + EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err); + EXPECT_NEAR(result.first.longitudinal_velocity_mps, 15.0, abs_err); + EXPECT_NEAR(result.first.acceleration_mps2, 15.0, abs_err); // Ambiguous projections: possibility with the lowest index is used pose.position.x = 0.5; pose.position.y = 0.5; result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); - EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); - EXPECT_NEAR(result.pose.position.y, 0.0, abs_err); - EXPECT_NEAR(result.longitudinal_velocity_mps, 15.0, abs_err); - EXPECT_NEAR(result.acceleration_mps2, 15.0, abs_err); + EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.first.pose.position.y, 0.0, abs_err); + EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err); + EXPECT_NEAR(result.first.longitudinal_velocity_mps, 15.0, abs_err); + EXPECT_NEAR(result.first.acceleration_mps2, 15.0, abs_err); } TEST(TestLongitudinalControllerUtils, applyDiffLimitFilter) @@ -484,3 +502,63 @@ TEST(TestLongitudinalControllerUtils, applyDiffLimitFilter) prev = new_val; } } + +TEST(TestLongitudinalControllerUtils, findTrajectoryPoseAfterDistance) +{ + using autoware_auto_planning_msgs::msg::Trajectory; + using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using geometry_msgs::msg::Pose; + const double abs_err = 1e-5; + Trajectory traj; + TrajectoryPoint point; + point.pose.position.x = 0.0; + point.pose.position.y = 0.0; + point.pose.position.z = 0.0; + traj.points.push_back(point); + point.pose.position.x = 1.0; + point.pose.position.y = 0.0; + point.pose.position.z = 0.0; + traj.points.push_back(point); + point.pose.position.x = 1.0; + point.pose.position.y = 1.0; + point.pose.position.z = 1.0; + traj.points.push_back(point); + point.pose.position.x = 2.0; + point.pose.position.y = 1.0; + point.pose.position.z = 2.0; + traj.points.push_back(point); + size_t src_idx = 0; + double distance = 0.0; + Pose result = longitudinal_utils::findTrajectoryPoseAfterDistance(src_idx, distance, traj); + EXPECT_NEAR(result.position.x, 0.0, abs_err); + EXPECT_NEAR(result.position.y, 0.0, abs_err); + EXPECT_NEAR(result.position.z, 0.0, abs_err); + + src_idx = 0; + distance = 0.5; + result = longitudinal_utils::findTrajectoryPoseAfterDistance(src_idx, distance, traj); + EXPECT_NEAR(result.position.x, 0.5, abs_err); + EXPECT_NEAR(result.position.y, 0.0, abs_err); + EXPECT_NEAR(result.position.z, 0.0, abs_err); + + src_idx = 0; + distance = 1.0; + result = longitudinal_utils::findTrajectoryPoseAfterDistance(src_idx, distance, traj); + EXPECT_NEAR(result.position.x, 1.0, abs_err); + EXPECT_NEAR(result.position.y, 0.0, abs_err); + EXPECT_NEAR(result.position.z, 0.0, abs_err); + + src_idx = 0; + distance = 1.5; + result = longitudinal_utils::findTrajectoryPoseAfterDistance(src_idx, distance, traj); + EXPECT_NEAR(result.position.x, 1.0, abs_err); + EXPECT_NEAR(result.position.y, 1.0 / (2.0 * sqrt(2.0)), abs_err); + EXPECT_NEAR(result.position.z, 1.0 / (2.0 * sqrt(2.0)), abs_err); + + src_idx = 0; + distance = 20.0; // beyond the trajectory, should return the last point + result = longitudinal_utils::findTrajectoryPoseAfterDistance(src_idx, distance, traj); + EXPECT_NEAR(result.position.x, 2.0, abs_err); + EXPECT_NEAR(result.position.y, 1.0, abs_err); + EXPECT_NEAR(result.position.z, 2.0, abs_err); +} diff --git a/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp index d21b11e5b68de..1d7791955b576 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp @@ -15,8 +15,8 @@ #ifndef PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ #define PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ +#include #include -#include #include #include #include diff --git a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp index e3c3b5cccfb8f..3ce5728521141 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp index daaebaacde8de..ca0d77140b195 100644 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp +++ b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp @@ -38,7 +38,7 @@ #include "trajectory_follower_base/lateral_controller_base.hpp" #include -#include +#include #include #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" 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/CMakeLists.txt b/control/trajectory_follower_node/CMakeLists.txt index c64a51b3dde7f..653b02eb39ed6 100644 --- a/control/trajectory_follower_node/CMakeLists.txt +++ b/control/trajectory_follower_node/CMakeLists.txt @@ -62,4 +62,5 @@ ament_auto_package( param test launch + config ) diff --git a/control/trajectory_follower_node/config/simple_trajectory_follower.param.yaml b/control/trajectory_follower_node/config/simple_trajectory_follower.param.yaml new file mode 100644 index 0000000000000..8194990a136ea --- /dev/null +++ b/control/trajectory_follower_node/config/simple_trajectory_follower.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + use_external_target_vel: $(var use_external_target_vel) + external_target_vel: $(var external_target_vel) + lateral_deviation: $(var lateral_deviation) diff --git a/control/trajectory_follower_node/launch/simple_trajectory_follower.launch.xml b/control/trajectory_follower_node/launch/simple_trajectory_follower.launch.xml index 8ce799e17e632..0c4c3faac73a9 100644 --- a/control/trajectory_follower_node/launch/simple_trajectory_follower.launch.xml +++ b/control/trajectory_follower_node/launch/simple_trajectory_follower.launch.xml @@ -5,9 +5,7 @@ - - - + diff --git a/control/trajectory_follower_node/param/lateral/mpc.param.yaml b/control/trajectory_follower_node/param/lateral/mpc.param.yaml index 4222082d40deb..9998b6aadf656 100644 --- a/control/trajectory_follower_node/param/lateral/mpc.param.yaml +++ b/control/trajectory_follower_node/param/lateral/mpc.param.yaml @@ -74,3 +74,5 @@ update_steer_threshold: 0.035 average_num: 1000 steering_offset_limit: 0.02 + + debug_publish_predicted_trajectory: false # publish debug predicted trajectory in Frenet coordinate 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/config/vehicle_cmd_gate.param.yaml b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml index 7ad685217f13d..54c87f45b6a96 100644 --- a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml +++ b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml @@ -3,7 +3,7 @@ update_rate: 10.0 system_emergency_heartbeat_timeout: 0.5 use_emergency_handling: false - check_external_emergency_heartbeat: false + check_external_emergency_heartbeat: $(var check_external_emergency_heartbeat) use_start_request: false enable_cmd_limit_filter: true filter_activated_count_threshold: 5 diff --git a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml b/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml index a6cf7cdcf8c08..c5368276b488a 100644 --- a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml +++ b/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml @@ -1,8 +1,6 @@ - - @@ -44,10 +42,7 @@ - + - - - 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/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index 1bed4ee67f412..aec2ede53d9e7 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -430,13 +430,17 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands) // Check engage if (!is_engaged_) { - filtered_commands.control = createStopControlCmd(); + filtered_commands.control.longitudinal = createLongitudinalStopControlCmd(); } // Check pause. Place this check after all other checks as it needs the final output. adapi_pause_->update(filtered_commands.control); if (adapi_pause_->is_paused()) { - filtered_commands.control = createStopControlCmd(); + if (is_engaged_) { + filtered_commands.control.longitudinal = createLongitudinalStopControlCmd(); + } else { + filtered_commands.control = createStopControlCmd(); + } } // Check if command filtering option is enable @@ -599,6 +603,17 @@ AckermannControlCommand VehicleCmdGate::createStopControlCmd() const return cmd; } +LongitudinalCommand VehicleCmdGate::createLongitudinalStopControlCmd() const +{ + LongitudinalCommand cmd; + const auto t = this->now(); + cmd.stamp = t; + cmd.speed = 0.0; + cmd.acceleration = stop_hold_acceleration_; + + return cmd; +} + AckermannControlCommand VehicleCmdGate::createEmergencyStopControlCmd() const { AckermannControlCommand cmd; diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index c36aab240ae79..b79c58d120443 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -55,6 +55,7 @@ namespace vehicle_cmd_gate using autoware_adapi_v1_msgs::msg::MrmState; using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_auto_control_msgs::msg::LongitudinalCommand; using autoware_auto_vehicle_msgs::msg::GearCommand; using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; using autoware_auto_vehicle_msgs::msg::SteeringReport; @@ -220,6 +221,7 @@ class VehicleCmdGate : public rclcpp::Node // Algorithm AckermannControlCommand prev_control_cmd_; AckermannControlCommand createStopControlCmd() const; + LongitudinalCommand createLongitudinalStopControlCmd() const; AckermannControlCommand createEmergencyStopControlCmd() const; std::shared_ptr prev_time_; diff --git a/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp b/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp index f02235ed1ecbf..f9f3087a6b69c 100644 --- a/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp +++ b/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp @@ -331,6 +331,9 @@ std::shared_ptr generateNode() {"--ros-args", "--params-file", vehicle_cmd_gate_dir + "/config/vehicle_cmd_gate.param.yaml", "--ros-args", "--params-file", vehicle_info_util_dir + "/config/vehicle_info.param.yaml"}); + node_options.append_parameter_override( + "check_external_emergency_heartbeat", true); // This parameter has to be set when launching. + const auto override = [&](const auto s, const std::vector v) { node_options.append_parameter_override>(s, v); }; diff --git a/docs/assets/images/autoware_universe_front.png b/docs/assets/images/autoware_universe_front.png new file mode 100644 index 0000000000000..e03e35d12f78b Binary files /dev/null and b/docs/assets/images/autoware_universe_front.png differ diff --git a/evaluator/diagnostic_converter/include/converter_node.hpp b/evaluator/diagnostic_converter/include/diagnostic_converter/converter_node.hpp similarity index 93% rename from evaluator/diagnostic_converter/include/converter_node.hpp rename to evaluator/diagnostic_converter/include/diagnostic_converter/converter_node.hpp index 53c762dac0ffe..59bb02ebf301f 100644 --- a/evaluator/diagnostic_converter/include/converter_node.hpp +++ b/evaluator/diagnostic_converter/include/diagnostic_converter/converter_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CONVERTER_NODE_HPP_ -#define CONVERTER_NODE_HPP_ +#ifndef DIAGNOSTIC_CONVERTER__CONVERTER_NODE_HPP_ +#define DIAGNOSTIC_CONVERTER__CONVERTER_NODE_HPP_ #include @@ -63,4 +63,4 @@ class DiagnosticConverter : public rclcpp::Node }; } // namespace diagnostic_converter -#endif // CONVERTER_NODE_HPP_ +#endif // DIAGNOSTIC_CONVERTER__CONVERTER_NODE_HPP_ diff --git a/evaluator/diagnostic_converter/src/converter_node.cpp b/evaluator/diagnostic_converter/src/converter_node.cpp index e61b19833d2fe..2a2574732694c 100644 --- a/evaluator/diagnostic_converter/src/converter_node.cpp +++ b/evaluator/diagnostic_converter/src/converter_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "converter_node.hpp" +#include "diagnostic_converter/converter_node.hpp" #include diff --git a/evaluator/diagnostic_converter/test/test_converter_node.cpp b/evaluator/diagnostic_converter/test/test_converter_node.cpp index 47f42d018ea71..167421f0777df 100644 --- a/evaluator/diagnostic_converter/test/test_converter_node.cpp +++ b/evaluator/diagnostic_converter/test/test_converter_node.cpp @@ -12,8 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "converter_node.hpp" -#include "gtest/gtest.h" +#include "diagnostic_converter/converter_node.hpp" #include @@ -21,6 +20,8 @@ #include "tier4_simulation_msgs/msg/user_defined_value.hpp" #include "tier4_simulation_msgs/msg/user_defined_value_type.hpp" +#include + #include #include #include diff --git a/evaluator/perception_online_evaluator/CMakeLists.txt b/evaluator/perception_online_evaluator/CMakeLists.txt new file mode 100644 index 0000000000000..f9cc0f4fa256c --- /dev/null +++ b/evaluator/perception_online_evaluator/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 3.14) +project(perception_online_evaluator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(pluginlib REQUIRED) + +find_package(glog REQUIRED) + +ament_auto_add_library(${PROJECT_NAME}_node SHARED + src/metrics_calculator.cpp + src/${PROJECT_NAME}_node.cpp + src/metrics/deviation_metrics.cpp + src/utils/marker_utils.cpp + src/utils/objects_filtering.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME}_node + PLUGIN "perception_diagnostics::PerceptionOnlineEvaluatorNode" + EXECUTABLE ${PROJECT_NAME} +) + +rclcpp_components_register_node(${PROJECT_NAME}_node + PLUGIN "perception_diagnostics::MotionEvaluatorNode" + EXECUTABLE motion_evaluator +) + +target_link_libraries(${PROJECT_NAME}_node glog::glog) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_perception_online_evaluator_node.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME}_node + ) +endif() + +ament_auto_package( + INSTALL_TO_SHARE + param + launch +) diff --git a/evaluator/perception_online_evaluator/README.md b/evaluator/perception_online_evaluator/README.md new file mode 100644 index 0000000000000..b801e5f418cef --- /dev/null +++ b/evaluator/perception_online_evaluator/README.md @@ -0,0 +1,43 @@ +# Perception Evaluator + +A node for evaluating the output of perception systems. + +## Purpose + +This module allows for the evaluation of how accurately perception results are generated without the need for annotations. It is capable of confirming performance and can evaluate results from a few seconds prior, enabling online execution. + +## Inner-workings / Algorithms + +- Calculates lateral deviation between the predicted path and the actual traveled trajectory. +- Calculates lateral deviation between the smoothed traveled trajectory and the perceived position to evaluate the stability of lateral position recognition. +- Calculates yaw deviation between the smoothed traveled trajectory and the perceived position to evaluate the stability of yaw recognition. + +## Inputs / Outputs + +| Name | Type | Description | +| ----------------- | ------------------------------------------------------ | ------------------------------------------------- | +| `~/input/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | The predicted objects to evaluate. | +| `~/metrics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostic information about perception accuracy. | +| `~/markers` | `visualization_msgs::msg::MarkerArray` | Visual markers for debugging and visualization. | + +## Parameters + +| Name | Type | Description | +| --------------------------------- | ------------ | ------------------------------------------------------------------------------------------------ | +| `selected_metrics` | List | Metrics to be evaluated, such as lateral deviation, yaw deviation, and predicted path deviation. | +| `smoothing_window_size` | Integer | Determines the window size for smoothing path, should be an odd number. | +| `prediction_time_horizons` | list[double] | Time horizons for prediction evaluation in seconds. | +| `target_object.*.check_deviation` | bool | Whether to check deviation for specific object types (car, truck, etc.). | +| `debug_marker.*` | bool | Debugging parameters for marker visualization (history path, predicted path, etc.). | + +## Assumptions / Known limits + +It is assumed that the current positions of PredictedObjects are reasonably accurate. + +## Future extensions / Unimplemented parts + +- Increase rate in recognition per class +- Metrics for objects with strange physical behavior (e.g., going through a fence) +- Metrics for splitting objects +- Metrics for problems with objects that are normally stationary but move +- Disappearing object metrics diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp new file mode 100644 index 0000000000000..da7a23b6980b6 --- /dev/null +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp @@ -0,0 +1,53 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PERCEPTION_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ + +#include "perception_online_evaluator/stat.hpp" + +#include +#include + +#include + +namespace perception_diagnostics +{ +namespace metrics +{ +using autoware_auto_perception_msgs::msg::PredictedPath; +using geometry_msgs::msg::Pose; + +/** + * @brief calculate lateral deviation of the given path from the reference path + * @param [in] ref_path reference path + * @param [in] pred_path predicted path + * @return calculated statistics + */ +double calcLateralDeviation(const std::vector & ref_path, const Pose & target_pose); + +/** + * @brief calculate yaw deviation of the given path from the reference path + * @param [in] ref_path reference path + * @param [in] pred_path predicted path + * @return calculated statistics + */ +double calcYawDeviation(const std::vector & ref_path, const Pose & target_pose); + +std::vector calcPredictedPathDeviation( + const std::vector & ref_path, const PredictedPath & pred_path); +} // namespace metrics +} // namespace perception_diagnostics + +#endif // PERCEPTION_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp new file mode 100644 index 0000000000000..8a2cddca476d4 --- /dev/null +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp @@ -0,0 +1,75 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_ + +#include "perception_online_evaluator/stat.hpp" + +#include +#include +#include +#include + +namespace perception_diagnostics +{ +/** + * @brief Enumeration of trajectory metrics + */ +enum class Metric { + lateral_deviation, + yaw_deviation, + predicted_path_deviation, + SIZE, +}; + +using MetricStatMap = std::unordered_map>; + +static const std::unordered_map str_to_metric = { + {"lateral_deviation", Metric::lateral_deviation}, + {"yaw_deviation", Metric::yaw_deviation}, + {"predicted_path_deviation", Metric::predicted_path_deviation}}; + +static const std::unordered_map metric_to_str = { + {Metric::lateral_deviation, "lateral_deviation"}, + {Metric::yaw_deviation, "yaw_deviation"}, + {Metric::predicted_path_deviation, "predicted_path_deviation"}}; + +// Metrics descriptions +static const std::unordered_map metric_descriptions = { + {Metric::lateral_deviation, "Lateral_deviation[m]"}, + {Metric::yaw_deviation, "Yaw_deviation[rad]"}, + {Metric::predicted_path_deviation, "Predicted_path_deviation[m]"}}; + +namespace details +{ +static struct CheckCorrectMaps +{ + CheckCorrectMaps() + { + if ( + str_to_metric.size() != static_cast(Metric::SIZE) || + metric_to_str.size() != static_cast(Metric::SIZE) || + metric_descriptions.size() != static_cast(Metric::SIZE)) { + std::cerr << "[metrics/metrics.hpp] Maps are not defined for all metrics: "; + std::cerr << str_to_metric.size() << " " << metric_to_str.size() << " " + << metric_descriptions.size() << std::endl; + } + } +} check; + +} // namespace details +} // namespace perception_diagnostics + +#endif // PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_ diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp new file mode 100644 index 0000000000000..dd6756a17f194 --- /dev/null +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp @@ -0,0 +1,141 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PERCEPTION_ONLINE_EVALUATOR__METRICS_CALCULATOR_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__METRICS_CALCULATOR_HPP_ + +#include "perception_online_evaluator/metrics/deviation_metrics.hpp" +#include "perception_online_evaluator/metrics/metric.hpp" +#include "perception_online_evaluator/parameters.hpp" +#include "perception_online_evaluator/stat.hpp" + +#include + +#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include + +#include +#include +#include +#include +#include + +namespace perception_diagnostics +{ +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using unique_identifier_msgs::msg::UUID; + +struct ObjectData +{ + PredictedObject object; + std::vector> path_pairs; + + std::vector getPredictedPath() const + { + std::vector path; + path.resize(path_pairs.size()); + std::transform( + path_pairs.begin(), path_pairs.end(), path.begin(), + [](const std::pair & pair) -> Pose { return pair.first; }); + return path; + } + + std::vector getHistoryPath() const + { + std::vector path; + path.resize(path_pairs.size()); + std::transform( + path_pairs.begin(), path_pairs.end(), path.begin(), + [](const std::pair & pair) -> Pose { return pair.second; }); + return path; + } +}; +using ObjectDataMap = std::unordered_map; + +// pair of history_path and smoothed_history_path for each object id +using HistoryPathMap = + std::unordered_map, std::vector>>; + +class MetricsCalculator +{ +public: + explicit MetricsCalculator(const std::shared_ptr & parameters) + : parameters_(parameters){}; + + /** + * @brief calculate + * @param [in] metric Metric enum value + * @return map of string describing the requested metric and the calculated value + */ + std::optional calculate(const Metric & metric) const; + + /** + * @brief set the dynamic objects used to calculate obstacle metrics + * @param [in] objects predicted objects + */ + void setPredictedObjects(const PredictedObjects & objects); + + HistoryPathMap getHistoryPathMap() const { return history_path_map_; } + ObjectDataMap getDebugObjectData() const { return debug_target_object_; } + +private: + std::shared_ptr parameters_; + + // Store predicted objects information and calculation results + std::unordered_map> object_map_; + HistoryPathMap history_path_map_; + + rclcpp::Time current_stamp_; + + // debug + mutable ObjectDataMap debug_target_object_; + + // Functions to calculate history path + void updateHistoryPath(); + std::vector averageFilterPath( + const std::vector & path, const size_t window_size) const; + std::vector generateHistoryPathWithPrev( + const std::vector & prev_history_path, const Pose & new_pose, const size_t window_size); + + // Update object data + void updateObjects( + const std::string uuid, const rclcpp::Time stamp, const PredictedObject & object); + void deleteOldObjects(const rclcpp::Time stamp); + + // Calculate metrics + MetricStatMap calcLateralDeviationMetrics(const PredictedObjects & objects) const; + MetricStatMap calcYawDeviationMetrics(const PredictedObjects & objects) const; + MetricStatMap calcPredictedPathDeviationMetrics(const PredictedObjects & objects) const; + Stat calcPredictedPathDeviationMetrics( + const PredictedObjects & objects, const double time_horizon) const; + + bool hasPassedTime(const rclcpp::Time stamp) const; + bool hasPassedTime(const std::string uuid, const rclcpp::Time stamp) const; + double getTimeDelay() const; + + // Extract object + rclcpp::Time getClosestStamp(const rclcpp::Time stamp) const; + std::optional getObjectByStamp( + const std::string uuid, const rclcpp::Time stamp) const; + PredictedObjects getObjectsByStamp(const rclcpp::Time stamp) const; + +}; // class MetricsCalculator + +} // namespace perception_diagnostics + +#endif // PERCEPTION_ONLINE_EVALUATOR__METRICS_CALCULATOR_HPP_ diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/parameters.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/parameters.hpp new file mode 100644 index 0000000000000..98cd3c25b71a3 --- /dev/null +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/parameters.hpp @@ -0,0 +1,58 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PERCEPTION_ONLINE_EVALUATOR__PARAMETERS_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__PARAMETERS_HPP_ + +#include "perception_online_evaluator/metrics/metric.hpp" + +#include +#include + +namespace perception_diagnostics +{ +/** + * @brief Enumeration of perception metrics + */ + +struct ObjectParameter +{ + bool check_deviation{false}; +}; + +struct DebugMarkerParameter +{ + bool show_history_path{false}; + bool show_history_path_arrows{false}; + bool show_smoothed_history_path{true}; + bool show_smoothed_history_path_arrows{false}; + bool show_predicted_path{true}; + bool show_predicted_path_gt{true}; + bool show_deviation_lines{true}; + bool show_object_polygon{true}; +}; + +struct Parameters +{ + std::vector metrics; + size_t smoothing_window_size{0}; + std::vector prediction_time_horizons; + DebugMarkerParameter debug_marker_parameters; + // parameters depend on object class + std::unordered_map object_parameters; +}; + +} // namespace perception_diagnostics + +#endif // PERCEPTION_ONLINE_EVALUATOR__PARAMETERS_HPP_ diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp new file mode 100644 index 0000000000000..b7535935ccd5f --- /dev/null +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp @@ -0,0 +1,91 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PERCEPTION_ONLINE_EVALUATOR__PERCEPTION_ONLINE_EVALUATOR_NODE_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__PERCEPTION_ONLINE_EVALUATOR_NODE_HPP_ + +#include "perception_online_evaluator/metrics_calculator.hpp" +#include "perception_online_evaluator/parameters.hpp" +#include "perception_online_evaluator/stat.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "diagnostic_msgs/msg/diagnostic_array.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include +#include +#include +#include +#include + +namespace perception_diagnostics +{ +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using diagnostic_msgs::msg::DiagnosticArray; +using diagnostic_msgs::msg::DiagnosticStatus; +using nav_msgs::msg::Odometry; + +using MarkerArray = visualization_msgs::msg::MarkerArray; + +/** + * @brief Node for perception evaluation + */ +class PerceptionOnlineEvaluatorNode : public rclcpp::Node +{ +public: + explicit PerceptionOnlineEvaluatorNode(const rclcpp::NodeOptions & node_options); + ~PerceptionOnlineEvaluatorNode() {} + + /** + * @brief callback on receiving a dynamic objects array + * @param [in] objects_msg received dynamic object array message + */ + void onObjects(const PredictedObjects::ConstSharedPtr objects_msg); + + DiagnosticStatus generateDiagnosticStatus( + const std::string metric, const Stat & metric_stat) const; + +private: + // Subscribers and publishers + rclcpp::Subscription::SharedPtr objects_sub_; + rclcpp::Publisher::SharedPtr metrics_pub_; + rclcpp::Publisher::SharedPtr pub_marker_; + + // TF + std::shared_ptr transform_listener_{nullptr}; + std::unique_ptr tf_buffer_; + + // Parameters + std::shared_ptr parameters_; + void initParameter(); + rcl_interfaces::msg::SetParametersResult onParameter( + const std::vector & parameters); + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + + // Metrics Calculator + MetricsCalculator metrics_calculator_; + std::deque stamps_; + void publishMetrics(); + + // Debug + void publishDebugMarker(); +}; +} // namespace perception_diagnostics + +#endif // PERCEPTION_ONLINE_EVALUATOR__PERCEPTION_ONLINE_EVALUATOR_NODE_HPP_ diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/stat.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/stat.hpp new file mode 100644 index 0000000000000..63494f90d02ee --- /dev/null +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/stat.hpp @@ -0,0 +1,93 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#ifndef PERCEPTION_ONLINE_EVALUATOR__STAT_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__STAT_HPP_ + +namespace perception_diagnostics +{ +/** + * @brief class to incrementally build statistics + * @typedef T type of the values (default to double) + */ +template +class Stat +{ +public: + /** + * @brief add a value + * @param value value to add + */ + void add(const T & value) + { + if (value < min_) { + min_ = value; + } + if (value > max_) { + max_ = value; + } + ++count_; + mean_ = mean_ + (value - mean_) / count_; + } + + /** + * @brief get the mean value + */ + long double mean() const { return mean_; } + + /** + * @brief get the minimum value + */ + T min() const { return min_; } + + /** + * @brief get the maximum value + */ + T max() const { return max_; } + + /** + * @brief get the number of values used to build this statistic + */ + unsigned int count() const { return count_; } + + template + friend std::ostream & operator<<(std::ostream & os, const Stat & stat); + +private: + T min_ = std::numeric_limits::max(); + T max_ = std::numeric_limits::min(); + long double mean_ = 0.0; + unsigned int count_ = 0; +}; + +/** + * @brief overload << operator for easy print to output stream + */ +template +std::ostream & operator<<(std::ostream & os, const Stat & stat) +{ + if (stat.count() == 0) { + os << "None None None"; + } else { + os << stat.min() << " " << stat.max() << " " << stat.mean(); + } + return os; +} + +} // namespace perception_diagnostics + +#endif // PERCEPTION_ONLINE_EVALUATOR__STAT_HPP_ diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp new file mode 100644 index 0000000000000..3ac4c1db9efbd --- /dev/null +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp @@ -0,0 +1,82 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PERCEPTION_ONLINE_EVALUATOR__UTILS__MARKER_UTILS_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__UTILS__MARKER_UTILS_HPP_ + +#include + +#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include +#include + +#include + +#include +#include +#include + +namespace marker_utils +{ + +using autoware_auto_perception_msgs::msg::PredictedObject; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using std_msgs::msg::ColorRGBA; +using tier4_autoware_utils::Polygon2d; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +inline int64_t bitShift(int64_t original_id) +{ + return original_id << (sizeof(int32_t) * 8 / 2); +} + +void addFootprintMarker( + visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose, + const vehicle_info_util::VehicleInfo & vehicle_info); + +MarkerArray createFootprintMarkerArray( + const Pose & base_link_pose, const vehicle_info_util::VehicleInfo vehicle_info, + const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); + +MarkerArray createPointsMarkerArray( + const std::vector points, const std::string & ns, const int32_t id, const float r, + const float g, const float b); +MarkerArray createPointsMarkerArray( + const std::vector poses, const std::string & ns, const int32_t id, const float r, + const float g, const float b); + +MarkerArray createPoseMarkerArray( + const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g, + const float & b); + +MarkerArray createPosesMarkerArray( + const std::vector poses, std::string && ns, const float & r, const float & g, + const float & b, const float & x_scale = 0.5, const float & y_scale = 0.2, + const float & z_scale = 0.2); + +std_msgs::msg::ColorRGBA createColorFromString(const std::string & str); + +MarkerArray createObjectPolygonMarkerArray( + const PredictedObject & object, std::string && ns, const int32_t & id, const float & r, + const float & g, const float & b); + +MarkerArray createDeviationLines( + const std::vector poses1, const std::vector poses2, const std::string & ns, + const float r, const float g, const float b); + +} // namespace marker_utils + +#endif // PERCEPTION_ONLINE_EVALUATOR__UTILS__MARKER_UTILS_HPP_ diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp new file mode 100644 index 0000000000000..5d4238f45dec9 --- /dev/null +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp @@ -0,0 +1,128 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PERCEPTION_ONLINE_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_ + +#include "perception_online_evaluator/parameters.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +/** + * most of this file is copied from objects_filtering.hpp in safety_check of behavior_path_planner + */ + +namespace perception_diagnostics +{ + +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; + +std::uint8_t getHighestProbLabel(const std::vector & classification); + +/** + * @brief Specifies which object class should be checked. + */ +struct ObjectTypesToCheck +{ + bool check_car{true}; ///< Check for cars. + bool check_truck{true}; ///< Check for trucks. + bool check_bus{true}; ///< Check for buses. + bool check_trailer{true}; ///< Check for trailers. + bool check_unknown{true}; ///< Check for unknown object types. + bool check_bicycle{true}; ///< Check for bicycles. + bool check_motorcycle{true}; ///< Check for motorcycles. + bool check_pedestrian{true}; ///< Check for pedestrians. +}; + +/** + * @brief Determines whether the predicted object type matches any of the target object types + * specified by the user. + * + * @param object The predicted object whose type is to be checked. + * @param target_object_types A structure containing boolean flags for each object type that the + * user is interested in checking. + * + * @return Returns true if the predicted object's highest probability label matches any of the + * specified target object types. + */ +bool isTargetObjectType( + const PredictedObject & object, const ObjectTypesToCheck & target_object_types); + +/** + * @brief Filters objects in the 'selected' container based on the provided filter function. + * + * This function partitions the 'selected' container based on the 'filter' function + * and moves objects that satisfy the filter condition to the 'removed' container. + * + * @tparam Func The type of the filter function. + * @param selected [in,out] The container of objects to be filtered. + * @param removed [out] The container where objects not satisfying the filter condition are moved. + * @param filter The filter function that determines whether an object should be removed. + */ +template +void filterObjects(PredictedObjects & selected, PredictedObjects & removed, Func filter) +{ + auto partitioned = std::partition(selected.objects.begin(), selected.objects.end(), filter); + removed.objects.insert(removed.objects.end(), partitioned, selected.objects.end()); + selected.objects.erase(partitioned, selected.objects.end()); +} + +/** + * @brief Filters objects in the 'objects' container based on the provided filter function. + * + * This function is an overload that simplifies filtering when you don't need to specify a + * separate 'removed' container. It internally creates a 'removed_objects' container and calls the + * main 'filterObjects' function. + * + * @tparam Func The type of the filter function. + * @param objects [in,out] The container of objects to be filtered. + * @param filter The filter function that determines whether an object should be removed. + */ +template +void filterObjects(PredictedObjects & objects, Func filter) +{ + [[maybe_unused]] PredictedObjects removed_objects{}; + filterObjects(objects, removed_objects, filter); +} + +/** + * @brief Filters the provided objects based on their classification. + * + * @param objects The predicted objects to be filtered. + * @param target_object_types The types of objects to retain after filtering. + */ +void filterObjectsByClass( + PredictedObjects & objects, const ObjectTypesToCheck & target_object_types); + +/** + * @brief Filters the provided objects based on their classification. + * + * @param objects The predicted objects to be filtered. + * @param params The parameters for each object class. + */ +void filterDeviationCheckObjects( + PredictedObjects & objects, const std::unordered_map & params); + +} // namespace perception_diagnostics + +#endif // PERCEPTION_ONLINE_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_ diff --git a/evaluator/perception_online_evaluator/launch/motion_evaluator.launch.xml b/evaluator/perception_online_evaluator/launch/motion_evaluator.launch.xml new file mode 100644 index 0000000000000..08c4e11126ec1 --- /dev/null +++ b/evaluator/perception_online_evaluator/launch/motion_evaluator.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/evaluator/perception_online_evaluator/launch/perception_online_evaluator.launch.xml b/evaluator/perception_online_evaluator/launch/perception_online_evaluator.launch.xml new file mode 100644 index 0000000000000..2ef179dbe3ff9 --- /dev/null +++ b/evaluator/perception_online_evaluator/launch/perception_online_evaluator.launch.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/evaluator/perception_online_evaluator/package.xml b/evaluator/perception_online_evaluator/package.xml new file mode 100644 index 0000000000000..bc0f7ef82049b --- /dev/null +++ b/evaluator/perception_online_evaluator/package.xml @@ -0,0 +1,42 @@ + + + + perception_online_evaluator + 0.1.0 + ROS 2 node for evaluating perception + Fumiya Watanabe + Kosuke Takeuchi + Kotaro Uetake + Kyoichi Sugahara + Shunsuke Miura + Yoshi Ri + Apache License 2.0 + + Kosuke Takeuchi + + ament_cmake_auto + autoware_cmake + + autoware_perception_msgs + diagnostic_msgs + eigen + geometry_msgs + libgoogle-glog-dev + motion_utils + nav_msgs + pluginlib + rclcpp + rclcpp_components + tf2 + tf2_ros + tier4_autoware_utils + vehicle_info_util + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml b/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml new file mode 100644 index 0000000000000..6749bac81aa35 --- /dev/null +++ b/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml @@ -0,0 +1,39 @@ +/**: + ros__parameters: + selected_metrics: + - lateral_deviation + - yaw_deviation + - predicted_path_deviation + + # this should be an odd number, because it includes the target point + smoothing_window_size: 11 + + prediction_time_horizons: [1.0, 2.0, 3.0, 5.0] + + target_object: + car: + check_deviation: true + truck: + check_deviation: true + bus: + check_deviation: true + trailer: + check_deviation: true + bicycle: + check_deviation: true + motorcycle: + check_deviation: true + pedestrian: + check_deviation: true + unknown: + check_deviation: false + + debug_marker: + history_path: false + history_path_arrows: false + smoothed_history_path: true + smoothed_history_path_arrows: false + predicted_path: true + predicted_path_gt: true + deviation_lines: true + object_polygon: true diff --git a/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp new file mode 100644 index 0000000000000..7a9435444c065 --- /dev/null +++ b/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp @@ -0,0 +1,65 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "perception_online_evaluator/metrics/deviation_metrics.hpp" + +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/geometry/pose_deviation.hpp" + +#include + +namespace perception_diagnostics +{ +namespace metrics +{ + +double calcLateralDeviation(const std::vector & ref_path, const Pose & target_pose) +{ + if (ref_path.empty()) { + return 0.0; + } + + const size_t nearest_index = motion_utils::findNearestIndex(ref_path, target_pose.position); + return std::abs( + tier4_autoware_utils::calcLateralDeviation(ref_path[nearest_index], target_pose.position)); +} + +double calcYawDeviation(const std::vector & ref_path, const Pose & target_pose) +{ + if (ref_path.empty()) { + return 0.0; + } + + const size_t nearest_index = motion_utils::findNearestIndex(ref_path, target_pose.position); + return std::abs(tier4_autoware_utils::calcYawDeviation(ref_path[nearest_index], target_pose)); +} + +std::vector calcPredictedPathDeviation( + const std::vector & ref_path, const PredictedPath & pred_path) +{ + std::vector deviations; + + if (ref_path.empty() || pred_path.path.empty()) { + return {}; + } + for (const Pose & p : pred_path.path) { + const size_t nearest_index = motion_utils::findNearestIndex(ref_path, p.position); + deviations.push_back( + tier4_autoware_utils::calcDistance2d(ref_path[nearest_index].position, p.position)); + } + + return deviations; +} +} // namespace metrics +} // namespace perception_diagnostics diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp new file mode 100644 index 0000000000000..bec6d84fcd7dd --- /dev/null +++ b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp @@ -0,0 +1,458 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "perception_online_evaluator/metrics_calculator.hpp" + +#include "motion_utils/trajectory/trajectory.hpp" +#include "perception_online_evaluator/utils/objects_filtering.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include + +namespace perception_diagnostics +{ +std::optional MetricsCalculator::calculate(const Metric & metric) const +{ + if (object_map_.empty()) { + return {}; + } + + // time delay is max element of parameters_->prediction_time_horizons + const double time_delay = getTimeDelay(); + const auto target_stamp = current_stamp_ - rclcpp::Duration::from_seconds(time_delay); + if (!hasPassedTime(target_stamp)) { + return {}; + } + const auto target_objects = getObjectsByStamp(target_stamp); + + switch (metric) { + case Metric::lateral_deviation: + return calcLateralDeviationMetrics(target_objects); + case Metric::yaw_deviation: + return calcYawDeviationMetrics(target_objects); + case Metric::predicted_path_deviation: + return calcPredictedPathDeviationMetrics(target_objects); + default: + return {}; + } +} + +double MetricsCalculator::getTimeDelay() const +{ + const auto max_element_it = std::max_element( + parameters_->prediction_time_horizons.begin(), parameters_->prediction_time_horizons.end()); + if (max_element_it != parameters_->prediction_time_horizons.end()) { + return *max_element_it; + } + throw std::runtime_error("prediction_time_horizons is empty"); +} + +bool MetricsCalculator::hasPassedTime(const std::string uuid, const rclcpp::Time stamp) const +{ + if (object_map_.find(uuid) == object_map_.end()) { + return false; + } + const auto oldest_stamp = object_map_.at(uuid).begin()->first; + return oldest_stamp <= stamp; +} + +bool MetricsCalculator::hasPassedTime(const rclcpp::Time stamp) const +{ + std::vector timestamps; + for (const auto & [uuid, stamp_and_objects] : object_map_) { + if (!stamp_and_objects.empty()) { + timestamps.push_back(stamp_and_objects.begin()->first); + } + } + + auto it = std::min_element(timestamps.begin(), timestamps.end()); + if (it != timestamps.end()) { + rclcpp::Time oldest_stamp = *it; + if (oldest_stamp > stamp) { + return false; + } + } + return true; +} + +rclcpp::Time MetricsCalculator::getClosestStamp(const rclcpp::Time stamp) const +{ + rclcpp::Time closest_stamp; + rclcpp::Duration min_duration = + rclcpp::Duration::from_nanoseconds(std::numeric_limits::max()); + + for (const auto & [uuid, stamp_and_objects] : object_map_) { + if (!stamp_and_objects.empty()) { + auto it = std::lower_bound( + stamp_and_objects.begin(), stamp_and_objects.end(), stamp, + [](const auto & pair, const rclcpp::Time & val) { return pair.first < val; }); + + // check the upper bound + if (it != stamp_and_objects.end()) { + const auto duration = it->first - stamp; + if (std::abs(duration.nanoseconds()) < min_duration.nanoseconds()) { + min_duration = duration; + closest_stamp = it->first; + } + } + + // check the lower bound (if it is not the first element) + if (it != stamp_and_objects.begin()) { + const auto prev_it = std::prev(it); + const auto duration = stamp - prev_it->first; + if (std::abs(duration.nanoseconds()) < min_duration.nanoseconds()) { + min_duration = duration; + closest_stamp = prev_it->first; + } + } + } + } + + return closest_stamp; +} + +std::optional MetricsCalculator::getObjectByStamp( + const std::string uuid, const rclcpp::Time stamp) const +{ + const auto closest_stamp = getClosestStamp(stamp); + auto it = std::lower_bound( + object_map_.at(uuid).begin(), object_map_.at(uuid).end(), closest_stamp, + [](const auto & pair, const rclcpp::Time & val) { return pair.first < val; }); + + if (it != object_map_.at(uuid).end() && it->first == closest_stamp) { + return it->second; + } + return std::nullopt; +} + +PredictedObjects MetricsCalculator::getObjectsByStamp(const rclcpp::Time stamp) const +{ + const auto closest_stamp = getClosestStamp(stamp); + + PredictedObjects objects; + objects.header.stamp = stamp; + for (const auto & [uuid, stamp_and_objects] : object_map_) { + auto it = std::lower_bound( + stamp_and_objects.begin(), stamp_and_objects.end(), closest_stamp, + [](const auto & pair, const rclcpp::Time & val) { return pair.first < val; }); + + // add the object only if the element pointed to by lower_bound is equal to stamp + if (it != stamp_and_objects.end() && it->first == closest_stamp) { + objects.objects.push_back(it->second); + } + } + return objects; +} + +MetricStatMap MetricsCalculator::calcLateralDeviationMetrics(const PredictedObjects & objects) const +{ + Stat stat{}; + const auto stamp = rclcpp::Time(objects.header.stamp); + + for (const auto & object : objects.objects) { + const auto uuid = tier4_autoware_utils::toHexString(object.object_id); + if (!hasPassedTime(uuid, stamp)) { + continue; + } + const auto object_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto history_path = history_path_map_.at(uuid).second; + if (history_path.empty()) { + continue; + } + stat.add(metrics::calcLateralDeviation(history_path, object_pose)); + } + + MetricStatMap metric_stat_map; + metric_stat_map["lateral_deviation"] = stat; + return metric_stat_map; +} + +MetricStatMap MetricsCalculator::calcYawDeviationMetrics(const PredictedObjects & objects) const +{ + Stat stat{}; + const auto stamp = rclcpp::Time(objects.header.stamp); + for (const auto & object : objects.objects) { + const auto uuid = tier4_autoware_utils::toHexString(object.object_id); + if (!hasPassedTime(uuid, stamp)) { + continue; + } + const auto object_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto history_path = history_path_map_.at(uuid).second; + if (history_path.empty()) { + continue; + } + stat.add(metrics::calcYawDeviation(history_path, object_pose)); + } + + MetricStatMap metric_stat_map; + metric_stat_map["yaw_deviation"] = stat; + return metric_stat_map; +} + +MetricStatMap MetricsCalculator::calcPredictedPathDeviationMetrics( + const PredictedObjects & objects) const +{ + const auto time_horizons = parameters_->prediction_time_horizons; + + MetricStatMap metric_stat_map; + for (const double time_horizon : time_horizons) { + const auto stat = calcPredictedPathDeviationMetrics(objects, time_horizon); + std::ostringstream stream; + stream << std::fixed << std::setprecision(2) << time_horizon; + std::string time_horizon_str = stream.str(); + metric_stat_map["predicted_path_deviation_" + time_horizon_str] = stat; + } + + return metric_stat_map; +} + +Stat MetricsCalculator::calcPredictedPathDeviationMetrics( + const PredictedObjects & objects, const double time_horizon) const +{ + // For each object, select the predicted path that is closest to the history path and store the + // distance to the history path + std::unordered_map>> + deviation_map_for_paths; + // For debugging. Save the pairs of predicted path pose and history path pose. + // Visualize the correspondence in rviz from the node. + std::unordered_map>> + debug_predicted_path_pairs_map; + + // Find the corresponding pose in the history path for each pose of the predicted path of each + // object, and record the distances + const auto stamp = objects.header.stamp; + for (const auto & object : objects.objects) { + const auto uuid = tier4_autoware_utils::toHexString(object.object_id); + const auto predicted_paths = object.kinematics.predicted_paths; + for (size_t i = 0; i < predicted_paths.size(); i++) { + const auto predicted_path = predicted_paths[i]; + const std::string path_id = uuid + "_" + std::to_string(i); + for (size_t j = 0; j < predicted_path.path.size(); j++) { + const double time_duration = + rclcpp::Duration(predicted_path.time_step).seconds() * static_cast(j); + if (time_duration > time_horizon) { + break; + } + const rclcpp::Time target_stamp = + rclcpp::Time(stamp) + rclcpp::Duration::from_seconds(time_duration); + if (!hasPassedTime(uuid, target_stamp)) { + continue; + } + const auto history_object_opt = getObjectByStamp(uuid, target_stamp); + if (!history_object_opt.has_value()) { + continue; + } + const auto history_object = history_object_opt.value(); + const auto history_pose = history_object.kinematics.initial_pose_with_covariance.pose; + const Pose & p = predicted_path.path[j]; + const double distance = + tier4_autoware_utils::calcDistance2d(p.position, history_pose.position); + deviation_map_for_paths[uuid][i].push_back(distance); + // debug + debug_predicted_path_pairs_map[path_id].push_back(std::make_pair(p, history_pose)); + } + } + } + + // Select the predicted path with the smallest deviation for each object + std::unordered_map> deviation_map_for_objects; + for (const auto & [uuid, deviation_map] : deviation_map_for_paths) { + size_t min_deviation_index = 0; + double min_sum_deviation = std::numeric_limits::max(); + for (const auto & [i, deviations] : deviation_map) { + if (deviations.empty()) { + continue; + } + const double sum = std::accumulate(deviations.begin(), deviations.end(), 0.0); + if (sum < min_sum_deviation) { + min_sum_deviation = sum; + min_deviation_index = i; + } + } + deviation_map_for_objects[uuid] = deviation_map.at(min_deviation_index); + + // debug: save the delayed target object and the corresponding predicted path + const auto path_id = uuid + "_" + std::to_string(min_deviation_index); + const auto target_stamp_object = getObjectByStamp(uuid, stamp); + if (target_stamp_object.has_value()) { + ObjectData object_data; + object_data.object = target_stamp_object.value(); + object_data.path_pairs = debug_predicted_path_pairs_map[path_id]; + debug_target_object_[uuid] = object_data; + } + } + + // Store the deviation as a metric + Stat stat; + for (const auto & [uuid, deviations] : deviation_map_for_objects) { + if (deviations.empty()) { + continue; + } + for (const auto & deviation : deviations) { + stat.add(deviation); + } + } + return stat; +} + +void MetricsCalculator::setPredictedObjects(const PredictedObjects & objects) +{ + // using TimeStamp = builtin_interfaces::msg::Time; + current_stamp_ = objects.header.stamp; + + // store objects to check deviation + { + auto deviation_check_objects = objects; + filterDeviationCheckObjects(deviation_check_objects, parameters_->object_parameters); + using tier4_autoware_utils::toHexString; + for (const auto & object : deviation_check_objects.objects) { + std::string uuid = toHexString(object.object_id); + updateObjects(uuid, current_stamp_, object); + } + deleteOldObjects(current_stamp_); + updateHistoryPath(); + } +} + +void MetricsCalculator::deleteOldObjects(const rclcpp::Time stamp) +{ + // delete the data older than 2*time_delay_ + const double time_delay = getTimeDelay(); + for (auto & [uuid, stamp_and_objects] : object_map_) { + for (auto it = stamp_and_objects.begin(); it != stamp_and_objects.end();) { + if (it->first < stamp - rclcpp::Duration::from_seconds(time_delay * 2)) { + it = stamp_and_objects.erase(it); + } else { + break; + } + } + } + + const auto object_map = object_map_; + for (const auto & [uuid, stamp_and_objects] : object_map) { + if (stamp_and_objects.empty()) { + object_map_.erase(uuid); + history_path_map_.erase(uuid); + debug_target_object_.erase(uuid); // debug + } + } +} + +void MetricsCalculator::updateObjects( + const std::string uuid, const rclcpp::Time stamp, const PredictedObject & object) +{ + object_map_[uuid][stamp] = object; +} + +void MetricsCalculator::updateHistoryPath() +{ + const double window_size = parameters_->smoothing_window_size; + + for (const auto & [uuid, stamp_and_objects] : object_map_) { + std::vector history_path; + for (const auto & [stamp, object] : stamp_and_objects) { + history_path.push_back(object.kinematics.initial_pose_with_covariance.pose); + } + + // pair of history_path(raw) and smoothed_history_path + // history_path(raw) is just for debugging + history_path_map_[uuid] = + std::make_pair(history_path, averageFilterPath(history_path, window_size)); + } +} + +std::vector MetricsCalculator::generateHistoryPathWithPrev( + const std::vector & prev_history_path, const Pose & new_pose, const size_t window_size) +{ + std::vector history_path; + const size_t half_window_size = static_cast(window_size / 2); + history_path.insert( + history_path.end(), prev_history_path.begin(), prev_history_path.end() - half_window_size); + + std::vector updated_poses; + updated_poses.insert( + updated_poses.end(), prev_history_path.end() - half_window_size * 2, prev_history_path.end()); + updated_poses.push_back(new_pose); + + updated_poses = averageFilterPath(updated_poses, window_size); + history_path.insert( + history_path.end(), updated_poses.begin() + half_window_size, updated_poses.end()); + return history_path; +} + +std::vector MetricsCalculator::averageFilterPath( + const std::vector & path, const size_t window_size) const +{ + using tier4_autoware_utils::calcAzimuthAngle; + using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::createQuaternionFromYaw; + + std::vector filtered_path; + filtered_path.reserve(path.size()); // Reserve space to avoid reallocations + + const size_t half_window = static_cast(window_size / 2); + + // Calculate the moving average for positions + for (size_t i = 0; i < path.size(); ++i) { + double sum_x = 0.0, sum_y = 0.0, sum_z = 0.0; + size_t valid_points = 0; // Correctly initialize and use as counter + + for (size_t j = std::max(i - half_window, static_cast(0)); + j <= std::min(i + half_window, path.size() - 1); ++j) { + sum_x += path[j].position.x; + sum_y += path[j].position.y; + sum_z += path[j].position.z; + ++valid_points; + } + + Pose average_pose; + if (valid_points > 0) { // Prevent division by zero + average_pose.position.x = sum_x / valid_points; + average_pose.position.y = sum_y / valid_points; + average_pose.position.z = sum_z / valid_points; + } else { + average_pose.position = path.at(i).position; + } + + // Placeholder for orientation to ensure structure integrity + average_pose.orientation = path.at(i).orientation; + filtered_path.push_back(average_pose); + } + + // Calculate yaw and convert to quaternion after averaging positions + for (size_t i = 0; i < filtered_path.size(); ++i) { + Pose & p = filtered_path[i]; + + // if the current pose is too close to the previous one, use the previous orientation + if (i > 0) { + const Pose & p_prev = filtered_path[i - 1]; + if (calcDistance2d(p_prev.position, p.position) < 0.1) { + p.orientation = p_prev.orientation; + continue; + } + } + + if (i < filtered_path.size() - 1) { + const double yaw = calcAzimuthAngle(p.position, filtered_path[i + 1].position); + filtered_path[i].orientation = createQuaternionFromYaw(yaw); + } else if (filtered_path.size() > 1) { + // For the last point, use the orientation of the second-to-last point + p.orientation = filtered_path[i - 1].orientation; + } + } + + return filtered_path; +} + +} // namespace perception_diagnostics diff --git a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp new file mode 100644 index 0000000000000..cf98ccc4c5460 --- /dev/null +++ b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -0,0 +1,337 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "perception_online_evaluator/perception_online_evaluator_node.hpp" + +#include "perception_online_evaluator/utils/marker_utils.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" +#include "tier4_autoware_utils/ros/parameter.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + +#include + +#include "boost/lexical_cast.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace perception_diagnostics +{ +PerceptionOnlineEvaluatorNode::PerceptionOnlineEvaluatorNode( + const rclcpp::NodeOptions & node_options) +: Node("perception_online_evaluator", node_options), + parameters_(std::make_shared()), + metrics_calculator_(parameters_) +{ + using std::placeholders::_1; + + google::InitGoogleLogging("perception_online_evaluator_node"); + google::InstallFailureSignalHandler(); + + objects_sub_ = create_subscription( + "~/input/objects", 1, std::bind(&PerceptionOnlineEvaluatorNode::onObjects, this, _1)); + metrics_pub_ = create_publisher("~/metrics", 1); + pub_marker_ = create_publisher("~/markers", 10); + + tf_buffer_ = std::make_unique(this->get_clock()); + transform_listener_ = std::make_shared(*tf_buffer_); + + // Parameters + initParameter(); + + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&PerceptionOnlineEvaluatorNode::onParameter, this, std::placeholders::_1)); +} + +void PerceptionOnlineEvaluatorNode::publishMetrics() +{ + DiagnosticArray metrics_msg; + + // calculate metrics + for (const Metric & metric : parameters_->metrics) { + const auto metric_stat_map = metrics_calculator_.calculate(Metric(metric)); + if (!metric_stat_map.has_value()) { + continue; + } + + for (const auto & [metric, stat] : metric_stat_map.value()) { + if (stat.count() > 0) { + metrics_msg.status.push_back(generateDiagnosticStatus(metric, stat)); + } + } + } + + // publish metrics + if (!metrics_msg.status.empty()) { + metrics_msg.header.stamp = now(); + metrics_pub_->publish(metrics_msg); + } + publishDebugMarker(); +} + +DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus( + const std::string metric, const Stat & metric_stat) const +{ + DiagnosticStatus status; + + status.level = status.OK; + status.name = metric; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "min"; + key_value.value = std::to_string(metric_stat.min()); + status.values.push_back(key_value); + key_value.key = "max"; + key_value.value = std::to_string(metric_stat.max()); + status.values.push_back(key_value); + key_value.key = "mean"; + key_value.value = std::to_string(metric_stat.mean()); + status.values.push_back(key_value); + + return status; +} + +void PerceptionOnlineEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg) +{ + metrics_calculator_.setPredictedObjects(*objects_msg); + publishMetrics(); +} + +void PerceptionOnlineEvaluatorNode::publishDebugMarker() +{ + using marker_utils::createColorFromString; + using marker_utils::createDeviationLines; + using marker_utils::createObjectPolygonMarkerArray; + using marker_utils::createPointsMarkerArray; + using marker_utils::createPosesMarkerArray; + + MarkerArray marker; + + const auto add = [&marker](MarkerArray added) { + for (auto & marker : added.markers) { + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + } + tier4_autoware_utils::appendMarkerArray(added, &marker); + }; + + const auto history_path_map = metrics_calculator_.getHistoryPathMap(); + const auto & p = parameters_->debug_marker_parameters; + + for (const auto & [uuid, history_path] : history_path_map) { + { + const auto c = createColorFromString(uuid + "_raw"); + if (p.show_history_path) { + add(createPointsMarkerArray(history_path.first, "history_path_" + uuid, 0, c.r, c.g, c.b)); + } + if (p.show_history_path_arrows) { + add(createPosesMarkerArray( + history_path.first, "history_path_arrows_" + uuid, c.r, c.g, c.b, 0.1, 0.05, 0.05)); + } + } + { + const auto c = createColorFromString(uuid); + if (p.show_smoothed_history_path) { + add(createPointsMarkerArray( + history_path.second, "smoothed_history_path_" + uuid, 0, c.r, c.g, c.b)); + } + if (p.show_smoothed_history_path_arrows) { + add(createPosesMarkerArray( + history_path.second, "smoothed_history_path_arrows_" + uuid, c.r, c.g, c.b, 0.1, 0.05, + 0.05)); + } + } + } + const auto object_data_map = metrics_calculator_.getDebugObjectData(); + for (const auto & [uuid, object_data] : object_data_map) { + const auto c = createColorFromString(uuid); + const auto predicted_path = object_data.getPredictedPath(); + const auto history_path = object_data.getHistoryPath(); + if (p.show_predicted_path) { + add(createPosesMarkerArray(predicted_path, "predicted_path_" + uuid, 0, 0, 1)); + } + if (p.show_predicted_path_gt) { + add(createPosesMarkerArray(history_path, "predicted_path_gt_" + uuid, 1, 0, 0)); + } + if (p.show_deviation_lines) { + add(createDeviationLines(predicted_path, history_path, "deviation_lines_" + uuid, 1, 1, 1)); + } + if (p.show_object_polygon) { + add(createObjectPolygonMarkerArray( + object_data.object, "object_polygon_" + uuid, 0, c.r, c.g, c.b)); + } + } + + pub_marker_->publish(marker); +} + +rcl_interfaces::msg::SetParametersResult PerceptionOnlineEvaluatorNode::onParameter( + const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + + auto & p = parameters_; + + updateParam(parameters, "smoothing_window_size", p->smoothing_window_size); + + // update metrics + { + std::vector metrics_str; + updateParam>(parameters, "selected_metrics", metrics_str); + std::vector metrics; + for (const std::string & selected_metric : metrics_str) { + const Metric metric = str_to_metric.at(selected_metric); + metrics.push_back(metric); + } + p->metrics = metrics; + } + + // update parameters for each object class + { + const auto get_object_param = [&](std::string && ns) -> std::optional { + ObjectParameter param{}; + if (updateParam(parameters, ns + "check_deviation", param.check_deviation)) { + return param; + } + return std::nullopt; + }; + + const std::string ns = "target_object."; + if (const auto new_param = get_object_param(ns + "car.")) { + p->object_parameters.at(ObjectClassification::CAR) = *new_param; + } + if (const auto new_param = get_object_param(ns + "truck.")) { + p->object_parameters.at(ObjectClassification::TRUCK) = *new_param; + } + if (const auto new_param = get_object_param(ns + "bus.")) { + p->object_parameters.at(ObjectClassification::BUS) = *new_param; + } + if (const auto new_param = get_object_param(ns + "trailer.")) { + p->object_parameters.at(ObjectClassification::TRAILER) = *new_param; + } + if (const auto new_param = get_object_param(ns + "bicycle.")) { + p->object_parameters.at(ObjectClassification::BICYCLE) = *new_param; + } + if (const auto new_param = get_object_param(ns + "motorcycle.")) { + p->object_parameters.at(ObjectClassification::MOTORCYCLE) = *new_param; + } + if (const auto new_param = get_object_param(ns + "pedestrian.")) { + p->object_parameters.at(ObjectClassification::PEDESTRIAN) = *new_param; + } + if (const auto new_param = get_object_param(ns + "unknown.")) { + p->object_parameters.at(ObjectClassification::UNKNOWN) = *new_param; + } + } + + // update debug marker parameters + { + const std::string ns = "debug_marker."; + updateParam( + parameters, ns + "history_path", p->debug_marker_parameters.show_history_path); + updateParam( + parameters, ns + "history_path_arrows", p->debug_marker_parameters.show_history_path_arrows); + updateParam( + parameters, ns + "smoothed_history_path", + p->debug_marker_parameters.show_smoothed_history_path); + updateParam( + parameters, ns + "smoothed_history_path_arrows", + p->debug_marker_parameters.show_smoothed_history_path_arrows); + updateParam( + parameters, ns + "predicted_path", p->debug_marker_parameters.show_predicted_path); + updateParam( + parameters, ns + "predicted_path_gt", p->debug_marker_parameters.show_predicted_path_gt); + updateParam( + parameters, ns + "deviation_lines", p->debug_marker_parameters.show_deviation_lines); + updateParam( + parameters, ns + "object_polygon", p->debug_marker_parameters.show_object_polygon); + } + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + return result; +} + +void PerceptionOnlineEvaluatorNode::initParameter() +{ + using tier4_autoware_utils::getOrDeclareParameter; + using tier4_autoware_utils::updateParam; + + auto & p = parameters_; + + p->smoothing_window_size = getOrDeclareParameter(*this, "smoothing_window_size"); + p->prediction_time_horizons = + getOrDeclareParameter>(*this, "prediction_time_horizons"); + + // set metrics + const auto selected_metrics = + getOrDeclareParameter>(*this, "selected_metrics"); + for (const std::string & selected_metric : selected_metrics) { + const Metric metric = str_to_metric.at(selected_metric); + parameters_->metrics.push_back(metric); + } + + // set parameters for each object class + { + const auto get_object_param = [&](std::string && ns) -> ObjectParameter { + ObjectParameter param{}; + param.check_deviation = getOrDeclareParameter(*this, ns + "check_deviation"); + return param; + }; + + const std::string ns = "target_object."; + p->object_parameters.emplace(ObjectClassification::CAR, get_object_param(ns + "car.")); + p->object_parameters.emplace(ObjectClassification::TRUCK, get_object_param(ns + "truck.")); + p->object_parameters.emplace(ObjectClassification::BUS, get_object_param(ns + "bus.")); + p->object_parameters.emplace(ObjectClassification::TRAILER, get_object_param(ns + "trailer.")); + p->object_parameters.emplace(ObjectClassification::BICYCLE, get_object_param(ns + "bicycle.")); + p->object_parameters.emplace( + ObjectClassification::MOTORCYCLE, get_object_param(ns + "motorcycle.")); + p->object_parameters.emplace( + ObjectClassification::PEDESTRIAN, get_object_param(ns + "pedestrian.")); + p->object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown.")); + } + + // set debug marker parameters + { + const std::string ns = "debug_marker."; + p->debug_marker_parameters.show_history_path = + getOrDeclareParameter(*this, ns + "history_path"); + p->debug_marker_parameters.show_history_path_arrows = + getOrDeclareParameter(*this, ns + "history_path_arrows"); + p->debug_marker_parameters.show_smoothed_history_path = + getOrDeclareParameter(*this, ns + "smoothed_history_path"); + p->debug_marker_parameters.show_smoothed_history_path_arrows = + getOrDeclareParameter(*this, ns + "smoothed_history_path_arrows"); + p->debug_marker_parameters.show_predicted_path = + getOrDeclareParameter(*this, ns + "predicted_path"); + p->debug_marker_parameters.show_predicted_path_gt = + getOrDeclareParameter(*this, ns + "predicted_path_gt"); + p->debug_marker_parameters.show_deviation_lines = + getOrDeclareParameter(*this, ns + "deviation_lines"); + p->debug_marker_parameters.show_object_polygon = + getOrDeclareParameter(*this, ns + "object_polygon"); + } +} +} // namespace perception_diagnostics + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(perception_diagnostics::PerceptionOnlineEvaluatorNode) diff --git a/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp new file mode 100644 index 0000000000000..75af92e83dcd8 --- /dev/null +++ b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp @@ -0,0 +1,196 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "perception_online_evaluator/utils/marker_utils.hpp" + +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include +#include + +#include +#include +#include + +#include + +namespace marker_utils +{ +using std_msgs::msg::ColorRGBA; +using tier4_autoware_utils::calcOffsetPose; +using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerOrientation; +using tier4_autoware_utils::createMarkerScale; +using tier4_autoware_utils::createPoint; +using visualization_msgs::msg::Marker; + +void addFootprintMarker( + visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose, + const vehicle_info_util::VehicleInfo & vehicle_info) +{ + const double half_width = vehicle_info.vehicle_width_m / 2.0; + const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m; + const double base_to_rear = vehicle_info.rear_overhang_m; + + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(pose, base_to_front, -half_width, 0.0).position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(pose, base_to_front, half_width, 0.0).position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, half_width, 0.0).position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, -half_width, 0.0).position); + marker.points.push_back(marker.points.front()); +} + +MarkerArray createFootprintMarkerArray( + const Pose & base_link_pose, const vehicle_info_util::VehicleInfo vehicle_info, + const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b) +{ + const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + MarkerArray msg; + + Marker marker = createDefaultMarker( + "map", current_time, ns, id, Marker::LINE_STRIP, createMarkerScale(0.2, 0.2, 0.2), + createMarkerColor(r, g, b, 0.999)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + MarkerArray marker_array; + + addFootprintMarker(marker, base_link_pose, vehicle_info); + msg.markers.push_back(marker); + return msg; +} + +MarkerArray createPointsMarkerArray( + const std::vector points, const std::string & ns, const int32_t id, const float r, + const float g, const float b) +{ + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::LINE_STRIP, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); + + for (const auto & point : points) { + marker.points.push_back(point); + } + + MarkerArray msg; + msg.markers.push_back(marker); + return msg; +} + +MarkerArray createPointsMarkerArray( + const std::vector poses, const std::string & ns, const int32_t id, const float r, + const float g, const float b) +{ + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::LINE_STRIP, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); + + for (const auto & pose : poses) { + marker.points.push_back(pose.position); + } + + MarkerArray msg; + msg.markers.push_back(marker); + return msg; +} + +MarkerArray createDeviationLines( + const std::vector poses1, const std::vector poses2, const std::string & ns, + const float r, const float g, const float b) +{ + MarkerArray msg; + + const size_t max_idx = std::min(poses1.size(), poses2.size()); + for (size_t i = 0; i < max_idx; ++i) { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i, Marker::LINE_STRIP, + createMarkerScale(0.005, 0.0, 0.0), createMarkerColor(r, g, b, 0.5)); + marker.points.push_back(poses1.at(i).position); + marker.points.push_back(poses2.at(i).position); + msg.markers.push_back(marker); + } + + return msg; +} + +MarkerArray createPoseMarkerArray( + const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g, + const float & b) +{ + MarkerArray msg; + + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::ARROW, + createMarkerScale(0.7, 0.3, 0.3), createMarkerColor(r, g, b, 0.999)); + marker.pose = pose; + msg.markers.push_back(marker); + + return msg; +} + +MarkerArray createPosesMarkerArray( + const std::vector poses, std::string && ns, const float & r, const float & g, + const float & b, const float & x_scale, const float & y_scale, const float & z_scale) +{ + MarkerArray msg; + + for (size_t i = 0; i < poses.size(); ++i) { + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i, Marker::ARROW, + createMarkerScale(x_scale, y_scale, z_scale), createMarkerColor(r, g, b, 0.5)); + marker.pose = poses.at(i); + msg.markers.push_back(marker); + } + + return msg; +} + +std_msgs::msg::ColorRGBA createColorFromString(const std::string & str) +{ + const auto hash = std::hash{}(str); + const auto r = (hash & 0xFF) / 255.0; + const auto g = ((hash >> 8) & 0xFF) / 255.0; + const auto b = ((hash >> 16) & 0xFF) / 255.0; + return tier4_autoware_utils::createMarkerColor(r, g, b, 0.5); +} + +MarkerArray createObjectPolygonMarkerArray( + const PredictedObject & object, std::string && ns, const int32_t & id, const float & r, + const float & g, const float & b) +{ + MarkerArray msg; + + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::LINE_STRIP, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); + + const double z = object.kinematics.initial_pose_with_covariance.pose.position.z; + const double height = object.shape.dimensions.z; + const auto polygon = tier4_autoware_utils::toPolygon2d( + object.kinematics.initial_pose_with_covariance.pose, object.shape); + for (const auto & p : polygon.outer()) { + marker.points.push_back(createPoint(p.x(), p.y(), z - height / 2)); + marker.points.push_back(createPoint(p.x(), p.y(), z + height / 2)); + } + marker.id = id; + msg.markers.push_back(marker); + + return msg; +} + +} // namespace marker_utils diff --git a/evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp b/evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp new file mode 100644 index 0000000000000..97daee36175f3 --- /dev/null +++ b/evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp @@ -0,0 +1,103 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "perception_online_evaluator/utils/objects_filtering.hpp" + +namespace perception_diagnostics +{ +ObjectTypesToCheck getDeviationCheckObjectTypes( + const std::unordered_map & params) +{ + ObjectTypesToCheck object_types_to_check; + for (const auto & [object_class, object_param] : params) { + switch (object_class) { + case ObjectClassification::CAR: + object_types_to_check.check_car = object_param.check_deviation; + break; + case ObjectClassification::TRUCK: + object_types_to_check.check_truck = object_param.check_deviation; + break; + case ObjectClassification::BUS: + object_types_to_check.check_bus = object_param.check_deviation; + break; + case ObjectClassification::TRAILER: + object_types_to_check.check_trailer = object_param.check_deviation; + break; + case ObjectClassification::BICYCLE: + object_types_to_check.check_bicycle = object_param.check_deviation; + break; + case ObjectClassification::MOTORCYCLE: + object_types_to_check.check_motorcycle = object_param.check_deviation; + break; + case ObjectClassification::PEDESTRIAN: + object_types_to_check.check_pedestrian = object_param.check_deviation; + break; + case ObjectClassification::UNKNOWN: + object_types_to_check.check_unknown = object_param.check_deviation; + break; + default: + break; + } + } + return object_types_to_check; +} + +std::uint8_t getHighestProbLabel(const std::vector & classification) +{ + std::uint8_t label = ObjectClassification::UNKNOWN; + float highest_prob = 0.0; + for (const auto & _class : classification) { + if (highest_prob < _class.probability) { + highest_prob = _class.probability; + label = _class.label; + } + } + + return label; +} + +bool isTargetObjectType( + const PredictedObject & object, const ObjectTypesToCheck & target_object_types) +{ + const auto t = getHighestProbLabel(object.classification); + + return ( + (t == ObjectClassification::CAR && target_object_types.check_car) || + (t == ObjectClassification::TRUCK && target_object_types.check_truck) || + (t == ObjectClassification::BUS && target_object_types.check_bus) || + (t == ObjectClassification::TRAILER && target_object_types.check_trailer) || + (t == ObjectClassification::UNKNOWN && target_object_types.check_unknown) || + (t == ObjectClassification::BICYCLE && target_object_types.check_bicycle) || + (t == ObjectClassification::MOTORCYCLE && target_object_types.check_motorcycle) || + (t == ObjectClassification::PEDESTRIAN && target_object_types.check_pedestrian)); +} + +void filterObjectsByClass( + PredictedObjects & objects, const ObjectTypesToCheck & target_object_types) +{ + const auto filter = [&](const auto & object) { + return isTargetObjectType(object, target_object_types); + }; + + filterObjects(objects, filter); +} + +void filterDeviationCheckObjects( + PredictedObjects & objects, const std::unordered_map & params) +{ + const auto object_types = getDeviationCheckObjectTypes(params); + filterObjectsByClass(objects, object_types); +} + +} // namespace perception_diagnostics diff --git a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp new file mode 100644 index 0000000000000..095fb130426f0 --- /dev/null +++ b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp @@ -0,0 +1,521 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +#include "boost/lexical_cast.hpp" + +#include +#include + +#include +#include +#include +#include + +using EvalNode = perception_diagnostics::PerceptionOnlineEvaluatorNode; +using PredictedObjects = autoware_auto_perception_msgs::msg::PredictedObjects; +using PredictedObject = autoware_auto_perception_msgs::msg::PredictedObject; +using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; +using MarkerArray = visualization_msgs::msg::MarkerArray; +using ObjectClassification = autoware_auto_perception_msgs::msg::ObjectClassification; + +using tier4_autoware_utils::generateUUID; + +constexpr double epsilon = 1e-6; + +class EvalTest : public ::testing::Test +{ +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + + rclcpp::NodeOptions options; + const auto share_dir = + ament_index_cpp::get_package_share_directory("perception_online_evaluator"); + options.arguments( + {"--ros-args", "--params-file", + share_dir + "/param/perception_online_evaluator.defaults.yaml"}); + options.append_parameter_override("prediction_time_horizons", std::vector{5.0}); + options.append_parameter_override("smoothing_window_size", 11); + + dummy_node = std::make_shared("perception_online_evaluator_test", options); + eval_node = std::make_shared(options); + // Enable all logging in the node + auto ret = rcutils_logging_set_logger_level( + dummy_node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + if (ret != RCUTILS_RET_OK) { + std::cerr << "Failed to set logging severity to DEBUG\n"; + } + ret = rcutils_logging_set_logger_level( + eval_node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + if (ret != RCUTILS_RET_OK) { + std::cerr << "Failed to set logging severity to DEBUG\n"; + } + objects_pub_ = rclcpp::create_publisher( + dummy_node, "/perception_online_evaluator/input/objects", 1); + + marker_sub_ = rclcpp::create_subscription( + eval_node, "perception_online_evaluator/markers", 10, + [this]([[maybe_unused]] const MarkerArray::SharedPtr msg) { has_received_marker_ = true; }); + uuid_ = generateUUID(); + } + + ~EvalTest() override + { + rclcpp::shutdown(); + google::ShutdownGoogleLogging(); + } + + void setTargetMetric(perception_diagnostics::Metric metric) + { + const auto metric_str = perception_diagnostics::metric_to_str.at(metric); + setTargetMetric(metric_str); + } + + void setTargetMetric(std::string metric_str) + { + const auto is_target_metric = [metric_str](const auto & status) { + return status.name == metric_str; + }; + metric_sub_ = rclcpp::create_subscription( + eval_node, "/perception_online_evaluator/metrics", 1, + [=](const DiagnosticArray::ConstSharedPtr msg) { + const auto it = std::find_if(msg->status.begin(), msg->status.end(), is_target_metric); + if (it != msg->status.end()) { + std::cerr << it->values[0].key << " " << it->values[0].value << " " << it->values[1].key + << " " << it->values[1].value << " " << it->values[2].key << " " + << it->values[2].value << std::endl; + metric_value_ = boost::lexical_cast(it->values[2].value); + metric_updated_ = true; + } + }); + } + + PredictedObject makePredictedObject(const std::vector> & predicted_path) + { + PredictedObject object; + object.object_id = uuid_; + ObjectClassification classification; + classification.label = ObjectClassification::CAR; + classification.probability = 1.0; + + object.classification = {classification}; + + object.kinematics.initial_pose_with_covariance.pose.position.x = predicted_path.front().first; + object.kinematics.initial_pose_with_covariance.pose.position.y = predicted_path.front().second; + object.kinematics.initial_pose_with_covariance.pose.position.z = 0.0; + object.kinematics.initial_pose_with_covariance.pose.orientation.x = 0.0; + object.kinematics.initial_pose_with_covariance.pose.orientation.y = 0.0; + object.kinematics.initial_pose_with_covariance.pose.orientation.z = 0.0; + object.kinematics.initial_pose_with_covariance.pose.orientation.w = 1.0; + + autoware_auto_perception_msgs::msg::PredictedPath path; + for (size_t i = 0; i < predicted_path.size(); ++i) { + geometry_msgs::msg::Pose pose; + pose.position.x = predicted_path[i].first; + pose.position.y = predicted_path[i].second; + pose.position.z = 0.0; + pose.orientation.x = 0.0; + pose.orientation.y = 0.0; + pose.orientation.z = 0.0; + pose.orientation.w = 1.0; + path.path.push_back(pose); + } + + path.confidence = 1.0; + path.time_step = rclcpp::Duration::from_seconds(time_step_); + object.kinematics.predicted_paths.push_back(path); + + return object; + } + + PredictedObjects makePredictedObjects( + const std::vector> & predicted_path) + { + PredictedObjects objects; + objects.objects.push_back(makePredictedObject(predicted_path)); + objects.header.stamp = rclcpp::Time(0); + return objects; + } + + PredictedObjects makeStraightPredictedObjects(const double time) + { + std::vector> predicted_path; + for (size_t i = 0; i <= time_horizon_ / time_step_; i++) { + predicted_path.push_back({velocity_ * (time + i * time_step_), 0.0}); + } + auto objects = makePredictedObjects(predicted_path); + objects.header.stamp = rclcpp::Time(0) + rclcpp::Duration::from_seconds(time); + return objects; + } + + PredictedObjects makeDeviatedStraightPredictedObjects(const double time, const double deviation) + { + std::vector> predicted_path; + for (size_t i = 0; i <= time_horizon_ / time_step_; i++) { + predicted_path.push_back({velocity_ * (time + i * time_step_), deviation}); + } + auto objects = makePredictedObjects(predicted_path); + objects.header.stamp = rclcpp::Time(0) + rclcpp::Duration::from_seconds(time); + return objects; + } + + PredictedObjects rotateObjects(const PredictedObjects objects, const double yaw) + { + PredictedObjects rotated_objects = objects; + for (auto & object : rotated_objects.objects) { + object.kinematics.initial_pose_with_covariance.pose.orientation.z = sin(yaw / 2); + object.kinematics.initial_pose_with_covariance.pose.orientation.w = cos(yaw / 2); + } + return rotated_objects; + } + + double publishObjectsAndGetMetric(const PredictedObjects & objects) + { + metric_updated_ = false; + objects_pub_->publish(objects); + const auto now = rclcpp::Clock().now(); + while (!metric_updated_) { + rclcpp::spin_some(dummy_node); + rclcpp::spin_some(eval_node); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + // timeout + if (rclcpp::Clock().now() - now > rclcpp::Duration::from_seconds(5)) { + throw std::runtime_error("Timeout while waiting for metric update"); + } + } + return metric_value_; + } + + void publishObjects(const PredictedObjects & objects) + { + objects_pub_->publish(objects); + rclcpp::spin_some(eval_node); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(dummy_node); + } + + void waitForDummyNode() + { + // wait for the marker to be published + publishObjects(makeStraightPredictedObjects(0)); + while (!has_received_marker_) { + rclcpp::spin_some(dummy_node); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(eval_node); + } + } + + // Latest metric value + bool metric_updated_ = false; + double metric_value_; + // Node + rclcpp::Node::SharedPtr dummy_node; + EvalNode::SharedPtr eval_node; + + // Pub/Sub + rclcpp::Publisher::SharedPtr objects_pub_; + rclcpp::Subscription::SharedPtr metric_sub_; + rclcpp::Subscription::SharedPtr marker_sub_; + bool has_received_marker_{false}; + + double time_delay_ = 5.0; + double time_step_ = 0.5; + double time_horizon_ = 10.0; + double velocity_ = 2.0; + + unique_identifier_msgs::msg::UUID uuid_; +}; + +// ========================================================================================== +// lateral deviation +TEST_F(EvalTest, testLateralDeviation_deviation0) +{ + waitForDummyNode(); + setTargetMetric("lateral_deviation"); + + const double deviation = 0.0; + for (double time = 0; time < time_delay_; time += time_step_) { + const auto objects = makeDeviatedStraightPredictedObjects(time, deviation); + publishObjects(objects); + } + + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_, deviation); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0.0, epsilon); +} + +TEST_F(EvalTest, testLateralDeviation_deviation1) +{ + waitForDummyNode(); + setTargetMetric("lateral_deviation"); + + const double deviation = 1.0; + for (double time = 0; time < time_delay_; time += time_step_) { + const auto objects = makeDeviatedStraightPredictedObjects(time, deviation); + publishObjects(objects); + } + + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_ * 2, deviation); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0.0, epsilon); +} + +TEST_F(EvalTest, testLateralDeviation_oscillation) +{ + waitForDummyNode(); + setTargetMetric("lateral_deviation"); + + const double deviation = 1.0; + double sign = 1.0; + for (double time = 0; time < time_delay_ * 2; time += time_step_) { + PredictedObjects objects; + if (time == time_delay_) { + objects = makeDeviatedStraightPredictedObjects(time, 0); + } else { + objects = makeDeviatedStraightPredictedObjects(time, deviation * sign); + sign *= -1.0; + } + publishObjects(objects); + } + + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_ * 2, deviation); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0.0, epsilon); +} + +TEST_F(EvalTest, testLateralDeviation_distortion) +{ + waitForDummyNode(); + setTargetMetric("lateral_deviation"); + + const double deviation = 1.0; + for (double time = 0; time < time_delay_ * 2; time += time_step_) { + PredictedObjects objects; + if (time == time_delay_) { + objects = makeDeviatedStraightPredictedObjects(time, deviation); + } else if (time == time_delay_ + time_step_) { + objects = makeDeviatedStraightPredictedObjects(time, -deviation); + } else { + objects = makeDeviatedStraightPredictedObjects(time, 0); + } + publishObjects(objects); + } + + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_ * 2, deviation); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), deviation, epsilon); +} +// ========================================================================================== + +// ========================================================================================== +// yaw deviation +TEST_F(EvalTest, testYawDeviation_deviation0) +{ + waitForDummyNode(); + setTargetMetric("yaw_deviation"); + + const double deviation = 0.0; + for (double time = 0; time < time_delay_; time += time_step_) { + const auto objects = makeDeviatedStraightPredictedObjects(time, deviation); + publishObjects(objects); + } + + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_, deviation); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0.0, epsilon); +} + +TEST_F(EvalTest, testYawDeviation_deviation1) +{ + waitForDummyNode(); + setTargetMetric("yaw_deviation"); + + const double deviation = 1.0; + for (double time = 0; time < time_delay_; time += time_step_) { + const auto objects = makeDeviatedStraightPredictedObjects(time, deviation); + publishObjects(objects); + } + + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_, deviation); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0.0, epsilon); +} + +TEST_F(EvalTest, testYawDeviation_oscillation) +{ + waitForDummyNode(); + setTargetMetric("yaw_deviation"); + + const double deviation = 1.0; + double sign = 1.0; + for (double time = 0; time < time_delay_ * 2; time += time_step_) { + PredictedObjects objects; + if (time == time_delay_) { + objects = makeDeviatedStraightPredictedObjects(time, 0); + } else { + objects = makeDeviatedStraightPredictedObjects(time, deviation * sign); + sign *= -1.0; + } + publishObjects(objects); + } + + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_ * 2, deviation); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0.0, epsilon); +} + +TEST_F(EvalTest, testYawDeviation_distortion) +{ + waitForDummyNode(); + setTargetMetric("yaw_deviation"); + + const double deviation = 1.0; + for (double time = 0; time < time_delay_ * 2; time += time_step_) { + PredictedObjects objects; + if (time == time_delay_) { + objects = makeDeviatedStraightPredictedObjects(time, deviation); + } else if (time == time_delay_ + time_step_) { + objects = makeDeviatedStraightPredictedObjects(time, -deviation); + } else { + objects = makeDeviatedStraightPredictedObjects(time, 0); + } + publishObjects(objects); + } + + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_ * 2, deviation); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0, epsilon); +} + +TEST_F(EvalTest, testYawDeviation_oscillation_rotate) +{ + waitForDummyNode(); + setTargetMetric("yaw_deviation"); + + const double deviation = 1.0; + const double yaw = M_PI / 4; + double sign = 1.0; + for (double time = 0; time < time_delay_ * 2; time += time_step_) { + PredictedObjects objects; + if (time == time_delay_) { + objects = rotateObjects(makeDeviatedStraightPredictedObjects(time, 0), yaw); + } else { + objects = rotateObjects( + makeDeviatedStraightPredictedObjects(time, deviation * sign), 2 * M_PI * std::rand()); + sign *= -1.0; + } + publishObjects(objects); + } + + const auto last_objects = rotateObjects( + makeDeviatedStraightPredictedObjects(time_delay_ * 2, deviation), 2 * M_PI * std::rand()); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), yaw, epsilon); +} + +TEST_F(EvalTest, testYawDeviation_distortion_rotate) +{ + waitForDummyNode(); + setTargetMetric("yaw_deviation"); + + const double deviation = 1.0; + const double yaw = M_PI / 4; + for (double time = 0; time < time_delay_ * 2; time += time_step_) { + PredictedObjects objects; + if (time == time_delay_) { + objects = rotateObjects(makeDeviatedStraightPredictedObjects(time, deviation), yaw); + } else if (time == time_delay_ + time_step_) { + objects = rotateObjects( + makeDeviatedStraightPredictedObjects(time, -deviation), 2 * M_PI * std::rand()); + } else { + objects = + rotateObjects(makeDeviatedStraightPredictedObjects(time, 0), 2 * M_PI * std::rand()); + } + publishObjects(objects); + } + + const auto last_objects = rotateObjects( + makeDeviatedStraightPredictedObjects(time_delay_ * 2, deviation), 2 * M_PI * std::rand()); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), yaw, epsilon); +} + +// ========================================================================================== +// predicted path deviation{ +TEST_F(EvalTest, testPredictedPathDeviation_deviation0) +{ + waitForDummyNode(); + + setTargetMetric("predicted_path_deviation_5.00"); + + const auto init_objects = makeStraightPredictedObjects(0); + publishObjects(init_objects); + + const double deviation = 0.0; + for (double time = time_step_; time < time_delay_; time += time_step_) { + const auto objects = makeDeviatedStraightPredictedObjects(time, deviation); + publishObjects(objects); + } + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_, deviation); + + const double num_points = time_delay_ / time_step_ + 1; + const double mean_deviation = deviation * (num_points - 1) / num_points; + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), mean_deviation, epsilon); +} + +TEST_F(EvalTest, testPredictedPathDeviation_deviation1) +{ + waitForDummyNode(); + + setTargetMetric("predicted_path_deviation_5.00"); + + const auto init_objects = makeStraightPredictedObjects(0); + publishObjects(init_objects); + + const double deviation = 1.0; + for (double time = time_step_; time < time_delay_; time += time_step_) { + const auto objects = makeDeviatedStraightPredictedObjects(time, deviation); + publishObjects(objects); + } + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_, deviation); + + const double num_points = time_delay_ / time_step_ + 1; + const double mean_deviation = deviation * (num_points - 1) / num_points; + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), mean_deviation, epsilon); +} + +TEST_F(EvalTest, testPredictedPathDeviation_deviation2) +{ + waitForDummyNode(); + + setTargetMetric("predicted_path_deviation_5.00"); + + const auto init_objects = makeStraightPredictedObjects(0); + publishObjects(init_objects); + + const double deviation = 2.0; + for (double time = time_step_; time < time_delay_; time += time_step_) { + const auto objects = makeDeviatedStraightPredictedObjects(time, deviation); + publishObjects(objects); + } + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_, deviation); + + const double num_points = time_delay_ / time_step_ + 1; + const double mean_deviation = deviation * (num_points - 1) / num_points; + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), mean_deviation, epsilon); +} +// ========================================================================================== diff --git a/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp b/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp index d0d8d61c4c231..53265872bffa8 100644 --- a/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp @@ -125,7 +125,7 @@ Stat calcTrajectoryLength(const Trajectory & traj) Stat calcTrajectoryDuration(const Trajectory & traj) { double duration = 0.0; - for (size_t i = 0; i < traj.points.size() - 1; ++i) { + for (size_t i = 0; i + 1 < traj.points.size(); ++i) { const double length = calcDistance2d(traj.points.at(i), traj.points.at(i + 1)); const double velocity = traj.points.at(i).longitudinal_velocity_mps; if (velocity != 0) { @@ -158,7 +158,7 @@ Stat calcTrajectoryAcceleration(const Trajectory & traj) Stat calcTrajectoryJerk(const Trajectory & traj) { Stat stat; - for (size_t i = 0; i < traj.points.size() - 1; ++i) { + for (size_t i = 0; i + 1 < traj.points.size(); ++i) { const double vel = traj.points.at(i).longitudinal_velocity_mps; if (vel != 0) { const double duration = diff --git a/evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt b/evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..4c1e8cf58c262 --- /dev/null +++ b/evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_metrics_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets Charts) +set(QT_WIDGETS_LIB Qt5::Widgets) +set(QT_CHARTS_LIB Qt5::Charts) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) +add_definitions(-DQT_NO_KEYWORDS) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/metrics_visualize_panel.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${QT_WIDGETS_LIB} + ${QT_CHARTS_LIB} +) + +target_compile_options(${PROJECT_NAME} PUBLIC -Wno-error=deprecated-copy -Wno-error=pedantic) +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + icons + plugins +) diff --git a/evaluator/tier4_metrics_rviz_plugin/README.md b/evaluator/tier4_metrics_rviz_plugin/README.md new file mode 100644 index 0000000000000..be94141254030 --- /dev/null +++ b/evaluator/tier4_metrics_rviz_plugin/README.md @@ -0,0 +1,16 @@ +# tier4_metrics_rviz_plugin + +## Purpose + +This plugin panel to visualize `planning_evaluator` output. + +## Inputs / Outputs + +| Name | Type | Description | +| ---------------------------------------- | --------------------------------------- | ------------------------------------- | +| `/diagnostic/planning_evaluator/metrics` | `diagnostic_msgs::msg::DiagnosticArray` | Subscribe `planning_evaluator` output | + +## HowToUse + +1. Start rviz and select panels/Add new panel. +2. Select MetricsVisualizePanel and press OK. diff --git a/evaluator/tier4_metrics_rviz_plugin/icons/classes/MetricsVisualizePanel.png b/evaluator/tier4_metrics_rviz_plugin/icons/classes/MetricsVisualizePanel.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/evaluator/tier4_metrics_rviz_plugin/icons/classes/MetricsVisualizePanel.png differ diff --git a/evaluator/tier4_metrics_rviz_plugin/package.xml b/evaluator/tier4_metrics_rviz_plugin/package.xml new file mode 100644 index 0000000000000..fe84a0ab81760 --- /dev/null +++ b/evaluator/tier4_metrics_rviz_plugin/package.xml @@ -0,0 +1,31 @@ + + + + tier4_metrics_rviz_plugin + 0.0.0 + The tier4_metrics_rviz_plugin package + Satoshi OTA + Kyoichi Sugahara + Maxime CLEMENT + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + diagnostic_msgs + libqt5-charts-dev + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rclcpp + rviz_common + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/evaluator/tier4_metrics_rviz_plugin/plugins/plugin_description.xml b/evaluator/tier4_metrics_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..5aca5bd7faa54 --- /dev/null +++ b/evaluator/tier4_metrics_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,5 @@ + + + MetricsVisualizePanel + + diff --git a/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp b/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp new file mode 100644 index 0000000000000..79da02b9b65c6 --- /dev/null +++ b/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp @@ -0,0 +1,86 @@ +// Copyright 2024 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// + +#include "metrics_visualize_panel.hpp" + +#include + +#include + +#include +#include +#include +#include + +namespace rviz_plugins +{ +MetricsVisualizePanel::MetricsVisualizePanel(QWidget * parent) +: rviz_common::Panel(parent), grid_(new QGridLayout()) +{ + setLayout(grid_); +} + +void MetricsVisualizePanel::onInitialize() +{ + using std::placeholders::_1; + + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + + sub_ = raw_node_->create_subscription( + "/diagnostic/planning_evaluator/metrics", rclcpp::QoS{1}, + std::bind(&MetricsVisualizePanel::onMetrics, this, _1)); + + const auto period = std::chrono::milliseconds(static_cast(1e3 / 10)); + timer_ = raw_node_->create_wall_timer(period, [&]() { onTimer(); }); +} + +void MetricsVisualizePanel::onTimer() +{ + std::lock_guard message_lock(mutex_); + + for (auto & [name, metric] : metrics_) { + metric.updateGraph(); + metric.updateTable(); + } +} + +void MetricsVisualizePanel::onMetrics(const DiagnosticArray::ConstSharedPtr msg) +{ + std::lock_guard message_lock(mutex_); + + const auto time = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9; + + constexpr size_t GRAPH_COL_SIZE = 5; + for (size_t i = 0; i < msg->status.size(); ++i) { + const auto & status = msg->status.at(i); + + if (metrics_.count(status.name) == 0) { + auto metric = Metric(status); + metrics_.emplace(status.name, metric); + grid_->addWidget(metric.getTable(), i / GRAPH_COL_SIZE * 2, i % GRAPH_COL_SIZE); + grid_->setRowStretch(i / GRAPH_COL_SIZE * 2, false); + grid_->addWidget(metric.getChartView(), i / GRAPH_COL_SIZE * 2 + 1, i % GRAPH_COL_SIZE); + grid_->setRowStretch(i / GRAPH_COL_SIZE * 2 + 1, true); + grid_->setColumnStretch(i % GRAPH_COL_SIZE, true); + } + + metrics_.at(status.name).updateData(time, status); + } +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::MetricsVisualizePanel, rviz_common::Panel) diff --git a/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp b/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp new file mode 100644 index 0000000000000..6708ecd7071e9 --- /dev/null +++ b/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp @@ -0,0 +1,205 @@ +// Copyright 2024 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// + +#ifndef METRICS_VISUALIZE_PANEL_HPP_ +#define METRICS_VISUALIZE_PANEL_HPP_ + +#ifndef Q_MOC_RUN +#include +#include +#include +#include +#include +#include +#include +#include +#include +#endif + +#include +#include + +#include + +#include +#include +#include + +namespace rviz_plugins +{ + +using diagnostic_msgs::msg::DiagnosticArray; +using diagnostic_msgs::msg::DiagnosticStatus; +using diagnostic_msgs::msg::KeyValue; +using QtCharts::QChart; +using QtCharts::QChartView; +using QtCharts::QLineSeries; + +struct Metric +{ +public: + explicit Metric(const DiagnosticStatus & status) : chart(new QChartView), table(new QTableWidget) + { + init(status); + } + + void init(const DiagnosticStatus & status) + { + QStringList header{}; + + { + auto label = new QLabel; + label->setAlignment(Qt::AlignCenter); + label->setText("metric_name"); + labels.emplace("metric_name", label); + + header.push_back(QString::fromStdString(status.name)); + } + + for (const auto & [key, value] : status.values) { + auto label = new QLabel; + label->setAlignment(Qt::AlignCenter); + labels.emplace(key, label); + + auto plot = new QLineSeries; + plot->setName(QString::fromStdString(key)); + plots.emplace(key, plot); + chart->chart()->addSeries(plot); + chart->chart()->createDefaultAxes(); + + header.push_back(QString::fromStdString(key)); + } + + { + chart->chart()->setTitle(QString::fromStdString(status.name)); + chart->chart()->legend()->setVisible(true); + chart->chart()->legend()->detachFromChart(); + chart->setRenderHint(QPainter::Antialiasing); + } + + { + table->setColumnCount(status.values.size() + 1); + table->setHorizontalHeaderLabels(header); + table->verticalHeader()->hide(); + table->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch); + table->setRowCount(1); + table->setSizeAdjustPolicy(QAbstractScrollArea::AdjustToContents); + } + } + + void updateData(const double time, const DiagnosticStatus & status) + { + for (const auto & [key, value] : status.values) { + const double data = std::stod(value); + labels.at(key)->setText(QString::fromStdString(toString(data))); + plots.at(key)->append(time, data); + updateMinMax(data); + } + + { + const auto area = chart->chart()->plotArea(); + const auto rect = chart->chart()->legend()->rect(); + chart->chart()->legend()->setGeometry( + QRectF(area.x(), area.y(), area.width(), rect.height())); + chart->chart()->axes(Qt::Horizontal).front()->setRange(time - 100.0, time); + } + + { + table->setCellWidget(0, 0, labels.at("metric_name")); + } + + for (size_t i = 0; i < status.values.size(); ++i) { + table->setCellWidget(0, i + 1, labels.at(status.values.at(i).key)); + } + } + + void updateMinMax(double data) + { + if (data < y_range_min) { + y_range_min = data > 0.0 ? 0.9 * data : 1.1 * data; + chart->chart()->axes(Qt::Vertical).front()->setMin(y_range_min); + } + + if (data > y_range_max) { + y_range_max = data > 0.0 ? 1.1 * data : 0.9 * data; + chart->chart()->axes(Qt::Vertical).front()->setMax(y_range_max); + } + } + + void updateTable() { table->update(); } + + void updateGraph() { chart->update(); } + + QChartView * getChartView() const { return chart; } + + QTableWidget * getTable() const { return table; } + +private: + static std::optional getValue(const DiagnosticStatus & status, std::string && key) + { + const auto itr = std::find_if( + status.values.begin(), status.values.end(), + [&](const auto & value) { return value.key == key; }); + + if (itr == status.values.end()) { + return std::nullopt; + } + + return itr->value; + } + + static std::string toString(const double & value) + { + std::stringstream ss; + ss << std::scientific << std::setprecision(2) << value; + return ss.str(); + } + + QChartView * chart; + QTableWidget * table; + + std::unordered_map labels; + std::unordered_map plots; + + double y_range_min{std::numeric_limits::max()}; + double y_range_max{std::numeric_limits::lowest()}; +}; + +class MetricsVisualizePanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit MetricsVisualizePanel(QWidget * parent = nullptr); + void onInitialize() override; + +private Q_SLOTS: + +private: + rclcpp::Node::SharedPtr raw_node_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Subscription::SharedPtr sub_; + + void onTimer(); + void onMetrics(const DiagnosticArray::ConstSharedPtr msg); + + QGridLayout * grid_; + + std::mutex mutex_; + std::unordered_map metrics_; +}; +} // namespace rviz_plugins + +#endif // METRICS_VISUALIZE_PANEL_HPP_ diff --git a/launch/tier4_autoware_api_launch/package.xml b/launch/tier4_autoware_api_launch/package.xml index 4d41ad0ac4a83..c55f1c8038e83 100644 --- a/launch/tier4_autoware_api_launch/package.xml +++ b/launch/tier4_autoware_api_launch/package.xml @@ -5,8 +5,6 @@ 0.0.0 The tier4_autoware_api_launch package Takagi, Isamu - yabuta - Kah Hooi Tan Ryohsuke Mitsudome Apache License 2.0 diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 06718a2cf0ffa..b49ff7bde2ed3 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -116,23 +116,6 @@ def launch_setup(context, *args, **kwargs): parameters=[nearest_search_param, lane_departure_checker_param, vehicle_info_param], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) - # control validator checker - control_validator_component = ComposableNode( - package="control_validator", - plugin="control_validator::ControlValidator", - name="control_validator", - remappings=[ - ("~/input/kinematics", "/localization/kinematic_state"), - ("~/input/reference_trajectory", "/planning/scenario_planning/trajectory"), - ( - "~/input/predicted_trajectory", - "/control/trajectory_follower/lateral/predicted_trajectory", - ), - ("~/output/validation_status", "~/validation_status"), - ], - parameters=[control_validator_param], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) # shift decider shift_decider_component = ComposableNode( @@ -351,7 +334,6 @@ def launch_setup(context, *args, **kwargs): executable=LaunchConfiguration("container_executable"), composable_node_descriptions=[ controller_component, - control_validator_component, lane_departure_component, shift_decider_component, vehicle_cmd_gate_component, @@ -360,6 +342,23 @@ def launch_setup(context, *args, **kwargs): ], ) + # control validator checker + control_validator_component = ComposableNode( + package="control_validator", + plugin="control_validator::ControlValidator", + name="control_validator", + remappings=[ + ("~/input/kinematics", "/localization/kinematic_state"), + ("~/input/reference_trajectory", "/planning/scenario_planning/trajectory"), + ( + "~/input/predicted_trajectory", + "/control/trajectory_follower/lateral/predicted_trajectory", + ), + ("~/output/validation_status", "~/validation_status"), + ], + parameters=[control_validator_param], + ) + group = GroupAction( [ PushRosNamespace("control"), @@ -372,7 +371,26 @@ def launch_setup(context, *args, **kwargs): ] ) - return [group] + control_validator_group = GroupAction( + [ + PushRosNamespace("control"), + ComposableNodeContainer( + name="control_validator_container", + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=[ + control_validator_component, + ComposableNode( + package="glog_component", + plugin="GlogComponent", + name="glog_validator_component", + ), + ], + ), + ] + ) + return [group, control_validator_group] def generate_launch_description(): diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml index 8538179253480..9d3fba05b4a6e 100644 --- a/launch/tier4_localization_launch/launch/localization.launch.xml +++ b/launch/tier4_localization_launch/launch/localization.launch.xml @@ -1,28 +1,32 @@ - - + - - - - + + + + + + + + - - - + + - + + + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml index 181f470a00800..225c7ab9d1146 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml @@ -1,17 +1,15 @@ - - - - - - - - - - - - + + + + + + + + + + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml index e4b04d3d4a32e..686e6d6cf12bc 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml @@ -2,14 +2,17 @@ - - + + + - + + + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml index 7733b4e3117a1..c00c90d467090 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml @@ -1,148 +1,116 @@ + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - + + + - - - - - - - - - - - - - - - - - - - - + + + + + + + + - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + - - + + + - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml index 23a1201a84958..bdea584bd58ae 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml @@ -27,10 +27,10 @@ - + diff --git a/launch/tier4_localization_launch/launch/util/util.launch.py b/launch/tier4_localization_launch/launch/util/util.launch.py deleted file mode 100644 index 1a34429f438ed..0000000000000 --- a/launch/tier4_localization_launch/launch/util/util.launch.py +++ /dev/null @@ -1,146 +0,0 @@ -# Copyright 2020 Tier IV, Inc. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import launch -from launch.actions import DeclareLaunchArgument -from launch.actions import OpaqueFunction -from launch.conditions import LaunchConfigurationNotEquals -from launch.conditions import UnlessCondition -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import LoadComposableNodes -from launch_ros.descriptions import ComposableNode -import yaml - - -def launch_setup(context, *args, **kwargs): - # https://github.com/ros2/launch_ros/issues/156 - def load_composable_node_param(param_path): - with open(LaunchConfiguration(param_path).perform(context), "r") as f: - return yaml.safe_load(f)["/**"]["ros__parameters"] - - crop_box_component = ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="crop_box_filter_measurement_range", - remappings=[ - ("input", LaunchConfiguration("input_pointcloud")), - ("output", "measurement_range/pointcloud"), - ], - parameters=[ - load_composable_node_param("crop_box_filter_measurement_range_param_path"), - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - voxel_grid_downsample_component = ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent", - name="voxel_grid_downsample_filter", - remappings=[ - ("input", "measurement_range/pointcloud"), - ("output", "voxel_grid_downsample/pointcloud"), - ], - parameters=[load_composable_node_param("voxel_grid_downsample_filter_param_path")], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - random_downsample_component = ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::RandomDownsampleFilterComponent", - name="random_downsample_filter", - remappings=[ - ("input", "voxel_grid_downsample/pointcloud"), - ("output", LaunchConfiguration("output/pointcloud")), - ], - parameters=[load_composable_node_param("random_downsample_filter_param_path")], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - composable_nodes = [ - crop_box_component, - voxel_grid_downsample_component, - random_downsample_component, - ] - - target_container = ( - "/sensing/lidar/top/pointcloud_preprocessor/pointcloud_container" - if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context) - else LaunchConfiguration("pointcloud_container_name") - ) - - load_composable_nodes = LoadComposableNodes( - condition=LaunchConfigurationNotEquals(target_container, ""), - composable_node_descriptions=composable_nodes, - target_container=target_container, - ) - - return [load_composable_nodes] - - -def generate_launch_description(): - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None, description=None): - arg = DeclareLaunchArgument(name, default_value=default_value, description=description) - launch_arguments.append(arg) - - add_launch_arg( - "crop_box_filter_measurement_range_param_path", - [ - LaunchConfiguration("crop_box_filter_measurement_range_param_path"), - ], - "path to the parameter file of crop_box_filter_measurement_range", - ) - add_launch_arg( - "voxel_grid_downsample_filter_param_path", - [ - LaunchConfiguration("voxel_grid_downsample_filter_param_path"), - ], - "path to the parameter file of voxel_grid_downsample_filter", - ) - add_launch_arg( - "random_downsample_filter_param_path", - [ - LaunchConfiguration("random_downsample_filter_param_path"), - ], - "path to the parameter file of random_downsample_filter", - ) - add_launch_arg("use_intra_process", "true", "use ROS 2 component container communication") - add_launch_arg("use_pointcloud_container", "True", "use pointcloud container") - add_launch_arg( - "pointcloud_container_name", - "/pointcloud_container", - "container name", - ) - - add_launch_arg( - "output/pointcloud", - "downsample/pointcloud", - "final output topic name", - ) - add_launch_arg( - "output_measurement_range_sensor_points_topic", - "measurement_range/pointcloud", - "output topic name for crop box filter", - ) - add_launch_arg( - "output_voxel_grid_downsample_sensor_points_topic", - "voxel_grid_downsample/pointcloud", - "output topic name for voxel grid downsample filter", - ) - add_launch_arg( - "output_downsample_sensor_points_topic", - "downsample/pointcloud", - "output topic name for downsample filter. this is final output", - ) - - return launch.LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/launch/tier4_localization_launch/launch/util/util.launch.xml b/launch/tier4_localization_launch/launch/util/util.launch.xml new file mode 100644 index 0000000000000..aeba276308646 --- /dev/null +++ b/launch/tier4_localization_launch/launch/util/util.launch.xml @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index c4de9c04dcaf2..d0046acf294b5 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -8,6 +8,10 @@ Koji Minoda Kento Yabuuchi Ryu Yamamoto + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Masahiro Sakamoto Apache License 2.0 Yamato Ando @@ -24,6 +28,7 @@ gyro_odometer ndt_scan_matcher pointcloud_preprocessor + pose_estimator_arbiter pose_initializer pose_instability_detector topic_tools diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py deleted file mode 100644 index 23bd2fc337c97..0000000000000 --- a/launch/tier4_map_launch/launch/map.launch.py +++ /dev/null @@ -1,223 +0,0 @@ -# Copyright 2021 Tier IV, Inc. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import os - -from ament_index_python import get_package_share_directory -import launch -from launch.actions import DeclareLaunchArgument -from launch.actions import GroupAction -from launch.actions import IncludeLaunchDescription -from launch.actions import OpaqueFunction -from launch.actions import SetLaunchConfiguration -from launch.conditions import IfCondition -from launch.conditions import UnlessCondition -from launch.launch_description_sources import AnyLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer -from launch_ros.actions import Node -from launch_ros.actions import PushRosNamespace -from launch_ros.descriptions import ComposableNode -from launch_ros.substitutions import FindPackageShare -import yaml - - -def launch_setup(context, *args, **kwargs): - lanelet2_map_loader_param_path = LaunchConfiguration("lanelet2_map_loader_param_path").perform( - context - ) - pointcloud_map_loader_param_path = LaunchConfiguration( - "pointcloud_map_loader_param_path" - ).perform(context) - - with open(lanelet2_map_loader_param_path, "r") as f: - lanelet2_map_loader_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(pointcloud_map_loader_param_path, "r") as f: - pointcloud_map_loader_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - map_hash_generator = Node( - package="map_loader", - executable="map_hash_generator", - name="map_hash_generator", - parameters=[ - { - "lanelet2_map_path": LaunchConfiguration("lanelet2_map_path"), - "pointcloud_map_path": LaunchConfiguration("pointcloud_map_path"), - } - ], - ) - - lanelet2_map_loader = ComposableNode( - package="map_loader", - plugin="Lanelet2MapLoaderNode", - name="lanelet2_map_loader", - remappings=[ - ("output/lanelet2_map", "vector_map"), - ], - parameters=[ - { - "lanelet2_map_path": LaunchConfiguration("lanelet2_map_path"), - }, - lanelet2_map_loader_param, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - lanelet2_map_visualization = ComposableNode( - package="map_loader", - plugin="Lanelet2MapVisualizationNode", - name="lanelet2_map_visualization", - remappings=[ - ("input/lanelet2_map", "vector_map"), - ("output/lanelet2_map_marker", "vector_map_marker"), - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - pointcloud_map_loader = ComposableNode( - package="map_loader", - plugin="PointCloudMapLoaderNode", - name="pointcloud_map_loader", - remappings=[ - ("output/pointcloud_map", "pointcloud_map"), - ("output/pointcloud_map_metadata", "pointcloud_map_metadata"), - ("service/get_partial_pcd_map", "/map/get_partial_pointcloud_map"), - ("service/get_differential_pcd_map", "/map/get_differential_pointcloud_map"), - ("service/get_selected_pcd_map", "/map/get_selected_pointcloud_map"), - ], - parameters=[ - {"pcd_paths_or_directory": ["[", LaunchConfiguration("pointcloud_map_path"), "]"]}, - {"pcd_metadata_path": [LaunchConfiguration("pointcloud_map_metadata_path")]}, - pointcloud_map_loader_param, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - map_tf_generator = ComposableNode( - package="map_tf_generator", - plugin="VectorMapTFGeneratorNode", - name="vector_map_tf_generator", - parameters=[ - { - "map_frame": "map", - "viewer_frame": "viewer", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - map_projection_loader_launch_file = os.path.join( - get_package_share_directory("map_projection_loader"), - "launch", - "map_projection_loader.launch.xml", - ) - map_projection_loader = IncludeLaunchDescription( - AnyLaunchDescriptionSource(map_projection_loader_launch_file), - launch_arguments={ - "map_projector_info_path": LaunchConfiguration("map_projector_info_path"), - "lanelet2_map_path": LaunchConfiguration("lanelet2_map_path"), - }.items(), - ) - - container = ComposableNodeContainer( - name="map_container", - namespace="", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=[ - lanelet2_map_loader, - lanelet2_map_visualization, - pointcloud_map_loader, - map_tf_generator, - ], - output="screen", - ) - - group = GroupAction( - [ - PushRosNamespace("map"), - container, - map_hash_generator, - map_projection_loader, - ] - ) - - return [group] - - -def generate_launch_description(): - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None, description=None): - launch_arguments.append( - DeclareLaunchArgument(name, default_value=default_value, description=description) - ) - - add_launch_arg("map_path", "", "path to map directory"), - add_launch_arg( - "lanelet2_map_path", - [LaunchConfiguration("map_path"), "/lanelet2_map.osm"], - "path to lanelet2 map file", - ), - add_launch_arg( - "pointcloud_map_path", - [LaunchConfiguration("map_path"), "/pointcloud_map.pcd"], - "path to pointcloud map file", - ), - add_launch_arg( - "pointcloud_map_metadata_path", - [LaunchConfiguration("map_path"), "/pointcloud_map_metadata.yaml"], - "path to pointcloud map metadata file", - ), - add_launch_arg( - "map_projector_info_path", - [LaunchConfiguration("map_path"), "/map_projector_info.yaml"], - "path to map projector info yaml file", - ), - add_launch_arg( - "lanelet2_map_loader_param_path", - [ - FindPackageShare("map_loader"), - "/config/lanelet2_map_loader.param.yaml", - ], - "path to lanelet2_map_loader param file", - ), - add_launch_arg( - "pointcloud_map_loader_param_path", - None, - "path to pointcloud_map_loader param file", - ), - add_launch_arg("use_intra_process", "false", "use ROS 2 component container communication"), - add_launch_arg("use_multithread", "false", "use multithread"), - - set_container_executable = SetLaunchConfiguration( - "container_executable", - "component_container", - condition=UnlessCondition(LaunchConfiguration("use_multithread")), - ) - - set_container_mt_executable = SetLaunchConfiguration( - "container_executable", - "component_container_mt", - condition=IfCondition(LaunchConfiguration("use_multithread")), - ) - - return launch.LaunchDescription( - launch_arguments - + [ - set_container_executable, - set_container_mt_executable, - ] - + [OpaqueFunction(function=launch_setup)] - ) diff --git a/launch/tier4_map_launch/launch/map.launch.xml b/launch/tier4_map_launch/launch/map.launch.xml index 26b2d462dd53f..132d1ec9be3ea 100644 --- a/launch/tier4_map_launch/launch/map.launch.xml +++ b/launch/tier4_map_launch/launch/map.launch.xml @@ -1,26 +1,68 @@ + + + + + + + + + + + - - - + + + + - - - - - - - - + + + + + + + + + + + + - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_map_launch/package.xml b/launch/tier4_map_launch/package.xml index d8f69c124526a..8fb41076204a8 100644 --- a/launch/tier4_map_launch/package.xml +++ b/launch/tier4_map_launch/package.xml @@ -7,6 +7,12 @@ Ryohsuke Mitsudome Ryu Yamamoto Koji Minoda + Kento Yabuuchi + Yamato Ando + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Masahiro Sakamoto Apache License 2.0 ament_cmake_auto 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 deleted file mode 100644 index b3e9e9148ee60..0000000000000 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ /dev/null @@ -1,383 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 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 deleted file mode 100644 index 1b4c3f38e038d..0000000000000 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ /dev/null @@ -1,434 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 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..9c3e815e8abb4 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 @@ -3,12 +3,11 @@ - + - - + @@ -31,17 +30,11 @@ - - - - - - - - + - + + @@ -62,25 +55,59 @@ - - - - - - + - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + - + + @@ -101,51 +128,113 @@ - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + - + + + + + + + + + + + + + + + + + + - - - - - - + + + + + + + + + + + + + + - + - + + + + + - - - - + + + + + + + - + + - + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml new file mode 100644 index 0000000000000..ddcbbbaeae107 --- /dev/null +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml @@ -0,0 +1,233 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml new file mode 100644 index 0000000000000..f9b65fd5c30eb --- /dev/null +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml new file mode 100644 index 0000000000000..0fabd5e98e45f --- /dev/null +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml new file mode 100644 index 0000000000000..b09684281d33a --- /dev/null +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 deleted file mode 100644 index ac521934265d7..0000000000000 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ /dev/null @@ -1,181 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml deleted file mode 100644 index ed37f6270c913..0000000000000 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml new file mode 100644 index 0000000000000..2f62e83ae0ef5 --- /dev/null +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml @@ -0,0 +1,124 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml new file mode 100644 index 0000000000000..b4d19c9052a63 --- /dev/null +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml @@ -0,0 +1,154 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml new file mode 100644 index 0000000000000..0ecc89c8743e8 --- /dev/null +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml @@ -0,0 +1,89 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py index 93d395ca3e466..a5af24595c593 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py @@ -20,7 +20,6 @@ from launch.conditions import IfCondition from launch.conditions import UnlessCondition from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode import yaml @@ -140,21 +139,11 @@ def launch_setup(context, *args, **kwargs): pipeline = PointcloudMapFilterPipeline(context) components = [] components.extend(pipeline.create_pipeline()) - individual_container = ComposableNodeContainer( - name=LaunchConfiguration("container_name"), - namespace="", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=components, - condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), - output="screen", - ) pointcloud_container_loader = LoadComposableNodes( composable_node_descriptions=components, - target_container=LaunchConfiguration("container_name"), - condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), + target_container=LaunchConfiguration("pointcloud_container_name"), ) - return [individual_container, pointcloud_container_loader] + return [pointcloud_container_loader] def generate_launch_description(): @@ -167,8 +156,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("output_topic", "") add_launch_arg("use_multithread", "False") add_launch_arg("use_intra_process", "True") - add_launch_arg("use_pointcloud_container", "False") - add_launch_arg("container_name", "pointcloud_map_filter_pipeline_container") + add_launch_arg("pointcloud_container_name", "pointcloud_container") add_launch_arg("use_pointcloud_map", "true") set_container_executable = SetLaunchConfiguration( "container_executable", diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml deleted file mode 100644 index 26e7af07646cd..0000000000000 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml +++ /dev/null @@ -1,46 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 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..fed268084792a 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,19 +26,17 @@ - - + - - + diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index 12190fb659235..99074d4ae4a98 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -21,7 +21,6 @@ from launch.conditions import UnlessCondition from launch.substitutions import LaunchConfiguration from launch.substitutions import PathJoinSubstitution -from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode from launch_ros.substitutions import FindPackageShare @@ -78,7 +77,7 @@ def create_additional_pipeline(self, lidar_name): plugin="pointcloud_preprocessor::CropBoxFilterComponent", name=f"{lidar_name}_crop_box_filter", remappings=[ - ("input", f"/sensing/lidar/{lidar_name}/outlier_filtered/pointcloud"), + ("input", f"/sensing/lidar/{lidar_name}/pointcloud"), ("output", f"{lidar_name}/range_cropped/pointcloud"), ], parameters=[ @@ -504,21 +503,11 @@ def launch_setup(context, *args, **kwargs): output_topic=pipeline.output_topic, ) ) - individual_container = ComposableNodeContainer( - name=LaunchConfiguration("container_name"), - namespace="", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=components, - condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), - output="screen", - ) pointcloud_container_loader = LoadComposableNodes( composable_node_descriptions=components, - target_container=LaunchConfiguration("container_name"), - condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), + target_container=LaunchConfiguration("pointcloud_container_name"), ) - return [individual_container, pointcloud_container_loader] + return [pointcloud_container_loader] def generate_launch_description(): @@ -530,8 +519,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("base_frame", "base_link") add_launch_arg("use_multithread", "False") add_launch_arg("use_intra_process", "True") - add_launch_arg("use_pointcloud_container", "False") - add_launch_arg("container_name", "perception_pipeline_container") + add_launch_arg("pointcloud_container_name", "pointcloud_container") add_launch_arg("input/pointcloud", "/sensing/lidar/concatenated/pointcloud") set_container_executable = SetLaunchConfiguration( diff --git a/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml b/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml index 751eeea66c7b6..fce3da96ce888 100644 --- a/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml +++ b/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml @@ -6,9 +6,8 @@ - - - + + @@ -23,8 +22,8 @@ - - + + @@ -39,8 +38,8 @@ - - + + @@ -48,4 +47,18 @@ + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 6582dfd32d91d..16396ef2961d8 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -10,10 +10,19 @@ + + + - + + + + + + + @@ -30,6 +39,9 @@ + + + @@ -70,7 +82,6 @@ default="false" description="if use_empty_dynamic_object_publisher:=true, /perception/object_recognition/objects topic has an empty DynamicObjectArray" /> - @@ -85,18 +96,18 @@ default="$(var data_path)/traffic_light_fine_detector" description="options: `tlr_yolox_s_batch_**`. The batch number must be either one of 1, 4, 6" /> - + - - - - - + @@ -119,8 +130,7 @@ - - + @@ -134,8 +144,7 @@ - - + @@ -171,22 +180,21 @@ + + + - + - - - - - + @@ -230,7 +238,18 @@ + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml index 58e9e231e4aa0..aa47d094d824e 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml @@ -7,8 +7,10 @@ - - + + + + @@ -22,10 +24,9 @@ - - - - + + + @@ -34,6 +35,8 @@ + + @@ -43,10 +46,11 @@ - - + + + @@ -54,15 +58,15 @@ - - - - - + + + + + @@ -72,11 +76,13 @@ - + + + @@ -84,6 +90,8 @@ + + @@ -93,10 +101,11 @@ - - + + + @@ -104,15 +113,15 @@ - - - - - + + + + + @@ -122,7 +131,8 @@ - + + diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py index a6bcb40e81252..e467aba5d4e98 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py @@ -17,6 +17,7 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction from launch.actions import SetLaunchConfiguration from launch.conditions import IfCondition from launch.conditions import UnlessCondition @@ -24,64 +25,29 @@ from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode +from launch_ros.parameter_descriptions import ParameterFile -def generate_launch_description(): - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None, description=None): - # a default_value of None is equivalent to not passing that kwarg at all - launch_arguments.append( - DeclareLaunchArgument(name, default_value=default_value, description=description) - ) - - fine_detector_share_dir = get_package_share_directory("traffic_light_fine_detector") - classifier_share_dir = get_package_share_directory("traffic_light_classifier") - add_launch_arg("enable_image_decompressor", "True") - add_launch_arg("enable_fine_detection", "True") - add_launch_arg("input/image", "/sensing/camera/traffic_light/image_raw") - add_launch_arg("output/rois", "/perception/traffic_light_recognition/rois") - add_launch_arg( - "output/traffic_signals", "/perception/traffic_light_recognition/traffic_signals" - ) - - # traffic_light_fine_detector - add_launch_arg( - "fine_detector_model_path", - os.path.join(fine_detector_share_dir, "data", "tlr_yolox_s.onnx"), - ) - add_launch_arg( - "fine_detector_label_path", - os.path.join(fine_detector_share_dir, "data", "tlr_labels.txt"), - ) - add_launch_arg("fine_detector_precision", "fp16") - add_launch_arg("fine_detector_score_thresh", "0.3") - add_launch_arg("fine_detector_nms_thresh", "0.65") - - add_launch_arg("approximate_sync", "False") - - # traffic_light_classifier - add_launch_arg("classifier_type", "1") - add_launch_arg( - "classifier_model_path", - os.path.join(classifier_share_dir, "data", "traffic_light_classifier_efficientNet_b1.onnx"), - ) - add_launch_arg( - "classifier_label_path", os.path.join(classifier_share_dir, "data", "lamp_labels.txt") - ) - add_launch_arg("classifier_precision", "fp16") - add_launch_arg("classifier_mean", "[123.675, 116.28, 103.53]") - add_launch_arg("classifier_std", "[58.395, 57.12, 57.375]") - - add_launch_arg("use_intra_process", "False") - add_launch_arg("use_multithread", "False") - +def launch_setup(context, *args, **kwargs): def create_parameter_dict(*args): result = {} for x in args: result[x] = LaunchConfiguration(x) return result + fine_detector_model_param = ParameterFile( + param_file=LaunchConfiguration("fine_detector_param_path").perform(context), + allow_substs=True, + ) + car_traffic_light_classifier_model_param = ParameterFile( + param_file=LaunchConfiguration("car_classifier_param_path").perform(context), + allow_substs=True, + ) + pedestrian_traffic_light_classifier_model_param = ParameterFile( + param_file=LaunchConfiguration("pedestrian_classifier_param_path").perform(context), + allow_substs=True, + ) + container = ComposableNodeContainer( name="traffic_light_node_container", namespace="", @@ -91,23 +57,28 @@ def create_parameter_dict(*args): ComposableNode( package="traffic_light_classifier", plugin="traffic_light::TrafficLightClassifierNodelet", - name="traffic_light_classifier", + name="car_traffic_light_classifier", namespace="classification", - parameters=[ - create_parameter_dict( - "approximate_sync", - "classifier_type", - "classifier_model_path", - "classifier_label_path", - "classifier_precision", - "classifier_mean", - "classifier_std", - ) + parameters=[car_traffic_light_classifier_model_param], + remappings=[ + ("~/input/image", LaunchConfiguration("input/image")), + ("~/input/rois", LaunchConfiguration("output/rois")), + ("~/output/traffic_signals", "classified/car/traffic_signals"), ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ), + ComposableNode( + package="traffic_light_classifier", + plugin="traffic_light::TrafficLightClassifierNodelet", + name="pedestrian_traffic_light_classifier", + namespace="classification", + parameters=[pedestrian_traffic_light_classifier_model_param], remappings=[ ("~/input/image", LaunchConfiguration("input/image")), ("~/input/rois", LaunchConfiguration("output/rois")), - ("~/output/traffic_signals", "classified/traffic_signals"), + ("~/output/traffic_signals", "classified/pedestrian/traffic_signals"), ], extra_arguments=[ {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} @@ -122,7 +93,10 @@ def create_parameter_dict(*args): ("~/input/image", LaunchConfiguration("input/image")), ("~/input/rois", LaunchConfiguration("output/rois")), ("~/input/rough/rois", "detection/rough/rois"), - ("~/input/traffic_signals", LaunchConfiguration("output/traffic_signals")), + ( + "~/input/traffic_signals", + LaunchConfiguration("output/traffic_signals"), + ), ("~/output/image", "debug/rois"), ("~/output/image/compressed", "debug/rois/compressed"), ("~/output/image/compressedDepth", "debug/rois/compressedDepth"), @@ -159,14 +133,6 @@ def create_parameter_dict(*args): condition=IfCondition(LaunchConfiguration("enable_image_decompressor")), ) - fine_detector_param = create_parameter_dict( - "fine_detector_model_path", - "fine_detector_label_path", - "fine_detector_precision", - "fine_detector_score_thresh", - "fine_detector_nms_thresh", - ) - fine_detector_loader = LoadComposableNodes( composable_node_descriptions=[ ComposableNode( @@ -174,7 +140,7 @@ def create_parameter_dict(*args): plugin="traffic_light::TrafficLightFineDetectorNodelet", name="traffic_light_fine_detector", namespace="detection", - parameters=[fine_detector_param], + parameters=[fine_detector_model_param], remappings=[ ("~/input/image", LaunchConfiguration("input/image")), ("~/input/rois", "rough/rois"), @@ -190,6 +156,61 @@ def create_parameter_dict(*args): condition=IfCondition(LaunchConfiguration("enable_fine_detection")), ) + return [container, decompressor_loader, fine_detector_loader] + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None, description=None): + # a default_value of None is equivalent to not passing that kwarg at all + launch_arguments.append( + DeclareLaunchArgument(name, default_value=default_value, description=description) + ) + + fine_detector_share_dir = get_package_share_directory("traffic_light_fine_detector") + classifier_share_dir = get_package_share_directory("traffic_light_classifier") + add_launch_arg("enable_image_decompressor", "True") + add_launch_arg("enable_fine_detection", "True") + add_launch_arg("input/image", "/sensing/camera/traffic_light/image_raw") + add_launch_arg("output/rois", "/perception/traffic_light_recognition/rois") + add_launch_arg( + "output/traffic_signals", + "/perception/traffic_light_recognition/traffic_signals", + ) + add_launch_arg( + "output/car/traffic_signals", "/perception/traffic_light_recognition/car/traffic_signals" + ) + add_launch_arg( + "output/pedestrian/traffic_signals", + "/perception/traffic_light_recognition/pedestrian/traffic_signals", + ) + + # traffic_light_fine_detector + add_launch_arg( + "fine_detector_param_path", + os.path.join(fine_detector_share_dir, "config", "traffic_light_fine_detector.param.yaml"), + ) + + # traffic_light_classifier + add_launch_arg( + "car_classifier_param_path", + os.path.join( + classifier_share_dir, "config", "car_traffic_light_classifier_efficientNet.param.yaml" + ), + ) + add_launch_arg( + "pedestrian_classifier_param_path", + os.path.join( + classifier_share_dir, + "config", + "pedestrian_traffic_light_classifier_efficientNet.param.yaml", + ), + ) + + add_launch_arg("use_intra_process", "False") + add_launch_arg("use_multithread", "False") + set_container_executable = SetLaunchConfiguration( "container_executable", "component_container", @@ -207,8 +228,6 @@ def create_parameter_dict(*args): *launch_arguments, set_container_executable, set_container_mt_executable, - container, - decompressor_loader, - fine_detector_loader, + OpaqueFunction(function=launch_setup), ] ) diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml index 7e6ba83747a8b..2d007ae8c5ff0 100644 --- a/launch/tier4_perception_launch/package.xml +++ b/launch/tier4_perception_launch/package.xml @@ -37,7 +37,6 @@ radar_object_clustering radar_object_tracker shape_estimation - tensorrt_yolo topic_tools tracking_object_merger traffic_light_arbiter diff --git a/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml b/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml index ec7545956e774..0885f48a6827d 100644 --- a/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml @@ -1,19 +1,10 @@ - - - - - - - - - - - - - - + + + + + diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index bf81125286256..710fb20631a45 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -7,6 +7,9 @@ + + + @@ -21,7 +24,10 @@ - + + + + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml index 73e466afc0bda..1ec74793b741b 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml @@ -1,4 +1,7 @@ + + + @@ -8,6 +11,8 @@ + + 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..d2aa009d9c699 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 @@ -4,6 +4,8 @@ + + @@ -14,6 +16,7 @@ + @@ -31,6 +34,68 @@ + + + + + + + + + + + + + + + + + + @@ -109,7 +174,12 @@ value="$(eval "'$(var behavior_velocity_planner_launch_modules)' + 'behavior_velocity_planner::NoDrivableLaneModulePlugin, '")" if="$(var launch_no_drivable_lane_module)" /> - + + @@ -129,19 +199,12 @@ - - - - - - - - - - + + + @@ -149,10 +212,10 @@ + - @@ -183,6 +246,8 @@ + + @@ -198,6 +263,7 @@ + @@ -206,59 +272,28 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + - - - + + + + + + + + + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml index 66c90ef2ff833..0a30204ca3c99 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml @@ -1,4 +1,6 @@ + + @@ -50,7 +52,10 @@ - + + + + diff --git a/launch/tier4_sensing_launch/launch/sensing.launch.xml b/launch/tier4_sensing_launch/launch/sensing.launch.xml index af9e4481a1430..391a1f8bad641 100644 --- a/launch/tier4_sensing_launch/launch/sensing.launch.xml +++ b/launch/tier4_sensing_launch/launch/sensing.launch.xml @@ -2,7 +2,6 @@ - @@ -14,7 +13,6 @@ - diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 4da6720b0af47..3801c448eaed4 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -85,6 +85,11 @@ + + + + + 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/launch/tier4_vehicle_launch/launch/vehicle.launch.xml b/launch/tier4_vehicle_launch/launch/vehicle.launch.xml index 0493e1ae0bbcf..0e1d22bfd1827 100644 --- a/launch/tier4_vehicle_launch/launch/vehicle.launch.xml +++ b/launch/tier4_vehicle_launch/launch/vehicle.launch.xml @@ -6,6 +6,7 @@ + @@ -21,6 +22,7 @@ + diff --git a/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md index 6492f20331a66..1b09420f5047d 100644 --- a/localization/ekf_localizer/README.md +++ b/localization/ekf_localizer/README.md @@ -32,71 +32,33 @@ This package includes the following features:

-## Launch - -The `ekf_localizer` starts with the default parameters with the following command. - -```sh -roslaunch ekf_localizer ekf_localizer.launch -``` - -The parameters and input topic names can be set in the `ekf_localizer.launch` file. - ## Node ### Subscribed Topics -- measured_pose_with_covariance (geometry_msgs/PoseWithCovarianceStamped) - - Input pose source with the measurement covariance matrix. - -- measured_twist_with_covariance (geometry_msgs/TwistWithCovarianceStamped) - - Input twist source with the measurement covariance matrix. - -- initialpose (geometry_msgs/PoseWithCovarianceStamped) - - Initial pose for EKF. The estimated pose is initialized with zeros at the start. It is initialized with this message whenever published. +| Name | Type | Description | +| -------------------------------- | ------------------------------------------------ | ---------------------------------------------------------------------------------------------------------------------------------------- | +| `measured_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Input pose source with the measurement covariance matrix. | +| `measured_twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | Input twist source with the measurement covariance matrix. | +| `initialpose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Initial pose for EKF. The estimated pose is initialized with zeros at the start. It is initialized with this message whenever published. | ### Published Topics -- ekf_odom (nav_msgs/Odometry) - - Estimated odometry. - -- ekf_pose (geometry_msgs/PoseStamped) - - Estimated pose. - -- ekf_pose_with_covariance (geometry_msgs/PoseWithCovarianceStamped) - - Estimated pose with covariance. - -- ekf_biased_pose (geometry_msgs/PoseStamped) - - Estimated pose including the yaw bias - -- ekf_biased_pose_with_covariance (geometry_msgs/PoseWithCovarianceStamped) - - Estimated pose with covariance including the yaw bias - -- ekf_twist (geometry_msgs/TwistStamped) - - Estimated twist. - -- ekf_twist_with_covariance (geometry_msgs/TwistWithCovarianceStamped) - - The estimated twist with covariance. - -- diagnostics (diagnostic_msgs/DiagnosticArray) - - The diagnostic information. +| Name | Type | Description | +| --------------------------------- | ------------------------------------------------ | ----------------------------------------------------- | +| `ekf_odom` | `nav_msgs::msg::Odometry` | Estimated odometry. | +| `ekf_pose` | `geometry_msgs::msg::PoseStamped` | Estimated pose. | +| `ekf_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose with covariance. | +| `ekf_biased_pose` | `geometry_msgs::msg::PoseStamped` | Estimated pose including the yaw bias | +| `ekf_biased_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose with covariance including the yaw bias | +| `ekf_twist` | `geometry_msgs::msg::TwistStamped` | Estimated twist. | +| `ekf_twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | The estimated twist with covariance. | +| `diagnostics` | `diagnostics_msgs::msg::DiagnosticArray` | The diagnostic information. | ### Published TF - base_link - - TF from "map" coordinate to estimated pose. + TF from `map` coordinate to estimated pose. ## Functions @@ -116,56 +78,33 @@ The parameters are set in `launch/ekf_localizer.launch` . ### For Node -| Name | Type | Description | Default value | -| :------------------------- | :----- | :---------------------------------------------------------------------------------------- | :------------ | -| show_debug_info | bool | Flag to display debug info | false | -| predict_frequency | double | Frequency for filtering and publishing [Hz] | 50.0 | -| tf_rate | double | Frequency for tf broadcasting [Hz] | 10.0 | -| extend_state_step | int | Max delay step which can be dealt with in EKF. Large number increases computational cost. | 50 | -| enable_yaw_bias_estimation | bool | Flag to enable yaw bias estimation | true | +{{ json_to_markdown("localization/ekf_localizer/schema/sub/node.sub_schema.json") }} ### For pose measurement -| Name | Type | Description | Default value | -| :---------------------------- | :----- | :------------------------------------------------------------ | :------------ | -| pose_additional_delay | double | Additional delay time for pose measurement [s] | 0.0 | -| pose_measure_uncertainty_time | double | Measured time uncertainty used for covariance calculation [s] | 0.01 | -| pose_smoothing_steps | int | A value for smoothing steps | 5 | -| pose_gate_dist | double | Limit of Mahalanobis distance used for outliers detection | 10000.0 | +{{ json_to_markdown("localization/ekf_localizer/schema/sub/pose_measurement.sub_schema.json") }} ### For twist measurement -| Name | Type | Description | Default value | -| :--------------------- | :----- | :-------------------------------------------------------- | :------------ | -| twist_additional_delay | double | Additional delay time for twist [s] | 0.0 | -| twist_smoothing_steps | int | A value for smoothing steps | 2 | -| twist_gate_dist | double | Limit of Mahalanobis distance used for outliers detection | 10000.0 | +{{ json_to_markdown("localization/ekf_localizer/schema/sub/twist_measurement.sub_schema.json") }} ### For process noise -| Name | Type | Description | Default value | -| :--------------------- | :----- | :--------------------------------------------------------------------------------------------------------------- | :------------ | -| proc_stddev_vx_c | double | Standard deviation of process noise in time differentiation expression of linear velocity x, noise for d_vx = 0 | 2.0 | -| proc_stddev_wz_c | double | Standard deviation of process noise in time differentiation expression of angular velocity z, noise for d_wz = 0 | 0.2 | -| proc_stddev_yaw_c | double | Standard deviation of process noise in time differentiation expression of yaw, noise for d_yaw = omega | 0.005 | -| proc_stddev_yaw_bias_c | double | Standard deviation of process noise in time differentiation expression of yaw_bias, noise for d_yaw_bias = 0 | 0.001 | +{{ json_to_markdown("localization/ekf_localizer/schema/sub/process_noise.sub_schema.json") }} note: process noise for positions x & y are calculated automatically from nonlinear dynamics. +### Simple 1D Filter Parameters + +{{ json_to_markdown("localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json") }} + ### For diagnostics -| Name | Type | Description | Default value | -| :------------------------------------ | :----- | :----------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| pose_no_update_count_threshold_warn | size_t | The threshold at which a WARN state is triggered due to the Pose Topic update not happening continuously for a certain number of times. | 50 | -| pose_no_update_count_threshold_error | size_t | The threshold at which an ERROR state is triggered due to the Pose Topic update not happening continuously for a certain number of times. | 250 | -| twist_no_update_count_threshold_warn | size_t | The threshold at which a WARN state is triggered due to the Twist Topic update not happening continuously for a certain number of times. | 50 | -| twist_no_update_count_threshold_error | size_t | The threshold at which an ERROR state is triggered due to the Twist Topic update not happening continuously for a certain number of times. | 250 | +{{ json_to_markdown("localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json") }} ### Misc -| Name | Type | Description | Default value | -| :-------------------------------- | :----- | :------------------------------------------------------------------------------------------------- | :------------- | -| threshold_observable_velocity_mps | double | Minimum value for velocity that will be used for EKF. Mainly used for dead zone in velocity sensor | 0.0 (disabled) | +{{ json_to_markdown("localization/ekf_localizer/schema/sub/misc.sub_schema.json") }} ## How to tune EKF parameters @@ -197,7 +136,9 @@ Increasing the number will improve the smoothness of the estimation, but may hav -where `b_k` is the yawbias. +where, $\theta_k$ represents the vehicle's heading angle, including the mounting angle bias. +$b_k$ is a correction term for the yaw bias, and it is modeled so that $(\theta_k+b_k)$ becomes the heading angle of the base_link. +The pose_estimator is expected to publish the base_link in the map coordinate system. However, the yaw angle may be offset due to calibration errors. This model compensates this error and improves estimation accuracy. ### time delay model @@ -232,7 +173,7 @@ Note that, although the dimension gets larger since the analytical expansion can ## Known issues -- In the presence of multiple inputs with yaw estimation, yaw bias `b_k` in the current EKF state would not make any sense, since it is intended to capture the extrinsic parameter's calibration error of a sensor. Thus, future work includes introducing yaw bias for each sensor with yaw estimation. +- If multiple pose_estimators are used, the input to the EKF will include multiple yaw biases corresponding to each source. However, the current EKF assumes the existence of only one yaw bias. Therefore, yaw bias `b_k` in the current EKF state would not make any sense and cannot correctly handle these multiple yaw biases. Thus, future work includes introducing yaw bias for each sensor with yaw estimation. ## reference diff --git a/localization/ekf_localizer/config/ekf_localizer.param.yaml b/localization/ekf_localizer/config/ekf_localizer.param.yaml index 1c16624605907..4cc71e0904ca8 100644 --- a/localization/ekf_localizer/config/ekf_localizer.param.yaml +++ b/localization/ekf_localizer/config/ekf_localizer.param.yaml @@ -1,32 +1,46 @@ /**: ros__parameters: - show_debug_info: false - enable_yaw_bias_estimation: true - predict_frequency: 50.0 - tf_rate: 50.0 - extend_state_step: 50 + node: + show_debug_info: false + enable_yaw_bias_estimation: true + predict_frequency: 50.0 + tf_rate: 50.0 + publish_tf: true + extend_state_step: 50 - # for Pose measurement - pose_additional_delay: 0.0 - pose_measure_uncertainty_time: 0.01 - pose_smoothing_steps: 5 - pose_gate_dist: 10000.0 + pose_measurement: + # for Pose measurement + pose_additional_delay: 0.0 + pose_measure_uncertainty_time: 0.01 + pose_smoothing_steps: 5 + pose_gate_dist: 10000.0 - # for twist measurement - twist_additional_delay: 0.0 - twist_smoothing_steps: 2 - twist_gate_dist: 10000.0 + twist_measurement: + # for twist measurement + twist_additional_delay: 0.0 + twist_smoothing_steps: 2 + twist_gate_dist: 10000.0 - # for process model - proc_stddev_yaw_c: 0.005 - proc_stddev_vx_c: 10.0 - proc_stddev_wz_c: 5.0 + process_noise: + # for process model + proc_stddev_yaw_c: 0.005 + proc_stddev_vx_c: 10.0 + proc_stddev_wz_c: 5.0 - # for diagnostics - pose_no_update_count_threshold_warn: 50 - pose_no_update_count_threshold_error: 100 - twist_no_update_count_threshold_warn: 50 - twist_no_update_count_threshold_error: 100 + simple_1d_filter_parameters: + #Simple1DFilter parameters + z_filter_proc_dev: 1.0 + roll_filter_proc_dev: 0.01 + pitch_filter_proc_dev: 0.01 - # for velocity measurement limitation (Set 0.0 if you want to ignore) - threshold_observable_velocity_mps: 0.0 # [m/s] + diagnostics: + # for diagnostics + pose_no_update_count_threshold_warn: 50 + pose_no_update_count_threshold_error: 100 + twist_no_update_count_threshold_warn: 50 + twist_no_update_count_threshold_error: 100 + + misc: + # for velocity measurement limitation (Set 0.0 if you want to ignore) + threshold_observable_velocity_mps: 0.0 # [m/s] + pose_frame_id: "map" diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index e2ffff2195645..6925e8b63c66b 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 */ @@ -200,7 +199,7 @@ class EKFLocalizer : public rclcpp::Node /** * @brief update predict frequency */ - void updatePredictFrequency(); + void updatePredictFrequency(const rclcpp::Time & current_time); /** * @brief get transform from frame_id @@ -220,7 +219,7 @@ class EKFLocalizer : public rclcpp::Node /** * @brief publish diagnostics message */ - void publishDiagnostics(); + void publishDiagnostics(const rclcpp::Time & current_time); /** * @brief update simple1DFilter 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/hyper_parameters.hpp b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp index 01ef658cf445d..56a13d1ecaf55 100644 --- a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp @@ -24,32 +24,41 @@ class HyperParameters { public: explicit HyperParameters(rclcpp::Node * node) - : show_debug_info(node->declare_parameter("show_debug_info", false)), - ekf_rate(node->declare_parameter("predict_frequency", 50.0)), + : show_debug_info(node->declare_parameter("node.show_debug_info")), + ekf_rate(node->declare_parameter("node.predict_frequency")), ekf_dt(1.0 / std::max(ekf_rate, 0.1)), - tf_rate_(node->declare_parameter("tf_rate", 10.0)), - enable_yaw_bias_estimation(node->declare_parameter("enable_yaw_bias_estimation", true)), - extend_state_step(node->declare_parameter("extend_state_step", 50)), - pose_frame_id(node->declare_parameter("pose_frame_id", std::string("map"))), - pose_additional_delay(node->declare_parameter("pose_additional_delay", 0.0)), - pose_gate_dist(node->declare_parameter("pose_gate_dist", 10000.0)), - pose_smoothing_steps(node->declare_parameter("pose_smoothing_steps", 5)), - twist_additional_delay(node->declare_parameter("twist_additional_delay", 0.0)), - twist_gate_dist(node->declare_parameter("twist_gate_dist", 10000.0)), - twist_smoothing_steps(node->declare_parameter("twist_smoothing_steps", 2)), - proc_stddev_vx_c(node->declare_parameter("proc_stddev_vx_c", 5.0)), - proc_stddev_wz_c(node->declare_parameter("proc_stddev_wz_c", 1.0)), - proc_stddev_yaw_c(node->declare_parameter("proc_stddev_yaw_c", 0.005)), + tf_rate_(node->declare_parameter("node.tf_rate")), + publish_tf_(node->declare_parameter("node.publish_tf")), + enable_yaw_bias_estimation(node->declare_parameter("node.enable_yaw_bias_estimation")), + extend_state_step(node->declare_parameter("node.extend_state_step")), + pose_frame_id(node->declare_parameter("misc.pose_frame_id")), + pose_additional_delay( + node->declare_parameter("pose_measurement.pose_additional_delay")), + pose_gate_dist(node->declare_parameter("pose_measurement.pose_gate_dist")), + pose_smoothing_steps(node->declare_parameter("pose_measurement.pose_smoothing_steps")), + twist_additional_delay( + node->declare_parameter("twist_measurement.twist_additional_delay")), + twist_gate_dist(node->declare_parameter("twist_measurement.twist_gate_dist")), + twist_smoothing_steps(node->declare_parameter("twist_measurement.twist_smoothing_steps")), + proc_stddev_vx_c(node->declare_parameter("process_noise.proc_stddev_vx_c")), + proc_stddev_wz_c(node->declare_parameter("process_noise.proc_stddev_wz_c")), + proc_stddev_yaw_c(node->declare_parameter("process_noise.proc_stddev_yaw_c")), + z_filter_proc_dev( + node->declare_parameter("simple_1d_filter_parameters.z_filter_proc_dev")), + roll_filter_proc_dev( + node->declare_parameter("simple_1d_filter_parameters.roll_filter_proc_dev")), + pitch_filter_proc_dev( + node->declare_parameter("simple_1d_filter_parameters.pitch_filter_proc_dev")), pose_no_update_count_threshold_warn( - node->declare_parameter("pose_no_update_count_threshold_warn", 50)), + node->declare_parameter("diagnostics.pose_no_update_count_threshold_warn")), pose_no_update_count_threshold_error( - node->declare_parameter("pose_no_update_count_threshold_error", 250)), + node->declare_parameter("diagnostics.pose_no_update_count_threshold_error")), twist_no_update_count_threshold_warn( - node->declare_parameter("twist_no_update_count_threshold_warn", 50)), + node->declare_parameter("diagnostics.twist_no_update_count_threshold_warn")), twist_no_update_count_threshold_error( - node->declare_parameter("twist_no_update_count_threshold_error", 250)), + node->declare_parameter("diagnostics.twist_no_update_count_threshold_error")), threshold_observable_velocity_mps( - node->declare_parameter("threshold_observable_velocity_mps", 0.5)) + node->declare_parameter("misc.threshold_observable_velocity_mps")) { } @@ -57,6 +66,7 @@ class HyperParameters const double ekf_rate; const double ekf_dt; const double tf_rate_; + const bool publish_tf_; const bool enable_yaw_bias_estimation; const int extend_state_step; const std::string pose_frame_id; @@ -69,6 +79,9 @@ class HyperParameters const double proc_stddev_vx_c; //!< @brief vx process noise const double proc_stddev_wz_c; //!< @brief wz process noise const double proc_stddev_yaw_c; //!< @brief yaw process noise + const double z_filter_proc_dev; + const double roll_filter_proc_dev; + const double pitch_filter_proc_dev; const size_t pose_no_update_count_threshold_warn; const size_t pose_no_update_count_threshold_error; const size_t twist_no_update_count_threshold_warn; 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/launch/ekf_localizer.launch.xml b/localization/ekf_localizer/launch/ekf_localizer.launch.xml index ee0504293602a..b6a1bd06185c2 100644 --- a/localization/ekf_localizer/launch/ekf_localizer.launch.xml +++ b/localization/ekf_localizer/launch/ekf_localizer.launch.xml @@ -25,8 +25,6 @@ - - diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml index 4101bae88b6e2..e9d756e547f26 100644 --- a/localization/ekf_localizer/package.xml +++ b/localization/ekf_localizer/package.xml @@ -9,6 +9,11 @@ Yamato Ando Takeshi Ishita Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Takamasa Horibe diff --git a/localization/ekf_localizer/schema/ekf_localizer.schema.json b/localization/ekf_localizer/schema/ekf_localizer.schema.json new file mode 100644 index 0000000000000..61fbcc2973aae --- /dev/null +++ b/localization/ekf_localizer/schema/ekf_localizer.schema.json @@ -0,0 +1,52 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "EKF Localizer Configuration", + "type": "object", + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "type": "object", + "properties": { + "node": { + "$ref": "sub/node.sub_schema.json#/definitions/node" + }, + "pose_measurement": { + "$ref": "sub/pose_measurement.sub_schema.json#/definitions/pose_measurement" + }, + "twist_measurement": { + "$ref": "sub/twist_measurement.sub_schema.json#/definitions/twist_measurement" + }, + "process_noise": { + "$ref": "sub/process_noise.sub_schema.json#/definitions/process_noise" + }, + "simple_1d_filter_parameters": { + "$ref": "sub/simple_1d_filter_parameters.sub_schema.json#/definitions/simple_1d_filter_parameters" + }, + "diagnostics": { + "$ref": "sub/diagnostics.sub_schema.json#/definitions/diagnostics" + }, + "misc": { + "$ref": "sub/misc.sub_schema.json#/definitions/misc" + } + }, + "required": [ + "node", + "pose_measurement", + "twist_measurement", + "process_noise", + "simple_1d_filter_parameters", + "diagnostics", + "misc" + ], + "additionalProperties": false + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json b/localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json new file mode 100644 index 0000000000000..2e2dca411971e --- /dev/null +++ b/localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json @@ -0,0 +1,38 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "EKF Localizer Configuration for Diagnostics", + "definitions": { + "diagnostics": { + "type": "object", + "properties": { + "pose_no_update_count_threshold_warn": { + "type": "integer", + "description": "The threshold at which a WARN state is triggered due to the Pose Topic update not happening continuously for a certain number of times", + "default": 50 + }, + "pose_no_update_count_threshold_error": { + "type": "integer", + "description": "The threshold at which an ERROR state is triggered due to the Pose Topic update not happening continuously for a certain number of times", + "default": 100 + }, + "twist_no_update_count_threshold_warn": { + "type": "integer", + "description": "The threshold at which a WARN state is triggered due to the Twist Topic update not happening continuously for a certain number of times", + "default": 50 + }, + "twist_no_update_count_threshold_error": { + "type": "integer", + "description": "The threshold at which an ERROR state is triggered due to the Twist Topic update not happening continuously for a certain number of times", + "default": 100 + } + }, + "required": [ + "pose_no_update_count_threshold_warn", + "pose_no_update_count_threshold_error", + "twist_no_update_count_threshold_warn", + "twist_no_update_count_threshold_error" + ], + "additionalProperties": false + } + } +} diff --git a/localization/ekf_localizer/schema/sub/misc.sub_schema.json b/localization/ekf_localizer/schema/sub/misc.sub_schema.json new file mode 100644 index 0000000000000..cc36a5bf41ec6 --- /dev/null +++ b/localization/ekf_localizer/schema/sub/misc.sub_schema.json @@ -0,0 +1,23 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "EKF Localizer Configuration for MISC", + "definitions": { + "misc": { + "type": "object", + "properties": { + "threshold_observable_velocity_mps": { + "type": "number", + "description": "Minimum value for velocity that will be used for EKF. Mainly used for dead zone in velocity sensor [m/s] (0.0 means disabled)", + "default": 0.0 + }, + "pose_frame_id": { + "type": "string", + "description": "Parent frame_id of EKF output pose", + "default": "map" + } + }, + "required": ["threshold_observable_velocity_mps", "pose_frame_id"], + "additionalProperties": false + } + } +} diff --git a/localization/ekf_localizer/schema/sub/node.sub_schema.json b/localization/ekf_localizer/schema/sub/node.sub_schema.json new file mode 100644 index 0000000000000..92e083b92d3e7 --- /dev/null +++ b/localization/ekf_localizer/schema/sub/node.sub_schema.json @@ -0,0 +1,50 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "EKF Localizer Configuration for node", + "definitions": { + "node": { + "type": "object", + "properties": { + "show_debug_info": { + "type": "boolean", + "description": "Flag to display debug info", + "default": false + }, + "predict_frequency": { + "type": "number", + "description": "Frequency for filtering and publishing [Hz]", + "default": 50.0 + }, + "tf_rate": { + "type": "number", + "description": "Frequency for tf broadcasting [Hz]", + "default": 50.0 + }, + "publish_tf": { + "type": "boolean", + "description": "Whether to publish tf", + "default": true + }, + "extend_state_step": { + "type": "integer", + "description": "Max delay step which can be dealt with in EKF. Large number increases computational cost.", + "default": 50 + }, + "enable_yaw_bias_estimation": { + "type": "boolean", + "description": "Flag to enable yaw bias estimation", + "default": true + } + }, + "required": [ + "show_debug_info", + "predict_frequency", + "tf_rate", + "publish_tf", + "extend_state_step", + "enable_yaw_bias_estimation" + ], + "additionalProperties": false + } + } +} diff --git a/localization/ekf_localizer/schema/sub/pose_measurement.sub_schema.json b/localization/ekf_localizer/schema/sub/pose_measurement.sub_schema.json new file mode 100644 index 0000000000000..8b931d5f68d12 --- /dev/null +++ b/localization/ekf_localizer/schema/sub/pose_measurement.sub_schema.json @@ -0,0 +1,38 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "EKF Localizer Configuration for Pose Measurement", + "definitions": { + "pose_measurement": { + "type": "object", + "properties": { + "pose_additional_delay": { + "type": "number", + "description": "Additional delay time for pose measurement [s]", + "default": 0.0 + }, + "pose_measure_uncertainty_time": { + "type": "number", + "description": "Measured time uncertainty used for covariance calculation [s]", + "default": 0.01 + }, + "pose_smoothing_steps": { + "type": "integer", + "description": "A value for smoothing steps", + "default": 5 + }, + "pose_gate_dist": { + "type": "number", + "description": "Limit of Mahalanobis distance used for outliers detection", + "default": 10000.0 + } + }, + "required": [ + "pose_additional_delay", + "pose_measure_uncertainty_time", + "pose_smoothing_steps", + "pose_gate_dist" + ], + "additionalProperties": false + } + } +} diff --git a/localization/ekf_localizer/schema/sub/process_noise.sub_schema.json b/localization/ekf_localizer/schema/sub/process_noise.sub_schema.json new file mode 100644 index 0000000000000..37a8c248edd2c --- /dev/null +++ b/localization/ekf_localizer/schema/sub/process_noise.sub_schema.json @@ -0,0 +1,28 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "EKF Localizer Configuration for Process Noise", + "definitions": { + "process_noise": { + "type": "object", + "properties": { + "proc_stddev_vx_c": { + "type": "number", + "description": "Standard deviation of process noise in time differentiation expression of linear velocity x, noise for d_vx = 0", + "default": 10.0 + }, + "proc_stddev_wz_c": { + "type": "number", + "description": "Standard deviation of process noise in time differentiation expression of angular velocity z, noise for d_wz = 0", + "default": 5.0 + }, + "proc_stddev_yaw_c": { + "type": "number", + "description": "Standard deviation of process noise in time differentiation expression of yaw, noise for d_yaw = omega", + "default": 0.005 + } + }, + "required": ["proc_stddev_yaw_c", "proc_stddev_vx_c", "proc_stddev_wz_c"], + "additionalProperties": false + } + } +} diff --git a/localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json b/localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json new file mode 100644 index 0000000000000..ad2f638a18d5f --- /dev/null +++ b/localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json @@ -0,0 +1,28 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "EKF Localizer Configuration of Simple 1D Filter Parameters", + "definitions": { + "simple_1d_filter_parameters": { + "type": "object", + "properties": { + "z_filter_proc_dev": { + "type": "number", + "description": "Simple1DFilter - Z filter process deviation", + "default": 1.0 + }, + "roll_filter_proc_dev": { + "type": "number", + "description": "Simple1DFilter - Roll filter process deviation", + "default": 0.01 + }, + "pitch_filter_proc_dev": { + "type": "number", + "description": "Simple1DFilter - Pitch filter process deviation", + "default": 0.01 + } + }, + "required": ["z_filter_proc_dev", "roll_filter_proc_dev", "pitch_filter_proc_dev"], + "additionalProperties": false + } + } +} diff --git a/localization/ekf_localizer/schema/sub/twist_measurement.sub_schema.json b/localization/ekf_localizer/schema/sub/twist_measurement.sub_schema.json new file mode 100644 index 0000000000000..727830a90a288 --- /dev/null +++ b/localization/ekf_localizer/schema/sub/twist_measurement.sub_schema.json @@ -0,0 +1,28 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "EKF Localizer Configuration for Twist Measurement", + "definitions": { + "twist_measurement": { + "type": "object", + "properties": { + "twist_additional_delay": { + "type": "number", + "description": "Additional delay time for twist [s]", + "default": 0.0 + }, + "twist_smoothing_steps": { + "type": "integer", + "description": "A value for smoothing steps", + "default": 2 + }, + "twist_gate_dist": { + "type": "number", + "description": "Limit of Mahalanobis distance used for outliers detection", + "default": 10000.0 + } + }, + "required": ["twist_additional_delay", "twist_smoothing_steps", "twist_gate_dist"], + "additionalProperties": false + } + } +} diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 5dd78a00f2fbd..f77481d45a534 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) @@ -61,9 +60,11 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti this, get_clock(), rclcpp::Duration::from_seconds(ekf_dt_), std::bind(&EKFLocalizer::timerCallback, this)); - timer_tf_ = rclcpp::create_timer( - this, get_clock(), rclcpp::Rate(params_.tf_rate_).period(), - std::bind(&EKFLocalizer::timerTFCallback, this)); + if (params_.publish_tf_) { + timer_tf_ = rclcpp::create_timer( + this, get_clock(), rclcpp::Rate(params_.tf_rate_).period(), + std::bind(&EKFLocalizer::timerTFCallback, this)); + } pub_pose_ = create_publisher("ekf_pose", 1); pub_pose_cov_ = @@ -95,23 +96,36 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti ekf_module_ = std::make_unique(warning_, params_); logger_configure_ = std::make_unique(this); - z_filter_.set_proc_dev(1.0); - roll_filter_.set_proc_dev(0.01); - pitch_filter_.set_proc_dev(0.01); + z_filter_.set_proc_dev(params_.z_filter_proc_dev); + roll_filter_.set_proc_dev(params_.roll_filter_proc_dev); + pitch_filter_.set_proc_dev(params_.pitch_filter_proc_dev); } /* * updatePredictFrequency */ -void EKFLocalizer::updatePredictFrequency() +void EKFLocalizer::updatePredictFrequency(const rclcpp::Time & current_time) { if (last_predict_time_) { - if (get_clock()->now() < *last_predict_time_) { + if (current_time < *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_ = (current_time - *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); @@ -119,7 +133,7 @@ void EKFLocalizer::updatePredictFrequency() proc_cov_yaw_d_ = std::pow(params_.proc_stddev_yaw_c * ekf_dt_, 2.0); } } - last_predict_time_ = std::make_shared(get_clock()->now()); + last_predict_time_ = std::make_shared(current_time); } /* @@ -127,17 +141,19 @@ void EKFLocalizer::updatePredictFrequency() */ void EKFLocalizer::timerCallback() { + const rclcpp::Time current_time = this->now(); + if (!is_activated_) { warning_->warnThrottle( "The node is not activated. Provide initial pose to pose_initializer", 2000); - publishDiagnostics(); + publishDiagnostics(current_time); return; } DEBUG_INFO(get_logger(), "========================= timer called ========================="); /* update predict frequency with measured timer rate */ - updatePredictFrequency(); + updatePredictFrequency(current_time); /* predict model in EKF */ stop_watch_.tic(); @@ -161,11 +177,11 @@ void EKFLocalizer::timerCallback() stop_watch_.tic(); // save the initial size because the queue size can change in the loop - const auto t_curr = this->now(); + const auto t_curr = current_time; const size_t n = pose_queue_.size(); for (size_t i = 0; i < n; ++i) { const auto pose = pose_queue_.pop_increment_age(); - bool is_updated = ekf_module_->measurementUpdatePose(*pose, 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; @@ -196,12 +212,11 @@ void EKFLocalizer::timerCallback() stop_watch_.tic(); // save the initial size because the queue size can change in the loop - const auto t_curr = this->now(); + const auto t_curr = current_time; const size_t n = twist_queue_.size(); for (size_t i = 0; i < n; ++i) { const auto twist = twist_queue_.pop_increment_age(); - bool is_updated = - ekf_module_->measurementUpdateTwist(*twist, 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; } @@ -215,15 +230,15 @@ void EKFLocalizer::timerCallback() const double roll = roll_filter_.get_x(); const double pitch = pitch_filter_.get_x(); const geometry_msgs::msg::PoseStamped current_ekf_pose = - ekf_module_->getCurrentPose(this->now(), z, roll, pitch, false); + ekf_module_->getCurrentPose(current_time, z, roll, pitch, false); const geometry_msgs::msg::PoseStamped current_biased_ekf_pose = - ekf_module_->getCurrentPose(this->now(), z, roll, pitch, true); + ekf_module_->getCurrentPose(current_time, z, roll, pitch, true); const geometry_msgs::msg::TwistStamped current_ekf_twist = - ekf_module_->getCurrentTwist(this->now()); + ekf_module_->getCurrentTwist(current_time); /* publish ekf result */ publishEstimateResult(current_ekf_pose, current_biased_ekf_pose, current_ekf_twist); - publishDiagnostics(); + publishDiagnostics(current_time); } /* @@ -243,10 +258,12 @@ void EKFLocalizer::timerTFCallback() const double roll = roll_filter_.get_x(); const double pitch = pitch_filter_.get_x(); + const rclcpp::Time current_time = this->now(); + geometry_msgs::msg::TransformStamped transform_stamped; transform_stamped = tier4_autoware_utils::pose2transform( - ekf_module_->getCurrentPose(this->now(), z, roll, pitch, false), "base_link"); - transform_stamped.header.stamp = this->now(); + ekf_module_->getCurrentPose(current_time, z, roll, pitch, false), "base_link"); + transform_stamped.header.stamp = current_time; tf_br_->sendTransform(transform_stamped); } @@ -327,15 +344,13 @@ void EKFLocalizer::publishEstimateResult( const geometry_msgs::msg::PoseStamped & current_biased_ekf_pose, const geometry_msgs::msg::TwistStamped & current_ekf_twist) { - rclcpp::Time current_time = this->now(); - /* publish latest pose */ pub_pose_->publish(current_ekf_pose); pub_biased_pose_->publish(current_biased_ekf_pose); /* publish latest pose with covariance */ geometry_msgs::msg::PoseWithCovarianceStamped pose_cov; - pose_cov.header.stamp = current_time; + pose_cov.header.stamp = current_ekf_pose.header.stamp; pose_cov.header.frame_id = current_ekf_pose.header.frame_id; pose_cov.pose.pose = current_ekf_pose.pose; pose_cov.pose.covariance = ekf_module_->getCurrentPoseCovariance(); @@ -350,7 +365,7 @@ void EKFLocalizer::publishEstimateResult( /* publish latest twist with covariance */ geometry_msgs::msg::TwistWithCovarianceStamped twist_cov; - twist_cov.header.stamp = current_time; + twist_cov.header.stamp = current_ekf_twist.header.stamp; twist_cov.header.frame_id = current_ekf_twist.header.frame_id; twist_cov.twist.twist = current_ekf_twist.twist; twist_cov.twist.covariance = ekf_module_->getCurrentTwistCovariance(); @@ -358,13 +373,13 @@ void EKFLocalizer::publishEstimateResult( /* publish yaw bias */ tier4_debug_msgs::msg::Float64Stamped yawb; - yawb.stamp = current_time; + yawb.stamp = current_ekf_twist.header.stamp; yawb.data = ekf_module_->getYawBias(); pub_yaw_bias_->publish(yawb); /* publish latest odometry */ nav_msgs::msg::Odometry odometry; - odometry.header.stamp = current_time; + odometry.header.stamp = current_ekf_pose.header.stamp; odometry.header.frame_id = current_ekf_pose.header.frame_id; odometry.child_frame_id = "base_link"; odometry.pose = pose_cov.pose; @@ -372,7 +387,7 @@ void EKFLocalizer::publishEstimateResult( pub_odom_->publish(odometry); } -void EKFLocalizer::publishDiagnostics() +void EKFLocalizer::publishDiagnostics(const rclcpp::Time & current_time) { std::vector diag_status_array; @@ -408,7 +423,7 @@ void EKFLocalizer::publishDiagnostics() diag_merged_status.hardware_id = this->get_name(); diagnostic_msgs::msg::DiagnosticArray diag_msg; - diag_msg.header.stamp = this->now(); + diag_msg.header.stamp = current_time; diag_msg.status.push_back(diag_merged_status); pub_diag_->publish(diag_msg); } diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index 0a5e3c98c96c8..8977d82f34138 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); @@ -86,9 +87,15 @@ geometry_msgs::msg::PoseStamped EKFModule::getCurrentPose( { const double x = kalman_filter_.getXelement(IDX::X); const double y = kalman_filter_.getXelement(IDX::Y); + /* + getXelement(IDX::YAW) is surely `biased_yaw`. + Please note how `yaw` and `yaw_bias` are used in the state transition model and + how the observed pose is handled in the measurement pose update. + */ const double biased_yaw = kalman_filter_.getXelement(IDX::YAW); const double yaw_bias = kalman_filter_.getXelement(IDX::YAWB); const double yaw = biased_yaw + yaw_bias; + Pose current_ekf_pose; current_ekf_pose.header.frame_id = params_.pose_frame_id; current_ekf_pose.header.stamp = current_time; @@ -131,6 +138,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 +196,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,18 +218,21 @@ 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; } - /* Set yaw */ + /* Since the kalman filter cannot handle the rotation angle directly, + offset the yaw angle so that the difference from the yaw angle that ekf holds internally is less + than 2 pi. */ double yaw = tf2::getYaw(pose.pose.pose.orientation); const double ekf_yaw = kalman_filter_.getXelement(delay_step * dim_x_ + IDX::YAW); const double yaw_error = normalizeYaw(yaw - ekf_yaw); // normalize the error not to exceed 2 pi @@ -243,7 +294,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 +313,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/geo_pose_projector/package.xml b/localization/geo_pose_projector/package.xml index d424ff14b9c99..c0e2db59deb64 100644 --- a/localization/geo_pose_projector/package.xml +++ b/localization/geo_pose_projector/package.xml @@ -6,6 +6,12 @@ The geo_pose_projector package Yamato Ando Koji Minoda + Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Koji Minoda diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml index ad4a865a2014a..a0a982ddedc16 100644 --- a/localization/gyro_odometer/package.xml +++ b/localization/gyro_odometer/package.xml @@ -6,6 +6,11 @@ The gyro_odometer package as a ROS 2 node Yamato Ando Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Yamato Ando diff --git a/localization/landmark_based_localizer/README.md b/localization/landmark_based_localizer/README.md index e4c79b7941b12..49588a19ac620 100644 --- a/localization/landmark_based_localizer/README.md +++ b/localization/landmark_based_localizer/README.md @@ -43,65 +43,21 @@ So, if the 4 vertices are considered as forming a tetrahedron and its volume exc ## Map specifications -For this package to work correctly, the poses of the landmarks must be specified in the Lanelet2 map format that `map_loader` and `landmark_manager` can interpret. - -The four vertices of a landmark are defined counterclockwise. - -The order of the four vertices is defined as follows. In the coordinate system of a landmark, - -- the x-axis is parallel to the vector from the first vertex to the second vertex -- the y-axis is parallel to the vector from the second vertex to the third vertex - -![lanelet2 data structure](./doc_image/lanelet2_data_structure.drawio.svg) - -### Example of `lanelet2_map.osm` - -The values provided below are placeholders. -Ensure to input the correct coordinates corresponding to the actual location where the landmark is placed, such as `lat`, `lon`, `mgrs_code`, `local_x`, `local_y`. - -For example, when using the AR tag, it would look like this. - -```xml -... - - - - - - - - - - - - - - - - - - - - - - - - - - -... - - - - - - - - - - - - -... - -``` +See + +## 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/README.md b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md index f0c54d6393f7e..1a776bd810fff 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/README.md +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md @@ -2,7 +2,7 @@ **ArTagBasedLocalizer** is a vision-based localization node. - +ar_tag_image This node uses [the ArUco library](https://index.ros.org/p/aruco/) to detect AR-Tags from camera images and calculates and publishes the pose of the ego vehicle based on these detections. The positions and orientations of the AR-Tags are assumed to be written in the Lanelet2 format. @@ -30,6 +30,10 @@ The positions and orientations of the AR-Tags are assumed to be written in the L | `/tf` | `geometry_msgs::msg::TransformStamped` | [debug topic] TF from camera to detected tag | | `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostics outputs | +## Parameters + +{{ json_to_markdown("localization/landmark_based_localizer/ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json") }} + ## How to launch When launching Autoware, set `artag` for `pose_source`. 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/launch/ar_tag_based_localizer.launch.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml index 6437455875cc2..272338905c3f0 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml @@ -8,18 +8,24 @@ - + + + + - + + + + diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml index 7e83220dadf2a..072479cc7aaf5 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml @@ -8,6 +8,9 @@ Masahiro Sakamoto Yamato Ando Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Ryu Yamamoto Apache License 2.0 ament_cmake @@ -17,7 +20,6 @@ aruco cv_bridge diagnostic_msgs - image_transport landmark_manager lanelet2_extension localization_util diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json b/localization/landmark_based_localizer/ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json new file mode 100644 index 0000000000000..bde5221aa1bbc --- /dev/null +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json @@ -0,0 +1,87 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for ar_tag_based_localizer", + "type": "object", + "definitions": { + "ar_tag_based_localizer": { + "type": "object", + "properties": { + "marker_size": { + "type": "number", + "description": "marker_size", + "default": 0.6 + }, + "target_tag_ids": { + "type": "array", + "description": "target_tag_ids", + "default": "['0','1','2','3','4','5','6']" + }, + "base_covariance": { + "type": "array", + "description": "base_covariance", + "default": [ + 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.02 + ] + }, + "distance_threshold": { + "type": "number", + "description": "distance_threshold(m)", + "default": "13.0" + }, + "consider_orientation": { + "type": "boolean", + "description": "consider_orientation", + "default": "false" + }, + "detection_mode": { + "type": "string", + "description": "detection_mode select from [DM_NORMAL, DM_FAST, DM_VIDEO_FAST]", + "default": "DM_NORMAL" + }, + "min_marker_size": { + "type": "number", + "description": "min_marker_size", + "default": 0.02 + }, + "ekf_time_tolerance": { + "type": "number", + "description": "ekf_time_tolerance(sec)", + "default": 5.0 + }, + "ekf_position_tolerance": { + "type": "number", + "description": "ekf_position_tolerance(m)", + "default": 10.0 + } + }, + "required": [ + "marker_size", + "target_tag_ids", + "base_covariance", + "distance_threshold", + "consider_orientation", + "detection_mode", + "min_marker_size", + "ekf_time_tolerance", + "ekf_position_tolerance" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/ar_tag_based_localizer" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} 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..43ac1e1098453 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,23 +49,23 @@ #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) -{ -} - -bool ArTagBasedLocalizer::setup() { /* Declare node parameters @@ -73,7 +73,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"); @@ -87,8 +88,10 @@ bool ArTagBasedLocalizer::setup() } else { // Error RCLCPP_ERROR_STREAM(this->get_logger(), "Invalid detection_mode: " << detection_mode); - return false; + return; } + ekf_pose_buffer_ = std::make_unique( + this->get_logger(), ekf_time_tolerance_, ekf_position_tolerance_); /* Log parameter info @@ -103,128 +106,121 @@ 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 - */ - it_ = std::make_unique(shared_from_this()); /* Subscribers */ + using std::placeholders::_1; map_bin_sub_ = this->create_subscription( "~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal), - std::bind(&ArTagBasedLocalizer::map_bin_callback, this, std::placeholders::_1)); + std::bind(&ArTagBasedLocalizer::map_bin_callback, this, _1)); rclcpp::QoS qos_sub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); qos_sub.best_effort(); image_sub_ = this->create_subscription( - "~/input/image", qos_sub, - std::bind(&ArTagBasedLocalizer::image_callback, this, std::placeholders::_1)); + "~/input/image", qos_sub, std::bind(&ArTagBasedLocalizer::image_callback, this, _1)); cam_info_sub_ = this->create_subscription( - "~/input/camera_info", qos_sub, - std::bind(&ArTagBasedLocalizer::cam_info_callback, this, std::placeholders::_1)); + "~/input/camera_info", qos_sub, std::bind(&ArTagBasedLocalizer::cam_info_callback, this, _1)); ekf_pose_sub_ = this->create_subscription( - "~/input/ekf_pose", qos_sub, - std::bind(&ArTagBasedLocalizer::ekf_pose_callback, this, std::placeholders::_1)); + "~/input/ekf_pose", qos_sub, std::bind(&ArTagBasedLocalizer::ekf_pose_callback, this, _1)); /* Publishers */ - rclcpp::QoS qos_marker = rclcpp::QoS(rclcpp::KeepLast(10)); - qos_marker.transient_local(); - qos_marker.reliable(); - marker_pub_ = this->create_publisher("~/debug/marker", qos_marker); - rclcpp::QoS qos_pub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); - image_pub_ = it_->advertise("~/debug/result", 1); - pose_pub_ = - this->create_publisher("~/output/pose_with_covariance", qos_pub); - diag_pub_ = this->create_publisher("/diagnostics", qos_pub); + const rclcpp::QoS qos_pub_once = rclcpp::QoS(rclcpp::KeepLast(10)).transient_local().reliable(); + const rclcpp::QoS qos_pub_periodic(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); + pose_pub_ = this->create_publisher( + "~/output/pose_with_covariance", qos_pub_periodic); + image_pub_ = this->create_publisher("~/debug/image", qos_pub_periodic); + mapped_tag_pose_pub_ = this->create_publisher("~/debug/mapped_tag", qos_pub_once); + detected_tag_pose_pub_ = + this->create_publisher("~/debug/detected_tag", qos_pub_periodic); + diag_pub_ = this->create_publisher("/diagnostics", qos_pub_periodic); RCLCPP_INFO(this->get_logger(), "Setup of ar_tag_based_localizer node is successful!"); - return true; } 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); - marker_pub_->publish(marker_msg); + landmark_manager_.parse_landmarks(msg, "apriltag_16h5"); + const MarkerArray marker_msg = landmark_manager_.get_landmarks_as_marker_array_msg(); + mapped_tag_pose_pub_->publish(marker_msg); } void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg) { - if ((image_pub_.getNumSubscribers() == 0) && (pose_pub_->get_subscription_count() == 0)) { + // check subscribers + if ((image_pub_->get_subscription_count() == 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 (detected_tag_pose_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 & 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); + } + detected_tag_pose_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 +248,101 @@ 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]; - - cv::Mat distortion_coeff(4, 1, CV_64FC1); - for (int i = 0; i < 4; ++i) { - distortion_coeff.at(i, 0) = 0; - } + // copy camera matrix + cv::Mat camera_matrix(3, 4, CV_64FC1); + std::copy(msg->p.begin(), msg->p.end(), camera_matrix.begin()); + + // all 0 + cv::Mat distortion_coeff(4, 1, CV_64FC1, 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{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_->get_subscription_count() > 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..f70821a39ffe8 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,14 @@ #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 #include #include @@ -59,6 +62,7 @@ #include #include +#include #include #include #include @@ -72,37 +76,34 @@ 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; + using Landmark = landmark_manager::Landmark; public: explicit ArTagBasedLocalizer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - bool setup(); 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_; // Subscribers rclcpp::Subscription::SharedPtr map_bin_sub_; @@ -111,17 +112,18 @@ class ArTagBasedLocalizer : public rclcpp::Node rclcpp::Subscription::SharedPtr ekf_pose_sub_; // Publishers - rclcpp::Publisher::SharedPtr marker_pub_; - image_transport::Publisher image_pub_; rclcpp::Publisher::SharedPtr pose_pub_; + rclcpp::Publisher::SharedPtr image_pub_; + rclcpp::Publisher::SharedPtr detected_tag_pose_pub_; + rclcpp::Publisher::SharedPtr mapped_tag_pose_pub_; rclcpp::Publisher::SharedPtr diag_pub_; // Others 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/ar_tag_based_localizer/src/main.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp index cbcd57b4b222a..8ef1dd6195580 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp @@ -48,7 +48,6 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); std::shared_ptr ptr = std::make_shared(); - ptr->setup(); rclcpp::spin(ptr); rclcpp::shutdown(); } diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp index 1b44327fd9e8b..5d05dd7e3755a 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp @@ -55,7 +55,8 @@ class TestArTagBasedLocalizer : public ::testing::Test TEST_F(TestArTagBasedLocalizer, test_setup) // NOLINT { - EXPECT_TRUE(node_->setup()); + // Check if the constructor finishes successfully + EXPECT_TRUE(true); } int main(int argc, char ** argv) 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/CMakeLists.txt b/localization/landmark_based_localizer/landmark_manager/CMakeLists.txt index 1b6126c892d2e..1b57e5cc89018 100644 --- a/localization/landmark_based_localizer/landmark_manager/CMakeLists.txt +++ b/localization/landmark_based_localizer/landmark_manager/CMakeLists.txt @@ -12,7 +12,7 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -ament_auto_add_library(landmark_manager +ament_auto_add_library(landmark_manager SHARED src/landmark_manager.cpp ) 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..c2b0751d91f9a 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 @@ -15,6 +15,8 @@ #ifndef LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ #define LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ +#include "lanelet2_extension/localization/landmark.hpp" + #include #include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" @@ -22,6 +24,7 @@ #include #include +#include #include #include @@ -34,12 +37,27 @@ 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); +class LandmarkManager +{ +public: + void parse_landmarks( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, + const std::string & target_subtype); + + [[nodiscard]] std::vector get_landmarks() const; -visualization_msgs::msg::MarkerArray convert_landmarks_to_marker_array_msg( - const std::vector & landmarks); + [[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..be19de9334e84 100644 --- a/localization/landmark_based_localizer/landmark_manager/package.xml +++ b/localization/landmark_based_localizer/landmark_manager/package.xml @@ -8,6 +8,9 @@ Masahiro Sakamoto Yamato Ando Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Ryu Yamamoto Apache License 2.0 ament_cmake @@ -18,6 +21,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..57bfcde461af6 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,52 +27,31 @@ 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) + const std::string & target_subtype) { - RCLCPP_INFO_STREAM(logger, "msg->format_version: " << msg->format_version); - RCLCPP_INFO_STREAM(logger, "msg->map_format: " << msg->map_format); - RCLCPP_INFO_STREAM(logger, "msg->map_version: " << msg->map_version); - RCLCPP_INFO_STREAM(logger, "msg->data.size(): " << msg->data.size()); - 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") { - continue; - } - const std::string subtype{poly.attributeOr(lanelet::AttributeName::Subtype, "none")}; - if (subtype != target_subtype) { - continue; - } - - // Get marker_id - const std::string marker_id = poly.attributeOr("marker_id", "none"); + std::vector landmarks = + lanelet::localization::parseLandmarkPolygons(msg, target_subtype); + for (const lanelet::Polygon3d & poly : landmarks) { + // Get landmark_id + const std::string landmark_id = poly.attributeOr("marker_id", "none"); // Get 4 vertices const auto & vertices = poly.basicPolygon(); if (vertices.size() != 4) { - RCLCPP_WARN_STREAM(logger, "vertices.size() (" << vertices.size() << ") != 4"); 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]; const auto & v2 = vertices[2]; const auto & v3 = vertices[3]; const double volume = (v1 - v0).cross(v2 - v0).dot(v3 - v0) / 6.0; - RCLCPP_INFO_STREAM(logger, "volume = " << volume); const double volume_threshold = 1e-5; if (volume > volume_threshold) { - RCLCPP_WARN_STREAM( - logger, - "volume (" << volume << ") > threshold (" << volume_threshold << "), This is not plane."); continue; } @@ -98,63 +79,135 @@ std::vector parse_landmarks( pose.orientation.w = q.w(); // Add - landmarks.push_back(Landmark{marker_id, pose}); - RCLCPP_INFO_STREAM(logger, "id: " << marker_id); - RCLCPP_INFO_STREAM( - logger, - "(x, y, z) = " << pose.position.x << ", " << pose.position.y << ", " << pose.position.z); - RCLCPP_INFO_STREAM( - logger, "q = " << pose.orientation.x << ", " << pose.orientation.y << ", " - << pose.orientation.z << ", " << pose.orientation.w); + landmarks_map_[landmark_id].push_back(pose); + } +} + +std::vector LandmarkManager::get_landmarks() const +{ + std::vector landmarks; + + landmark_manager::Landmark landmark; + for (const auto & [landmark_id_str, landmark_poses] : landmarks_map_) { + for (const auto & pose : landmark_poses) { + landmark.id = landmark_id_str; + landmark.pose = pose; + landmarks.push_back(landmark); + } } 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_error_monitor/README.md b/localization/localization_error_monitor/README.md index 2ddf41e4eac70..7912ad843ef19 100644 --- a/localization/localization_error_monitor/README.md +++ b/localization/localization_error_monitor/README.md @@ -29,10 +29,4 @@ The package monitors the following two values: ## Parameters -| Name | Type | Description | -| -------------------------------------- | ------ | -------------------------------------------------------------------------------------------- | -| `scale` | double | scale factor for monitored values (default: 3.0) | -| `error_ellipse_size` | double | error threshold for long radius of confidence ellipse [m] (default: 1.0) | -| `warn_ellipse_size` | double | warning threshold for long radius of confidence ellipse [m] (default: 0.8) | -| `error_ellipse_size_lateral_direction` | double | error threshold for size of confidence ellipse along lateral direction [m] (default: 0.3) | -| `warn_ellipse_size_lateral_direction` | double | warning threshold for size of confidence ellipse along lateral direction [m] (default: 0.25) | +{{ json_to_markdown("localization/localization_error_monitor/schema/localization_error_monitor.schema.json") }} diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml index 266ae6c848834..a1b352d911a0d 100644 --- a/localization/localization_error_monitor/package.xml +++ b/localization/localization_error_monitor/package.xml @@ -7,6 +7,11 @@ Yamato Ando Koji Minoda Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Taichi Higashide diff --git a/localization/localization_error_monitor/schema/localization_error_monitor.schema.json b/localization/localization_error_monitor/schema/localization_error_monitor.schema.json new file mode 100644 index 0000000000000..0cdd5fb946756 --- /dev/null +++ b/localization/localization_error_monitor/schema/localization_error_monitor.schema.json @@ -0,0 +1,56 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Localization Error Monitor node", + "type": "object", + "definitions": { + "localization_error_monitor": { + "type": "object", + "properties": { + "scale": { + "type": "number", + "default": 3.0, + "description": "scale factor for monitored values" + }, + "error_ellipse_size": { + "type": "number", + "default": 1.5, + "description": "error threshold for long radius of confidence ellipse [m]" + }, + "warn_ellipse_size": { + "type": "number", + "default": 1.2, + "description": "warning threshold for long radius of confidence ellipse [m]" + }, + "error_ellipse_size_lateral_direction": { + "type": "number", + "default": 0.3, + "description": "error threshold for size of confidence ellipse along lateral direction [m]" + }, + "warn_ellipse_size_lateral_direction": { + "type": "number", + "default": 0.25, + "description": "warning threshold for size of confidence ellipse along lateral direction [m]" + } + }, + "required": [ + "scale", + "error_ellipse_size", + "warn_ellipse_size", + "error_ellipse_size_lateral_direction", + "warn_ellipse_size_lateral_direction" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/localization_error_monitor" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/localization/localization_error_monitor/src/diagnostics.cpp b/localization/localization_error_monitor/src/diagnostics.cpp index 0c5a4a7800639..375fccfa06787 100644 --- a/localization/localization_error_monitor/src/diagnostics.cpp +++ b/localization/localization_error_monitor/src/diagnostics.cpp @@ -23,7 +23,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracy( diagnostic_msgs::msg::DiagnosticStatus stat; diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "localization_accuracy"; + key_value.key = "localization_error_ellipse"; key_value.value = std::to_string(ellipse_size); stat.values.push_back(key_value); @@ -47,7 +47,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracyLateralDirection diagnostic_msgs::msg::DiagnosticStatus stat; diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "localization_accuracy_lateral_direction"; + key_value.key = "localization_error_ellipse_lateral_direction"; key_value.value = std::to_string(ellipse_size); stat.values.push_back(key_value); diff --git a/localization/localization_util/CMakeLists.txt b/localization/localization_util/CMakeLists.txt index 9490ffb67723b..fe65077d89649 100644 --- a/localization/localization_util/CMakeLists.txt +++ b/localization/localization_util/CMakeLists.txt @@ -6,10 +6,17 @@ 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/pose_array_interpolator.hpp deleted file mode 100644 index 998d8465733f5..0000000000000 --- a/localization/localization_util/include/localization_util/pose_array_interpolator.hpp +++ /dev/null @@ -1,62 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef LOCALIZATION_UTIL__POSE_ARRAY_INTERPOLATOR_HPP_ -#define LOCALIZATION_UTIL__POSE_ARRAY_INTERPOLATOR_HPP_ - -#include "localization_util/util_func.hpp" - -#include - -#include - -#include - -class PoseArrayInterpolator -{ -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); - - PoseArrayInterpolator( - rclcpp::Node * node, const rclcpp::Time & target_ros_time, - const std::deque & pose_msg_ptr_array); - - PoseWithCovarianceStamped get_current_pose(); - PoseWithCovarianceStamped get_old_pose(); - PoseWithCovarianceStamped get_new_pose(); - [[nodiscard]] bool is_success() const; - -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_; - - [[nodiscard]] bool validate_time_stamp_difference( - const rclcpp::Time & target_time, const rclcpp::Time & reference_time, - const double time_tolerance_sec) const; - [[nodiscard]] bool validate_position_difference( - const geometry_msgs::msg::Point & target_point, - const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_) const; -}; - -#endif // LOCALIZATION_UTIL__POSE_ARRAY_INTERPOLATOR_HPP_ diff --git a/localization/localization_util/include/localization_util/smart_pose_buffer.hpp b/localization/localization_util/include/localization_util/smart_pose_buffer.hpp new file mode 100644 index 0000000000000..2a2a775a9280c --- /dev/null +++ b/localization/localization_util/include/localization_util/smart_pose_buffer.hpp @@ -0,0 +1,68 @@ +// Copyright 2015-2019 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ +#define LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ + +#include "localization_util/util_func.hpp" + +#include + +#include + +#include + +class SmartPoseBuffer +{ +private: + using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + +public: + struct InterpolateResult + { + PoseWithCovarianceStamped old_pose; + PoseWithCovarianceStamped new_pose; + PoseWithCovarianceStamped interpolated_pose; + }; + + SmartPoseBuffer() = delete; + SmartPoseBuffer( + const rclcpp::Logger & logger, const double & pose_timeout_sec, + const double & pose_distance_tolerance_meters); + + 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_; + 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, + const double time_tolerance_sec) const; + [[nodiscard]] bool validate_position_difference( + const geometry_msgs::msg::Point & target_point, + const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_) const; +}; + +#endif // LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ diff --git a/localization/localization_util/include/localization_util/tf2_listener_module.hpp b/localization/localization_util/include/localization_util/tf2_listener_module.hpp deleted file mode 100644 index b332f9effe0e0..0000000000000 --- a/localization/localization_util/include/localization_util/tf2_listener_module.hpp +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef LOCALIZATION_UTIL__TF2_LISTENER_MODULE_HPP_ -#define LOCALIZATION_UTIL__TF2_LISTENER_MODULE_HPP_ - -#include - -#include -#include -#include - -#include - -class Tf2ListenerModule -{ - using TransformStamped = geometry_msgs::msg::TransformStamped; - -public: - explicit Tf2ListenerModule(rclcpp::Node * node); - bool get_transform( - const builtin_interfaces::msg::Time & timestamp, const std::string & target_frame, - const std::string & source_frame, - const geometry_msgs::msg::TransformStamped::SharedPtr & transform_stamped_ptr) const; - -private: - rclcpp::Logger logger_; - tf2_ros::Buffer tf2_buffer_; - tf2_ros::TransformListener tf2_listener_; -}; - -#endif // LOCALIZATION_UTIL__TF2_LISTENER_MODULE_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/package.xml b/localization/localization_util/package.xml index 20f9b160212b5..fa8da2aa1231a 100644 --- a/localization/localization_util/package.xml +++ b/localization/localization_util/package.xml @@ -8,6 +8,9 @@ Masahiro Sakamoto Shintaro Sakoda Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Ryu Yamamoto Apache License 2.0 Yamato Ando 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..bc62bfa690946 --- /dev/null +++ b/localization/localization_util/src/smart_pose_buffer.cpp @@ -0,0 +1,154 @@ +// 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) { + return std::nullopt; + } + + // [time_last < target_ros_time] is acceptable here. + // It is possible that the target_ros_time (often sensor timestamp) is newer than the latest + // timestamp of buffered pose (often EKF). + // However, if the timestamp difference is too large, + // it will later be rejected by validate_time_stamp_difference. + + // 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/tf2_listener_module.cpp b/localization/localization_util/src/tf2_listener_module.cpp deleted file mode 100644 index 8a19ceec9f30d..0000000000000 --- a/localization/localization_util/src/tf2_listener_module.cpp +++ /dev/null @@ -1,60 +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/tf2_listener_module.hpp" - -#include - -geometry_msgs::msg::TransformStamped identity_transform_stamped( - const builtin_interfaces::msg::Time & timestamp, const std::string & header_frame_id, - const std::string & child_frame_id) -{ - geometry_msgs::msg::TransformStamped transform; - transform.header.stamp = timestamp; - transform.header.frame_id = header_frame_id; - transform.child_frame_id = child_frame_id; - transform.transform.rotation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); - transform.transform.translation = tier4_autoware_utils::createTranslation(0.0, 0.0, 0.0); - return transform; -} - -Tf2ListenerModule::Tf2ListenerModule(rclcpp::Node * node) -: logger_(node->get_logger()), tf2_buffer_(node->get_clock()), tf2_listener_(tf2_buffer_) -{ -} - -bool Tf2ListenerModule::get_transform( - const builtin_interfaces::msg::Time & timestamp, const std::string & target_frame, - const std::string & source_frame, const TransformStamped::SharedPtr & transform_stamped_ptr) const -{ - const TransformStamped identity = - identity_transform_stamped(timestamp, target_frame, source_frame); - - if (target_frame == source_frame) { - *transform_stamped_ptr = identity; - return true; - } - - try { - *transform_stamped_ptr = - tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); - } catch (tf2::TransformException & ex) { - RCLCPP_WARN(logger_, "%s", ex.what()); - RCLCPP_ERROR(logger_, "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); - - *transform_stamped_ptr = identity; - return false; - } - return true; -} 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..b4d656cb1810b 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -27,10 +27,9 @@ 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 src/map_update_module.cpp ) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index d7a7b1c5c37f3..74d49e13a4c21 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -18,7 +18,6 @@ One optional function is regularization. Please see the regularization chapter i | Name | Type | Description | | ----------------------------------- | ----------------------------------------------- | ------------------------------------- | | `ekf_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | initial pose | -| `pointcloud_map` | `sensor_msgs::msg::PointCloud2` | map pointcloud | | `points_raw` | `sensor_msgs::msg::PointCloud2` | sensor pointcloud | | `sensing/gnss/pose_with_covariance` | `sensor_msgs::msg::PoseWithCovarianceStamped` | base position for regularization term | @@ -32,13 +31,13 @@ One optional function is regularization. Please see the regularization chapter i | `ndt_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | estimated pose with covariance | | `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | diagnostics | | `points_aligned` | `sensor_msgs::msg::PointCloud2` | [debug topic] pointcloud aligned by scan matching | -| `points_aligned_no_ground` | `sensor_msgs::msg::PointCloud2` | [debug topic] de-grounded pointcloud aligned by scan matching | +| `points_aligned_no_ground` | `sensor_msgs::msg::PointCloud2` | [debug topic] no ground pointcloud aligned by scan matching | | `initial_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | [debug topic] initial pose used in scan matching | | `multi_ndt_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] estimated poses from multiple initial poses in real-time covariance estimation | | `multi_initial_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] initial poses for real-time covariance estimation | | `exe_time_ms` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] execution time for scan matching [ms] | | `transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching | -| `no_ground_transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching based on de-grounded LiDAR scan | +| `no_ground_transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching based on no ground LiDAR scan | | `iteration_num` | `tier4_debug_msgs::msg::Int32Stamped` | [debug topic] number of scan matching iterations | | `initial_to_result_relative_pose` | `geometry_msgs::msg::PoseStamped` | [debug topic] relative pose between the initial point and the convergence point | | `initial_to_result_distance` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the initial point and the convergence point [m] | @@ -57,28 +56,29 @@ One optional function is regularization. Please see the regularization chapter i ### Core Parameters -| Name | Type | Description | -| --------------------------------------------------------- | ---------------------- | -------------------------------------------------------------------------------------------------- | -| `base_frame` | string | Vehicle reference frame | -| `ndt_base_frame` | string | NDT reference frame | -| `map_frame` | string | map frame | -| `input_sensor_points_queue_size` | int | Subscriber queue size | -| `trans_epsilon` | double | The max difference between two consecutive transformations to consider convergence | -| `step_size` | double | The newton line search maximum step length | -| `resolution` | double | The ND voxel grid resolution [m] | -| `max_iterations` | int | The number of iterations required to calculate alignment | -| `converged_param_type` | int | The type of indicators for scan matching score (0: TP, 1: NVTL) | -| `converged_param_transform_probability` | double | TP threshold for deciding whether to trust the estimation result (when converged_param_type = 0) | -| `converged_param_nearest_voxel_transformation_likelihood` | double | NVTL threshold for deciding whether to trust the estimation result (when converged_param_type = 1) | -| `initial_estimate_particles_num` | int | The number of particles to estimate initial pose | -| `n_startup_trials` | int | The number of initial random trials in the TPE (Tree-Structured Parzen Estimator). | -| `lidar_topic_timeout_sec` | double | Tolerance of timestamp difference between current time and sensor pointcloud | -| `initial_pose_timeout_sec` | int | Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] | -| `initial_pose_distance_tolerance_m` | double | Tolerance of distance difference between two initial poses used for linear interpolation. [m] | -| `num_threads` | int | Number of threads used for parallel computing | -| `output_pose_covariance` | std::array | The covariance of output pose | - -(TP: Transform Probability, NVTL: Nearest Voxel Transform Probability) +#### Frame + +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/frame.json") }} + +#### Ndt + +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/ndt.json") }} + +#### Initial Pose Estimation + +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json") }} + +#### Validation + +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/validation.json") }} + +#### Score Estimation + +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/score_estimation.json") }} + +#### Covariance + +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/covariance.json") }} ## Regularization @@ -154,10 +154,7 @@ This is because if the base position is far off from the true value, NDT scan ma ### Parameters -| Name | Type | Description | -| ----------------------------- | ------ | ---------------------------------------------------------------------- | -| `regularization_enabled` | bool | Flag to add regularization term to NDT optimization (FALSE by default) | -| `regularization_scale_factor` | double | Coefficient of the regularization term. | +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/ndt_regularization.json") }} Regularization is disabled by default because GNSS is not always accurate enough to serve the appropriate base position in any scenes. @@ -193,12 +190,6 @@ Using the feature, `ndt_scan_matcher` can theoretically handle any large size ma ### Additional interfaces -#### Additional inputs - -| Name | Type | Description | -| ---------------- | ------------------------- | ----------------------------------------------------------- | -| `input_ekf_odom` | `nav_msgs::msg::Odometry` | Vehicle localization results (used for map update decision) | - #### Additional outputs | Name | Type | Description | @@ -213,20 +204,11 @@ Using the feature, `ndt_scan_matcher` can theoretically handle any large size ma ### Parameters -| Name | Type | Description | -| ------------------------------------- | ------ | -------------------------------------------------------------------- | -| `use_dynamic_map_loading` | bool | Flag to enable dynamic map loading feature for NDT (TRUE by default) | -| `dynamic_map_loading_update_distance` | double | Distance traveled to load new map(s) | -| `dynamic_map_loading_map_radius` | double | Map loading radius for every update | -| `lidar_radius` | double | LiDAR radius used for localization (only used for diagnosis) | - -### Enabling the dynamic map loading feature +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/dynamic_map_loading.json") }} -To use dynamic map loading feature for `ndt_scan_matcher`, you also need to appropriately configure some other settings outside of this node. -Follow the next two instructions. +### Notes for dynamic map loading -1. enable dynamic map loading interface in `pointcloud_map_loader` (by setting `enable_differential_load` to true in the package) -2. split the PCD files into grids (recommended size: 20[m] x 20[m]) +To use dynamic map loading feature for `ndt_scan_matcher`, you also need to split the PCD files into grids (recommended size: 20[m] x 20[m]) Note that the dynamic map loading may FAIL if the map is split into two or more large size map (e.g. 1000[m] x 1000[m]). Please provide either of @@ -235,30 +217,21 @@ Note that the dynamic map loading may FAIL if the map is split into two or more Here is a split PCD map for `sample-map-rosbag` from Autoware tutorial: [`sample-map-rosbag_split.zip`](https://github.com/autowarefoundation/autoware.universe/files/10349104/sample-map-rosbag_split.zip) -| PCD files | `use_dynamic_map_loading` | `enable_differential_load` | How NDT loads map(s) | -| :------------: | :-----------------------: | :------------------------: | :------------------: | -| single file | true | true | at once (standard) | -| single file | true | false | **does NOT work** | -| single file | false | true/false | at once (standard) | -| multiple files | true | true | dynamically | -| multiple files | true | false | **does NOT work** | -| multiple files | false | true/false | at once (standard) | +| PCD files | How NDT loads map(s) | +| :------------: | :------------------: | +| single file | at once (standard) | +| multiple files | dynamically | -## Scan matching score based on de-grounded LiDAR scan +## Scan matching score based on no ground LiDAR scan ### Abstract -This is a function that uses de-grounded LiDAR scan to estimate the scan matching score. This score can reflect the current localization performance more accurately. +This is a function that uses no ground LiDAR scan to estimate the scan matching score. This score can reflect the current localization performance more accurately. [related issue](https://github.com/autowarefoundation/autoware.universe/issues/2044). ### Parameters - - -| Name | Type | Description | -| ------------------------------------- | ------ | ------------------------------------------------------------------------------------- | -| `estimate_scores_for_degrounded_scan` | bool | Flag for using scan matching score based on de-grounded LiDAR scan (FALSE by default) | -| `z_margin_for_ground_removal` | double | Z-value margin for removal ground points | +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/score_estimation_no_ground_points.json") }} ## 2D real-time covariance estimation @@ -277,8 +250,4 @@ Note that this function may spoil healthy system behavior if it consumes much ca initial_pose_offset_model is rotated around (x,y) = (0,0) in the direction of the first principal component of the Hessian matrix. initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements. -| Name | Type | Description | -| ----------------------------- | ------------------- | ----------------------------------------------------------------- | -| `use_covariance_estimation` | bool | Flag for using real-time covariance estimation (FALSE by default) | -| `initial_pose_offset_model_x` | std::vector | X-axis offset [m] | -| `initial_pose_offset_model_y` | std::vector | Y-axis offset [m] | +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json") }} diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index 1a6c26cd9c351..144449ce75084 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -1,106 +1,114 @@ /**: ros__parameters: - # Use dynamic map loading - use_dynamic_map_loading: true + frame: + # Vehicle reference frame + base_frame: "base_link" - # Vehicle reference frame - base_frame: "base_link" + # NDT reference frame + ndt_base_frame: "ndt_base_link" - # NDT reference frame - ndt_base_frame: "ndt_base_link" + # Map frame + map_frame: "map" - # map frame - map_frame: "map" - # Subscriber queue size - input_sensor_points_queue_size: 1 + ndt: + # The maximum difference between two consecutive + # transformations in order to consider convergence + trans_epsilon: 0.01 - # The maximum difference between two consecutive - # transformations in order to consider convergence - trans_epsilon: 0.01 + # The newton line search maximum step length + step_size: 0.1 - # The newton line search maximum step length - step_size: 0.1 + # The ND voxel grid resolution + resolution: 2.0 - # The ND voxel grid resolution - resolution: 2.0 + # The number of iterations required to calculate alignment + max_iterations: 30 - # The number of iterations required to calculate alignment - max_iterations: 30 + # Number of threads used for parallel computing + num_threads: 4 - # Converged param type - # 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD - converged_param_type: 1 + regularization: + enable: false - # If converged_param_type is 0 - # Threshold for deciding whether to trust the estimation result - converged_param_transform_probability: 3.0 + # Regularization scale factor + scale_factor: 0.01 - # If converged_param_type is 1 - # Threshold for deciding whether to trust the estimation result - converged_param_nearest_voxel_transformation_likelihood: 2.3 - # The number of particles to estimate initial pose - initial_estimate_particles_num: 200 + initial_pose_estimation: + # The number of particles to estimate initial pose + particles_num: 200 - # The number of initial random trials in the TPE (Tree-Structured Parzen Estimator). - # This value should be equal to or less than 'initial_estimate_particles_num' and more than 0. - # If it is equal to 'initial_estimate_particles_num', the search will be the same as a full random search. - n_startup_trials: 20 + # The number of initial random trials in the TPE (Tree-Structured Parzen Estimator). + # This value should be equal to or less than 'initial_estimate_particles_num' and more than 0. + # If it is equal to 'initial_estimate_particles_num', the search will be the same as a full random search. + n_startup_trials: 20 - # Tolerance of timestamp difference between current time and sensor pointcloud. [sec] - lidar_topic_timeout_sec: 1.0 - # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] - initial_pose_timeout_sec: 1.0 + validation: + # Tolerance of timestamp difference between current time and sensor pointcloud. [sec] + lidar_topic_timeout_sec: 1.0 - # Tolerance of distance difference between two initial poses used for linear interpolation. [m] - initial_pose_distance_tolerance_m: 10.0 + # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] + initial_pose_timeout_sec: 1.0 - # Number of threads used for parallel computing - num_threads: 4 + # Tolerance of distance difference between two initial poses used for linear interpolation. [m] + initial_pose_distance_tolerance_m: 10.0 - # The covariance of output pose - # Note that this covariance matrix is empirically derived - output_pose_covariance: - [ - 0.0225, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0225, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0225, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.000625, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.000625, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.000625, - ] + # The execution time which means probably NDT cannot matches scans properly. [ms] + critical_upper_bound_exe_time_ms: 100.0 - # 2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value) - use_covariance_estimation: false - # Offset arrangement in covariance estimation [m] - # initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements. - initial_pose_offset_model_x: [0.0, 0.0, 0.5, -0.5, 1.0, -1.0] - initial_pose_offset_model_y: [0.5, -0.5, 0.0, 0.0, 0.0, 0.0] + score_estimation: + # Converged param type + # 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD + converged_param_type: 1 - # Regularization switch - regularization_enabled: false + # If converged_param_type is 0 + # Threshold for deciding whether to trust the estimation result + converged_param_transform_probability: 3.0 - # Regularization scale factor - regularization_scale_factor: 0.01 + # If converged_param_type is 1 + # Threshold for deciding whether to trust the estimation result + converged_param_nearest_voxel_transformation_likelihood: 2.3 - # Dynamic map loading distance - dynamic_map_loading_update_distance: 20.0 + # Scan matching score based on no ground LiDAR scan + no_ground_points: + enable: false - # Dynamic map loading loading radius - dynamic_map_loading_map_radius: 150.0 + # If lidar_point.z - base_link.z <= this threshold , the point will be removed + z_margin_for_ground_removal: 0.8 - # Radius of input LiDAR range (used for diagnostics of dynamic map loading) - lidar_radius: 100.0 - # cspell: ignore degrounded - # A flag for using scan matching score based on de-grounded LiDAR scan - estimate_scores_for_degrounded_scan: false + covariance: + # The covariance of output pose + # Note that this covariance matrix is empirically derived + output_pose_covariance: + [ + 0.0225, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0225, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0225, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.000625, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.000625, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.000625, + ] - # If lidar_point.z - base_link.z <= this threshold , the point will be removed - z_margin_for_ground_removal: 0.8 + # 2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value) + covariance_estimation: + enable: false - # The execution time which means probably NDT cannot matches scans properly. [ms] - critical_upper_bound_exe_time_ms: 100 + # Offset arrangement in covariance estimation [m] + # initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements. + initial_pose_offset_model_x: [0.0, 0.0, 0.5, -0.5, 1.0, -1.0] + initial_pose_offset_model_y: [0.5, -0.5, 0.0, 0.0, 0.0, 0.0] + + + dynamic_map_loading: + # Dynamic map loading distance + update_distance: 20.0 + + # Dynamic map loading loading radius + map_radius: 150.0 + + # Radius of input LiDAR range (used for diagnostics of dynamic map loading) + lidar_radius: 100.0 diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/debug.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/debug.hpp deleted file mode 100644 index ccc3b59e63328..0000000000000 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/debug.hpp +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NDT_SCAN_MATCHER__DEBUG_HPP_ -#define NDT_SCAN_MATCHER__DEBUG_HPP_ - -#include "ndt_scan_matcher/particle.hpp" - -#include - -#include - -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); - -#endif // NDT_SCAN_MATCHER__DEBUG_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp new file mode 100644 index 0000000000000..2955e3cb9a5f7 --- /dev/null +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp @@ -0,0 +1,173 @@ +// Copyright 2024 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use node file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ +#define NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ + +#include + +#include + +#include +#include +#include + +enum class ConvergedParamType { + TRANSFORM_PROBABILITY = 0, + NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD = 1 +}; + +struct HyperParameters +{ + struct Frame + { + std::string base_frame; + std::string ndt_base_frame; + std::string map_frame; + } frame; + + pclomp::NdtParams ndt; + bool ndt_regularization_enable; + + struct InitialPoseEstimation + { + int64_t particles_num; + int64_t n_startup_trials; + } initial_pose_estimation; + + struct Validation + { + double lidar_topic_timeout_sec; + double initial_pose_timeout_sec; + double initial_pose_distance_tolerance_m; + double critical_upper_bound_exe_time_ms; + } validation; + + struct ScoreEstimation + { + ConvergedParamType converged_param_type; + double converged_param_transform_probability; + double converged_param_nearest_voxel_transformation_likelihood; + struct NoGroundPoints + { + bool enable; + double z_margin_for_ground_removal; + } no_ground_points; + } score_estimation; + + struct Covariance + { + std::array output_pose_covariance; + + struct CovarianceEstimation + { + bool enable; + std::vector initial_pose_offset_model; + } covariance_estimation; + } covariance; + + struct DynamicMapLoading + { + double update_distance; + double map_radius; + double lidar_radius; + } dynamic_map_loading; + +public: + explicit HyperParameters(rclcpp::Node * node) + { + frame.base_frame = node->declare_parameter("frame.base_frame"); + frame.ndt_base_frame = node->declare_parameter("frame.ndt_base_frame"); + frame.map_frame = node->declare_parameter("frame.map_frame"); + + ndt.trans_epsilon = node->declare_parameter("ndt.trans_epsilon"); + ndt.step_size = node->declare_parameter("ndt.step_size"); + ndt.resolution = node->declare_parameter("ndt.resolution"); + ndt.max_iterations = static_cast(node->declare_parameter("ndt.max_iterations")); + ndt.num_threads = static_cast(node->declare_parameter("ndt.num_threads")); + ndt.num_threads = std::max(ndt.num_threads, 1); + ndt_regularization_enable = node->declare_parameter("ndt.regularization.enable"); + ndt.regularization_scale_factor = + static_cast(node->declare_parameter("ndt.regularization.scale_factor")); + + initial_pose_estimation.particles_num = + node->declare_parameter("initial_pose_estimation.particles_num"); + initial_pose_estimation.n_startup_trials = + node->declare_parameter("initial_pose_estimation.n_startup_trials"); + + validation.lidar_topic_timeout_sec = + node->declare_parameter("validation.lidar_topic_timeout_sec"); + validation.initial_pose_timeout_sec = + node->declare_parameter("validation.initial_pose_timeout_sec"); + validation.initial_pose_distance_tolerance_m = + node->declare_parameter("validation.initial_pose_distance_tolerance_m"); + validation.critical_upper_bound_exe_time_ms = + node->declare_parameter("validation.critical_upper_bound_exe_time_ms"); + + const int64_t converged_param_type_tmp = + node->declare_parameter("score_estimation.converged_param_type"); + score_estimation.converged_param_type = + static_cast(converged_param_type_tmp); + score_estimation.converged_param_transform_probability = + node->declare_parameter("score_estimation.converged_param_transform_probability"); + score_estimation.converged_param_nearest_voxel_transformation_likelihood = + node->declare_parameter( + "score_estimation.converged_param_nearest_voxel_transformation_likelihood"); + score_estimation.no_ground_points.enable = + node->declare_parameter("score_estimation.no_ground_points.enable"); + score_estimation.no_ground_points.z_margin_for_ground_removal = node->declare_parameter( + "score_estimation.no_ground_points.z_margin_for_ground_removal"); + + std::vector output_pose_covariance = + node->declare_parameter>("covariance.output_pose_covariance"); + for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) { + covariance.output_pose_covariance[i] = output_pose_covariance[i]; + } + covariance.covariance_estimation.enable = + node->declare_parameter("covariance.covariance_estimation.enable"); + if (covariance.covariance_estimation.enable) { + std::vector initial_pose_offset_model_x = + node->declare_parameter>( + "covariance.covariance_estimation.initial_pose_offset_model_x"); + std::vector initial_pose_offset_model_y = + node->declare_parameter>( + "covariance.covariance_estimation.initial_pose_offset_model_y"); + + if (initial_pose_offset_model_x.size() == initial_pose_offset_model_y.size()) { + const size_t size = initial_pose_offset_model_x.size(); + covariance.covariance_estimation.initial_pose_offset_model.resize(size); + for (size_t i = 0; i < size; i++) { + covariance.covariance_estimation.initial_pose_offset_model[i].x() = + initial_pose_offset_model_x[i]; + covariance.covariance_estimation.initial_pose_offset_model[i].y() = + initial_pose_offset_model_y[i]; + } + } else { + RCLCPP_WARN( + node->get_logger(), + "Invalid initial pose offset model parameters. Disable covariance estimation."); + covariance.covariance_estimation.enable = false; + } + } + + dynamic_map_loading.update_distance = + node->declare_parameter("dynamic_map_loading.update_distance"); + dynamic_map_loading.map_radius = + node->declare_parameter("dynamic_map_loading.map_radius"); + dynamic_map_loading.lidar_radius = + node->declare_parameter("dynamic_map_loading.lidar_radius"); + } +}; + +#endif // NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp deleted file mode 100644 index 79454e89b9ed0..0000000000000 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NDT_SCAN_MATCHER__MAP_MODULE_HPP_ -#define NDT_SCAN_MATCHER__MAP_MODULE_HPP_ - -#include - -#include - -#include -#include -#include - -#include - -class MapModule -{ - using PointSource = pcl::PointXYZ; - using PointTarget = pcl::PointXYZ; - using NormalDistributionsTransform = - pclomp::MultiGridNormalDistributionsTransform; - -public: - MapModule( - rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr, - rclcpp::CallbackGroup::SharedPtr map_callback_group); - -private: - void callback_map_points(sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr); - - rclcpp::Subscription::SharedPtr map_points_sub_; - std::shared_ptr ndt_ptr_; - std::mutex * ndt_ptr_mutex_; -}; - -#endif // NDT_SCAN_MATCHER__MAP_MODULE_HPP_ 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..04c762ae950a4 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 @@ -15,9 +15,8 @@ #ifndef NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ #define NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ -#include "localization_util/tf2_listener_module.hpp" #include "localization_util/util_func.hpp" -#include "ndt_scan_matcher/debug.hpp" +#include "ndt_scan_matcher/hyper_parameters.hpp" #include "ndt_scan_matcher/particle.hpp" #include @@ -37,56 +36,47 @@ #include #include #include +#include #include class MapUpdateModule { using PointSource = pcl::PointXYZ; using PointTarget = pcl::PointXYZ; - using NormalDistributionsTransform = - pclomp::MultiGridNormalDistributionsTransform; + using NdtType = pclomp::MultiGridNormalDistributionsTransform; + using NdtPtrType = std::shared_ptr; public: MapUpdateModule( - rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr, - std::shared_ptr tf2_listener_module, std::string map_frame, - rclcpp::CallbackGroup::SharedPtr main_callback_group); + rclcpp::Node * node, std::mutex * ndt_ptr_mutex, NdtPtrType & ndt_ptr, + HyperParameters::DynamicMapLoading param); private: friend class NDTScanMatcher; - void callback_ekf_odom(nav_msgs::msg::Odometry::ConstSharedPtr odom_ptr); - void map_update_timer_callback(); - void update_ndt( - const std::vector & maps_to_add, - const std::vector & map_ids_to_remove); + // Update the specified NDT + bool update_ndt(const geometry_msgs::msg::Point & position, NdtType & ndt); void update_map(const geometry_msgs::msg::Point & position); - [[nodiscard]] bool should_update_map(const geometry_msgs::msg::Point & position) const; + [[nodiscard]] bool should_update_map(const geometry_msgs::msg::Point & position); void publish_partial_pcd_map(); rclcpp::Publisher::SharedPtr loaded_pcd_pub_; rclcpp::Client::SharedPtr pcd_loader_client_; - rclcpp::TimerBase::SharedPtr map_update_timer_; - rclcpp::Subscription::SharedPtr ekf_odom_sub_; - - rclcpp::CallbackGroup::SharedPtr map_callback_group_; - - std::shared_ptr ndt_ptr_; + NdtPtrType & ndt_ptr_; std::mutex * ndt_ptr_mutex_; - std::string map_frame_; rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; - std::shared_ptr tf2_listener_module_; std::optional last_update_position_ = std::nullopt; - std::optional current_position_ = std::nullopt; - const double dynamic_map_loading_update_distance_; - const double dynamic_map_loading_map_radius_; - const double lidar_radius_; + + HyperParameters::DynamicMapLoading param_; + + // Indicate if there is a prefetch thread waiting for being collected + NdtPtrType secondary_ndt_ptr_; + bool need_rebuild_; }; #endif // NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ 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..ca69576061e21 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,8 +17,8 @@ #define FMT_HEADER_ONLY -#include "localization_util/tf2_listener_module.hpp" -#include "ndt_scan_matcher/map_module.hpp" +#include "localization_util/smart_pose_buffer.hpp" +#include "ndt_scan_matcher/hyper_parameters.hpp" #include "ndt_scan_matcher/map_update_module.hpp" #include @@ -40,6 +40,8 @@ #include #include #include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -64,11 +66,6 @@ #include #include -enum class ConvergedParamType { - TRANSFORM_PROBABILITY = 0, - NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD = 1 -}; - class NDTScanMatcher : public rclcpp::Node { using PointSource = pcl::PointXYZ; @@ -87,6 +84,7 @@ class NDTScanMatcher : public rclcpp::Node const std_srvs::srv::SetBool::Request::SharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr res); + void callback_timer(); void callback_sensor_points( sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_msg_in_sensor_frame); void callback_initial_pose( @@ -123,17 +121,19 @@ class NDTScanMatcher : public rclcpp::Node const double score, const double score_threshold, const std::string & score_name); bool validate_converged_param( const double & transform_probability, const double & nearest_voxel_transformation_likelihood); + static int count_oscillation(const std::vector & result_pose_msg_array); + std::array rotate_covariance( + const std::array & src_covariance, const Eigen::Matrix3d & rotation) const; std::array estimate_covariance( const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, const rclcpp::Time & sensor_ros_time); - std::optional interpolate_regularization_pose( - const rclcpp::Time & sensor_ros_time); void add_regularization_pose(const rclcpp::Time & sensor_ros_time); void publish_diagnostic(); + rclcpp::TimerBase::SharedPtr map_update_timer_; rclcpp::Subscription::SharedPtr initial_pose_sub_; rclcpp::Subscription::SharedPtr sensor_points_sub_; rclcpp::Subscription::SharedPtr @@ -174,55 +174,30 @@ class NDTScanMatcher : public rclcpp::Node rclcpp::Service::SharedPtr service_trigger_node_; tf2_ros::TransformBroadcaster tf2_broadcaster_; + tf2_ros::Buffer tf2_buffer_; + tf2_ros::TransformListener tf2_listener_; + + rclcpp::CallbackGroup::SharedPtr timer_callback_group_; std::shared_ptr ndt_ptr_; std::shared_ptr> state_ptr_; Eigen::Matrix4f base_to_sensor_matrix_; - std::string base_frame_; - std::string ndt_base_frame_; - std::string map_frame_; - - ConvergedParamType converged_param_type_; - double converged_param_transform_probability_; - double converged_param_nearest_voxel_transformation_likelihood_; - - int initial_estimate_particles_num_; - int n_startup_trials_; - double lidar_topic_timeout_sec_; - double initial_pose_timeout_sec_; - double initial_pose_distance_tolerance_m_; - float inversion_vector_threshold_; - float oscillation_threshold_; - bool use_cov_estimation_; - 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_; + + // Keep latest position for dynamic map loading + std::mutex latest_ekf_position_mtx_; + std::optional latest_ekf_position_ = std::nullopt; - // 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_; - std::unique_ptr map_module_; + std::atomic is_activated_; std::unique_ptr map_update_module_; std::unique_ptr logger_configure_; - // cspell: ignore degrounded - bool estimate_scores_for_degrounded_scan_; - double z_margin_for_ground_removal_; - - bool use_dynamic_map_loading_; - - // The execution time which means probably NDT cannot matches scans properly - int critical_upper_bound_exe_time_ms_; + HyperParameters param_; }; #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/launch/ndt_scan_matcher.launch.xml b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml index b442ac1b3d843..cbcb0a9f74bc4 100644 --- a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml +++ b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml @@ -2,32 +2,28 @@ - - - - + + + - - + - - + - + + - - + - diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index dcfdd77cb5a18..0913ee04174f5 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -8,6 +8,10 @@ Kento Yabuuchi Koji Minoda Masahiro Sakamoto + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Yamato Ando diff --git a/localization/ndt_scan_matcher/schema/ndt_scan_matcher.schema.json b/localization/ndt_scan_matcher/schema/ndt_scan_matcher.schema.json new file mode 100644 index 0000000000000..a42ee37858f46 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/ndt_scan_matcher.schema.json @@ -0,0 +1,44 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "type": "object", + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "type": "object", + "properties": { + "frame": { "$ref": "sub/frame.json#/definitions/frame" }, + "ndt": { "$ref": "sub/ndt.json#/definitions/ndt" }, + "regularization": { "$ref": "ndt_regularization.json#/definitions/ndt/regularization" }, + "initial_pose_estimation": { + "$ref": "sub/initial_pose_estimation.json#/definitions/initial_pose_estimation" + }, + "validation": { "$ref": "sub/validation.json#/definitions/validation" }, + "score_estimation": { + "$ref": "sub/score_estimation.json#/definitions/score_estimation" + }, + "covariance": { "$ref": "sub/covariance.json#/definitions/covariance" }, + "dynamic_map_loading": { + "$ref": "sub/dynamic_map_loading.json#/definitions/dynamic_map_loading" + } + }, + "required": [ + "frame", + "ndt", + "initial_pose_estimation", + "validation", + "score_estimation", + "covariance", + "dynamic_map_loading" + ], + "additionalProperties": false + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/ndt_scan_matcher/schema/sub/covariance.json b/localization/ndt_scan_matcher/schema/sub/covariance.json new file mode 100644 index 0000000000000..655edabbdf871 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/covariance.json @@ -0,0 +1,25 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "covariance": { + "type": "object", + "properties": { + "output_pose_covariance": { + "type": "array", + "description": "The covariance of output pose. Note that this covariance matrix is empirically derived.", + "default": [ + 0.0225, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0225, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0225, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.000625, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.000625, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.000625 + ] + }, + "covariance_estimation": { + "$ref": "covariance_covariance_estimation.json#/definitions/covariance_estimation" + } + }, + "required": ["output_pose_covariance", "covariance_estimation"], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json b/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json new file mode 100644 index 0000000000000..d978e5bcf7357 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json @@ -0,0 +1,28 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "covariance_estimation": { + "type": "object", + "properties": { + "enable": { + "type": "boolean", + "description": "2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value).", + "default": false + }, + "initial_pose_offset_model_x": { + "type": "array", + "description": "Offset arrangement in covariance estimation [m]. initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements.", + "default": [0.0, 0.0, 0.5, -0.5, 1.0, -1.0] + }, + "initial_pose_offset_model_y": { + "type": "array", + "description": "Offset arrangement in covariance estimation [m]. initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements.", + "default": [0.5, -0.5, 0.0, 0.0, 0.0, 0.0] + } + }, + "required": ["enable", "initial_pose_offset_model_x", "initial_pose_offset_model_y"], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/schema/sub/dynamic_map_loading.json b/localization/ndt_scan_matcher/schema/sub/dynamic_map_loading.json new file mode 100644 index 0000000000000..9776fbed350f2 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/dynamic_map_loading.json @@ -0,0 +1,31 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "dynamic_map_loading": { + "type": "object", + "properties": { + "update_distance": { + "type": "number", + "description": "Dynamic map loading distance.", + "default": 20.0, + "minimum": 0.0 + }, + "map_radius": { + "type": "number", + "description": "Dynamic map loading loading radius.", + "default": 150.0, + "minimum": 0.0 + }, + "lidar_radius": { + "type": "number", + "description": "Radius of input LiDAR range (used for diagnostics of dynamic map loading).", + "default": 100.0, + "minimum": 0.0 + } + }, + "required": ["update_distance", "map_radius", "lidar_radius"], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/schema/sub/frame.json b/localization/ndt_scan_matcher/schema/sub/frame.json new file mode 100644 index 0000000000000..98bf7abe711c3 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/frame.json @@ -0,0 +1,28 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "frame": { + "type": "object", + "properties": { + "base_frame": { + "type": "string", + "description": "Vehicle reference frame.", + "default": "base_link" + }, + "ndt_base_frame": { + "type": "string", + "description": "NDT reference frame.", + "default": "ndt_base_link" + }, + "map_frame": { + "type": "string", + "description": "Map frame.", + "default": "map" + } + }, + "required": ["base_frame", "ndt_base_frame", "map_frame"], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json b/localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json new file mode 100644 index 0000000000000..9817f3145bbd3 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json @@ -0,0 +1,25 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "initial_pose_estimation": { + "type": "object", + "properties": { + "particles_num": { + "type": "number", + "description": "The number of particles to estimate initial pose.", + "default": 200, + "minimum": 1 + }, + "n_startup_trials": { + "type": "number", + "description": "The number of initial random trials in the TPE (Tree-Structured Parzen Estimator). This value should be equal to or less than 'initial_estimate_particles_num' and more than 0. If it is equal to 'initial_estimate_particles_num', the search will be the same as a full random search.", + "default": 20, + "minimum": 1 + } + }, + "required": ["particles_num", "n_startup_trials"], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/schema/sub/ndt.json b/localization/ndt_scan_matcher/schema/sub/ndt.json new file mode 100644 index 0000000000000..850e48c2db33b --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/ndt.json @@ -0,0 +1,53 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "ndt": { + "type": "object", + "properties": { + "trans_epsilon": { + "type": "number", + "description": "The maximum difference between two consecutive transformations in order to consider convergence.", + "default": 0.01, + "minimum": 0.0 + }, + "step_size": { + "type": "number", + "description": "The newton line search maximum step length.", + "default": 0.1, + "minimum": 0.0 + }, + "resolution": { + "type": "number", + "description": "The ND voxel grid resolution.", + "default": 2.0, + "minimum": 0.0 + }, + "max_iterations": { + "type": "number", + "description": "The number of iterations required to calculate alignment.", + "default": 30, + "minimum": 1 + }, + "num_threads": { + "type": "number", + "description": "Number of threads used for parallel computing.", + "default": 4, + "minimum": 1 + }, + "regularization": { + "$ref": "ndt_regularization.json#/definitions/regularization" + } + }, + "required": [ + "trans_epsilon", + "step_size", + "resolution", + "max_iterations", + "num_threads", + "regularization" + ], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/schema/sub/ndt_regularization.json b/localization/ndt_scan_matcher/schema/sub/ndt_regularization.json new file mode 100644 index 0000000000000..1de74de792700 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/ndt_regularization.json @@ -0,0 +1,24 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "regularization": { + "type": "object", + "properties": { + "enable": { + "type": "boolean", + "description": "Regularization switch.", + "default": false + }, + "scale_factor": { + "type": "number", + "description": "Regularization scale factor.", + "default": 0.01, + "minimum": 0.0 + } + }, + "required": ["enable", "scale_factor"], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/schema/sub/score_estimation.json b/localization/ndt_scan_matcher/schema/sub/score_estimation.json new file mode 100644 index 0000000000000..c8a1195f99080 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/score_estimation.json @@ -0,0 +1,39 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "score_estimation": { + "type": "object", + "properties": { + "converged_param_type": { + "type": "number", + "description": "Converged param type. 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD", + "default": 1, + "minimum": 0, + "maximum": 1 + }, + "converged_param_transform_probability": { + "type": "number", + "description": "If converged_param_type is 0, threshold for deciding whether to trust the estimation result.", + "default": 3.0, + "minimum": 0.0 + }, + "converged_param_nearest_voxel_transformation_likelihood": { + "type": "number", + "description": "If converged_param_type is 1, threshold for deciding whether to trust the estimation result.", + "default": 2.3, + "minimum": 0.0 + }, + "no_ground_points": { + "$ref": "score_estimation_no_ground_points.json#/definitions/no_ground_points" + } + }, + "required": [ + "converged_param_type", + "converged_param_transform_probability", + "no_ground_points" + ], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/schema/sub/score_estimation_no_ground_points.json b/localization/ndt_scan_matcher/schema/sub/score_estimation_no_ground_points.json new file mode 100644 index 0000000000000..aa3f0fe162bc0 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/score_estimation_no_ground_points.json @@ -0,0 +1,24 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "no_ground_points": { + "type": "object", + "properties": { + "enable": { + "type": "boolean", + "description": "A flag for using scan matching score based on de-grounded LiDAR scan.", + "default": false + }, + "z_margin_for_ground_removal": { + "type": "number", + "description": "If lidar_point.z - base_link.z <= this threshold , the point will be removed.", + "default": 0.8, + "minimum": 0.0 + } + }, + "required": ["enable", "z_margin_for_ground_removal"], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/schema/sub/validation.json b/localization/ndt_scan_matcher/schema/sub/validation.json new file mode 100644 index 0000000000000..5ad40ceb99577 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/validation.json @@ -0,0 +1,42 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "validation": { + "type": "object", + "properties": { + "lidar_topic_timeout_sec": { + "type": "number", + "description": "Tolerance of timestamp difference between current time and sensor pointcloud. [sec]", + "default": 1.0, + "minimum": 0.0 + }, + "initial_pose_timeout_sec": { + "type": "number", + "description": "Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec]", + "default": 1.0, + "minimum": 0.0 + }, + "initial_pose_distance_tolerance_m": { + "type": "number", + "description": "Tolerance of distance difference between two initial poses used for linear interpolation. [m]", + "default": 10.0, + "minimum": 0.0 + }, + "critical_upper_bound_exe_time_ms": { + "type": "number", + "description": "The execution time which means probably NDT cannot matches scans properly. [ms]", + "default": 100.0, + "minimum": 0.0 + } + }, + "required": [ + "lidar_topic_timeout_sec", + "initial_pose_timeout_sec", + "initial_pose_distance_tolerance_m", + "critical_upper_bound_exe_time_ms" + ], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/src/debug.cpp b/localization/ndt_scan_matcher/src/debug.cpp deleted file mode 100644 index 941f682de5803..0000000000000 --- a/localization/ndt_scan_matcher/src/debug.cpp +++ /dev/null @@ -1,67 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ndt_scan_matcher/debug.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) -{ - // 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.id = static_cast(i); - marker.lifetime = rclcpp::Duration::from_seconds(10.0); // 10.0 is the lifetime in seconds. - - marker.ns = "initial_pose_transform_probability_color_marker"; - marker.pose = particle.initial_pose; - marker.color = exchange_color_crc(particle.score / 4.5); - marker_array.markers.push_back(marker); - - marker.ns = "initial_pose_iteration_color_marker"; - marker.pose = particle.initial_pose; - marker.color = exchange_color_crc((1.0 * particle.iteration) / 30.0); - marker_array.markers.push_back(marker); - - marker.ns = "initial_pose_index_color_marker"; - marker.pose = particle.initial_pose; - marker.color = exchange_color_crc(static_cast(i) / 100.0); - marker_array.markers.push_back(marker); - - marker.ns = "result_pose_transform_probability_color_marker"; - marker.pose = particle.result_pose; - marker.color = exchange_color_crc(particle.score / 4.5); - marker_array.markers.push_back(marker); - - marker.ns = "result_pose_iteration_color_marker"; - marker.pose = particle.result_pose; - marker.color = exchange_color_crc((1.0 * particle.iteration) / 30.0); - marker_array.markers.push_back(marker); - - marker.ns = "result_pose_index_color_marker"; - 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/ndt_scan_matcher/src/map_module.cpp b/localization/ndt_scan_matcher/src/map_module.cpp deleted file mode 100644 index d6088a1b14949..0000000000000 --- a/localization/ndt_scan_matcher/src/map_module.cpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ndt_scan_matcher/map_module.hpp" - -MapModule::MapModule( - rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr, - rclcpp::CallbackGroup::SharedPtr map_callback_group) -: ndt_ptr_(std::move(ndt_ptr)), ndt_ptr_mutex_(ndt_ptr_mutex) -{ - auto map_sub_opt = rclcpp::SubscriptionOptions(); - map_sub_opt.callback_group = map_callback_group; - - map_points_sub_ = node->create_subscription( - "pointcloud_map", rclcpp::QoS{1}.transient_local(), - std::bind(&MapModule::callback_map_points, this, std::placeholders::_1), map_sub_opt); -} - -void MapModule::callback_map_points( - sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr) -{ - NormalDistributionsTransform new_ndt; - new_ndt.setParams(ndt_ptr_->getParams()); - - pcl::shared_ptr> map_points_ptr(new pcl::PointCloud); - pcl::fromROSMsg(*map_points_msg_ptr, *map_points_ptr); - new_ndt.setInputTarget(map_points_ptr); - // create Thread - // detach - auto output_cloud = std::make_shared>(); - new_ndt.align(*output_cloud); - - // swap - ndt_ptr_mutex_->lock(); - *ndt_ptr_ = new_ndt; - ndt_ptr_mutex_->unlock(); -} diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index f0a583417fb76..950457f0d49b1 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -14,106 +14,117 @@ #include "ndt_scan_matcher/map_update_module.hpp" -template -double norm_xy(const T p1, const U p2) -{ - double dx = p1.x - p2.x; - double dy = p1.y - p2.y; - return std::sqrt(dx * dx + dy * dy); -} - MapUpdateModule::MapUpdateModule( - rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr, - std::shared_ptr tf2_listener_module, std::string map_frame, - rclcpp::CallbackGroup::SharedPtr main_callback_group) -: ndt_ptr_(std::move(ndt_ptr)), + rclcpp::Node * node, std::mutex * ndt_ptr_mutex, NdtPtrType & ndt_ptr, + HyperParameters::DynamicMapLoading param) +: ndt_ptr_(ndt_ptr), ndt_ptr_mutex_(ndt_ptr_mutex), - map_frame_(std::move(map_frame)), logger_(node->get_logger()), clock_(node->get_clock()), - tf2_listener_module_(std::move(tf2_listener_module)), - dynamic_map_loading_update_distance_( - node->declare_parameter("dynamic_map_loading_update_distance")), - dynamic_map_loading_map_radius_( - node->declare_parameter("dynamic_map_loading_map_radius")), - lidar_radius_(node->declare_parameter("lidar_radius")) + param_(param) { - auto main_sub_opt = rclcpp::SubscriptionOptions(); - main_sub_opt.callback_group = main_callback_group; - - map_callback_group_ = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - ekf_odom_sub_ = node->create_subscription( - "ekf_odom", 100, std::bind(&MapUpdateModule::callback_ekf_odom, this, std::placeholders::_1), - main_sub_opt); - loaded_pcd_pub_ = node->create_publisher( "debug/loaded_pointcloud_map", rclcpp::QoS{1}.transient_local()); pcd_loader_client_ = node->create_client("pcd_loader_service"); - double map_update_dt = 1.0; - auto period_ns = std::chrono::duration_cast( - std::chrono::duration(map_update_dt)); - map_update_timer_ = rclcpp::create_timer( - node, clock_, period_ns, std::bind(&MapUpdateModule::map_update_timer_callback, this), - map_callback_group_); + secondary_ndt_ptr_.reset(new NdtType); + + if (ndt_ptr_) { + *secondary_ndt_ptr_ = *ndt_ptr_; + } else { + RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, "Attempt to update a null NDT pointer."); + } + + // Initially, a direct map update on ndt_ptr_ is needed. + // ndt_ptr_'s mutex is locked until it is fully rebuilt. + // From the second update, the update is done on secondary_ndt_ptr_, + // and ndt_ptr_ is only locked when swapping its pointer with + // secondary_ndt_ptr_. + need_rebuild_ = true; } -void MapUpdateModule::callback_ekf_odom(nav_msgs::msg::Odometry::ConstSharedPtr odom_ptr) +bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & position) { - current_position_ = odom_ptr->pose.pose.position; - if (last_update_position_ == std::nullopt) { - return; + return false; } - double distance = norm_xy(current_position_.value(), last_update_position_.value()); - if (distance + lidar_radius_ > dynamic_map_loading_map_radius_) { - RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1, "Dynamic map loading is not keeping up."); + + const double dx = position.x - last_update_position_.value().x; + const double dy = position.y - last_update_position_.value().y; + const double distance = std::hypot(dx, dy); + if (distance + param_.lidar_radius > param_.map_radius) { + RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, "Dynamic map loading is not keeping up."); + // If the map does not keep up with the current position, + // lock ndt_ptr_ entirely until it is fully rebuilt. + need_rebuild_ = true; } + + return distance > param_.update_distance; } -void MapUpdateModule::map_update_timer_callback() +void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) { - if (current_position_ == std::nullopt) { - RCLCPP_ERROR_STREAM_THROTTLE( - logger_, *clock_, 1, - "Cannot find the reference position for map update. Please check if the EKF odometry is " - "provided to NDT."); - return; - } - if (last_update_position_ == std::nullopt) return; + // If the current position is super far from the previous loading position, + // lock and rebuild ndt_ptr_ + if (need_rebuild_) { + ndt_ptr_mutex_->lock(); + + auto param = ndt_ptr_->getParams(); + auto input_source = ndt_ptr_->getInputSource(); + + ndt_ptr_.reset(new NdtType); + + ndt_ptr_->setParams(param); + + update_ndt(position, *ndt_ptr_); + ndt_ptr_->setInputSource(input_source); + ndt_ptr_mutex_->unlock(); + need_rebuild_ = false; + } else { + // Load map to the secondary_ndt_ptr, which does not require a mutex lock + // Since the update of the secondary ndt ptr and the NDT align (done on + // the main ndt_ptr_) overlap, the latency of updating/alignment reduces partly. + // If the updating is done the main ndt_ptr_, either the update or the NDT + // align will be blocked by the other. + const bool updated = update_ndt(position, *secondary_ndt_ptr_); + if (!updated) { + last_update_position_ = position; + return; + } - // continue only if we should update the map - if (should_update_map(current_position_.value())) { - RCLCPP_INFO(logger_, "Start updating NDT map (timer_callback)"); - update_map(current_position_.value()); - last_update_position_ = current_position_; + ndt_ptr_mutex_->lock(); + auto dummy_ptr = ndt_ptr_; + auto input_source = ndt_ptr_->getInputSource(); + ndt_ptr_ = secondary_ndt_ptr_; + ndt_ptr_->setInputSource(input_source); + ndt_ptr_mutex_->unlock(); + + dummy_ptr.reset(); } -} -bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & position) const -{ - if (last_update_position_ == std::nullopt) return false; - double distance = norm_xy(position, last_update_position_.value()); - return distance > dynamic_map_loading_update_distance_; + secondary_ndt_ptr_.reset(new NdtType); + *secondary_ndt_ptr_ = *ndt_ptr_; + + // Memorize the position of the last update + last_update_position_ = position; + + // Publish the new ndt maps + publish_partial_pcd_map(); } -void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) +bool MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, NdtType & ndt) { auto request = std::make_shared(); + request->area.center_x = static_cast(position.x); request->area.center_y = static_cast(position.y); - request->area.radius = static_cast(dynamic_map_loading_map_radius_); - request->cached_ids = ndt_ptr_->getCurrentMapIDs(); + request->area.radius = static_cast(param_.map_radius); + request->cached_ids = ndt.getCurrentMapIDs(); while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { - RCLCPP_INFO( - logger_, - "Waiting for pcd loader service. Check if the enable_differential_load in " - "pointcloud_map_loader is set `true`."); + RCLCPP_INFO(logger_, "Waiting for pcd loader service. Check the pointcloud_map_loader."); } // send a request to map_loader @@ -125,63 +136,54 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) while (status != std::future_status::ready) { RCLCPP_INFO(logger_, "waiting response"); if (!rclcpp::ok()) { - return; + return false; // No update } status = result.wait_for(std::chrono::seconds(1)); } - update_ndt(result.get()->new_pointcloud_with_ids, result.get()->ids_to_remove); - last_update_position_ = position; -} -void MapUpdateModule::update_ndt( - const std::vector & maps_to_add, - const std::vector & map_ids_to_remove) -{ + auto & maps_to_add = result.get()->new_pointcloud_with_ids; + auto & map_ids_to_remove = result.get()->ids_to_remove; + RCLCPP_INFO( logger_, "Update map (Add: %lu, Remove: %lu)", maps_to_add.size(), map_ids_to_remove.size()); if (maps_to_add.empty() && map_ids_to_remove.empty()) { RCLCPP_INFO(logger_, "Skip map update"); - return; + return false; // No update } + const auto exe_start_time = std::chrono::system_clock::now(); + const size_t add_size = maps_to_add.size(); + // Perform heavy processing outside of the lock scope + std::vector>> points_pcl(add_size); - NormalDistributionsTransform backup_ndt = *ndt_ptr_; + for (size_t i = 0; i < add_size; i++) { + points_pcl[i] = pcl::make_shared>(); + pcl::fromROSMsg(maps_to_add[i].pointcloud, *points_pcl[i]); + } // Add pcd - for (const auto & map_to_add : maps_to_add) { - pcl::shared_ptr> map_points_ptr(new pcl::PointCloud); - pcl::fromROSMsg(map_to_add.pointcloud, *map_points_ptr); - backup_ndt.addTarget(map_points_ptr, map_to_add.cell_id); + for (size_t i = 0; i < add_size; i++) { + ndt.addTarget(points_pcl[i], maps_to_add[i].cell_id); } // Remove pcd for (const std::string & map_id_to_remove : map_ids_to_remove) { - backup_ndt.removeTarget(map_id_to_remove); + ndt.removeTarget(map_id_to_remove); } - backup_ndt.createVoxelKdtree(); + ndt.createVoxelKdtree(); const auto exe_end_time = std::chrono::system_clock::now(); const auto duration_micro_sec = std::chrono::duration_cast(exe_end_time - exe_start_time).count(); const auto exe_time = static_cast(duration_micro_sec) / 1000.0; RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time); - - // swap - (*ndt_ptr_mutex_).lock(); - // ToDo (kminoda): Here negligible NDT copy occurs during the new map loading phase, which should - // ideally be avoided. But I will leave this for now since I cannot come up with a solution other - // than using pointer of pointer. - *ndt_ptr_ = backup_ndt; - (*ndt_ptr_mutex_).unlock(); - - publish_partial_pcd_map(); + return true; // Updated } void MapUpdateModule::publish_partial_pcd_map() { pcl::PointCloud map_pcl = ndt_ptr_->getVoxelPCD(); - sensor_msgs::msg::PointCloud2 map_msg; pcl::toROSMsg(map_pcl, map_msg); map_msg.header.frame_id = "map"; 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..cd637791f04b6 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,156 +60,52 @@ 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( - const std::vector & result_pose_msg_array, - const float oscillation_threshold, const float inversion_vector_threshold) -{ - bool prev_oscillation = false; - int oscillation_cnt = 0; - - for (size_t i = 2; i < result_pose_msg_array.size(); ++i) { - const Eigen::Vector3d current_pose = point_to_vector3d(result_pose_msg_array.at(i).position); - const Eigen::Vector3d prev_pose = point_to_vector3d(result_pose_msg_array.at(i - 1).position); - const Eigen::Vector3d prev_prev_pose = - point_to_vector3d(result_pose_msg_array.at(i - 2).position); - const auto current_vec = current_pose - prev_pose; - const auto prev_vec = (prev_pose - prev_prev_pose).normalized(); - const bool oscillation = prev_vec.dot(current_vec) < inversion_vector_threshold; - if (prev_oscillation && oscillation) { - if (static_cast(oscillation_cnt) > oscillation_threshold) { - return true; - } - ++oscillation_cnt; - } else { - oscillation_cnt = 0; - } - prev_oscillation = oscillation; - } - return false; -} - -// cspell: ignore degrounded NDTScanMatcher::NDTScanMatcher() : Node("ndt_scan_matcher"), tf2_broadcaster_(*this), + tf2_buffer_(this->get_clock()), + tf2_listener_(tf2_buffer_), ndt_ptr_(new NormalDistributionsTransform), 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 - regularization_enabled_(declare_parameter("regularization_enabled")) + is_activated_(false), + param_(this) { (*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); - - base_frame_ = this->declare_parameter("base_frame"); - RCLCPP_INFO(get_logger(), "base_frame_id: %s", base_frame_.c_str()); - - ndt_base_frame_ = this->declare_parameter("ndt_base_frame"); - RCLCPP_INFO(get_logger(), "ndt_base_frame_id: %s", ndt_base_frame_.c_str()); - - map_frame_ = this->declare_parameter("map_frame"); - RCLCPP_INFO(get_logger(), "map_frame_id: %s", map_frame_.c_str()); - - pclomp::NdtParams ndt_params{}; - 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.num_threads = std::max(ndt_params.num_threads, 1); - ndt_params.regularization_scale_factor = - static_cast(this->declare_parameter("regularization_scale_factor")); - ndt_ptr_->setParams(ndt_params); - - RCLCPP_INFO( - get_logger(), "trans_epsilon: %lf, step_size: %lf, resolution: %lf, max_iterations: %d", - 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"); - converged_param_type_ = static_cast(converged_param_type_tmp); - - converged_param_transform_probability_ = - this->declare_parameter("converged_param_transform_probability"); - converged_param_nearest_voxel_transformation_likelihood_ = - this->declare_parameter("converged_param_nearest_voxel_transformation_likelihood"); - - 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"); - initial_pose_timeout_sec_ = this->declare_parameter("initial_pose_timeout_sec"); - - initial_pose_distance_tolerance_m_ = - this->declare_parameter("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 = - this->declare_parameter>("initial_pose_offset_model_x"); - std::vector initial_pose_offset_model_y = - 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); - } else { - RCLCPP_WARN( - get_logger(), - "Invalid initial pose offset model parameters. Disable covariance estimation."); - use_cov_estimation_ = false; - } - } - - std::vector output_pose_covariance = - this->declare_parameter>("output_pose_covariance"); - for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) { - 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"); - - estimate_scores_for_degrounded_scan_ = - this->declare_parameter("estimate_scores_for_degrounded_scan"); - - z_margin_for_ground_removal_ = this->declare_parameter("z_margin_for_ground_removal"); - - rclcpp::CallbackGroup::SharedPtr initial_pose_callback_group; - initial_pose_callback_group = + timer_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::CallbackGroup::SharedPtr initial_pose_callback_group = + this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::CallbackGroup::SharedPtr sensor_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - rclcpp::CallbackGroup::SharedPtr main_callback_group; - main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); auto initial_pose_sub_opt = rclcpp::SubscriptionOptions(); initial_pose_sub_opt.callback_group = initial_pose_callback_group; - - auto main_sub_opt = rclcpp::SubscriptionOptions(); - main_sub_opt.callback_group = main_callback_group; - + auto sensor_sub_opt = rclcpp::SubscriptionOptions(); + sensor_sub_opt.callback_group = sensor_callback_group; + + constexpr double map_update_dt = 1.0; + constexpr auto period_ns = std::chrono::duration_cast( + std::chrono::duration(map_update_dt)); + map_update_timer_ = rclcpp::create_timer( + this, this->get_clock(), period_ns, std::bind(&NDTScanMatcher::callback_timer, this), + timer_callback_group_); initial_pose_sub_ = this->create_subscription( "ekf_pose_with_covariance", 100, std::bind(&NDTScanMatcher::callback_initial_pose, this, std::placeholders::_1), initial_pose_sub_opt); sensor_points_sub_ = this->create_subscription( - "points_raw", rclcpp::SensorDataQoS().keep_last(points_queue_size), - std::bind(&NDTScanMatcher::callback_sensor_points, this, std::placeholders::_1), main_sub_opt); + "points_raw", rclcpp::SensorDataQoS().keep_last(1), + std::bind(&NDTScanMatcher::callback_sensor_points, this, std::placeholders::_1), + sensor_sub_opt); // Only if regularization is enabled, subscribe to the regularization base pose - if (regularization_enabled_) { + if (param_.ndt_regularization_enable) { // NOTE: The reason that the regularization subscriber does not belong to the - // main_callback_group is to ensure that the regularization callback is called even if + // sensor_callback_group is to ensure that the regularization callback is called even if // sensor_callback takes long time to process. // Both callback_initial_pose and callback_regularization_pose must not miss receiving data for // proper interpolation. @@ -232,6 +114,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_ = @@ -283,22 +168,21 @@ NDTScanMatcher::NDTScanMatcher() "ndt_align_srv", std::bind( &NDTScanMatcher::service_ndt_align, this, std::placeholders::_1, std::placeholders::_2), - rclcpp::ServicesQoS().get_rmw_qos_profile(), main_callback_group); + rclcpp::ServicesQoS().get_rmw_qos_profile(), sensor_callback_group); service_trigger_node_ = this->create_service( "trigger_node_srv", std::bind( &NDTScanMatcher::service_trigger_node, this, std::placeholders::_1, std::placeholders::_2), - rclcpp::ServicesQoS().get_rmw_qos_profile(), main_callback_group); + rclcpp::ServicesQoS().get_rmw_qos_profile(), sensor_callback_group); - tf2_listener_module_ = std::make_shared(this); + ndt_ptr_->setParams(param_.ndt); - use_dynamic_map_loading_ = this->declare_parameter("use_dynamic_map_loading"); - if (use_dynamic_map_loading_) { - map_update_module_ = std::make_unique( - this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, map_frame_, main_callback_group); - } else { - map_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, main_callback_group); - } + initial_pose_buffer_ = std::make_unique( + this->get_logger(), param_.validation.initial_pose_timeout_sec, + param_.validation.initial_pose_distance_tolerance_m); + + map_update_module_ = + std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, param_.dynamic_map_loading); logger_configure_ = std::make_unique(this); } @@ -324,7 +208,8 @@ void NDTScanMatcher::publish_diagnostic() } if ( state_ptr_->count("lidar_topic_delay_time_sec") && - std::stod((*state_ptr_)["lidar_topic_delay_time_sec"]) > lidar_topic_timeout_sec_) { + std::stod((*state_ptr_)["lidar_topic_delay_time_sec"]) > + param_.validation.lidar_topic_timeout_sec) { diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "lidar_topic_delay_time_sec exceed limit. "; } @@ -344,13 +229,13 @@ void NDTScanMatcher::publish_diagnostic() if ( state_ptr_->count("nearest_voxel_transformation_likelihood") && std::stod((*state_ptr_)["nearest_voxel_transformation_likelihood"]) < - converged_param_nearest_voxel_transformation_likelihood_) { + param_.score_estimation.converged_param_nearest_voxel_transformation_likelihood) { diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "NDT score is unreliably low. "; } if ( - state_ptr_->count("execution_time") && - std::stod((*state_ptr_)["execution_time"]) >= critical_upper_bound_exe_time_ms_) { + state_ptr_->count("execution_time") && std::stod((*state_ptr_)["execution_time"]) >= + param_.validation.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])"; @@ -370,47 +255,52 @@ void NDTScanMatcher::publish_diagnostic() diagnostics_pub_->publish(diag_msg); } +void NDTScanMatcher::callback_timer() +{ + if (!is_activated_) { + return; + } + std::lock_guard lock(latest_ekf_position_mtx_); + if (latest_ekf_position_ == std::nullopt) { + RCLCPP_ERROR_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, + "Cannot find the reference position for map update. Please check if the EKF odometry is " + "provided to NDT."); + return; + } + // continue only if we should update the map + if (map_update_module_->should_update_map(latest_ekf_position_.value())) { + RCLCPP_INFO(this->get_logger(), "Start updating NDT map (timer_callback)"); + map_update_module_->update_map(latest_ekf_position_.value()); + } +} + void NDTScanMatcher::callback_initial_pose( const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr initial_pose_msg_ptr) { 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 == param_.frame.map_frame) { + initial_pose_buffer_->push_back(initial_pose_msg_ptr); + } else { + 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 " << param_.frame.map_frame + << ". Please check the frame_id in the input topic and ensure it is correct."); } - if (initial_pose_msg_ptr->header.frame_id == map_frame_) { - initial_pose_msg_ptr_array_.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); + { + // latest_ekf_position_ is also used by callback_timer, so it is necessary to acquire the lock + std::lock_guard lock(latest_ekf_position_mtx_); + latest_ekf_position_ = initial_pose_msg_ptr->pose.pose.position; } } 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( @@ -425,12 +315,12 @@ void NDTScanMatcher::callback_sensor_points( const double lidar_topic_delay_time_sec = (this->now() - sensor_ros_time).seconds(); (*state_ptr_)["lidar_topic_delay_time_sec"] = std::to_string(lidar_topic_delay_time_sec); - if (lidar_topic_delay_time_sec > lidar_topic_timeout_sec_) { + if (lidar_topic_delay_time_sec > param_.validation.lidar_topic_timeout_sec) { RCLCPP_WARN( this->get_logger(), "The LiDAR topic is experiencing latency. The delay time is %lf[sec] (the tolerance is " "%lf[sec])", - lidar_topic_delay_time_sec, lidar_topic_timeout_sec_); + lidar_topic_delay_time_sec, param_.validation.lidar_topic_timeout_sec); // If the delay time of the LiDAR topic exceeds the delay compensation time of ekf_localizer, // even if further processing continues, the estimated result will be rejected by ekf_localizer. @@ -454,25 +344,24 @@ void NDTScanMatcher::callback_sensor_points( pcl::fromROSMsg(*sensor_points_msg_in_sensor_frame, *sensor_points_in_sensor_frame); transform_sensor_measurement( - sensor_frame, base_frame_, sensor_points_in_sensor_frame, sensor_points_in_baselink_frame); + sensor_frame, param_.frame.base_frame, sensor_points_in_sensor_frame, + sensor_points_in_baselink_frame); ndt_ptr_->setInputSource(sensor_points_in_baselink_frame); 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_) { + if (param_.ndt_regularization_enable) { add_regularization_pose(sensor_ros_time); } @@ -483,7 +372,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(); @@ -498,10 +387,11 @@ void NDTScanMatcher::callback_sensor_points( // perform several validations bool is_ok_iteration_num = validate_num_iteration(ndt_result.iteration_num, ndt_ptr_->getMaximumIterations()); + const int oscillation_num = count_oscillation(transformation_msg_array); bool is_local_optimal_solution_oscillation = false; if (!is_ok_iteration_num) { - is_local_optimal_solution_oscillation = validate_local_optimal_solution_oscillation( - transformation_msg_array, oscillation_threshold_, inversion_vector_threshold_); + constexpr int oscillation_threshold = 10; + is_local_optimal_solution_oscillation = (oscillation_num > oscillation_threshold); } bool is_ok_converged_param = validate_converged_param( ndt_result.transform_probability, ndt_result.nearest_voxel_transformation_likelihood); @@ -515,8 +405,16 @@ void NDTScanMatcher::callback_sensor_points( } // covariance estimation - std::array ndt_covariance = output_pose_covariance_; - if (is_converged && use_cov_estimation_) { + const Eigen::Quaterniond map_to_base_link_quat = Eigen::Quaterniond( + result_pose_msg.orientation.w, result_pose_msg.orientation.x, result_pose_msg.orientation.y, + result_pose_msg.orientation.z); + const Eigen::Matrix3d map_to_base_link_rotation = + map_to_base_link_quat.normalized().toRotationMatrix(); + + std::array ndt_covariance = + rotate_covariance(param_.covariance.output_pose_covariance, map_to_base_link_rotation); + + if (is_converged && param_.covariance.covariance_estimation.enable) { const auto estimated_covariance = estimate_covariance(ndt_result, initial_pose_matrix, sensor_ros_time); ndt_covariance = estimated_covariance; @@ -528,7 +426,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,23 +437,25 @@ 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); tier4_autoware_utils::transformPointCloud( *sensor_points_in_baselink_frame, *sensor_points_in_map_ptr, ndt_result.pose); - publish_point_cloud(sensor_ros_time, map_frame_, sensor_points_in_map_ptr); + publish_point_cloud(sensor_ros_time, param_.frame.map_frame, sensor_points_in_map_ptr); - // whether use de-grounded points calculate score - if (estimate_scores_for_degrounded_scan_) { + // whether use no ground points to calculate score + if (param_.score_estimation.no_ground_points.enable) { // remove ground pcl::shared_ptr> no_ground_points_in_map_ptr( new pcl::PointCloud); for (std::size_t i = 0; i < sensor_points_in_map_ptr->size(); i++) { const float point_z = sensor_points_in_map_ptr->points[i].z; // NOLINT - if (point_z - matrix4f_to_pose(ndt_result.pose).position.z > z_margin_for_ground_removal_) { + if ( + point_z - matrix4f_to_pose(ndt_result.pose).position.z > + param_.score_estimation.no_ground_points.z_margin_for_ground_removal) { no_ground_points_in_map_ptr->points.push_back(sensor_points_in_map_ptr->points[i]); } } @@ -563,7 +463,7 @@ void NDTScanMatcher::callback_sensor_points( sensor_msgs::msg::PointCloud2 no_ground_points_msg_in_map; pcl::toROSMsg(*no_ground_points_in_map_ptr, no_ground_points_msg_in_map); no_ground_points_msg_in_map.header.stamp = sensor_ros_time; - no_ground_points_msg_in_map.header.frame_id = map_frame_; + no_ground_points_msg_in_map.header.frame_id = param_.frame.map_frame; no_ground_points_aligned_pose_pub_->publish(no_ground_points_msg_in_map); // calculate score const auto no_ground_transform_probability = static_cast( @@ -583,11 +483,9 @@ void NDTScanMatcher::callback_sensor_points( std::to_string(ndt_result.nearest_voxel_transformation_likelihood); (*state_ptr_)["iteration_num"] = std::to_string(ndt_result.iteration_num); (*state_ptr_)["skipping_publish_num"] = std::to_string(skipping_publish_num); - if (is_local_optimal_solution_oscillation) { - (*state_ptr_)["is_local_optimal_solution_oscillation"] = "1"; - } else { - (*state_ptr_)["is_local_optimal_solution_oscillation"] = "0"; - } + (*state_ptr_)["oscillation_count"] = std::to_string(oscillation_num); + (*state_ptr_)["is_local_optimal_solution_oscillation"] = + std::to_string(is_local_optimal_solution_oscillation); (*state_ptr_)["execution_time"] = std::to_string(exe_time); publish_diagnostic(); @@ -598,11 +496,25 @@ void NDTScanMatcher::transform_sensor_measurement( const pcl::shared_ptr> & sensor_points_input_ptr, pcl::shared_ptr> & sensor_points_output_ptr) { - auto tf_target_to_source_ptr = std::make_shared(); - tf2_listener_module_->get_transform( - this->now(), target_frame, source_frame, tf_target_to_source_ptr); + if (source_frame == target_frame) { + sensor_points_output_ptr = sensor_points_input_ptr; + return; + } + + geometry_msgs::msg::TransformStamped transform; + try { + transform = tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + RCLCPP_WARN( + this->get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); + // Since there is no clear error handling policy, temporarily return as is. + sensor_points_output_ptr = sensor_points_input_ptr; + return; + } + const geometry_msgs::msg::PoseStamped target_to_source_pose_stamped = - tier4_autoware_utils::transform2pose(*tf_target_to_source_ptr); + tier4_autoware_utils::transform2pose(transform); const Eigen::Matrix4f base_to_sensor_matrix = pose_to_matrix4f(target_to_source_pose_stamped.pose); tier4_autoware_utils::transformPointCloud( @@ -614,10 +526,10 @@ void NDTScanMatcher::publish_tf( { geometry_msgs::msg::PoseStamped result_pose_stamped_msg; result_pose_stamped_msg.header.stamp = sensor_ros_time; - result_pose_stamped_msg.header.frame_id = map_frame_; + result_pose_stamped_msg.header.frame_id = param_.frame.map_frame; result_pose_stamped_msg.pose = result_pose_msg; tf2_broadcaster_.sendTransform( - tier4_autoware_utils::pose2transform(result_pose_stamped_msg, ndt_base_frame_)); + tier4_autoware_utils::pose2transform(result_pose_stamped_msg, param_.frame.ndt_base_frame)); } void NDTScanMatcher::publish_pose( @@ -626,12 +538,12 @@ void NDTScanMatcher::publish_pose( { geometry_msgs::msg::PoseStamped result_pose_stamped_msg; result_pose_stamped_msg.header.stamp = sensor_ros_time; - result_pose_stamped_msg.header.frame_id = map_frame_; + result_pose_stamped_msg.header.frame_id = param_.frame.map_frame; result_pose_stamped_msg.pose = result_pose_msg; geometry_msgs::msg::PoseWithCovarianceStamped result_pose_with_cov_msg; result_pose_with_cov_msg.header.stamp = sensor_ros_time; - result_pose_with_cov_msg.header.frame_id = map_frame_; + result_pose_with_cov_msg.header.frame_id = param_.frame.map_frame; result_pose_with_cov_msg.pose.pose = result_pose_msg; result_pose_with_cov_msg.pose.covariance = ndt_covariance; @@ -658,7 +570,7 @@ void NDTScanMatcher::publish_marker( visualization_msgs::msg::MarkerArray marker_array; visualization_msgs::msg::Marker marker; marker.header.stamp = sensor_ros_time; - marker.header.frame_id = map_frame_; + marker.header.frame_id = param_.frame.map_frame; marker.type = visualization_msgs::msg::Marker::ARROW; marker.action = visualization_msgs::msg::Marker::ADD; marker.scale = tier4_autoware_utils::createMarkerScale(0.3, 0.1, 0.1); @@ -692,7 +604,7 @@ void NDTScanMatcher::publish_initial_to_result( initial_to_result_relative_pose_stamped.pose = tier4_autoware_utils::inverseTransformPose(result_pose_msg, initial_pose_cov_msg.pose.pose); initial_to_result_relative_pose_stamped.header.stamp = sensor_ros_time; - initial_to_result_relative_pose_stamped.header.frame_id = map_frame_; + initial_to_result_relative_pose_stamped.header.frame_id = param_.frame.map_frame; initial_to_result_relative_pose_pub_->publish(initial_to_result_relative_pose_stamped); const auto initial_to_result_distance = @@ -740,13 +652,16 @@ bool NDTScanMatcher::validate_converged_param( const double & transform_probability, const double & nearest_voxel_transformation_likelihood) { bool is_ok_converged_param = false; - if (converged_param_type_ == ConvergedParamType::TRANSFORM_PROBABILITY) { + if (param_.score_estimation.converged_param_type == ConvergedParamType::TRANSFORM_PROBABILITY) { is_ok_converged_param = validate_score( - transform_probability, converged_param_transform_probability_, "Transform Probability"); - } else if (converged_param_type_ == ConvergedParamType::NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) { + transform_probability, param_.score_estimation.converged_param_transform_probability, + "Transform Probability"); + } else if ( + param_.score_estimation.converged_param_type == + ConvergedParamType::NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) { is_ok_converged_param = validate_score( nearest_voxel_transformation_likelihood, - converged_param_nearest_voxel_transformation_likelihood_, + param_.score_estimation.converged_param_nearest_voxel_transformation_likelihood, "Nearest Voxel Transformation Likelihood"); } else { is_ok_converged_param = false; @@ -756,6 +671,55 @@ bool NDTScanMatcher::validate_converged_param( return is_ok_converged_param; } +int NDTScanMatcher::count_oscillation( + const std::vector & result_pose_msg_array) +{ + constexpr double inversion_vector_threshold = -0.9; + + int oscillation_cnt = 0; + int max_oscillation_cnt = 0; + + for (size_t i = 2; i < result_pose_msg_array.size(); ++i) { + const Eigen::Vector3d current_pose = point_to_vector3d(result_pose_msg_array.at(i).position); + const Eigen::Vector3d prev_pose = point_to_vector3d(result_pose_msg_array.at(i - 1).position); + const Eigen::Vector3d prev_prev_pose = + point_to_vector3d(result_pose_msg_array.at(i - 2).position); + const auto current_vec = (current_pose - prev_pose).normalized(); + const auto prev_vec = (prev_pose - prev_prev_pose).normalized(); + const double cosine_value = current_vec.dot(prev_vec); + const bool oscillation = cosine_value < inversion_vector_threshold; + if (oscillation) { + oscillation_cnt++; // count consecutive oscillation + } else { + oscillation_cnt = 0; // reset + } + max_oscillation_cnt = std::max(max_oscillation_cnt, oscillation_cnt); + } + return max_oscillation_cnt; +} + +std::array NDTScanMatcher::rotate_covariance( + const std::array & src_covariance, const Eigen::Matrix3d & rotation) const +{ + std::array ret_covariance = src_covariance; + + Eigen::Matrix3d src_cov; + src_cov << src_covariance[0], src_covariance[1], src_covariance[2], src_covariance[6], + src_covariance[7], src_covariance[8], src_covariance[12], src_covariance[13], + src_covariance[14]; + + Eigen::Matrix3d ret_cov; + ret_cov = rotation * src_cov * rotation.transpose(); + + for (Eigen::Index i = 0; i < 3; ++i) { + ret_covariance[i] = ret_cov(0, i); + ret_covariance[i + 6] = ret_cov(1, i); + ret_covariance[i + 12] = ret_cov(2, i); + } + + return ret_covariance; +} + std::array NDTScanMatcher::estimate_covariance( const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, const rclcpp::Time & sensor_ros_time) @@ -765,12 +729,13 @@ 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()); - return output_pose_covariance_; + RCLCPP_WARN(get_logger(), "Error in Eigen solver: %s", e.what()); + return param_.covariance.output_pose_covariance; } // first result is added to mean - const int n = initial_pose_offset_model_.size() + 1; + const int n = + static_cast(param_.covariance.covariance_estimation.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; @@ -780,14 +745,15 @@ std::array NDTScanMatcher::estimate_covariance( geometry_msgs::msg::PoseArray multi_ndt_result_msg; geometry_msgs::msg::PoseArray multi_initial_pose_msg; multi_ndt_result_msg.header.stamp = sensor_ros_time; - multi_ndt_result_msg.header.frame_id = map_frame_; + multi_ndt_result_msg.header.frame_id = param_.frame.map_frame; multi_initial_pose_msg.header.stamp = sensor_ros_time; - multi_initial_pose_msg.header.frame_id = map_frame_; + multi_initial_pose_msg.header.frame_id = param_.frame.map_frame; multi_ndt_result_msg.poses.push_back(matrix4f_to_pose(ndt_result.pose)); multi_initial_pose_msg.poses.push_back(matrix4f_to_pose(initial_pose_matrix)); // multiple searches - for (const auto & pose_offset : initial_pose_offset_model_) { + for (const auto & pose_offset : + param_.covariance.covariance_estimation.initial_pose_offset_model) { const Eigen::Vector2d rotated_pose_offset_2d = rot * pose_offset; Eigen::Matrix4f sub_initial_pose_matrix(Eigen::Matrix4f::Identity()); @@ -816,7 +782,7 @@ std::array NDTScanMatcher::estimate_covariance( } pca_covariance /= (n - 1); // unbiased covariance - std::array ndt_covariance = output_pose_covariance_; + std::array ndt_covariance = param_.covariance.output_pose_covariance; ndt_covariance[0 + 6 * 0] += pca_covariance(0, 0); ndt_covariance[1 + 6 * 0] += pca_covariance(1, 0); ndt_covariance[0 + 6 * 1] += pca_covariance(0, 1); @@ -828,42 +794,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 +816,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; } @@ -883,17 +826,29 @@ void NDTScanMatcher::service_ndt_align( tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res) { // get TF from pose_frame to map_frame - auto tf_pose_to_map_ptr = std::make_shared(); - tf2_listener_module_->get_transform( - get_clock()->now(), map_frame_, req->pose_with_covariance.header.frame_id, tf_pose_to_map_ptr); + const std::string & target_frame = param_.frame.map_frame; + const std::string & source_frame = req->pose_with_covariance.header.frame_id; - // transform pose_frame to map_frame - const auto initial_pose_msg_in_map_frame = - transform(req->pose_with_covariance, *tf_pose_to_map_ptr); - if (use_dynamic_map_loading_) { - map_update_module_->update_map(initial_pose_msg_in_map_frame.pose.pose.position); + geometry_msgs::msg::TransformStamped transform_s2t; + try { + transform_s2t = tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + // Note: Up to AWSIMv1.1.0, there is a known bug where the GNSS frame_id is incorrectly set to + // "gnss_link" instead of "map". The ndt_align is designed to return identity when this issue + // occurs. However, in the future, converting to a non-existent frame_id should be prohibited. + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + RCLCPP_WARN( + this->get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); + transform_s2t.header.stamp = get_clock()->now(); + transform_s2t.header.frame_id = target_frame; + transform_s2t.child_frame_id = source_frame; + transform_s2t.transform = tf2::toMsg(tf2::Transform::getIdentity()); } + // transform pose_frame to map_frame + const auto initial_pose_msg_in_map_frame = transform(req->pose_with_covariance, transform_s2t); + map_update_module_->update_map(initial_pose_msg_in_map_frame.pose.pose.position); + // mutex Map std::lock_guard lock(ndt_ptr_mtx_); @@ -936,16 +891,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. @@ -955,12 +910,18 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( // the ego vehicle is aligned with the ground to some extent about roll and pitch. const std::vector is_loop_variable = {false, false, false, false, false, true}; TreeStructuredParzenEstimator tpe( - TreeStructuredParzenEstimator::Direction::MAXIMIZE, n_startup_trials_, is_loop_variable); + TreeStructuredParzenEstimator::Direction::MAXIMIZE, + param_.initial_pose_estimation.n_startup_trials, is_loop_variable); 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 = param_.initial_pose_estimation.particles_num / publish_num; + + for (int64_t i = 0; i < param_.initial_pose_estimation.particles_num; i++) { const TreeStructuredParzenEstimator::Input input = tpe.get_next_input(); geometry_msgs::msg::Pose initial_pose; @@ -986,10 +947,12 @@ 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(), param_.frame.map_frame, particle, i); + if ( + (i + 1) % publish_interval == 0 || (i + 1) == param_.initial_pose_estimation.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); @@ -1015,7 +978,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( auto sensor_points_in_map_ptr = std::make_shared>(); tier4_autoware_utils::transformPointCloud( *ndt_ptr_->getInputSource(), *sensor_points_in_map_ptr, ndt_result.pose); - publish_point_cloud(initial_pose_with_cov.header.stamp, map_frame_, sensor_points_in_map_ptr); + publish_point_cloud( + initial_pose_with_cov.header.stamp, param_.frame.map_frame, sensor_points_in_map_ptr); } auto best_particle_ptr = std::max_element( @@ -1024,7 +988,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( geometry_msgs::msg::PoseWithCovarianceStamped result_pose_with_cov_msg; result_pose_with_cov_msg.header.stamp = initial_pose_with_cov.header.stamp; - result_pose_with_cov_msg.header.frame_id = map_frame_; + result_pose_with_cov_msg.header.frame_id = param_.frame.map_frame; result_pose_with_cov_msg.pose.pose = best_particle_ptr->result_pose; output_pose_with_cov_to_log(get_logger(), "align_pose_output", result_pose_with_cov_msg); 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/particle.cpp b/localization/ndt_scan_matcher/src/particle.cpp new file mode 100644 index 0000000000000..b30703c2761e8 --- /dev/null +++ b/localization/ndt_scan_matcher/src/particle.cpp @@ -0,0 +1,63 @@ +// Copyright 2015-2019 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ndt_scan_matcher/particle.hpp" + +#include "localization_util/util_func.hpp" + +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) +{ + 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.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. + + marker.ns = "initial_pose_transform_probability_color_marker"; + marker.pose = particle.initial_pose; + marker.color = exchange_color_crc(particle.score / 4.5); + marker_array.markers.push_back(marker); + + marker.ns = "initial_pose_iteration_color_marker"; + marker.pose = particle.initial_pose; + marker.color = exchange_color_crc((1.0 * particle.iteration) / 30.0); + marker_array.markers.push_back(marker); + + marker.ns = "initial_pose_index_color_marker"; + marker.pose = particle.initial_pose; + marker.color = exchange_color_crc(static_cast(i) / 100.0); + marker_array.markers.push_back(marker); + + marker.ns = "result_pose_transform_probability_color_marker"; + marker.pose = particle.result_pose; + marker.color = exchange_color_crc(particle.score / 4.5); + marker_array.markers.push_back(marker); + + marker.ns = "result_pose_iteration_color_marker"; + marker.pose = particle.result_pose; + marker.color = exchange_color_crc((1.0 * particle.iteration) / 30.0); + marker_array.markers.push_back(marker); + + marker.ns = "result_pose_index_color_marker"; + marker.pose = particle.result_pose; + marker.color = exchange_color_crc(static_cast(i) / 100.0); + marker_array.markers.push_back(marker); +} diff --git a/localization/pose2twist/package.xml b/localization/pose2twist/package.xml index 83bce5a718334..16c49bb51c218 100644 --- a/localization/pose2twist/package.xml +++ b/localization/pose2twist/package.xml @@ -6,6 +6,11 @@ The pose2twist package Yamato Ando Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Yamato Ando diff --git a/localization/pose_estimator_arbiter/CMakeLists.txt b/localization/pose_estimator_arbiter/CMakeLists.txt new file mode 100644 index 0000000000000..9a47b654a6ab4 --- /dev/null +++ b/localization/pose_estimator_arbiter/CMakeLists.txt @@ -0,0 +1,58 @@ +cmake_minimum_required(VERSION 3.14) +project(pose_estimator_arbiter) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(glog REQUIRED) + +find_package(PCL REQUIRED COMPONENTS common) +include_directories(SYSTEM ${PCL_INCLUDE_DIRS}) + +# ============================== +# switch rule library +ament_auto_add_library(switch_rule + SHARED + src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp +) +target_include_directories(switch_rule PUBLIC src) + +# ============================== +# pose estimator arbiter node +ament_auto_add_executable(${PROJECT_NAME} + src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp + src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC src) +target_link_libraries(${PROJECT_NAME} switch_rule glog::glog) + +# ============================== +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + # define test definition macro + function(add_testcase filepath) + get_filename_component(filename ${filepath} NAME) + string(REGEX REPLACE ".cpp" "" test_name ${filename}) + ament_add_gmock(${test_name} ${filepath}) + target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src) + target_link_libraries(${test_name} fmt) + ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) + endfunction() + + add_testcase(test/test_shared_data.cpp) + add_ros_test( + test/test_pose_estimator_arbiter.py + TIMEOUT "30" + ) +endif() + +# ============================== +# In practice, the example rule is not used as autoware code. +# It exists only for user reference and is tested only. +add_subdirectory(example_rule) + +# ============================== +ament_auto_package(INSTALL_TO_SHARE + launch +) diff --git a/localization/pose_estimator_arbiter/README.md b/localization/pose_estimator_arbiter/README.md new file mode 100644 index 0000000000000..7e1ad2b9ec6eb --- /dev/null +++ b/localization/pose_estimator_arbiter/README.md @@ -0,0 +1,284 @@ +# pose_estimator_arbiter + +Table of contents: + +- [Abstract](#abstract) +- [Interface](#interfaces) +- [Architecture](#architecture) +- [How to launch](#how-to-launch) +- [Switching Rules](#switching-rules) +- [Pose Initialization](#pose-initialization) +- [Future Plans](#future-plans) + +## Abstract + +This package launches multiple pose estimators and provides the capability to stop or resume specific pose estimators based on the situation. +It provides provisional switching rules and will be adaptable to a wide variety of rules in the future. + +Please refer to [this discussion](https://github.com/orgs/autowarefoundation/discussions/3878) about other ideas on implementation. + +### Why do we need a stop/resume mechanism? + +It is possible to launch multiple pose_estimators and fuse them using a Kalman filter by editing launch files. +However, this approach is not preferable due to computational costs. + +Particularly, NDT and YabLoc are computationally intensive, and it's not recommended to run them simultaneously. +Also, even if both can be activated at the same time, the Kalman Filter may be affected by one of them giving bad output. + +> [!NOTE] +> Currently, **there is ONLY A RULE implemented that always enables all pose_estimators.** +> If users want to toggle pose_estimator with their own rules, they need to add new rules. by referring to example_rule. +> The [example_rule](example_rule/README.md) has source code that can be used as a reference for implementing the rules. + +### Supporting pose_estimators + +- [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) +- [eagleye](https://autowarefoundation.github.io/autoware-documentation/main/how-to-guides/integrating-autoware/launch-autoware/localization/eagleye/) +- [yabloc](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/yabloc) +- [landmark_based_localizer](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/landmark_based_localizer) + +### Demonstration + +The following video demonstrates the switching of four different pose estimators. + +
+ +Users can reproduce the demonstration using the following data and launch command: + +[sample data (rosbag & map)](https://drive.google.com/file/d/1MxLo1Sw6PdvfkyOYf_9A5dZ9uli1vPvS/view) +The rosbag is simulated data created by [AWSIM](https://tier4.github.io/AWSIM/). +The map is an edited version of the [original map data](https://github.com/tier4/AWSIM/releases/download/v1.1.0/nishishinjuku_autoware_map.zip) published on the AWSIM documentation page to make it suitable for multiple pose_estimators. + +```bash +ros2 launch autoware_launch logging_simulator.launch.xml \ + map_path:= \ + vehicle_model:=sample_vehicle \ + sensor_model:=awsim_sensor_kit \ + pose_source:=ndt_yabloc_artag_eagleye +``` + +## Interfaces + +
+Click to show details + +### Parameters + +There are no parameters. + +### Services + +| Name | Type | Description | +| ---------------- | ------------------------------- | ------------------------------- | +| `/config_logger` | logging_demo::srv::ConfigLogger | service to modify logging level | + +### Clients + +| Name | Type | Description | +| --------------------- | --------------------- | --------------------------------- | +| `/yabloc_suspend_srv` | std_srv::srv::SetBool | service to stop or restart yabloc | + +### Subscriptions + +For pose estimator arbitration: + +| Name | Type | Description | +| ------------------------------------- | --------------------------------------------- | -------------- | +| `/input/artag/image` | sensor_msgs::msg::Image | ArTag input | +| `/input/yabloc/image` | sensor_msgs::msg::Image | YabLoc input | +| `/input/eagleye/pose_with_covariance` | geometry_msgs::msg::PoseWithCovarianceStamped | Eagleye output | +| `/input/ndt/pointcloud` | sensor_msgs::msg::PointCloud2 | NDT input | + +For switching rule: + +| Name | Type | Description | +| ----------------------------- | ------------------------------------------------------------ | --------------------------------- | +| `/input/vector_map` | autoware_auto_mapping_msgs::msg::HADMapBin | vector map | +| `/input/pose_with_covariance` | geometry_msgs::msg::PoseWithCovarianceStamped | localization final output | +| `/input/initialization_state` | autoware_adapi_v1_msgs::msg::LocalizationInitializationState | localization initialization state | + +### Publications + +| Name | Type | Description | +| -------------------------------------- | --------------------------------------------- | ------------------------------------------------------ | +| `/output/artag/image` | sensor_msgs::msg::Image | relayed ArTag input | +| `/output/yabloc/image` | sensor_msgs::msg::Image | relayed YabLoc input | +| `/output/eagleye/pose_with_covariance` | geometry_msgs::msg::PoseWithCovarianceStamped | relayed Eagleye output | +| `/output/ndt/pointcloud` | sensor_msgs::msg::PointCloud2 | relayed NDT input | +| `/output/debug/marker_array` | visualization_msgs::msg::MarkerArray | [debug topic] everything for visualization | +| `/output/debug/string` | visualization_msgs::msg::MarkerArray | [debug topic] debug information such as current status | + +
+ +## Trouble Shooting + +If it does not seems to work, users can get more information in the following ways. + +> [!TIP] +> +> ```bash +> ros2 service call /localization/pose_estimator_arbiter/config_logger logging_demo/srv/ConfigLogger \ +> '{logger_name: localization.pose_estimator_arbiter, level: debug}' +> ``` + +## Architecture + +
+Click to show details + +### Case of running a single pose estimator + +When each pose_estimator is run alone, this package does nothing. +Following figure shows the node configuration when NDT, YabLoc Eagleye and AR-Tag are run independently. + +drawing + +### Case of running multiple pose estimators + +When running multiple pose_estimators, pose_estimator_arbiter is executed. +It comprises a **switching rule** and **stoppers** corresponding to each pose_estimator. + +- Stoppers control the pose_estimator activity by relaying inputs or outputs, or by requesting a suspend service. +- Switching rules determine which pose_estimator to use. + +Which stoppers and switching rules are instantiated depends on the runtime arguments at startup. + +Following figure shows the node configuration when all pose_estimator are run simultaneously. + +drawing + +- **NDT** + +The NDT stopper relays topics in the front side of the point cloud pre-processor. + +- **YabLoc** + +The YabLoc stopper relays input image topics in the frontend of the image pre-processor. +YabLoc includes a particle filter process that operates on a timer, and even when image topics are not streamed, the particle prediction process continues to work. +To address this, the YabLoc stopper also has a service client for explicitly stopping and resuming YabLoc. + +- **Eagleye** + +The Eagleye stopper relays Eagleye's output pose topics in the backend of Eagleye's estimation process. +Eagleye performs time-series processing internally, and it can't afford to stop the input stream. +Furthermore, Eagleye's estimation process is lightweight enough to be run continuously without a significant load, so the relay is inserted in the backend. + +- **ArTag** + +The ArTag stopper relays image topics in the front side of the landmark localizer. + +
+ +## How to launch + +
+Click to show details + +The user can launch the desired pose_estimators by giving the pose_estimator names as a concatenation of underscores for the runtime argument `pose_source`. + +```bash +ros2 launch autoware_launch logging_simulator.launch.xml \ + map_path:= \ + vehicle_model:=sample_vehicle \ + sensor_model:=awsim_sensor_kit \ + pose_source:=ndt_yabloc_artag_eagleye +``` + +Even if `pose_source` includes an unexpected string, it will be filtered appropriately. +Please see the table below for details. + +| given runtime argument | parsed pose_estimator_arbiter's param (pose_sources) | +| ------------------------------------------- | ---------------------------------------------------- | +| `pose_source:=ndt` | `["ndt"]` | +| `pose_source:=nan` | `[]` | +| `pose_source:=yabloc_ndt` | `["ndt","yabloc"]` | +| `pose_source:=yabloc_ndt_ndt_ndt` | `["ndt","yabloc"]` | +| `pose_source:=ndt_yabloc_eagleye` | `["ndt","yabloc","eagleye"]` | +| `pose_source:=ndt_yabloc_nan_eagleye_artag` | `["ndt","yabloc","eagleye","artag"]` | + +
+ +## Switching Rules + +
+Click to show details + +Currently, **ONLY ONE RULE** (`enable_all_rule`) is implemented. +In the future, several rules will be implemented and users will be able to select rules. + +> [!TIP] +> There are presets available to extend the rules. If you want to extend the rules, please see [example_rule](./example_rule/README.md). + +### Enable All Rule + +This is the default and simplest rule. This rule enables all pose_estimators regardless of their current state. + +```mermaid +flowchart LR + A{ } + A --whatever --> _A[enable all pose_estimators] +``` + +
+ +## Pose Initialization + +When using multiple pose_estimators, it is necessary to appropriately adjust the parameters provided to the `pose_initializer`. + +
+Click to show details + +The following table is based on the runtime argument "pose_source" indicating which initial pose estimation methods are available and the parameters that should be provided to the pose_initialization node. +To avoid making the application too complicated, a priority is established so that NDT is always used when it is available. +(The pose_initializer will only perform NDT-based initial pose estimation when `ndt_enabled` and `yabloc_enabled` are both `true`). + +This table's usage is described from three perspectives: + +- **Autoware Users:** Autoware users do not need to consult this table. + They simply provide the desired combinations of pose_estimators, and the appropriate parameters are automatically provided to the pose_initializer. +- **Autoware Developers:** Autoware developers can consult this table to know which parameters are assigned. +- **Who implements New Pose Estimator Switching:** + Developers must extend this table and implement the assignment of appropriate parameters to the pose_initializer. + +| pose_source | invoked initialization method | `ndt_enabled` | `yabloc_enabled` | `gnss_enabled` | `sub_gnss_pose_cov` | +| :-------------------------: | ----------------------------- | ------------- | ---------------- | -------------- | -------------------------------------------- | +| ndt | ndt | true | false | true | /sensing/gnss/pose_with_covariance | +| yabloc | yabloc | false | true | true | /sensing/gnss/pose_with_covariance | +| eagleye | vehicle needs run for a while | false | false | true | /localization/pose_estimator/eagleye/... | +| artag | 2D Pose Estimate (RViz) | false | false | true | /sensing/gnss/pose_with_covariance | +| ndt, yabloc | ndt | true | true | true | /sensing/gnss/pose_with_covariance | +| ndt, eagleye | ndt | true | false | true | /sensing/gnss/pose_with_covariance | +| ndt, artag | ndt | true | false | true | /sensing/gnss/pose_with_covariance | +| yabloc, eagleye | yabloc | false | true | true | /sensing/gnss/pose_with_covariance | +| yabloc, artag | yabloc | false | true | true | /sensing/gnss/pose_with_covariance | +| eagleye, artag | vehicle needs run for a while | false | false | true | /localization/pose_estimator/eagleye/pose... | +| ndt, yabloc, eagleye | ndt | true | true | true | /sensing/gnss/pose_with_covariance | +| ndt, eagleye, artag | ndt | true | false | true | /sensing/gnss/pose_with_covariance | +| yabloc, eagleye, artag | yabloc | false | true | true | /sensing/gnss/pose_with_covariance | +| ndt, yabloc, eagleye, artag | ndt | true | true | true | /sensing/gnss/pose_with_covariance | + +
+ +## Future Plans + +
+Click to show details + +### gradually switching + +In the future, this package will provide not only ON/OFF switching, but also a mechanism for low frequency operation, such as 50% NDT & 50% YabLoc. + +### stopper for pose_estimators to be added in the future + +The basic strategy is to realize ON/OFF switching by relaying the input or output topics of that pose_estimator. +If pose_estimator involves time-series processing with heavy computations, it's not possible to pause and resume with just topic relaying. + +In such cases, there may not be generally applicable solutions, but the following methods may help: + +1. Completely stop and **reinitialize** time-series processing, as seen in the case of YabLoc. +2. Subscribe to `localization/kinematic_state` and **keep updating states** to ensure that the estimation does not break (relying on the output of the active pose_estimator). +3. The multiple pose_estimator **does not support** that particular pose_estimator. + +Please note that this issue is fundamental to realizing multiple pose_estimators, and it will arise regardless of the architecture proposed in this case. + +
diff --git a/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt b/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt new file mode 100644 index 0000000000000..333f92842b860 --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt @@ -0,0 +1,31 @@ +# example switch rule library +ament_auto_add_library(example_rule + SHARED + src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp + src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp + src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp + src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp +) +target_include_directories(example_rule PUBLIC src example_rule/src) +target_link_libraries(example_rule switch_rule) + +# ============================== +# define test definition macro +function(add_testcase filepath) + get_filename_component(filename ${filepath} NAME) + string(REGEX REPLACE ".cpp" "" test_name ${filename}) + + ament_add_gtest(${test_name} ${filepath}) + target_link_libraries("${test_name}" switch_rule example_rule) + target_include_directories(${test_name} PUBLIC src) + ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) +endfunction() + +# ============================== +# build test +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + add_testcase(test/test_rule_helper.cpp) + add_testcase(test/test_vector_map_based_rule.cpp) + add_testcase(test/test_pcd_map_based_rule.cpp) +endif() diff --git a/localization/pose_estimator_arbiter/example_rule/README.md b/localization/pose_estimator_arbiter/example_rule/README.md new file mode 100644 index 0000000000000..c5bb80912607f --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/README.md @@ -0,0 +1,99 @@ +# example rule + +The example rule provides a sample rule for controlling the arbiter. By combining the provided rules, it is possible to achieve demonstrations as follows. Users can extend the rules as needed by referencing this code, allowing them to control the arbiter as desired. + +## Demonstration + +The following video demonstrates the switching of four different pose estimators. + +
+ +## Switching Rules + +### Pcd Map Based Rule + +```mermaid +flowchart LR + A{PCD is enough dense } + A --true--> B[enable NDT] + A --false--> C[enable YabLoc] +``` + +### Vector Map Based Rule + +```mermaid +flowchart LR + A{ } + A --whatever --> _A[When the ego vehicle is in a predetermined pose_estimator_area,\n it enables the corresponding pose_estamtor.] +``` + +### Rule helpers + +Rule helpers are auxiliary tools for describing switching rules. + +- [PCD occupancy](#pcd-occupancy) +- [Pose estimator area](#pose-estimator-area) + +#### PCD occupancy + +drawing + +#### Pose estimator area + +The pose_estimator_area is a planar area described by polygon in lanelet2. +The height of the area is meaningless; it judges if the projection of its self-position is contained within the polygon or not. + +drawing + +A sample pose_estimator_area is shown below. The values provided below are placeholders. +To be correctly read, the area should have the type "pose_estimator_specify" and the subtype should be one of ndt, yabloc, eagleye, or artag. + +```xml + + + + + + + + + + + + + + + + + + + + + + + + + +... + + + + + + + + + + + + + + + + + + + + + +``` diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/grid_key.hpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/grid_key.hpp new file mode 100644 index 0000000000000..c366a7f02f4fe --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/grid_key.hpp @@ -0,0 +1,68 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__RULE_HELPER__GRID_KEY_HPP_ +#define POSE_ESTIMATOR_ARBITER__RULE_HELPER__GRID_KEY_HPP_ +#include + +#include + +#include + +namespace pose_estimator_arbiter::rule_helper +{ +struct GridKey +{ + const float unit_length = 10.f; + int x, y; + + GridKey() : x(0), y(0) {} + GridKey(float x, float y) : x(std::floor(x / unit_length)), y(std::floor(y / unit_length)) {} + + pcl::PointXYZ get_center_point() const + { + pcl::PointXYZ xyz; + xyz.x = unit_length * (static_cast(x) + 0.5f); + xyz.y = unit_length * (static_cast(y) + 0.5f); + xyz.z = 0.f; + return xyz; + } + + friend bool operator==(const GridKey & one, const GridKey & other) + { + return one.x == other.x && one.y == other.y; + } + friend bool operator!=(const GridKey & one, const GridKey & other) { return !(one == other); } +}; + +} // namespace pose_estimator_arbiter::rule_helper + +// This is for unordered_map and unordered_set +namespace std +{ +template <> +struct hash +{ +public: + size_t operator()(const pose_estimator_arbiter::rule_helper::GridKey & grid) const + { + std::size_t seed = 0; + boost::hash_combine(seed, grid.x); + boost::hash_combine(seed, grid.y); + return seed; + } +}; +} // namespace std + +#endif // POSE_ESTIMATOR_ARBITER__RULE_HELPER__GRID_KEY_HPP_ diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp new file mode 100644 index 0000000000000..72071b23b467f --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp @@ -0,0 +1,112 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp" + +#include "pose_estimator_arbiter/rule_helper/grid_key.hpp" + +#include + +#include + +namespace pose_estimator_arbiter::rule_helper +{ +PcdOccupancy::PcdOccupancy(int pcd_density_upper_threshold, int pcd_density_lower_threshold) +: pcd_density_upper_threshold_(pcd_density_upper_threshold), + pcd_density_lower_threshold_(pcd_density_lower_threshold) +{ +} + +bool PcdOccupancy::ndt_can_operate( + const geometry_msgs::msg::Point & position, std::string * optional_message) const +{ + if (!kdtree_) { + if (optional_message) { + *optional_message = "pcd is not subscribed yet"; + } + return false; + } + + const pcl::PointXYZ query{ + static_cast(position.x), static_cast(position.y), static_cast(position.z)}; + std::vector indices; + std::vector distances; + const int count = kdtree_->radiusSearch(query, 50, indices, distances, 0); + + static bool last_is_ndt_mode = true; + const bool is_ndt_mode = (last_is_ndt_mode) ? (count > pcd_density_lower_threshold_) + : (count > pcd_density_upper_threshold_); + last_is_ndt_mode = is_ndt_mode; + + std::stringstream ss; + ss << "pcd occupancy: " << count << " > " + << (last_is_ndt_mode ? pcd_density_lower_threshold_ : pcd_density_upper_threshold_); + + if (optional_message) { + *optional_message = ss.str(); + } + + return is_ndt_mode; +} + +visualization_msgs::msg::MarkerArray PcdOccupancy::debug_marker_array() const +{ + visualization_msgs::msg::Marker msg; + msg.ns = "pcd_occupancy"; + msg.id = 0; + msg.header.frame_id = "map"; + msg.scale.set__x(3.0f).set__y(3.0f).set__z(3.f); + msg.color.set__r(1.0f).set__g(1.0f).set__b(0.2f).set__a(1.0f); + + if (occupied_areas_) { + for (auto p : occupied_areas_->points) { + geometry_msgs::msg::Point geometry_point{}; + geometry_point.set__x(p.x).set__y(p.y).set__z(p.z); + msg.points.push_back(geometry_point); + } + } + + visualization_msgs::msg::MarkerArray msg_array; + msg_array.markers.push_back(msg); + + return msg_array; +} + +void PcdOccupancy::init(PointCloud2::ConstSharedPtr msg) +{ + if (kdtree_) { + // already initialized + return; + } + + pcl::PointCloud cloud; + pcl::fromROSMsg(*msg, cloud); + + std::unordered_map grid_point_count; + for (pcl::PointXYZ xyz : cloud) { + grid_point_count[GridKey(xyz.x, xyz.y)] += 1; + } + + occupied_areas_ = std::make_shared>(); + for (const auto [grid, count] : grid_point_count) { + if (count > 50) { + occupied_areas_->push_back(grid.get_center_point()); + } + } + + kdtree_ = pcl::make_shared>(); + kdtree_->setInputCloud(occupied_areas_); +} + +} // namespace pose_estimator_arbiter::rule_helper diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp new file mode 100644 index 0000000000000..e5eb4ff091a31 --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp @@ -0,0 +1,52 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__RULE_HELPER__PCD_OCCUPANCY_HPP_ +#define POSE_ESTIMATOR_ARBITER__RULE_HELPER__PCD_OCCUPANCY_HPP_ + +#include + +#include +#include + +#include +#include +#include + +namespace pose_estimator_arbiter::rule_helper +{ +class PcdOccupancy +{ + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using MarkerArray = visualization_msgs::msg::MarkerArray; + +public: + explicit PcdOccupancy(int pcd_density_upper_threshold, int pcd_density_lower_threshold); + + MarkerArray debug_marker_array() const; + void init(PointCloud2::ConstSharedPtr msg); + bool ndt_can_operate( + const geometry_msgs::msg::Point & position, std::string * optional_message = nullptr) const; + +private: + const int pcd_density_upper_threshold_; + const int pcd_density_lower_threshold_; + + pcl::PointCloud::Ptr occupied_areas_{nullptr}; + pcl::KdTreeFLANN::Ptr kdtree_{nullptr}; +}; + +} // namespace pose_estimator_arbiter::rule_helper + +#endif // POSE_ESTIMATOR_ARBITER__RULE_HELPER__PCD_OCCUPANCY_HPP_ diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp new file mode 100644 index 0000000000000..4eea34f9ae1ee --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp @@ -0,0 +1,144 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp" + +#include +#include +#include + +#include + +#include + +#include +#include +#include + +namespace pose_estimator_arbiter::rule_helper +{ +using BoostPoint = boost::geometry::model::d2::point_xy; +using BoostPolygon = boost::geometry::model::polygon; + +struct PoseEstimatorArea::Impl +{ + explicit Impl(rclcpp::Logger logger) : logger_(logger) {} + std::multimap bounding_boxes_; + + void init(HADMapBin::ConstSharedPtr msg); + bool within( + const geometry_msgs::msg::Point & point, const std::string & pose_estimator_name) const; + + MarkerArray debug_marker_array() const { return marker_array_; } + +private: + rclcpp::Logger logger_; + MarkerArray marker_array_; +}; + +PoseEstimatorArea::PoseEstimatorArea(const rclcpp::Logger & logger) : logger_(logger) +{ + impl_ = std::make_shared(logger_); +} + +PoseEstimatorArea::PoseEstimatorArea(rclcpp::Node * node) : PoseEstimatorArea(node->get_logger()) +{ +} + +void PoseEstimatorArea::init(HADMapBin::ConstSharedPtr msg) +{ + impl_->init(msg); +} + +bool PoseEstimatorArea::within( + const geometry_msgs::msg::Point & point, const std::string & pose_estimator_name) const +{ + return impl_->within(point, pose_estimator_name); +} + +PoseEstimatorArea::MarkerArray PoseEstimatorArea::debug_marker_array() const +{ + return impl_->debug_marker_array(); +} + +void PoseEstimatorArea::Impl::init(HADMapBin::ConstSharedPtr msg) +{ + if (!bounding_boxes_.empty()) { + // already initialized + return; + } + + lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap); + lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map); + + const auto & polygon_layer = lanelet_map->polygonLayer; + RCLCPP_DEBUG_STREAM(logger_, "Polygon layer size: " << polygon_layer.size()); + + const std::string pose_estimator_specifying_attribute = "pose_estimator_specify"; + for (const auto & polygon : polygon_layer) { + // + const std::string type{polygon.attributeOr(lanelet::AttributeName::Type, "none")}; + RCLCPP_DEBUG_STREAM(logger_, "polygon type: " << type); + if (pose_estimator_specifying_attribute != type) { + continue; + } + + // + const std::string subtype{polygon.attributeOr(lanelet::AttributeName::Subtype, "none")}; + RCLCPP_DEBUG_STREAM(logger_, "polygon sub type: " << subtype); + + // Create a marker for visualization + Marker marker; + marker.type = Marker::LINE_STRIP; + marker.scale.set__x(0.2f).set__y(0.2f).set__z(0.2f); + marker.color.set__r(1.0f).set__g(1.0f).set__b(0.0f).set__a(1.0f); + marker.ns = subtype; + marker.header.frame_id = "map"; + marker.id = marker_array_.markers.size(); + + // Enqueue the vertices of the polygon to bounding box and visualization marker + BoostPolygon poly; + for (const lanelet::ConstPoint3d & p : polygon) { + poly.outer().push_back(BoostPoint(p.x(), p.y())); + + geometry_msgs::msg::Point point_msg; + point_msg.set__x(p.x()).set__y(p.y()).set__z(p.z()); + marker.points.push_back(point_msg); + } + + // Push the first vertex again to enclose the polygon + poly.outer().push_back(poly.outer().front()); + marker.points.push_back(marker.points.front()); + + // Push back the items + bounding_boxes_.emplace(subtype, poly); + marker_array_.markers.push_back(marker); + } +} + +bool PoseEstimatorArea::Impl::within( + const geometry_msgs::msg::Point & point, const std::string & pose_estimator_name) const +{ + const BoostPoint boost_point(point.x, point.y); + + auto range = bounding_boxes_.equal_range(pose_estimator_name); + + for (auto itr = range.first; itr != range.second; ++itr) { + if (!boost::geometry::disjoint(itr->second, boost_point)) { + return true; + } + } + return false; +} +} // namespace pose_estimator_arbiter::rule_helper diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp new file mode 100644 index 0000000000000..d6901f9be2dbf --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp @@ -0,0 +1,56 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_ +#define POSE_ESTIMATOR_ARBITER__RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_ + +#include +#include + +#include +#include +#include + +#include +#include + +namespace pose_estimator_arbiter::rule_helper +{ +class PoseEstimatorArea +{ + using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using Marker = visualization_msgs::msg::Marker; + using MarkerArray = visualization_msgs::msg::MarkerArray; + +public: + explicit PoseEstimatorArea(rclcpp::Node * node); + explicit PoseEstimatorArea(const rclcpp::Logger & logger); + + // This is assumed to be called only once in the vector map callback. + void init(const HADMapBin::ConstSharedPtr msg); + + bool within( + const geometry_msgs::msg::Point & point, const std::string & pose_estimator_name) const; + + MarkerArray debug_marker_array() const; + +private: + struct Impl; + std::shared_ptr impl_{nullptr}; + rclcpp::Logger logger_; +}; + +} // namespace pose_estimator_arbiter::rule_helper + +#endif // POSE_ESTIMATOR_ARBITER__RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_ diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp new file mode 100644 index 0000000000000..4c3829316230b --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp @@ -0,0 +1,76 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp" + +#include + +namespace pose_estimator_arbiter::switch_rule +{ +PcdMapBasedRule::PcdMapBasedRule( + rclcpp::Node & node, const std::unordered_set & running_estimator_list, + const std::shared_ptr shared_data) +: BaseSwitchRule(node), running_estimator_list_(running_estimator_list), shared_data_(shared_data) +{ + const int pcd_density_upper_threshold = + tier4_autoware_utils::getOrDeclareParameter(node, "pcd_density_upper_threshold"); + const int pcd_density_lower_threshold = + tier4_autoware_utils::getOrDeclareParameter(node, "pcd_density_lower_threshold"); + + pcd_occupancy_ = std::make_unique( + pcd_density_upper_threshold, pcd_density_lower_threshold); + + // Register callback + shared_data_->point_cloud_map.register_callback( + [this](sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) -> void { + pcd_occupancy_->init(msg); + }); +} + +PcdMapBasedRule::MarkerArray PcdMapBasedRule::debug_marker_array() +{ + MarkerArray array_msg; + + if (pcd_occupancy_) { + const auto & additional = pcd_occupancy_->debug_marker_array().markers; + array_msg.markers.insert(array_msg.markers.end(), additional.begin(), additional.end()); + } + + return array_msg; +} + +std::unordered_map PcdMapBasedRule::update() +{ + const auto position = shared_data_->localization_pose_cov()->pose.pose.position; + const bool ndt_can_operate = pcd_occupancy_->ndt_can_operate(position); + + if (ndt_can_operate) { + debug_string_ = "Enable ndt: "; + return { + {PoseEstimatorType::ndt, true}, + {PoseEstimatorType::yabloc, false}, + {PoseEstimatorType::eagleye, false}, + {PoseEstimatorType::artag, false}, + }; + } else { + debug_string_ = "Enable yabloc: "; + return { + {PoseEstimatorType::ndt, false}, + {PoseEstimatorType::yabloc, true}, + {PoseEstimatorType::eagleye, false}, + {PoseEstimatorType::artag, false}}; + } +} + +} // namespace pose_estimator_arbiter::switch_rule diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp new file mode 100644 index 0000000000000..23fd0f812f700 --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_ +#define POSE_ESTIMATOR_ARBITER__SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_ + +#include "pose_estimator_arbiter/pose_estimator_type.hpp" +#include "pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp" +#include "pose_estimator_arbiter/shared_data.hpp" +#include "pose_estimator_arbiter/switch_rule/base_switch_rule.hpp" + +#include +#include +#include +#include + +namespace pose_estimator_arbiter::switch_rule +{ +class PcdMapBasedRule : public BaseSwitchRule +{ +public: + PcdMapBasedRule( + rclcpp::Node & node, const std::unordered_set & running_estimator_list, + const std::shared_ptr shared_data); + + std::unordered_map update() override; + + std::string debug_string() override { return debug_string_; } + + MarkerArray debug_marker_array() override; + +protected: + const std::unordered_set running_estimator_list_; + std::shared_ptr shared_data_{nullptr}; + + std::unique_ptr pcd_occupancy_{nullptr}; + + // Store the reason why which pose estimator is enabled + mutable std::string debug_string_; +}; +} // namespace pose_estimator_arbiter::switch_rule + +#endif // POSE_ESTIMATOR_ARBITER__SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_ diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp new file mode 100644 index 0000000000000..094434c62ac9c --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp @@ -0,0 +1,75 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp" + +#include + +namespace pose_estimator_arbiter::switch_rule +{ +VectorMapBasedRule::VectorMapBasedRule( + rclcpp::Node & node, const std::unordered_set & running_estimator_list, + const std::shared_ptr shared_data) +: BaseSwitchRule(node), running_estimator_list_(running_estimator_list), shared_data_(shared_data) +{ + pose_estimator_area_ = std::make_unique(node.get_logger()); + + // Register callback + shared_data_->vector_map.register_callback( + [this](autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) -> void { + pose_estimator_area_->init(msg); + }); + + RCLCPP_INFO_STREAM(get_logger(), "VectorMapBasedRule is initialized successfully"); +} + +VectorMapBasedRule::MarkerArray VectorMapBasedRule::debug_marker_array() +{ + MarkerArray array_msg; + + if (pose_estimator_area_) { + const auto & additional = pose_estimator_area_->debug_marker_array().markers; + array_msg.markers.insert(array_msg.markers.end(), additional.begin(), additional.end()); + } + + return array_msg; +} + +std::unordered_map VectorMapBasedRule::update() +{ + const auto ego_position = shared_data_->localization_pose_cov()->pose.pose.position; + + std::unordered_map enable_list; + bool at_least_one_is_enabled = false; + for (const auto & estimator_type : running_estimator_list_) { + const std::string estimator_name{magic_enum::enum_name(estimator_type)}; + const bool result = pose_estimator_area_->within(ego_position, estimator_name); + enable_list.emplace(estimator_type, result); + + at_least_one_is_enabled |= result; + } + if (at_least_one_is_enabled) { + debug_string_ = + "Enable at least one pose_estimators: self vehicle is within the area of at least one " + "pose_estimator_area"; + } else { + debug_string_ = + "Enable no pose_estimator: self vehicle is out of the area of all pose_estimator_area"; + } + RCLCPP_DEBUG(get_logger(), "%s", debug_string_.c_str()); + + return enable_list; +} + +} // namespace pose_estimator_arbiter::switch_rule diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp new file mode 100644 index 0000000000000..e3360f9692f86 --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp @@ -0,0 +1,55 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__SWITCH_RULE__VECTOR_MAP_BASED_RULE_HPP_ +#define POSE_ESTIMATOR_ARBITER__SWITCH_RULE__VECTOR_MAP_BASED_RULE_HPP_ + +#include "pose_estimator_arbiter/pose_estimator_type.hpp" +#include "pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp" +#include "pose_estimator_arbiter/shared_data.hpp" +#include "pose_estimator_arbiter/switch_rule/base_switch_rule.hpp" + +#include +#include +#include +#include +#include + +namespace pose_estimator_arbiter::switch_rule +{ +class VectorMapBasedRule : public BaseSwitchRule +{ +public: + VectorMapBasedRule( + rclcpp::Node & node, const std::unordered_set & running_estimator_list, + const std::shared_ptr shared_data); + + std::unordered_map update() override; + + std::string debug_string() override { return debug_string_; } + + MarkerArray debug_marker_array() override; + +protected: + const std::unordered_set running_estimator_list_; + std::shared_ptr shared_data_{nullptr}; + + // Store the reason why which pose estimator is enabled + mutable std::string debug_string_; + + std::unique_ptr pose_estimator_area_{nullptr}; +}; +} // namespace pose_estimator_arbiter::switch_rule + +#endif // POSE_ESTIMATOR_ARBITER__SWITCH_RULE__VECTOR_MAP_BASED_RULE_HPP_ diff --git a/localization/pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp b/localization/pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp new file mode 100644 index 0000000000000..5febfa403363e --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp @@ -0,0 +1,100 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp" + +#include +#include + +#include + +class MockNode : public ::testing::Test +{ +protected: + virtual void SetUp() + { + rclcpp::init(0, nullptr); + node = std::make_shared("test_node"); + node->declare_parameter("pcd_density_lower_threshold", 1); + node->declare_parameter("pcd_density_upper_threshold", 5); + + const auto running_estimator_list = + std::unordered_set{ + pose_estimator_arbiter::PoseEstimatorType::ndt, + pose_estimator_arbiter::PoseEstimatorType::yabloc, + pose_estimator_arbiter::PoseEstimatorType::eagleye, + pose_estimator_arbiter::PoseEstimatorType::artag}; + + shared_data_ = std::make_shared(); + + rule_ = std::make_shared( + *node, running_estimator_list, shared_data_); + } + + std::shared_ptr node; + std::shared_ptr shared_data_; + std::shared_ptr rule_; + + virtual void TearDown() { rclcpp::shutdown(); } +}; + +TEST_F(MockNode, pcdMapBasedRule) +{ + // Create dummy pcd and set + { + pcl::PointCloud cloud; + for (float x = -10; x < 10.0; x += 0.2) { + for (float y = -10; y < 10.0; y += 0.2) { + cloud.push_back(pcl::PointXYZ(x, y, 0)); + } + } + + using PointCloud2 = sensor_msgs::msg::PointCloud2; + PointCloud2 msg; + pcl::toROSMsg(cloud, msg); + + // Set + shared_data_->point_cloud_map.set_and_invoke(std::make_shared(msg)); + } + + using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + auto create_pose = [](double x, double y) -> PoseCovStamped { + PoseCovStamped msg; + msg.pose.pose.position.x = x; + msg.pose.pose.position.y = y; + return msg; + }; + + { + auto position = create_pose(5, 5); + shared_data_->localization_pose_cov.set_and_invoke( + std::make_shared(position)); + auto ret = rule_->update(); + EXPECT_TRUE(ret.at(pose_estimator_arbiter::PoseEstimatorType::ndt)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::yabloc)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::eagleye)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::artag)); + } + + { + auto position = create_pose(100, 100); + shared_data_->localization_pose_cov.set_and_invoke( + std::make_shared(position)); + auto ret = rule_->update(); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::ndt)); + EXPECT_TRUE(ret.at(pose_estimator_arbiter::PoseEstimatorType::yabloc)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::eagleye)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::artag)); + } +} diff --git a/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp b/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp new file mode 100644 index 0000000000000..35ed8b04bfcad --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp @@ -0,0 +1,106 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_estimator_arbiter/rule_helper/grid_key.hpp" +#include "pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp" +#include "pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp" + +#include + +#include + +#include + +#include +#include +#include + +#include + +class MockNode : public ::testing::Test +{ +protected: + virtual void SetUp() + { + rclcpp::init(0, nullptr); + node = std::make_shared("test_node"); + } + + std::shared_ptr node{nullptr}; + + virtual void TearDown() { rclcpp::shutdown(); } +}; + +TEST_F(MockNode, poseEstimatorArea) +{ + auto create_polygon3d = []() -> lanelet::Polygon3d { + lanelet::Polygon3d polygon; + polygon.setAttribute(lanelet::AttributeName::Type, {"pose_estimator_specify"}); + polygon.setAttribute(lanelet::AttributeName::Subtype, {"ndt"}); + lanelet::Id index = 0; + polygon.push_back(lanelet::Point3d(index++, 0, 0)); + polygon.push_back(lanelet::Point3d(index++, 10, 0)); + polygon.push_back(lanelet::Point3d(index++, 10, 10)); + polygon.push_back(lanelet::Point3d(index++, 0, 10)); + return polygon; + }; + + lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap); + lanelet_map->add(create_polygon3d()); + + using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using Point = geometry_msgs::msg::Point; + HADMapBin msg; + lanelet::utils::conversion::toBinMsg(lanelet_map, &msg); + + pose_estimator_arbiter::rule_helper::PoseEstimatorArea pose_estimator_area(&(*node)); + pose_estimator_area.init(std::make_shared(msg)); + + EXPECT_TRUE(pose_estimator_area.within(Point().set__x(5).set__y(5).set__z(0), "ndt")); + EXPECT_FALSE(pose_estimator_area.within(Point().set__x(5).set__y(5).set__z(0), "yabloc")); + EXPECT_FALSE(pose_estimator_area.within(Point().set__x(-5).set__y(-5).set__z(0), "ndt")); + EXPECT_FALSE(pose_estimator_area.within(Point().set__x(-5).set__y(-5).set__z(0), "yabloc")); +} + +TEST_F(MockNode, pcdOccupancy) +{ + using pose_estimator_arbiter::rule_helper::PcdOccupancy; + const int pcd_density_upper_threshold = 20; + const int pcd_density_lower_threshold = 10; + + pose_estimator_arbiter::rule_helper::PcdOccupancy pcd_occupancy( + pcd_density_upper_threshold, pcd_density_lower_threshold); + + geometry_msgs::msg::Point point; + std::string message; + + // Since we have not yet given a point cloud, this returns false. + EXPECT_FALSE(pcd_occupancy.ndt_can_operate(point, &message)); +} + +TEST_F(MockNode, gridKey) +{ + using pose_estimator_arbiter::rule_helper::GridKey; + EXPECT_TRUE(GridKey(10., -5.) == GridKey(10., -10.)); + EXPECT_TRUE(GridKey(10., -5.) != GridKey(10., -15.)); + + EXPECT_TRUE(GridKey(10., -5.).get_center_point().x == 15.f); + EXPECT_TRUE(GridKey(10., -5.).get_center_point().y == -5.f); + EXPECT_TRUE(GridKey(10., -5.).get_center_point().z == 0.f); + + std::unordered_set set; + set.emplace(10., -5.); + EXPECT_EQ(set.count(GridKey(10., -5.)), 1ul); + EXPECT_EQ(set.count(GridKey(10., -15.)), 0ul); +} diff --git a/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp b/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp new file mode 100644 index 0000000000000..a0a983e2ad3b7 --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp @@ -0,0 +1,106 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp" + +#include + +#include + +#include +#include +#include + +#include + +class MockNode : public ::testing::Test +{ +protected: + virtual void SetUp() + { + rclcpp::init(0, nullptr); + node = std::make_shared("test_node"); + + const auto running_estimator_list = + std::unordered_set{ + pose_estimator_arbiter::PoseEstimatorType::ndt, + pose_estimator_arbiter::PoseEstimatorType::yabloc, + pose_estimator_arbiter::PoseEstimatorType::eagleye, + pose_estimator_arbiter::PoseEstimatorType::artag}; + + shared_data_ = std::make_shared(); + + rule_ = std::make_shared( + *node, running_estimator_list, shared_data_); + } + + std::shared_ptr node; + std::shared_ptr shared_data_; + std::shared_ptr rule_; + + virtual void TearDown() { rclcpp::shutdown(); } +}; + +TEST_F(MockNode, vectorMapBasedRule) +{ + // Create dummy lanelet2 and set + { + auto create_polygon3d = []() -> lanelet::Polygon3d { + lanelet::Polygon3d polygon; + polygon.setAttribute(lanelet::AttributeName::Type, {"pose_estimator_specify"}); + polygon.setAttribute(lanelet::AttributeName::Subtype, {"ndt"}); + lanelet::Id index = 0; + polygon.push_back(lanelet::Point3d(index++, 0, 0)); + polygon.push_back(lanelet::Point3d(index++, 10, 0)); + polygon.push_back(lanelet::Point3d(index++, 10, 10)); + polygon.push_back(lanelet::Point3d(index++, 0, 10)); + return polygon; + }; + + lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap); + lanelet_map->add(create_polygon3d()); + + using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + HADMapBin msg; + lanelet::utils::conversion::toBinMsg(lanelet_map, &msg); + + // Set + shared_data_->vector_map.set_and_invoke(std::make_shared(msg)); + } + + using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + auto create_pose = [](double x, double y) -> PoseCovStamped::ConstSharedPtr { + PoseCovStamped msg; + msg.pose.pose.position.x = x; + msg.pose.pose.position.y = y; + return std::make_shared(msg); + }; + + { + shared_data_->localization_pose_cov.set_and_invoke(create_pose(5, 5)); + auto ret = rule_->update(); + EXPECT_TRUE(ret.at(pose_estimator_arbiter::PoseEstimatorType::ndt)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::yabloc)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::eagleye)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::artag)); + } + { + shared_data_->localization_pose_cov.set_and_invoke(create_pose(15, 15)); + auto ret = rule_->update(); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::ndt)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::yabloc)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::eagleye)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::artag)); + } +} diff --git a/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml b/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml new file mode 100644 index 0000000000000..0a708e3f48988 --- /dev/null +++ b/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/localization/pose_estimator_arbiter/media/architecture.drawio.svg b/localization/pose_estimator_arbiter/media/architecture.drawio.svg new file mode 100644 index 0000000000000..4a7a7fe355775 --- /dev/null +++ b/localization/pose_estimator_arbiter/media/architecture.drawio.svg @@ -0,0 +1,957 @@ + + + + + + + +
+
+
+ legend +
+
+
+ + legend + + + + + + +
+
+
+ pose_estimator_arbiter +
+
+
+ + pose_estimator_arbiter + + + + + + + +
+
+
+ /pose_estimator +
+ /pose_with_covariance +
+
+
+ + /pose_estimator... + + + + + + + +
+
+
+ /util/downsampled/pointcloud +
+
+
+ + /util/downsampled/pointcloud + + + + + + +
+
+
+ pointcluod_ +
+ preprocessor +
+
+
+ + pointcluod_... + + + + + + + +
+
+
+ /pose_estimator/yabloc/* +
+
+
+ + /pose_estimator/yabloc/* + + + + + + +
+
+
+ image_ +
+ preprocessor +
+
+
+ + image_... + + + + + + + +
+
+
+ /pose_estimator +
+ /pose_with_covariance +
+
+
+ + /pose_estimator... + + + + + + +
+
+
+ ndt_scan_matcher +
+
+
+ + ndt_scan_matcher + + + + + + + +
+
+
+ /pose_estimator +
+ /pose_with_covariance +
+
+
+ + /pose_estimator... + + + + + + +
+
+
+ yabloc_particle_filter +
+
+
+ + yabloc_particle_filt... + + + + + + + +
+
+
+ /pose_with_covariance +
+
+
+ + /pose_with_covariance + + + + + + + +
+
+
+ /twist_estimator +
+ /twist_with_covariance +
+
+
+ + /twist_estimator... + + + + + + +
+
+
+ twist_ +
+ estimator +
+
+
+ + twist_... + + + + + + + +
+
+
+ /sensing/camera +
+ /traffic_light/image_raw/relay +
+
+
+ + /sensing/camera... + + + + + + + + +
+
+
+ /sensing/lidar/top +
+ /outlier_filtered/pointcloud +
+
+
+ + /sensing/lidar/top... + + + + + + + +
+
+
+ /pose_estimator/eagleye +
+ /pose_with_covariance +
+ /to_relay +
+
+
+ + /pose_estimator/eagleye... + + + + + + +
+
+
+ eagleye +
+
+
+ + eagleye + + + + + + + +
+
+
+ /map/vector_map, +
+ /map/pointcloud_map, +
+ /initialization_state, +
+ /tf +
+
+
+ + /map/vector_map,... + + + + + + + +
+
+
+ /sensing/lidar/top +
+ /outlier_filtered/pointcloud/relay +
+
+
+ + /sensing/lidar/top... + + + + + + + + +
+
+
+ /sensing/camera +
+ /traffic_light/image_raw +
+
+
+ + /sensing/camera... + + + + + + +
+
+
+ EKF +
+ (pose_twist_fusion_filter) +
+
+
+ + EKF... + + + + + + + + +
+
+
+ /sensing/* +
+
+
+ + /sensing/* + + + + + + +
+
+
+ eagleye_stopper +
+
+
+ + eagleye_stopper + + + + + + + +
+
+
+ yabloc_suspend_srv +
+
+
+ + yabloc_suspend_srv + + + + + + +
+
+
+ yabloc_stopper +
+
+
+ + yabloc_stopper + + + + + + +
+
+
+ ndt_stopper +
+
+
+ + ndt_stopper + + + + + + +
+
+
+ switching rule +
+
+
+ + switching rule + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ module +
+
+
+ + module + + + + + + +
+
+
+ pointcluod_ +
+ preprocessor +
+
+
+ + pointcluod_... + + + + + + +
+
+
+ pointcluod_ +
+ preprocessor +
+
+
+ + pointcluod_... + + + + + + +
+
+
+ image_ +
+ preprocessor +
+
+
+ + image_... + + + + + + +
+
+
+ image_ +
+ preprocessor +
+
+
+ + image_... + + + + + + +
+
+
+ eagleye +
+
+
+ + eagleye + + + + + + +
+
+
+ eagleye +
+
+
+ + eagleye + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ some nodes +
+
+
+ + some nodes + + + + + + +
+
+
+ NDT & YabLoc & Eagleye & AR-Tag +
+
+
+ + + NDT & YabLoc & Eagleye & AR-Tag + + + + + + + + +
+
+
+ base +
+ stopper +
+
+
+ + base... + + + + + + +
+
+
+ stopper +
+
+
+ + stopper + + + + + + + +
+
+
+ Inheritance +
+
+
+ + Inheritance + + + + + +
+
+
+ It has ON/OFF inteface +
+
+
+ + It has ON/OFF inteface + + + + + +
+
+
+ It has pose_estimator- +
+ specific features +
+
+
+ + It has pose_estimator-... + + + + + + +
+
+
+ artag_stopper +
+
+
+ + artag_stopper + + + + + + +
+
+
+ artag +
+
+
+ + artag + + + + + + + +
+
+
+ /sensing/camera +
+ /traffic_light/image_raw/relay +
+
+
+ + /sensing/camera... + + + + + + + +
+
+
+ /pose_with_covariance +
+
+
+ + /pose_with_covariance + + + + + + + + +
+
+
+ /sensing/camera +
+ /traffic_light/image_raw +
+
+
+ + /sensing/camera... + + + + + + + Text is not SVG - cannot display + + + diff --git a/localization/pose_estimator_arbiter/media/pcd_occupancy.drawio.svg b/localization/pose_estimator_arbiter/media/pcd_occupancy.drawio.svg new file mode 100644 index 0000000000000..6a20728e27fd9 --- /dev/null +++ b/localization/pose_estimator_arbiter/media/pcd_occupancy.drawio.svg @@ -0,0 +1,878 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + + + + + + + + + + + + + + +
+
+
+ 3 +
+
+
+ + 3 + + + + + + + +
+
+
+ 4 +
+
+
+ + 4 + + + + + + + +
+
+
+ 5 +
+
+
+ + 5 + + + + + + + + + + + + + + + + + + + +
+
+
+ 6 +
+
+
+ + 6 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ point cloud +
+
+
+ + point cloud + + + + + + +
+
+
+ non points area +
+
+
+ + non points area + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ occupied grids +
+
+
+ + occupied grids + + + + + + +
+
+
+ not occupied grids +
+
+
+ + not occupied grids + + + + + + + + + + +
+
+
+ surrounding occupied grid=1 +
+
+
+ + surrounding occupied... + + + + + + +
+
+
+ surrounding occupied grids = 6 +
+
+
+ + surrounding occupied... + + + + + + + + + +
+
+
+ divide into grid +
+
+
+ + divide into grid + + + + + +
+
+
+ check occupancy for each grids +
+
+
+ + check occupancy for each grids + + + + + +
+
+
+ + Every period, count the number of occupied grids around the ego. +
+ +
+
+
+ + Every period, count the number of occupied grids around the ego. + + + + + +
+
+
+ ① +
+
+
+ + ① + + + + + +
+
+
+ ② +
+
+
+ + ② + + + + + +
+
+
+ ③ +
+
+
+ + ③ + + + + + +
+
+
+ ④ +
+
+
+ + ④ + + + + + +
+
+
+ grid_size +
+
+
+ + grid_size + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/localization/pose_estimator_arbiter/media/pose_estimator_area_in_vector_map.png b/localization/pose_estimator_arbiter/media/pose_estimator_area_in_vector_map.png new file mode 100644 index 0000000000000..68e6a9ff430e6 Binary files /dev/null and b/localization/pose_estimator_arbiter/media/pose_estimator_area_in_vector_map.png differ diff --git a/localization/pose_estimator_arbiter/media/single_pose_estimator.drawio.svg b/localization/pose_estimator_arbiter/media/single_pose_estimator.drawio.svg new file mode 100644 index 0000000000000..837571474b76a --- /dev/null +++ b/localization/pose_estimator_arbiter/media/single_pose_estimator.drawio.svg @@ -0,0 +1,880 @@ + + + + + + + +
+
+
+ legend +
+
+
+ + legend + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ module +
+
+
+ + module + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ some nodes +
+
+
+ + some nodes + + + + + + + +
+
+
+ /util/downsampled/pointcloud +
+
+
+ + /util/downsampled/pointcloud + + + + + + +
+
+
+ pointcluod_ +
+ preprocessor +
+
+
+ + pointcluod_... + + + + + + + +
+
+
+ /pose_estimator/yabloc/* +
+
+
+ + /pose_estimator/yabloc/* + + + + + + +
+
+
+ image_ +
+ preprocessor +
+
+
+ + image_... + + + + + + + +
+
+
+ /pose_estimator +
+ /pose_with_covariance +
+
+
+ + /pose_estimator... + + + + + + +
+
+
+ ndt_scan_matcher +
+
+
+ + ndt_scan_matcher + + + + + + + +
+
+
+ /pose_estimator +
+ /pose_with_covariance +
+
+
+ + /pose_estimator... + + + + + + +
+
+
+ yabloc_particle_filter +
+
+
+ + yabloc_particle_filt... + + + + + + + +
+
+
+ /pose_with_covariance +
+
+
+ + /pose_with_covariance + + + + + + + + +
+
+
+ /sensing/lidar/top +
+ /outlier_filtered/pointcloud +
+
+
+ + /sensing/lidar/top... + + + + + + + +
+
+
+ /pose_estimator +
+ /pose_with_covariance +
+
+
+ + /pose_estimator... + + + + + + +
+
+
+ eagleye +
+
+
+ + eagleye + + + + + + + + +
+
+
+ /yabloc/image_processing +
+ /undistorted/image_raw +
+
+
+ + /yabloc/image_processing... + + + + + + +
+
+
+ EKF +
+ (pose_twist_fusion_filter) +
+
+
+ + EKF... + + + + + + + + +
+
+
+ /sensing/* +
+
+
+ + /sensing/* + + + + + + +
+
+
+ pointcluod_ +
+ preprocessor +
+
+
+ + pointcluod_... + + + + + + +
+
+
+ pointcluod_ +
+ preprocessor +
+
+
+ + pointcluod_... + + + + + + +
+
+
+ image_ +
+ preprocessor +
+
+
+ + image_... + + + + + + +
+
+
+ image_ +
+ preprocessor +
+
+
+ + image_... + + + + + + +
+
+
+ eagleye +
+
+
+ + eagleye + + + + + + +
+
+
+ eagleye +
+
+
+ + eagleye + + + + + + +
+
+
+ EKF +
+ (pose_twist_fusion_filter) +
+
+
+ + EKF... + + + + + + +
+
+
+ EKF +
+ (pose_twist_fusion_filter) +
+
+
+ + EKF... + + + + + + + +
+
+
+ /twist_estimator +
+ /twist_with_covariance +
+
+
+ + /twist_estimator... + + + + + + +
+
+
+ twist_ +
+ estimator +
+
+
+ + twist_... + + + + + + +
+
+
+ twist_ +
+ estimator +
+
+
+ + twist_... + + + + + + + +
+
+
+ /twist_estimator +
+ /twist_with_covariance +
+
+
+ + /twist_estimator... + + + + + + +
+
+
+ twist_ +
+ estimator +
+
+
+ + twist_... + + + + + + + +
+
+
+ /twist_estimator +
+ /twist_with_covariance +
+
+
+ + /twist_estimator... + + + + + + + + + +
+
+
+ only NDT +
+
+
+ + only NDT + + + + + + +
+
+
+ only YabLoc +
+
+
+ + only YabLoc + + + + + + +
+
+
+ only Eagleye +
+
+
+ + only Eagleye + + + + + + +
+
+
+ artag +
+
+
+ + artag + + + + + + + + +
+
+
+ /sensing/camera/ +
+ traffic_light/image_raw +
+
+
+ + /sensing/camera/... + + + + + + +
+
+
+ EKF +
+ (pose_twist_fusion_filter) +
+
+
+ + EKF... + + + + + + + +
+
+
+ /twist_estimator +
+ /twist_with_covariance +
+
+
+ + /twist_estimator... + + + + + + + +
+
+
+ only AR-Tag +
+
+
+ + only AR-Tag + + + + + + +
+
+
+ twist_ +
+ estimator +
+
+
+ + twist_... + + + + + + + +
+
+
+ /pose_estimator +
+ /pose_with_covariance +
+
+
+ + /pose_estimator... + + + + + + + + Text is not SVG - cannot display + + + diff --git a/localization/pose_estimator_arbiter/package.xml b/localization/pose_estimator_arbiter/package.xml new file mode 100644 index 0000000000000..b416d42c8617f --- /dev/null +++ b/localization/pose_estimator_arbiter/package.xml @@ -0,0 +1,43 @@ + + + + pose_estimator_arbiter + 0.1.0 + The arbiter of multiple pose estimators + Kento Yabuuchi + Apache License 2.0 + Kento Yabuuchi + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + autoware_auto_mapping_msgs + diagnostic_msgs + geometry_msgs + lanelet2_extension + libgoogle-glog-dev + magic_enum + pcl_conversions + pcl_ros + pluginlib + sensor_msgs + std_msgs + std_srvs + tier4_autoware_utils + ublox_msgs + visualization_msgs + yabloc_particle_filter + + rclcpp + rclcpp_components + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + ros_testing + + + ament_cmake + + diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp new file mode 100644 index 0000000000000..9e67dfc063964 --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp @@ -0,0 +1,100 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_ARBITER_HPP_ +#define POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_ARBITER_HPP_ + +#include "pose_estimator_arbiter/shared_data.hpp" +#include "pose_estimator_arbiter/stopper/base_stopper.hpp" +#include "pose_estimator_arbiter/switch_rule/base_switch_rule.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace pose_estimator_arbiter +{ +class PoseEstimatorArbiter : public rclcpp::Node +{ + using SetBool = std_srvs::srv::SetBool; + using String = std_msgs::msg::String; + using MarkerArray = visualization_msgs::msg::MarkerArray; + using Image = sensor_msgs::msg::Image; + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using InitializationState = autoware_adapi_v1_msgs::msg::LocalizationInitializationState; + using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; + +public: + PoseEstimatorArbiter(); + +private: + // Set of running pose estimators specified by ros param `pose_sources` + const std::unordered_set running_estimator_list_; + // Configuration to allow changing the log level by service + const std::unique_ptr logger_configure_; + + // This is passed to several modules (stoppers & rule) so that all modules can access common data + // without passing them as arguments. Also, modules can register subscriber callbacks through + // shared_data, avoiding the need to define duplicate subscribers for each module. + std::shared_ptr shared_data_{nullptr}; + + // Timer callback + rclcpp::TimerBase::SharedPtr timer_; + // Publishers + rclcpp::Publisher::SharedPtr pub_diag_; + rclcpp::Publisher::SharedPtr pub_debug_marker_array_; + rclcpp::Publisher::SharedPtr pub_debug_string_; + // Subscribers for stoppers + rclcpp::Subscription::SharedPtr sub_yabloc_input_; + rclcpp::Subscription::SharedPtr sub_artag_input_; + rclcpp::Subscription::SharedPtr sub_ndt_input_; + rclcpp::Subscription::SharedPtr sub_eagleye_output_; + // Subscribers for switch rules + rclcpp::Subscription::SharedPtr sub_localization_pose_cov_; + rclcpp::Subscription::SharedPtr sub_point_cloud_map_; + rclcpp::Subscription::SharedPtr sub_vector_map_; + rclcpp::Subscription::SharedPtr sub_initialization_state_; + + // Stoppers which enable/disable pose estimators + std::unordered_map stoppers_; + // Abstract class to determine which pose estimator should be used + std::shared_ptr switch_rule_{nullptr}; + + // Instruct all stopper to enable/disable + void toggle_all(bool enabled); + // Instruct each stopper to enable/disable + void toggle_each(const std::unordered_map & toggle_list); + + // Load switching rule according to the condition + void load_switch_rule(); + // Publish diagnostic messages + void publish_diagnostics() const; + + // Timer callback + void on_timer(); +}; +} // namespace pose_estimator_arbiter + +#endif // POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_ARBITER_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp new file mode 100644 index 0000000000000..4fc3fd9b914a6 --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp @@ -0,0 +1,213 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_estimator_arbiter/pose_estimator_arbiter.hpp" +#include "pose_estimator_arbiter/pose_estimator_type.hpp" +#include "pose_estimator_arbiter/stopper/stopper_artag.hpp" +#include "pose_estimator_arbiter/stopper/stopper_eagleye.hpp" +#include "pose_estimator_arbiter/stopper/stopper_ndt.hpp" +#include "pose_estimator_arbiter/stopper/stopper_yabloc.hpp" +#include "pose_estimator_arbiter/switch_rule/enable_all_rule.hpp" + +#include + +namespace pose_estimator_arbiter +{ +// Parses ros param to get the estimator set that is running +static std::unordered_set parse_estimator_name_args( + const std::vector & arg, const rclcpp::Logger & logger) +{ + std::unordered_set running_estimator_list; + for (const auto & estimator_name : arg) { + const auto estimator = magic_enum::enum_cast(estimator_name); + + if (estimator.has_value()) { + running_estimator_list.insert(estimator.value()); + } else { + RCLCPP_ERROR_STREAM(logger, "invalid pose_estimator_name is specified: " << estimator_name); + } + } + + return running_estimator_list; +} + +PoseEstimatorArbiter::PoseEstimatorArbiter() +: Node("pose_estimator_arbiter"), + running_estimator_list_(parse_estimator_name_args( + declare_parameter>("pose_sources"), get_logger())), + logger_configure_(std::make_unique(this)) +{ + // Shared data + shared_data_ = std::make_shared(); + + // Publisher + pub_diag_ = create_publisher("/diagnostics", 10); + pub_debug_string_ = create_publisher("~/debug/string", 10); + pub_debug_marker_array_ = create_publisher("~/debug/marker_array", 10); + + // Define function to get running pose_estimator + const std::set running_estimator_set( + running_estimator_list_.begin(), running_estimator_list_.end()); + const auto is_running = [running_estimator_set](const PoseEstimatorType type) -> bool { + return running_estimator_set.count(type) != 0; + }; + + // QoS + const rclcpp::QoS sensor_qos = rclcpp::SensorDataQoS(); + const rclcpp::QoS latch_qos = rclcpp::QoS(1).transient_local().reliable(); + + // Create stoppers & subscribers + if (is_running(PoseEstimatorType::ndt)) { + stoppers_.emplace( + PoseEstimatorType::ndt, std::make_shared(this, shared_data_)); + sub_ndt_input_ = create_subscription( + "~/input/ndt/pointcloud", sensor_qos, shared_data_->ndt_input_points.create_callback()); + } + if (is_running(PoseEstimatorType::yabloc)) { + stoppers_.emplace( + PoseEstimatorType::yabloc, std::make_shared(this, shared_data_)); + sub_yabloc_input_ = create_subscription( + "~/input/yabloc/image", sensor_qos, shared_data_->yabloc_input_image.create_callback()); + } + if (is_running(PoseEstimatorType::eagleye)) { + stoppers_.emplace( + PoseEstimatorType::eagleye, std::make_shared(this, shared_data_)); + sub_eagleye_output_ = create_subscription( + "~/input/eagleye/pose_with_covariance", 5, /* this is not sensor topic */ + shared_data_->eagleye_output_pose_cov.create_callback()); + } + if (is_running(PoseEstimatorType::artag)) { + stoppers_.emplace( + PoseEstimatorType::artag, std::make_shared(this, shared_data_)); + sub_artag_input_ = create_subscription( + "~/input/artag/image", sensor_qos, shared_data_->artag_input_image.create_callback()); + } + + // Subscribers for switch rule + { + sub_localization_pose_cov_ = create_subscription( + "~/input/pose_with_covariance", 5, shared_data_->localization_pose_cov.create_callback()); + sub_point_cloud_map_ = create_subscription( + "~/input/pointcloud_map", latch_qos, shared_data_->point_cloud_map.create_callback()); + sub_vector_map_ = create_subscription( + "~/input/vector_map", latch_qos, shared_data_->vector_map.create_callback()); + sub_initialization_state_ = create_subscription( + "~/input/initialization_state", latch_qos, + shared_data_->initialization_state.create_callback()); + } + + // Load switching rule + load_switch_rule(); + + // Timer callback + auto on_timer = std::bind(&PoseEstimatorArbiter::on_timer, this); + timer_ = + rclcpp::create_timer(this, this->get_clock(), rclcpp::Rate(1).period(), std::move(on_timer)); + + // Enable all pose estimators at the first + toggle_all(true); +} + +void PoseEstimatorArbiter::load_switch_rule() +{ + // NOTE: In the future, some rule will be laid below + RCLCPP_INFO_STREAM(get_logger(), "load default switching rule"); + switch_rule_ = + std::make_shared(*this, running_estimator_list_, shared_data_); +} + +void PoseEstimatorArbiter::toggle_each( + const std::unordered_map & toggle_list) +{ + for (auto [type, stopper] : stoppers_) { + RCLCPP_DEBUG_STREAM( + get_logger(), magic_enum::enum_name(type) << " : " << std::boolalpha << toggle_list.at(type)); + + // If the rule implementation is perfect, toggle_list should contains all pose_estimator_type. + if (toggle_list.count(type) == 0) { + RCLCPP_ERROR_STREAM( + get_logger(), magic_enum::enum_name(type) << " is not included in toggle_list."); + continue; + } + + // Enable or disable according to toggle_list + if (toggle_list.at(type)) { + stopper->enable(); + } else { + stopper->disable(); + } + } +} + +void PoseEstimatorArbiter::toggle_all(bool enabled) +{ + // Create toggle_list + std::unordered_map toggle_list; + for (auto [type, stopper] : stoppers_) { + toggle_list.emplace(type, enabled); + } + + // Apply toggle_list + toggle_each(toggle_list); +} + +void PoseEstimatorArbiter::publish_diagnostics() const +{ + diagnostic_msgs::msg::DiagnosticStatus diag_status; + + // Temporary implementation + { + diag_status.name = "localization: " + std::string(this->get_name()); + diag_status.hardware_id = this->get_name(); + + diag_status.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diag_status.message = "OK"; + + // TODO(KYabuuchi) : Add more details + diagnostic_msgs::msg::KeyValue key_value_msg; + key_value_msg.key = "state"; + key_value_msg.value = "Further details have not been implemented yet."; + diag_status.values.push_back(key_value_msg); + } + + DiagnosticArray diag_msg; + diag_msg.header.stamp = this->now(); + diag_msg.status.push_back(diag_status); + + pub_diag_->publish(diag_msg); +} + +void PoseEstimatorArbiter::on_timer() +{ + // Toggle each stopper status + if (switch_rule_) { + const auto toggle_list = switch_rule_->update(); + toggle_each(toggle_list); + + // Publish std_msg::String for debug + pub_debug_string_->publish(String().set__data(switch_rule_->debug_string())); + // Publish visualization_msgs::MarkerArray for debug + pub_debug_marker_array_->publish(switch_rule_->debug_marker_array()); + + } else { + RCLCPP_WARN_STREAM( + get_logger(), "switch_rule is not activated. Therefore, enable all pose_estimators"); + toggle_all(true); + } + + // Publish diagnostic results periodically + publish_diagnostics(); +} + +} // namespace pose_estimator_arbiter diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp new file mode 100644 index 0000000000000..e48507948520b --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp @@ -0,0 +1,31 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_estimator_arbiter/pose_estimator_arbiter.hpp" + +#include + +int main(int argc, char * argv[]) +{ + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_type.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_type.hpp new file mode 100644 index 0000000000000..edf12537198a3 --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_type.hpp @@ -0,0 +1,23 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_TYPE_HPP_ +#define POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_TYPE_HPP_ + +namespace pose_estimator_arbiter +{ +enum class PoseEstimatorType : int { ndt = 1, yabloc = 2, eagleye = 4, artag = 8 }; +} + +#endif // POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_TYPE_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp new file mode 100644 index 0000000000000..b9c3bd45c9e24 --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp @@ -0,0 +1,101 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__SHARED_DATA_HPP_ +#define POSE_ESTIMATOR_ARBITER__SHARED_DATA_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace pose_estimator_arbiter +{ +template +struct CallbackInvokingVariable +{ + CallbackInvokingVariable() {} + + explicit CallbackInvokingVariable(T initial_data) : value(initial_data) {} + + // Set data and invoke all callbacks + void set_and_invoke(T new_value) + { + value = new_value; + + // Call all callbacks with the new value + for (const auto & callback : callbacks) { + callback(value.value()); + } + } + + // Same as std::optional::has_value() + bool has_value() const { return value.has_value(); } + + // Same as std::optional::value() + const T operator()() const { return value.value(); } + + // Register callback function which is invoked when set_and_invoke() is called + void register_callback(std::function callback) const { callbacks.push_back(callback); } + + // Create subscription callback function which is used as below: + // auto subscriber = create_subscription("topic_name", 10, + // callback_invoking_variable.create_callback()); + auto create_callback() + { + return std::bind(&CallbackInvokingVariable::set_and_invoke, this, std::placeholders::_1); + } + +private: + // The latest data + std::optional value{std::nullopt}; + + // These functions are expected not to change the value variable + mutable std::vector> callbacks; +}; + +// This structure is handed to several modules as shared_ptr so that all modules can access data. +struct SharedData +{ +public: + using Image = sensor_msgs::msg::Image; + using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using InitializationState = autoware_adapi_v1_msgs::msg::LocalizationInitializationState; + + SharedData() {} + + // Used for stoppers + CallbackInvokingVariable eagleye_output_pose_cov; + CallbackInvokingVariable artag_input_image; + CallbackInvokingVariable ndt_input_points; + CallbackInvokingVariable yabloc_input_image; + + // Used for switch rule + CallbackInvokingVariable localization_pose_cov; + CallbackInvokingVariable point_cloud_map; + CallbackInvokingVariable vector_map; + CallbackInvokingVariable initialization_state{ + std::make_shared( + InitializationState{}.set__state(InitializationState::UNINITIALIZED))}; +}; + +} // namespace pose_estimator_arbiter +#endif // POSE_ESTIMATOR_ARBITER__SHARED_DATA_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/base_stopper.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/base_stopper.hpp new file mode 100644 index 0000000000000..fc2242a0b27e9 --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/base_stopper.hpp @@ -0,0 +1,47 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__STOPPER__BASE_STOPPER_HPP_ +#define POSE_ESTIMATOR_ARBITER__STOPPER__BASE_STOPPER_HPP_ + +#include "pose_estimator_arbiter/shared_data.hpp" + +#include + +#include + +namespace pose_estimator_arbiter::stopper +{ +class BaseStopper +{ +public: + using SharedPtr = std::shared_ptr; + + explicit BaseStopper(rclcpp::Node * node, const std::shared_ptr shared_data) + : logger_(node->get_logger()), shared_data_(shared_data) + { + } + + void enable() { set_enable(true); } + void disable() { set_enable(false); } + + virtual void set_enable(bool enabled) = 0; + +protected: + rclcpp::Logger logger_; + std::shared_ptr shared_data_{nullptr}; +}; +} // namespace pose_estimator_arbiter::stopper + +#endif // POSE_ESTIMATOR_ARBITER__STOPPER__BASE_STOPPER_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_artag.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_artag.hpp new file mode 100644 index 0000000000000..72a3f5412add0 --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_artag.hpp @@ -0,0 +1,57 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_ARTAG_HPP_ +#define POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_ARTAG_HPP_ + +#include "pose_estimator_arbiter/stopper/base_stopper.hpp" + +#include +#include + +#include +namespace pose_estimator_arbiter::stopper +{ +class StopperArTag : public BaseStopper +{ + using Image = sensor_msgs::msg::Image; + using SetBool = std_srvs::srv::SetBool; + +public: + explicit StopperArTag(rclcpp::Node * node, const std::shared_ptr shared_data) + : BaseStopper(node, shared_data) + { + ar_tag_is_enabled_ = true; + pub_image_ = node->create_publisher("~/output/artag/image", rclcpp::SensorDataQoS()); + + // Register callback + shared_data_->artag_input_image.register_callback([this](Image::ConstSharedPtr msg) -> void { + if (ar_tag_is_enabled_) { + pub_image_->publish(*msg); + } + }); + } + + void set_enable(bool enabled) override { ar_tag_is_enabled_ = enabled; } + +protected: + rclcpp::CallbackGroup::SharedPtr service_callback_group_; + +private: + bool ar_tag_is_enabled_; + rclcpp::Publisher::SharedPtr pub_image_; +}; +} // namespace pose_estimator_arbiter::stopper + +#endif // POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_ARTAG_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_eagleye.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_eagleye.hpp new file mode 100644 index 0000000000000..12bbe8c9769a1 --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_eagleye.hpp @@ -0,0 +1,53 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_EAGLEYE_HPP_ +#define POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_EAGLEYE_HPP_ +#include "pose_estimator_arbiter/stopper/base_stopper.hpp" + +#include + +#include + +namespace pose_estimator_arbiter::stopper +{ +class StopperEagleye : public BaseStopper +{ + using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + +public: + explicit StopperEagleye(rclcpp::Node * node, const std::shared_ptr shared_data) + : BaseStopper(node, shared_data) + { + eagleye_is_enabled_ = true; + pub_pose_ = node->create_publisher("~/output/eagleye/pose_with_covariance", 5); + + // Register callback + shared_data_->eagleye_output_pose_cov.register_callback( + [this](PoseCovStamped::ConstSharedPtr msg) -> void { + if (eagleye_is_enabled_) { + pub_pose_->publish(*msg); + } + }); + } + + void set_enable(bool enabled) override { eagleye_is_enabled_ = enabled; } + +private: + bool eagleye_is_enabled_; + rclcpp::Publisher::SharedPtr pub_pose_; +}; +} // namespace pose_estimator_arbiter::stopper + +#endif // POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_EAGLEYE_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_ndt.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_ndt.hpp new file mode 100644 index 0000000000000..dacea02f77bde --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_ndt.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_NDT_HPP_ +#define POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_NDT_HPP_ +#include "pose_estimator_arbiter/stopper/base_stopper.hpp" + +#include + +#include + +namespace pose_estimator_arbiter::stopper +{ +class StopperNdt : public BaseStopper +{ + using PointCloud2 = sensor_msgs::msg::PointCloud2; + +public: + explicit StopperNdt(rclcpp::Node * node, const std::shared_ptr shared_data) + : BaseStopper(node, shared_data) + { + ndt_is_enabled_ = true; + pub_pointcloud_ = node->create_publisher( + "~/output/ndt/pointcloud", rclcpp::SensorDataQoS().keep_last(10)); + + // Register callback + shared_data_->ndt_input_points.register_callback( + [this](PointCloud2::ConstSharedPtr msg) -> void { + if (ndt_is_enabled_) { + pub_pointcloud_->publish(*msg); + } + }); + } + + void set_enable(bool enabled) override { ndt_is_enabled_ = enabled; } + +private: + rclcpp::Publisher::SharedPtr pub_pointcloud_; + bool ndt_is_enabled_; +}; +} // namespace pose_estimator_arbiter::stopper + +#endif // POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_NDT_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_yabloc.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_yabloc.hpp new file mode 100644 index 0000000000000..2cd8b66f4ffd4 --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_yabloc.hpp @@ -0,0 +1,90 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_YABLOC_HPP_ +#define POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_YABLOC_HPP_ +#include "pose_estimator_arbiter/stopper/base_stopper.hpp" + +#include +#include + +#include + +namespace pose_estimator_arbiter::stopper +{ +class StopperYabLoc : public BaseStopper +{ + using Image = sensor_msgs::msg::Image; + using SetBool = std_srvs::srv::SetBool; + +public: + explicit StopperYabLoc(rclcpp::Node * node, const std::shared_ptr shared_data) + : BaseStopper(node, shared_data) + { + yabloc_is_enabled_ = true; + pub_image_ = node->create_publisher("~/output/yabloc/image", rclcpp::SensorDataQoS()); + + service_callback_group_ = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + // Prepare suspend service server + using namespace std::literals::chrono_literals; + enable_service_client_ = node->create_client( + "~/yabloc_trigger_srv", rmw_qos_profile_services_default, service_callback_group_); + while (!enable_service_client_->wait_for_service(1s) && rclcpp::ok()) { + RCLCPP_INFO( + node->get_logger(), "Waiting for service : %s", enable_service_client_->get_service_name()); + } + + // Register callback + shared_data_->yabloc_input_image.register_callback([this](Image::ConstSharedPtr msg) -> void { + if (yabloc_is_enabled_) { + pub_image_->publish(*msg); + } + }); + } + + void set_enable(bool enabled) override + { + if (yabloc_is_enabled_ != enabled) { + request_service(enabled); + } + yabloc_is_enabled_ = enabled; + } + +protected: + rclcpp::CallbackGroup::SharedPtr service_callback_group_; + + void request_service(bool flag) + { + using namespace std::literals::chrono_literals; + auto request = std::make_shared(); + request->data = flag; + + auto result_future = enable_service_client_->async_send_request(request); + std::future_status status = result_future.wait_for(1000ms); + if (status == std::future_status::ready) { + RCLCPP_DEBUG_STREAM(logger_, "stopper_yabloc dis/enabling service exited successfully"); + } else { + RCLCPP_ERROR_STREAM(logger_, "stopper_yabloc dis/enabling service exited unexpectedly"); + } + } + +private: + bool yabloc_is_enabled_; + rclcpp::Client::SharedPtr enable_service_client_; + rclcpp::Publisher::SharedPtr pub_image_; +}; +} // namespace pose_estimator_arbiter::stopper +#endif // POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_YABLOC_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/base_switch_rule.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/base_switch_rule.hpp new file mode 100644 index 0000000000000..5770f18efa32c --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/base_switch_rule.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__SWITCH_RULE__BASE_SWITCH_RULE_HPP_ +#define POSE_ESTIMATOR_ARBITER__SWITCH_RULE__BASE_SWITCH_RULE_HPP_ + +#include "pose_estimator_arbiter/pose_estimator_type.hpp" + +#include + +#include + +#include +#include +#include +#include + +namespace pose_estimator_arbiter::switch_rule +{ +class BaseSwitchRule +{ +protected: + using MarkerArray = visualization_msgs::msg::MarkerArray; + +public: + explicit BaseSwitchRule(rclcpp::Node & node) + : logger_ptr_(std::make_shared(node.get_logger())) + { + } + + virtual ~BaseSwitchRule() = default; + virtual std::unordered_map update() = 0; + virtual std::string debug_string() { return std::string{}; } + virtual MarkerArray debug_marker_array() { return MarkerArray{}; } + +protected: + rclcpp::Logger get_logger() const { return *logger_ptr_; } + std::shared_ptr logger_ptr_{nullptr}; +}; + +} // namespace pose_estimator_arbiter::switch_rule + +#endif // POSE_ESTIMATOR_ARBITER__SWITCH_RULE__BASE_SWITCH_RULE_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp new file mode 100644 index 0000000000000..2b2e325c3d94b --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp @@ -0,0 +1,43 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_estimator_arbiter/switch_rule/enable_all_rule.hpp" + +#include + +#include +#include +#include +#include + +namespace pose_estimator_arbiter::switch_rule +{ +EnableAllRule::EnableAllRule( + rclcpp::Node & node, const std::unordered_set & running_estimator_list, + const std::shared_ptr) +: BaseSwitchRule(node), running_estimator_list_(running_estimator_list) +{ +} + +std::unordered_map EnableAllRule::update() +{ + return { + {PoseEstimatorType::ndt, true}, + {PoseEstimatorType::yabloc, true}, + {PoseEstimatorType::eagleye, true}, + {PoseEstimatorType::artag, true}, + }; +} + +} // namespace pose_estimator_arbiter::switch_rule diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.hpp new file mode 100644 index 0000000000000..568226985b2ff --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.hpp @@ -0,0 +1,45 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__SWITCH_RULE__ENABLE_ALL_RULE_HPP_ +#define POSE_ESTIMATOR_ARBITER__SWITCH_RULE__ENABLE_ALL_RULE_HPP_ + +#include "pose_estimator_arbiter/shared_data.hpp" +#include "pose_estimator_arbiter/switch_rule/base_switch_rule.hpp" + +#include +#include +#include +#include + +namespace pose_estimator_arbiter::switch_rule +{ +class EnableAllRule : public BaseSwitchRule +{ +public: + EnableAllRule( + rclcpp::Node & node, const std::unordered_set & running_estimator_list, + const std::shared_ptr shared_data); + + virtual ~EnableAllRule() = default; + + std::unordered_map update() override; + +protected: + const std::unordered_set running_estimator_list_; +}; + +} // namespace pose_estimator_arbiter::switch_rule + +#endif // POSE_ESTIMATOR_ARBITER__SWITCH_RULE__ENABLE_ALL_RULE_HPP_ diff --git a/localization/pose_estimator_arbiter/test/test_pose_estimator_arbiter.py b/localization/pose_estimator_arbiter/test/test_pose_estimator_arbiter.py new file mode 100644 index 0000000000000..c419fb68e0571 --- /dev/null +++ b/localization/pose_estimator_arbiter/test/test_pose_estimator_arbiter.py @@ -0,0 +1,180 @@ +#!/usr/bin/env python3 + +# 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. + +import os +import time +import unittest + +from ament_index_python import get_package_share_directory +from geometry_msgs.msg import PoseWithCovarianceStamped +import launch +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import AnyLaunchDescriptionSource +import launch_testing +import pytest +import rclpy +from rclpy.qos import QoSProfile +from rclpy.qos import ReliabilityPolicy +from sensor_msgs.msg import Image +from sensor_msgs.msg import PointCloud2 +from std_srvs.srv import SetBool + +# This test confirms that all topics are relayed by arbiter. + + +@pytest.mark.launch_test +def generate_test_description(): + test_pose_estimator_arbiter_launch_file = os.path.join( + get_package_share_directory("pose_estimator_arbiter"), + "launch", + "pose_estimator_arbiter.launch.xml", + ) + + pose_estimator_arbiter = IncludeLaunchDescription( + AnyLaunchDescriptionSource(test_pose_estimator_arbiter_launch_file), + launch_arguments={ + "pose_sources": "[ndt, yabloc, eagleye, artag]", + "input_pointcloud": "/sensing/lidar/top/pointcloud", + }.items(), + ) + + return launch.LaunchDescription( + [ + pose_estimator_arbiter, + # Start tests right away - no need to wait for anything + launch_testing.actions.ReadyToTest(), + ] + ) + + +class TestPoseEstimatorArbiter(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + + def tearDown(self): + self.test_node.destroy_node() + + def spin_for(self, duration_sec): + end_time = time.time() + duration_sec + while time.time() < end_time: + rclpy.spin_once(self.test_node, timeout_sec=0.1) + + def yabloc_suspend_service_callback(self, srv): + pass + + def publish_input_topics(self): + self.pub_ndt_input.publish(PointCloud2()) + rclpy.spin_once(self.test_node, timeout_sec=0.1) + self.pub_yabloc_input.publish(Image()) + rclpy.spin_once(self.test_node, timeout_sec=0.1) + self.pub_eagleye_input.publish(PoseWithCovarianceStamped()) + rclpy.spin_once(self.test_node, timeout_sec=0.1) + + def create_publishers_and_subscribers(self): + # Publisher + qos_profile = QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT) + + self.pub_ndt_input = self.test_node.create_publisher( + PointCloud2, "/sensing/lidar/top/pointcloud", qos_profile + ) + # pub_yabloc_input is used for both yabloc and artag + self.pub_yabloc_input = self.test_node.create_publisher( + Image, "/sensing/camera/traffic_light/image_raw", qos_profile + ) + self.pub_eagleye_input = self.test_node.create_publisher( + PoseWithCovarianceStamped, + "/localization/pose_estimator/eagleye/pose_with_covariance/to_relay", + QoSProfile(depth=10), + ) + + # Subscriber + self.test_node.create_subscription( + PointCloud2, + "/sensing/lidar/top/pointcloud/relay", + lambda msg: self.ndt_relayed.append(msg.header), + qos_profile, + ) + self.test_node.create_subscription( + Image, + "/sensing/camera/traffic_light/image_raw/yabloc_relay", + lambda msg: self.yabloc_relayed.append(msg.header), + qos_profile, + ) + self.test_node.create_subscription( + Image, + "/sensing/camera/traffic_light/image_raw/artag_relay", + lambda msg: self.artag_relayed.append(msg.header), + qos_profile, + ) + self.test_node.create_subscription( + PoseWithCovarianceStamped, + "/localization/pose_estimator/pose_with_covariance", + lambda msg: self.eagleye_relayed.append(msg.header), + qos_profile, + ) + + def test_node_link(self): + # The arbiter waits for the service to start, so here it instantiates a meaningless service server. + self.test_node.create_service( + SetBool, + "/localization/pose_estimator/yabloc/pf/yabloc_trigger_srv", + self.yabloc_suspend_service_callback, + ) + + # Define subscription buffer + self.ndt_relayed = [] + self.yabloc_relayed = [] + self.artag_relayed = [] + self.eagleye_relayed = [] + + # Create publishers and subscribers + self.create_publishers_and_subscribers() + + # Wait 0.5 second for node to be ready + self.spin_for(0.5) + + # Publish dummy input topics + for _ in range(10): + self.publish_input_topics() + rclpy.spin_once(self.test_node, timeout_sec=0.1) + + # Wait 0.5 second for all topics to be subscribed + self.spin_for(0.5) + + # Confirm both topics are relayed + # In reality, 10topics should be received, but with a margin, 5 is used as the threshold. + self.assertGreater(len(self.ndt_relayed), 5) + self.assertGreater(len(self.yabloc_relayed), 5) + self.assertGreater(len(self.eagleye_relayed), 5) + self.assertGreater(len(self.artag_relayed), 5) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/localization/pose_estimator_arbiter/test/test_shared_data.cpp b/localization/pose_estimator_arbiter/test/test_shared_data.cpp new file mode 100644 index 0000000000000..3c1fa50b502a1 --- /dev/null +++ b/localization/pose_estimator_arbiter/test/test_shared_data.cpp @@ -0,0 +1,59 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_estimator_arbiter/shared_data.hpp" + +#include + +TEST(SharedData, callback_invoked_correctly) +{ + pose_estimator_arbiter::CallbackInvokingVariable variable; + + // register callback + bool callback_invoked = false; + int processed_value = 0; + variable.register_callback([&](const int & value) { + callback_invoked = true; + processed_value = 2 * value; + }); + + EXPECT_FALSE(variable.has_value()); + EXPECT_FALSE(callback_invoked); + EXPECT_EQ(processed_value, 0); + + // set value and invoke callback + const int expected_value = 10; + variable.set_and_invoke(expected_value); + + EXPECT_TRUE(variable.has_value()); + EXPECT_TRUE(callback_invoked); + EXPECT_TRUE(variable() == expected_value); + EXPECT_TRUE(processed_value == 2 * expected_value); +} + +TEST(SharedData, multiple_callback_invoked_correctly) +{ + pose_estimator_arbiter::CallbackInvokingVariable variable; + + // register callback + int callback_invoked_num = 0; + variable.register_callback([&](const int &) { callback_invoked_num++; }); + variable.register_callback([&](const int &) { callback_invoked_num++; }); + variable.register_callback([&](const int &) { callback_invoked_num++; }); + + // set value and invoke callback + variable.set_and_invoke(10); + + EXPECT_EQ(callback_invoked_num, 3); +} diff --git a/localization/pose_initializer/README.md b/localization/pose_initializer/README.md index 961f3a8e3293c..7006becf00c2f 100644 --- a/localization/pose_initializer/README.md +++ b/localization/pose_initializer/README.md @@ -13,14 +13,7 @@ This node depends on the map height fitter library. ### Parameters -| Name | Type | Description | -| --------------------- | ---- | ---------------------------------------------------------------------------------------- | -| `ekf_enabled` | bool | If true, EKF localizer is activated. | -| `ndt_enabled` | bool | If true, the pose will be estimated by NDT scan matcher, otherwise it is passed through. | -| `stop_check_enabled` | bool | If true, initialization is accepted only when the vehicle is stopped. | -| `stop_check_duration` | bool | The duration used for the stop check above. | -| `gnss_enabled` | bool | If true, use the GNSS pose when no pose is specified. | -| `gnss_pose_timeout` | bool | The duration that the GNSS pose is valid. | +{{ json_to_markdown("localization/pose_initializer/schema/pose_initializer.schema.json") }} ### Services diff --git a/localization/pose_initializer/config/pose_initializer.param.yaml b/localization/pose_initializer/config/pose_initializer.param.yaml index a05cc7c35cb1a..88acbfb52611d 100644 --- a/localization/pose_initializer/config/pose_initializer.param.yaml +++ b/localization/pose_initializer/config/pose_initializer.param.yaml @@ -2,6 +2,11 @@ ros__parameters: gnss_pose_timeout: 3.0 # [sec] stop_check_duration: 3.0 # [sec] + ekf_enabled: $(var ekf_enabled) + gnss_enabled: $(var gnss_enabled) + yabloc_enabled: $(var yabloc_enabled) + ndt_enabled: $(var ndt_enabled) + stop_check_enabled: $(var stop_check_enabled) # from gnss gnss_particle_covariance: diff --git a/localization/pose_initializer/launch/pose_initializer.launch.xml b/localization/pose_initializer/launch/pose_initializer.launch.xml index 43e8d48c4fda0..3e4911e2c2936 100644 --- a/localization/pose_initializer/launch/pose_initializer.launch.xml +++ b/localization/pose_initializer/launch/pose_initializer.launch.xml @@ -7,15 +7,10 @@ + - - - - - - - + @@ -23,8 +18,10 @@ - + + + diff --git a/localization/pose_initializer/package.xml b/localization/pose_initializer/package.xml index 1a1a8e48b7e3b..85c9c26bd6c8c 100644 --- a/localization/pose_initializer/package.xml +++ b/localization/pose_initializer/package.xml @@ -7,6 +7,11 @@ Yamato Ando Takagi, Isamu Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Yamato Ando Takagi, Isamu diff --git a/localization/pose_initializer/schema/pose_initializer.schema.json b/localization/pose_initializer/schema/pose_initializer.schema.json new file mode 100644 index 0000000000000..ced4dc78b93b1 --- /dev/null +++ b/localization/pose_initializer/schema/pose_initializer.schema.json @@ -0,0 +1,85 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Pose Initializer Node", + "type": "object", + "definitions": { + "pose_initializer": { + "type": "object", + "properties": { + "gnss_pose_timeout": { + "type": "number", + "description": "The duration that the GNSS pose is valid. [sec]", + "default": "3.0", + "minimum": 0.0 + }, + "stop_check_enabled": { + "type": "string", + "description": "If true, initialization is accepted only when the vehicle is stopped.", + "default": "true" + }, + "stop_check_duration": { + "type": "number", + "description": "The duration used for the stop check above. [sec]", + "default": "3.0", + "minimum": 0.0 + }, + "ekf_enabled": { + "type": "string", + "description": "If true, EKF localizer is activated.", + "default": "true" + }, + "gnss_enabled": { + "type": "string", + "description": "If true, use the GNSS pose when no pose is specified.", + "default": "true" + }, + "yabloc_enabled": { + "type": "string", + "description": "If true, YabLocModule is used.", + "default": "true" + }, + "ndt_enabled": { + "type": "string", + "description": "If true, the pose will be estimated by NDT scan matcher, otherwise it is passed through.", + "default": "true" + }, + "gnss_particle_covariance": { + "type": "array", + "description": "gnss particle covariance", + "default": "[1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.0]" + }, + "output_pose_covariance": { + "type": "array", + "description": "output pose covariance", + "default": "[1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2]" + } + }, + "required": [ + "gnss_pose_timeout", + "stop_check_duration", + "ekf_enabled", + "gnss_enabled", + "yabloc_enabled", + "ndt_enabled", + "stop_check_enabled", + "gnss_particle_covariance", + "output_pose_covariance" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/pose_initializer" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/pose_instability_detector/config/pose_instability_detector.param.yaml b/localization/pose_instability_detector/config/pose_instability_detector.param.yaml index 29a25849d6b1c..d94de020a4a12 100644 --- a/localization/pose_instability_detector/config/pose_instability_detector.param.yaml +++ b/localization/pose_instability_detector/config/pose_instability_detector.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - interval_sec: 1.0 # [sec] + interval_sec: 0.5 # [sec] threshold_diff_position_x: 1.0 # [m] threshold_diff_position_y: 1.0 # [m] threshold_diff_position_z: 1.0 # [m] diff --git a/localization/pose_instability_detector/package.xml b/localization/pose_instability_detector/package.xml index 7774407a7990d..bf57e5589b747 100644 --- a/localization/pose_instability_detector/package.xml +++ b/localization/pose_instability_detector/package.xml @@ -10,6 +10,7 @@ Taiki Yamada Ryu Yamamoto Shintaro Sakoda + NGUYEN Viet Anh Apache License 2.0 Shintaro Sakoda diff --git a/localization/pose_instability_detector/schema/pose_instability_detector.schema.json b/localization/pose_instability_detector/schema/pose_instability_detector.schema.json index b45b43102444d..560d39a2d5bff 100644 --- a/localization/pose_instability_detector/schema/pose_instability_detector.schema.json +++ b/localization/pose_instability_detector/schema/pose_instability_detector.schema.json @@ -8,7 +8,7 @@ "properties": { "interval_sec": { "type": "number", - "default": 1.0, + "default": 0.5, "exclusiveMinimum": 0, "description": "The interval of timer_callback in seconds." }, 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/package.xml b/localization/stop_filter/package.xml index b17809aaa9948..83eaf1b9fa821 100644 --- a/localization/stop_filter/package.xml +++ b/localization/stop_filter/package.xml @@ -7,6 +7,11 @@ Koji Minoda Yamato Ando Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Koji Minoda 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/localization/tree_structured_parzen_estimator/package.xml b/localization/tree_structured_parzen_estimator/package.xml index b699d5c69e610..96d2b9ecf54cc 100644 --- a/localization/tree_structured_parzen_estimator/package.xml +++ b/localization/tree_structured_parzen_estimator/package.xml @@ -8,6 +8,9 @@ Masahiro Sakamoto Shintaro Sakoda Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Ryu Yamamoto Apache License 2.0 Shintaro Sakoda diff --git a/localization/twist2accel/CMakeLists.txt b/localization/twist2accel/CMakeLists.txt index 93b9546be0a36..59f23eacb2fb3 100644 --- a/localization/twist2accel/CMakeLists.txt +++ b/localization/twist2accel/CMakeLists.txt @@ -13,4 +13,5 @@ ament_target_dependencies(twist2accel) ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/localization/twist2accel/README.md b/localization/twist2accel/README.md index ec73c34052f99..5a540dca895d4 100644 --- a/localization/twist2accel/README.md +++ b/localization/twist2accel/README.md @@ -21,10 +21,7 @@ This package is responsible for estimating acceleration using the output of `ekf ## Parameters -| Name | Type | Description | -| -------------------- | ------ | ------------------------------------------------------------------------- | -| `use_odom` | bool | use odometry if true, else use twist input (default: true) | -| `accel_lowpass_gain` | double | lowpass gain for lowpass filter in estimating acceleration (default: 0.9) | +{{ json_to_markdown("localization/twist2accel/schema/twist2accel.schema.json") }} ## Future work diff --git a/localization/twist2accel/config/twist2accel.param.yaml b/localization/twist2accel/config/twist2accel.param.yaml new file mode 100644 index 0000000000000..e58e029248253 --- /dev/null +++ b/localization/twist2accel/config/twist2accel.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + use_odom: true + accel_lowpass_gain: 0.9 diff --git a/localization/twist2accel/launch/twist2accel.launch.xml b/localization/twist2accel/launch/twist2accel.launch.xml index 47b8a95d13a08..c4c9a3ed50bfc 100644 --- a/localization/twist2accel/launch/twist2accel.launch.xml +++ b/localization/twist2accel/launch/twist2accel.launch.xml @@ -1,6 +1,6 @@ - - + + @@ -8,7 +8,6 @@ - - + diff --git a/localization/twist2accel/package.xml b/localization/twist2accel/package.xml index c44cd9d144566..08dacf9185769 100644 --- a/localization/twist2accel/package.xml +++ b/localization/twist2accel/package.xml @@ -7,6 +7,11 @@ Koji Minoda Yamato Ando Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Koji Minoda diff --git a/localization/twist2accel/schema/twist2accel.schema.json b/localization/twist2accel/schema/twist2accel.schema.json new file mode 100644 index 0000000000000..6b3c2bd094166 --- /dev/null +++ b/localization/twist2accel/schema/twist2accel.schema.json @@ -0,0 +1,36 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for twist2accel Nodes", + "type": "object", + "definitions": { + "twist2accel": { + "type": "object", + "properties": { + "use_odom": { + "type": "boolean", + "default": true, + "description": "use odometry if true, else use twist input." + }, + "accel_lowpass_gain": { + "type": "number", + "default": 0.9, + "minimum": 0.0, + "description": "lowpass gain for lowpass filter in estimating acceleration." + } + }, + "required": ["use_odom", "accel_lowpass_gain"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/twist2accel" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/localization/twist2accel/src/twist2accel.cpp b/localization/twist2accel/src/twist2accel.cpp index a8a7d46f57056..68019f27e95fe 100644 --- a/localization/twist2accel/src/twist2accel.cpp +++ b/localization/twist2accel/src/twist2accel.cpp @@ -35,8 +35,8 @@ Twist2Accel::Twist2Accel(const std::string & node_name, const rclcpp::NodeOption pub_accel_ = create_publisher("output/accel", 1); prev_twist_ptr_ = nullptr; - accel_lowpass_gain_ = declare_parameter("accel_lowpass_gain", 0.5); - use_odom_ = declare_parameter("use_odom", true); + accel_lowpass_gain_ = declare_parameter("accel_lowpass_gain"); + use_odom_ = declare_parameter("use_odom"); lpf_alx_ptr_ = std::make_shared(accel_lowpass_gain_); lpf_aly_ptr_ = std::make_shared(accel_lowpass_gain_); diff --git a/localization/yabloc/yabloc_common/README.md b/localization/yabloc/yabloc_common/README.md index 1160f1b314b99..6368305bdbfad 100644 --- a/localization/yabloc/yabloc_common/README.md +++ b/localization/yabloc/yabloc_common/README.md @@ -32,11 +32,7 @@ It estimates the height and tilt of the ground from lanelet2. ### Parameters -| Name | Type | Description | -| ----------------- | ---- | -------------------------------------------------------- | -| `force_zero_tilt` | bool | if true, the tilt is always determined to be horizontal. | -| `K` | int | parameter for nearest k search | -| `R` | int | parameter for radius search | +{{ json_to_markdown("localization/yabloc/yabloc_common/schema/ground_server.schema.json") }} ## ll2_decomposer @@ -63,8 +59,4 @@ This node extracts the elements related to the road surface markings and yabloc ### Parameters -| Name | Type | Description | -| --------------------- | ---------------- | ---------------------------------------------------------------------- | -| `road_marking_labels` | vector\ | This label is used to extract the road surface markings from lanelet2. | -| `sign_board_labels` | vector\ | This label is used to extract the traffic sign boards from lanelet2. | -| `bounding_box_labels` | vector\ | This label is used to extract the bounding boxes from lanelet2. | +{{ json_to_markdown("localization/yabloc/yabloc_common/schema/ll2_decomposer.schema.json") }} diff --git a/localization/yabloc/yabloc_common/package.xml b/localization/yabloc/yabloc_common/package.xml index d7e94ebefa24b..710ec0aeb75f8 100644 --- a/localization/yabloc/yabloc_common/package.xml +++ b/localization/yabloc/yabloc_common/package.xml @@ -5,6 +5,11 @@ YabLoc common library Kento Yabuuchi Masahiro Sakamoto + Yamato Ando + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 ament_cmake_ros diff --git a/localization/yabloc/yabloc_common/schema/ground_server.schema.json b/localization/yabloc/yabloc_common/schema/ground_server.schema.json new file mode 100644 index 0000000000000..a9b0674cedd1c --- /dev/null +++ b/localization/yabloc/yabloc_common/schema/ground_server.schema.json @@ -0,0 +1,43 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for ground_server", + "type": "object", + "definitions": { + "ground_server": { + "type": "object", + "properties": { + "force_zero_tilt": { + "type": "boolean", + "description": "if true, the tilt is always determined to be horizontal", + "default": false + }, + "K": { + "type": "number", + "description": "the number of neighbors for ground search on a map", + "default": 50 + }, + "R": { + "type": "number", + "description": "radius for ground search on a map [m]", + "default": 10 + } + }, + "required": ["force_zero_tilt", "K", "R"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/ground_server" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/yabloc/yabloc_common/schema/ll2_decomposer.schema.json b/localization/yabloc/yabloc_common/schema/ll2_decomposer.schema.json new file mode 100644 index 0000000000000..fa5a7b37464f3 --- /dev/null +++ b/localization/yabloc/yabloc_common/schema/ll2_decomposer.schema.json @@ -0,0 +1,51 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for ll2_decomposer", + "type": "object", + "definitions": { + "ll2_decomposer": { + "type": "object", + "properties": { + "road_marking_labels": { + "type": "array", + "description": "line string types that indicating road surface markings in lanelet2", + "default": [ + "cross_walk", + "zebra_marking", + "line_thin", + "line_thick", + "pedestrian_marking", + "stop_line", + "road_border" + ] + }, + "sign_board_labels": { + "type": "array", + "description": "line string types that indicating traffic sign boards in lanelet2", + "default": ["sign-board"] + }, + "bounding_box_labels": { + "type": "array", + "description": "line string types that indicating not mapped areas in lanelet2", + "default": ["none"] + } + }, + "required": ["road_marking_labels", "sign_board_labels", "bounding_box_labels"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/ll2_decomposer" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/yabloc/yabloc_image_processing/README.md b/localization/yabloc/yabloc_image_processing/README.md index 85e1d408a1b4b..9816a02c48121 100644 --- a/localization/yabloc/yabloc_image_processing/README.md +++ b/localization/yabloc/yabloc_image_processing/README.md @@ -53,15 +53,7 @@ This node extract road surface region by [graph-based-segmentation](https://docs ### Parameters -| Name | Type | Description | -| --------------------------------- | ------ | ------------------------------------------------------------------ | -| `target_height_ratio` | double | height on the image to retrieve the candidate road surface | -| `target_candidate_box_width` | int | size of the square area to search for candidate road surfaces | -| `pickup_additional_graph_segment` | bool | if this is true, additional regions of similar color are retrieved | -| `similarity_score_threshold` | double | threshold for picking up additional areas | -| `sigma` | double | parameters for cv::ximgproc::segmentation | -| `k` | double | parameters for cv::ximgproc::segmentation | -| `min_size` | double | parameters for cv::ximgproc::segmentation | +{{ json_to_markdown("localization/yabloc/yabloc_image_processing/schema/graph_segment.schema.json") }} ## segment_filter @@ -89,14 +81,7 @@ This is a node that integrates the results of graph_segment and lsd to extract r ### Parameters -| Name | Type | Description | -| -------------------------------------- | ------ | ------------------------------------------------------------------- | -| `min_segment_length` | double | min length threshold (if it is negative, it is unlimited) | -| `max_segment_distance` | double | max distance threshold (if it is negative, it is unlimited) | -| `max_lateral_distance` | double | max lateral distance threshold (if it is negative, it is unlimited) | -| `publish_image_with_segment_for_debug` | bool | toggle whether to publish the filtered line segment for debug | -| `max_range` | double | range of debug projection visualization | -| `image_size` | int | image size of debug projection visualization | +{{ json_to_markdown("localization/yabloc/yabloc_image_processing/schema/segment_filter.schema.json") }} ## undistort @@ -127,11 +112,7 @@ This is to avoid redundant decompression within Autoware. ### Parameters -| Name | Type | Description | -| ------------------- | ------ | ---------------------------------------------------------------------------------------------- | -| `use_sensor_qos` | bool | where to use sensor qos or not | -| `width` | int | resized image width size | -| `override_frame_id` | string | value for overriding the camera's frame_id. if blank, frame_id of static_tf is not overwritten | +{{ json_to_markdown("localization/yabloc/yabloc_image_processing/schema/undistort.schema.json") }} #### about tf_static overriding diff --git a/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml b/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml index aa6fc10ee6667..a0cad16302c2b 100644 --- a/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml +++ b/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml @@ -1,7 +1,7 @@ - + diff --git a/localization/yabloc/yabloc_image_processing/package.xml b/localization/yabloc/yabloc_image_processing/package.xml index ffdcea25b4b6d..209f09fdaa7ac 100644 --- a/localization/yabloc/yabloc_image_processing/package.xml +++ b/localization/yabloc/yabloc_image_processing/package.xml @@ -6,6 +6,11 @@ YabLoc image processing package Kento Yabuuchi Masahiro Sakamoto + Yamato Ando + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 ament_cmake diff --git a/localization/yabloc/yabloc_image_processing/schema/graph_segment.schema.json b/localization/yabloc/yabloc_image_processing/schema/graph_segment.schema.json new file mode 100644 index 0000000000000..c0bf96dd0de7a --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/schema/graph_segment.schema.json @@ -0,0 +1,71 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for graph_segment", + "type": "object", + "definitions": { + "graph_segment": { + "type": "object", + "properties": { + "target_height_ratio": { + "type": "number", + "description": "height on the image to retrieve the candidate road surface", + "default": 0.85 + }, + "target_candidate_box_width": { + "type": "number", + "description": "size of the square area to search for candidate road surfaces", + "default": 15 + }, + "pickup_additional_graph_segment": { + "type": "boolean", + "description": "if this is true, additional regions of similar color are retrieved", + "default": true + }, + "similarity_score_threshold": { + "type": "number", + "description": "threshold for picking up additional areas", + "default": 0.8 + }, + "sigma": { + "type": "number", + "description": "parameter for cv::ximgproc::segmentation::GraphSegmentation", + "default": 0.5 + }, + "k": { + "type": "number", + "description": "parameter for cv::ximgproc::segmentation::GraphSegmentation", + "default": 300.0 + }, + "min_size": { + "type": "number", + "description": "parameter for cv::ximgproc::segmentation::GraphSegmentation", + "default": 100.0 + } + }, + "required": [ + "target_height_ratio", + "target_candidate_box_width", + "pickup_additional_graph_segment", + "similarity_score_threshold", + "sigma", + "k", + "min_size" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/graph_segment" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/yabloc/yabloc_image_processing/schema/segment_filter.schema.json b/localization/yabloc/yabloc_image_processing/schema/segment_filter.schema.json new file mode 100644 index 0000000000000..bfe74ea80569c --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/schema/segment_filter.schema.json @@ -0,0 +1,65 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for segment_filter", + "type": "object", + "definitions": { + "segment_filter": { + "type": "object", + "properties": { + "min_segment_length": { + "type": "number", + "description": "min length threshold (if it is negative, it is unlimited)", + "default": 1.5 + }, + "max_segment_distance": { + "type": "number", + "description": "max distance threshold (if it is negative, it is unlimited)", + "default": 30.0 + }, + "max_lateral_distance": { + "type": "number", + "description": "max lateral distance threshold (if it is negative, it is unlimited)", + "default": 10.0 + }, + "publish_image_with_segment_for_debug": { + "type": "boolean", + "description": "toggle whether to publish the filtered line segment for debug", + "default": true + }, + "max_range": { + "type": "number", + "description": "range of debug projection visualization", + "default": 20.0 + }, + "image_size": { + "type": "number", + "description": "image size of debug projection visualization", + "default": 800 + } + }, + "required": [ + "min_segment_length", + "max_segment_distance", + "max_lateral_distance", + "publish_image_with_segment_for_debug", + "max_range", + "image_size" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/segment_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/yabloc/yabloc_image_processing/schema/undistort.schema.json b/localization/yabloc/yabloc_image_processing/schema/undistort.schema.json new file mode 100644 index 0000000000000..d166d85024903 --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/schema/undistort.schema.json @@ -0,0 +1,43 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for undistort", + "type": "object", + "definitions": { + "undistort": { + "type": "object", + "properties": { + "use_sensor_qos": { + "type": "boolean", + "description": "whether to use sensor qos or not", + "default": true + }, + "width": { + "type": "number", + "description": "resized image width size", + "default": 800 + }, + "override_frame_id": { + "type": "string", + "description": "value for overriding the camera's frame_id. if blank, frame_id of static_tf is not overwritten", + "default": "" + } + }, + "required": ["use_sensor_qos", "width", "override_frame_id"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/undistort" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/yabloc/yabloc_monitor/README.md b/localization/yabloc/yabloc_monitor/README.md index ed4cdc36b6ba0..849429290b427 100644 --- a/localization/yabloc/yabloc_monitor/README.md +++ b/localization/yabloc/yabloc_monitor/README.md @@ -25,3 +25,7 @@ To be added, | Name | Type | Description | | -------------- | --------------------------------- | ------------------- | | `/diagnostics` | `diagnostic_msgs/DiagnosticArray` | Diagnostics outputs | + +### Parameters + +{{ json_to_markdown("localization/yabloc/yabloc_monitor/schema/yabloc_monitor.schema.json") }} diff --git a/localization/yabloc/yabloc_monitor/package.xml b/localization/yabloc/yabloc_monitor/package.xml index 8c92c3c6a4303..22a5d0eded6b6 100644 --- a/localization/yabloc/yabloc_monitor/package.xml +++ b/localization/yabloc/yabloc_monitor/package.xml @@ -7,6 +7,11 @@ Kento Yabuuchi Koji Minoda Masahiro Sakamoto + Yamato Ando + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 ament_cmake_ros diff --git a/localization/yabloc/yabloc_monitor/schema/yabloc_monitor.schema.json b/localization/yabloc/yabloc_monitor/schema/yabloc_monitor.schema.json new file mode 100644 index 0000000000000..c83a9d3ac45f3 --- /dev/null +++ b/localization/yabloc/yabloc_monitor/schema/yabloc_monitor.schema.json @@ -0,0 +1,33 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for yabloc_monitor", + "type": "object", + "definitions": { + "yabloc_monitor": { + "type": "object", + "properties": { + "availability/timestamp_tolerance": { + "type": "number", + "description": "tolerable time difference between current time and latest estimated pose", + "default": 1.0 + } + }, + "required": ["availability/timestamp_tolerance"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/yabloc_monitor" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/yabloc/yabloc_particle_filter/README.md b/localization/yabloc/yabloc_particle_filter/README.md index 41ab7b1b10039..f46a363102c8b 100644 --- a/localization/yabloc/yabloc_particle_filter/README.md +++ b/localization/yabloc/yabloc_particle_filter/README.md @@ -36,15 +36,13 @@ This package contains some executable nodes related to particle filter. ### Parameters -| Name | Type | Description | -| ----------------------------- | ---------------- | ----------------------------------------------------------------- | -| `visualize` | bool | whether particles are also published in visualization_msgs or not | -| `static_linear_covariance` | double | to override the covariance of `/twist_with_covariance` | -| `static_angular_covariance` | double | to override the covariance of `/twist_with_covariance` | -| `resampling_interval_seconds` | double | the interval of particle resampling | -| `num_of_particles` | int | the number of particles | -| `prediction_rate` | double | frequency of forecast updates, in Hz | -| `cov_xx_yy` | vector\ | the covariance of initial pose | +{{ json_to_markdown("localization/yabloc/yabloc_particle_filter/schema/predictor.schema.json") }} + +### Services + +| Name | Type | Description | +| -------------------- | ------------------------ | ------------------------------------------------ | +| `yabloc_trigger_srv` | `std_srvs::srv::SetBool` | activation and deactivation of yabloc estimation | ## gnss_particle_corrector @@ -74,19 +72,7 @@ This package contains some executable nodes related to particle filter. ### Parameters -| Name | Type | Description | -| -------------------------------- | ------ | ---------------------------------------------------------------------------------------- | -| `acceptable_max_delay` | double | how long to hold the predicted particles | -| `visualize` | double | whether publish particles as marker_array or not | -| `mahalanobis_distance_threshold` | double | if the Mahalanobis distance to the GNSS for particle exceeds this, the correction skips. | -| `for_fixed/max_weight` | bool | parameter for gnss weight distribution | -| `for_fixed/flat_radius` | bool | parameter for gnss weight distribution | -| `for_fixed/max_radius` | bool | parameter for gnss weight distribution | -| `for_fixed/min_weight` | bool | parameter for gnss weight distribution | -| `for_not_fixed/flat_radius` | bool | parameter for gnss weight distribution | -| `for_not_fixed/max_radius` | bool | parameter for gnss weight distribution | -| `for_not_fixed/min_weight` | bool | parameter for gnss weight distribution | -| `for_not_fixed/max_weight` | bool | parameter for gnss weight distribution | +{{ json_to_markdown("localization/yabloc/yabloc_particle_filter/schema/gnss_particle_corrector.schema.json") }} ## camera_particle_corrector @@ -121,16 +107,7 @@ This package contains some executable nodes related to particle filter. ### Parameters -| Name | Type | Description | -| ---------------------- | ------ | ----------------------------------------------------------------------------------------------------------------------------------------- | -| `acceptable_max_delay` | double | how long to hold the predicted particles | -| `visualize` | double | whether publish particles as marker_array or not | -| `image_size` | int | image size of debug/cost_map_image | -| `max_range` | double | width of hierarchical cost map | -| `gamma` | double | gamma value of the intensity gradient of the cost map | -| `min_prob` | double | minimum particle weight the corrector node gives | -| `far_weight_gain` | double | `exp(-far_weight_gain_ * squared_distance_from_camera)` is weight gain. if this is large, the nearby road markings will be more important | -| `enabled_at_first` | bool | if it is false, this node is not activated at first. you can activate by service call | +{{ json_to_markdown("localization/yabloc/yabloc_particle_filter/schema/camera_particle_corrector.schema.json") }} ### Services diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp index c7794a2be9f51..e02393ee14db6 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include @@ -46,6 +47,7 @@ class Predictor : public rclcpp::Node using TwistCovStamped = geometry_msgs::msg::TwistWithCovarianceStamped; using TwistStamped = geometry_msgs::msg::TwistStamped; using Marker = visualization_msgs::msg::Marker; + using SetBool = std_srvs::srv::SetBool; Predictor(); @@ -62,6 +64,7 @@ class Predictor : public rclcpp::Node const std::vector cov_xx_yy_; // Subscriber + rclcpp::Subscription::SharedPtr ekf_pose_sub_; rclcpp::Subscription::SharedPtr initialpose_sub_; rclcpp::Subscription::SharedPtr twist_cov_sub_; rclcpp::Subscription::SharedPtr particles_sub_; @@ -73,11 +76,15 @@ class Predictor : public rclcpp::Node rclcpp::Publisher::SharedPtr pose_cov_pub_; rclcpp::Publisher::SharedPtr marker_pub_; std::unique_ptr tf2_broadcaster_; + // Server + rclcpp::Service::SharedPtr yabloc_trigger_server_; // Timer callback rclcpp::TimerBase::SharedPtr timer_; float ground_height_{0}; + bool yabloc_activated_{true}; + PoseCovStamped::ConstSharedPtr latest_ekf_pose_ptr_{nullptr}; std::optional particle_array_opt_{std::nullopt}; std::optional latest_twist_opt_{std::nullopt}; @@ -92,6 +99,8 @@ class Predictor : public rclcpp::Node void on_twist_cov(const TwistCovStamped::ConstSharedPtr twist); void on_weighted_particles(const ParticleArray::ConstSharedPtr weighted_particles); void on_timer(); + void on_trigger_service( + SetBool::Request::ConstSharedPtr request, SetBool::Response::SharedPtr response); // void initialize_particles(const PoseCovStamped & initialpose); diff --git a/localization/yabloc/yabloc_particle_filter/launch/yabloc_particle_filter.launch.xml b/localization/yabloc/yabloc_particle_filter/launch/yabloc_particle_filter.launch.xml index f38264b828d34..5dd483f142174 100644 --- a/localization/yabloc/yabloc_particle_filter/launch/yabloc_particle_filter.launch.xml +++ b/localization/yabloc/yabloc_particle_filter/launch/yabloc_particle_filter.launch.xml @@ -16,6 +16,9 @@ + + + diff --git a/localization/yabloc/yabloc_particle_filter/package.xml b/localization/yabloc/yabloc_particle_filter/package.xml index aabb6009c2148..5db4aa0c29e88 100644 --- a/localization/yabloc/yabloc_particle_filter/package.xml +++ b/localization/yabloc/yabloc_particle_filter/package.xml @@ -6,6 +6,11 @@ YabLoc particle filter package Kento Yabuuchi Masahiro Sakamoto + Yamato Ando + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 ament_cmake_ros diff --git a/localization/yabloc/yabloc_particle_filter/schema/camera_particle_corrector.schema.json b/localization/yabloc/yabloc_particle_filter/schema/camera_particle_corrector.schema.json new file mode 100644 index 0000000000000..a8b4873347f52 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/schema/camera_particle_corrector.schema.json @@ -0,0 +1,77 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for camera_particle_corrector", + "type": "object", + "definitions": { + "camera_particle_corrector": { + "type": "object", + "properties": { + "acceptable_max_delay": { + "type": "number", + "description": "how long to hold the predicted particles", + "default": 1.0 + }, + "visualize": { + "type": "boolean", + "description": "whether publish particles as marker_array or not", + "default": false + }, + "image_size": { + "type": "number", + "description": "image size of debug/cost_map_image", + "default": 800 + }, + "max_range": { + "type": "number", + "description": "width of hierarchical cost map", + "default": 40.0 + }, + "gamma": { + "type": "number", + "description": "gamma value of the intensity gradient of the cost map", + "default": 5.0 + }, + "min_prob": { + "type": "number", + "description": "minimum particle weight the corrector node gives", + "default": 0.1 + }, + "far_weight_gain": { + "type": "number", + "description": "`exp(-far_weight_gain_ * squared_distance_from_camera)` is weight gain. if this is large, the nearby road markings will be more important", + "default": 0.001 + }, + "enabled_at_first": { + "type": "boolean", + "description": "if it is false, this node is not activated at first. you can activate by service call", + "default": true + } + }, + "required": [ + "acceptable_max_delay", + "visualize", + "image_size", + "max_range", + "gamma", + "min_prob", + "far_weight_gain", + "enabled_at_first" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/camera_particle_corrector" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/yabloc/yabloc_particle_filter/schema/gnss_particle_corrector.schema.json b/localization/yabloc/yabloc_particle_filter/schema/gnss_particle_corrector.schema.json new file mode 100644 index 0000000000000..a3990ad333981 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/schema/gnss_particle_corrector.schema.json @@ -0,0 +1,94 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for gnss_particle_corrector", + "type": "object", + "definitions": { + "gnss_particle_corrector": { + "type": "object", + "properties": { + "acceptable_max_delay": { + "type": "number", + "description": "how long to hold the predicted particles", + "default": 1.0 + }, + "visualize": { + "type": "boolean", + "description": "whether publish particles as marker_array or not", + "default": false + }, + "mahalanobis_distance_threshold": { + "type": "number", + "description": "if the Mahalanobis distance to the GNSS for particle exceeds this, the correction skips.", + "default": 30.0 + }, + "for_fixed/max_weight": { + "type": "number", + "description": "gnss weight distribution used when observation is fixed", + "default": 5.0 + }, + "for_fixed/flat_radius": { + "type": "number", + "description": "gnss weight distribution used when observation is fixed", + "default": 0.5 + }, + "for_fixed/max_radius": { + "type": "number", + "description": "gnss weight distribution used when observation is fixed", + "default": 10.0 + }, + "for_fixed/min_weight": { + "type": "number", + "description": "gnss weight distribution used when observation is fixed", + "default": 0.5 + }, + "for_not_fixed/max_weight": { + "type": "number", + "description": "gnss weight distribution used when observation is not fixed", + "default": 1.0 + }, + "for_not_fixed/flat_radius": { + "type": "number", + "description": "gnss weight distribution used when observation is not fixed", + "default": 5.0 + }, + "for_not_fixed/max_radius": { + "type": "number", + "description": "gnss weight distribution used when observation is not fixed", + "default": 20.0 + }, + "for_not_fixed/min_weight": { + "type": "number", + "description": "gnss weight distribution used when observation is not fixed", + "default": 0.5 + } + }, + "required": [ + "acceptable_max_delay", + "visualize", + "for_fixed/max_weight", + "for_fixed/flat_radius", + "for_fixed/max_radius", + "for_fixed/min_weight", + "for_not_fixed/max_weight", + "for_not_fixed/flat_radius", + "for_not_fixed/max_radius", + "for_not_fixed/min_weight" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/gnss_particle_corrector" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/yabloc/yabloc_particle_filter/schema/predictor.schema.json b/localization/yabloc/yabloc_particle_filter/schema/predictor.schema.json new file mode 100644 index 0000000000000..487dffdb00094 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/schema/predictor.schema.json @@ -0,0 +1,71 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for predictor", + "type": "object", + "definitions": { + "predictor": { + "type": "object", + "properties": { + "visualize": { + "type": "boolean", + "description": "whether particles are also published in visualization_msgs or not", + "default": true + }, + "static_linear_covariance": { + "type": "number", + "description": "overriding covariance of `/twist_with_covariance`", + "default": 0.04 + }, + "static_angular_covariance": { + "type": "number", + "description": "overriding covariance of `/twist_with_covariance`", + "default": 0.006 + }, + "resampling_interval_seconds": { + "type": "number", + "description": "the interval of particle resampling", + "default": 1.0 + }, + "num_of_particles": { + "type": "number", + "description": "the number of particles", + "default": 500 + }, + "prediction_rate": { + "type": "number", + "description": "frequency of forecast updates, in Hz", + "default": 50.0 + }, + "cov_xx_yy": { + "type": "array", + "description": "the covariance of initial pose", + "default": [2.0, 0.25] + } + }, + "required": [ + "visualize", + "static_linear_covariance", + "static_angular_covariance", + "resampling_interval_seconds", + "num_of_particles", + "prediction_rate", + "cov_xx_yy" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/predictor" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp b/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp index b16905ba19b41..764ef88bbde18 100644 --- a/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp @@ -52,7 +52,12 @@ Predictor::Predictor() auto on_initial = std::bind(&Predictor::on_initial_pose, this, _1); auto on_twist_cov = std::bind(&Predictor::on_twist_cov, this, _1); auto on_particle = std::bind(&Predictor::on_weighted_particles, this, _1); - auto on_height = [this](std_msgs::msg::Float32 m) -> void { this->ground_height_ = m.data; }; + auto on_height = [this](std_msgs::msg::Float32::ConstSharedPtr msg) -> void { + this->ground_height_ = msg->data; + }; + auto on_ekf_pose = [this](PoseCovStamped::ConstSharedPtr msg) -> void { + this->latest_ekf_pose_ptr_ = msg; + }; initialpose_sub_ = create_subscription("~/input/initialpose", 1, on_initial); particles_sub_ = @@ -60,6 +65,7 @@ Predictor::Predictor() height_sub_ = create_subscription("~/input/height", 10, on_height); twist_cov_sub_ = create_subscription("~/input/twist_with_covariance", 10, on_twist_cov); + ekf_pose_sub_ = create_subscription("~/input/ekf_pose", 10, on_ekf_pose); // Timer callback const double prediction_rate = declare_parameter("prediction_rate"); @@ -67,12 +73,41 @@ Predictor::Predictor() timer_ = rclcpp::create_timer( this, this->get_clock(), rclcpp::Rate(prediction_rate).period(), std::move(on_timer)); + // Service server + using std::placeholders::_2; + auto on_trigger_service = std::bind(&Predictor::on_trigger_service, this, _1, _2); + yabloc_trigger_server_ = create_service("~/yabloc_trigger_srv", on_trigger_service); + // Optional modules if (declare_parameter("visualize", false)) { visualizer_ptr_ = std::make_unique(*this); } } +void Predictor::on_trigger_service( + SetBool::Request::ConstSharedPtr request, SetBool::Response::SharedPtr response) +{ + if (request->data) { + RCLCPP_INFO_STREAM(get_logger(), "yabloc particle filter is activated"); + } else { + RCLCPP_INFO_STREAM(get_logger(), "yabloc particle filter is deactivated"); + } + + const bool before_activated_ = yabloc_activated_; + yabloc_activated_ = request->data; + response->success = true; + + if (yabloc_activated_ && (!before_activated_)) { + RCLCPP_INFO_STREAM(get_logger(), "restart particle filter"); + if (latest_ekf_pose_ptr_) { + on_initial_pose(latest_ekf_pose_ptr_); + } else { + yabloc_activated_ = false; + response->success = false; + } + } +} + void Predictor::on_initial_pose(const PoseCovStamped::ConstSharedPtr initialpose) { // Publish initial pose marker @@ -181,6 +216,10 @@ void Predictor::on_timer() { // ========================================================================== // Pre-check section + // Return if yabloc is not activated + if (!yabloc_activated_) { + return; + } // Return if particle_array is not initialized yet if (!particle_array_opt_.has_value()) { return; diff --git a/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt b/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt index 164f280becae8..91b272b413c4c 100644 --- a/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt +++ b/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt @@ -14,6 +14,9 @@ find_package(PCL REQUIRED COMPONENTS common kdtree) # Sophus find_package(Sophus REQUIRED) +# OpenCV +find_package(OpenCV REQUIRED) + # =================================================== # Executable # Camera @@ -28,6 +31,7 @@ ament_auto_add_executable(${TARGET} target_include_directories(${TARGET} PUBLIC include) target_include_directories(${TARGET} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) target_link_libraries(${TARGET} ${PCL_LIBRARIES} Sophus::Sophus) +ament_target_dependencies(${TARGET} OpenCV) # =================================================== ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/localization/yabloc/yabloc_pose_initializer/README.md b/localization/yabloc/yabloc_pose_initializer/README.md index 907b79c1459ec..33b9788cca3d3 100644 --- a/localization/yabloc/yabloc_pose_initializer/README.md +++ b/localization/yabloc/yabloc_pose_initializer/README.md @@ -59,9 +59,7 @@ Converted model URL ### Parameters -| Name | Type | Description | -| ------------------ | ---- | ----------------------------------------- | -| `angle_resolution` | int | how many divisions of 1 sigma angle range | +{{ json_to_markdown("localization/yabloc/yabloc_pose_initializer/schema/camera_pose_initializer.schema.json") }} ### Services diff --git a/localization/yabloc/yabloc_pose_initializer/package.xml b/localization/yabloc/yabloc_pose_initializer/package.xml index 4e3ca1c1e61d1..afd0d4aa86bcf 100644 --- a/localization/yabloc/yabloc_pose_initializer/package.xml +++ b/localization/yabloc/yabloc_pose_initializer/package.xml @@ -5,6 +5,11 @@ The pose initializer Kento Yabuuchi Masahiro Sakamoto + Yamato Ando + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 ament_cmake_ros @@ -12,8 +17,10 @@ rosidl_default_generators autoware_auto_mapping_msgs + cv_bridge geometry_msgs lanelet2_extension + libopencv-dev rclcpp sensor_msgs tier4_localization_msgs diff --git a/localization/yabloc/yabloc_pose_initializer/schema/camera_pose_initializer.schema.json b/localization/yabloc/yabloc_pose_initializer/schema/camera_pose_initializer.schema.json new file mode 100644 index 0000000000000..26a53e0fe408f --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/schema/camera_pose_initializer.schema.json @@ -0,0 +1,33 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for camera_pose_initializer", + "type": "object", + "definitions": { + "camera_pose_initializer": { + "type": "object", + "properties": { + "angle_resolution": { + "type": "number", + "description": "how many divisions of 1 sigma angle range", + "default": 30 + } + }, + "required": ["angle_resolution"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/camera_pose_initializer" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/map/map_height_fitter/CMakeLists.txt b/map/map_height_fitter/CMakeLists.txt index fb5b3feac6418..8821158c54757 100644 --- a/map/map_height_fitter/CMakeLists.txt +++ b/map/map_height_fitter/CMakeLists.txt @@ -10,8 +10,15 @@ ament_auto_add_library(map_height_fitter SHARED ) target_link_libraries(map_height_fitter ${PCL_LIBRARIES}) +# When adding `lanelet2_extension` to package.xml, many warnings are generated. +# These are treated as errors in compile, so pedantic warnings are disabled for this package. +target_compile_options(map_height_fitter PRIVATE -Wno-pedantic) + ament_auto_add_executable(node src/node.cpp ) -ament_auto_package(INSTALL_TO_SHARE launch) +ament_auto_package( + INSTALL_TO_SHARE + launch + config) diff --git a/map/map_height_fitter/README.md b/map/map_height_fitter/README.md index 9b768f87c432b..7da70a0ff7766 100644 --- a/map/map_height_fitter/README.md +++ b/map/map_height_fitter/README.md @@ -4,8 +4,18 @@ This library fits the given point with the ground of the point cloud map. The map loading operation is switched by the parameter `enable_partial_load` of the node specified by `map_loader_name`. The node using this library must use multi thread executor. -| Interface | Local Name | Description | -| ------------ | ------------------ | ---------------------------------------- | -| Parameter | map_loader_name | The point cloud map loader name. | -| Subscription | ~/pointcloud_map | The topic name to load the whole map | -| Client | ~/partial_map_load | The service name to load the partial map | +## Parameters + +{{ json_to_markdown("map/map_height_fitter/schema/map_height_fitter.schema.json") }} + +## Topic subscription + +| Topic Name | Description | +| ---------------- | -------------------------------------------------------------------------------------------- | +| ~/pointcloud_map | The topic containing the whole pointcloud map (only used when `enable_partial_load = false`) | + +## Service client + +| Service Name | Description | +| ------------------ | ----------------------------------- | +| ~/partial_map_load | The service to load the partial map | diff --git a/map/map_height_fitter/config/map_height_fitter.param.yaml b/map/map_height_fitter/config/map_height_fitter.param.yaml new file mode 100644 index 0000000000000..af6a7fddfa554 --- /dev/null +++ b/map/map_height_fitter/config/map_height_fitter.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + map_height_fitter: + map_loader_name: "/map/pointcloud_map_loader" + target: "pointcloud_map" diff --git a/map/map_height_fitter/launch/map_height_fitter.launch.xml b/map/map_height_fitter/launch/map_height_fitter.launch.xml index 2edbdb831590d..353f2151ee172 100644 --- a/map/map_height_fitter/launch/map_height_fitter.launch.xml +++ b/map/map_height_fitter/launch/map_height_fitter.launch.xml @@ -1,10 +1,13 @@ + + - + + diff --git a/map/map_height_fitter/package.xml b/map/map_height_fitter/package.xml index f50eba9218d67..7f384aa43ec7b 100644 --- a/map/map_height_fitter/package.xml +++ b/map/map_height_fitter/package.xml @@ -7,14 +7,21 @@ Takagi, Isamu Yamato Ando Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Takagi, Isamu ament_cmake autoware_cmake + autoware_auto_mapping_msgs autoware_map_msgs geometry_msgs + lanelet2_extension libpcl-common pcl_conversions rclcpp diff --git a/map/map_height_fitter/schema/map_height_fitter.schema.json b/map/map_height_fitter/schema/map_height_fitter.schema.json new file mode 100644 index 0000000000000..dc59eb76d9685 --- /dev/null +++ b/map/map_height_fitter/schema/map_height_fitter.schema.json @@ -0,0 +1,43 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for map_height_fitter", + "type": "object", + "definitions": { + "map_height_fitter": { + "type": "object", + "properties": { + "map_loader_name": { + "type": "string", + "description": "Node name of the map loader from which this map_height_fitter will retrieve its parameters", + "default": "/map/pointcloud_map_loader" + }, + "target": { + "type": "string", + "description": "Target map to fit (choose from 'pointcloud_map', 'vector_map')", + "default": "pointcloud_map" + } + }, + "required": ["map_loader_name", "target"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "type": "object", + "properties": { + "map_height_fitter": { + "$ref": "#/definitions/map_height_fitter" + } + } + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/map/map_height_fitter/src/map_height_fitter.cpp b/map/map_height_fitter/src/map_height_fitter.cpp index 1eb1edd04a68a..095574125d9a0 100644 --- a/map/map_height_fitter/src/map_height_fitter.cpp +++ b/map/map_height_fitter/src/map_height_fitter.cpp @@ -14,10 +14,16 @@ #include "map_height_fitter/map_height_fitter.hpp" +#include +#include + +#include #include #include #include +#include +#include #include #include #include @@ -31,52 +37,74 @@ struct MapHeightFitter::Impl static constexpr char enable_partial_load[] = "enable_partial_load"; explicit Impl(rclcpp::Node * node); - void on_map(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); + void on_pcd_map(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); + void on_vector_map(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); bool get_partial_point_cloud_map(const Point & point); - double get_ground_height(const tf2::Vector3 & point) const; + double get_ground_height(const Point & point) const; std::optional fit(const Point & position, const std::string & frame); tf2::BufferCore tf2_buffer_; tf2_ros::TransformListener tf2_listener_; std::string map_frame_; - pcl::PointCloud::Ptr map_cloud_; rclcpp::Node * node_; + std::string fit_target_; + + // for fitting by pointcloud_map_loader rclcpp::CallbackGroup::SharedPtr group_; - rclcpp::Client::SharedPtr cli_map_; - rclcpp::Subscription::SharedPtr sub_map_; - rclcpp::AsyncParametersClient::SharedPtr params_map_loader_; + pcl::PointCloud::Ptr map_cloud_; + rclcpp::Client::SharedPtr cli_pcd_map_; + rclcpp::Subscription::SharedPtr sub_pcd_map_; + rclcpp::AsyncParametersClient::SharedPtr params_pcd_map_loader_; + + // for fitting by vector_map_loader + lanelet::LaneletMapPtr vector_map_; + rclcpp::Subscription::SharedPtr sub_vector_map_; }; MapHeightFitter::Impl::Impl(rclcpp::Node * node) : tf2_listener_(tf2_buffer_), node_(node) { - const auto callback = [this](const std::shared_future> & future) { - bool partial_load = false; - for (const auto & param : future.get()) { - if (param.get_name() == enable_partial_load) { - partial_load = param.as_bool(); - } - } - - if (partial_load) { - group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - cli_map_ = node_->create_client( - "~/partial_map_load", rmw_qos_profile_default, group_); - } else { - const auto durable_qos = rclcpp::QoS(1).transient_local(); - sub_map_ = node_->create_subscription( - "~/pointcloud_map", durable_qos, - std::bind(&MapHeightFitter::Impl::on_map, this, std::placeholders::_1)); - } - }; - - const auto map_loader_name = node->declare_parameter("map_loader_name"); - params_map_loader_ = rclcpp::AsyncParametersClient::make_shared(node, map_loader_name); - params_map_loader_->wait_for_service(); - params_map_loader_->get_parameters({enable_partial_load}, callback); + fit_target_ = node->declare_parameter("map_height_fitter.target"); + if (fit_target_ == "pointcloud_map") { + const auto callback = + [this](const std::shared_future> & future) { + bool partial_load = false; + for (const auto & param : future.get()) { + if (param.get_name() == enable_partial_load) { + partial_load = param.as_bool(); + } + } + + if (partial_load) { + group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + cli_pcd_map_ = node_->create_client( + "~/partial_map_load", rmw_qos_profile_default, group_); + } else { + const auto durable_qos = rclcpp::QoS(1).transient_local(); + sub_pcd_map_ = node_->create_subscription( + "~/pointcloud_map", durable_qos, + std::bind(&MapHeightFitter::Impl::on_pcd_map, this, std::placeholders::_1)); + } + }; + + const auto map_loader_name = + node->declare_parameter("map_height_fitter.map_loader_name"); + params_pcd_map_loader_ = rclcpp::AsyncParametersClient::make_shared(node, map_loader_name); + params_pcd_map_loader_->wait_for_service(); + params_pcd_map_loader_->get_parameters({enable_partial_load}, callback); + + } else if (fit_target_ == "vector_map") { + const auto durable_qos = rclcpp::QoS(1).transient_local(); + sub_vector_map_ = node_->create_subscription( + "~/vector_map", durable_qos, + std::bind(&MapHeightFitter::Impl::on_vector_map, this, std::placeholders::_1)); + + } else { + throw std::runtime_error("invalid fit_target"); + } } -void MapHeightFitter::Impl::on_map(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) +void MapHeightFitter::Impl::on_pcd_map(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { map_frame_ = msg->header.frame_id; map_cloud_ = pcl::PointCloud::Ptr(new pcl::PointCloud); @@ -87,11 +115,11 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point) { const auto logger = node_->get_logger(); - if (!cli_map_) { + if (!cli_pcd_map_) { RCLCPP_WARN_STREAM(logger, "Partial map loading in pointcloud_map_loader is not enabled"); return false; } - if (!cli_map_->service_is_ready()) { + if (!cli_pcd_map_->service_is_ready()) { RCLCPP_WARN_STREAM(logger, "Partial map loading in pointcloud_map_loader is not ready"); return false; } @@ -101,11 +129,11 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point) req->area.center_y = point.y; req->area.radius = 50; - RCLCPP_INFO(logger, "Send request to map_loader"); - auto future = cli_map_->async_send_request(req); + RCLCPP_DEBUG(logger, "Send request to map_loader"); + auto future = cli_pcd_map_->async_send_request(req); auto status = future.wait_for(std::chrono::seconds(1)); while (status != std::future_status::ready) { - RCLCPP_INFO(logger, "waiting response"); + RCLCPP_DEBUG(logger, "waiting response"); if (!rclcpp::ok()) { return false; } @@ -113,7 +141,7 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point) } const auto res = future.get(); - RCLCPP_INFO( + RCLCPP_DEBUG( logger, "Loaded partial pcd map from map_loader (grid size: %lu)", res->new_pointcloud_with_ids.size()); @@ -134,72 +162,121 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point) return true; } -double MapHeightFitter::Impl::get_ground_height(const tf2::Vector3 & point) const +void MapHeightFitter::Impl::on_vector_map( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) { - const double x = point.getX(); - const double y = point.getY(); - - // find distance d to closest point - double min_dist2 = INFINITY; - for (const auto & p : map_cloud_->points) { - const double dx = x - p.x; - const double dy = y - p.y; - const double sd = (dx * dx) + (dy * dy); - min_dist2 = std::min(min_dist2, sd); - } + vector_map_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg(*msg, vector_map_); + map_frame_ = msg->header.frame_id; +} + +double MapHeightFitter::Impl::get_ground_height(const Point & point) const +{ + const auto logger = node_->get_logger(); + + const double x = point.x; + const double y = point.y; - // find lowest height within radius (d+1.0) - const double radius2 = std::pow(std::sqrt(min_dist2) + 1.0, 2.0); double height = INFINITY; - for (const auto & p : map_cloud_->points) { - const double dx = x - p.x; - const double dy = y - p.y; - const double sd = (dx * dx) + (dy * dy); - if (sd < radius2) { - height = std::min(height, static_cast(p.z)); + if (fit_target_ == "pointcloud_map") { + // find distance d to closest point + double min_dist2 = INFINITY; + for (const auto & p : map_cloud_->points) { + const double dx = x - p.x; + const double dy = y - p.y; + const double sd = (dx * dx) + (dy * dy); + min_dist2 = std::min(min_dist2, sd); + } + + // find lowest height within radius (d+1.0) + const double radius2 = std::pow(std::sqrt(min_dist2) + 1.0, 2.0); + + for (const auto & p : map_cloud_->points) { + const double dx = x - p.x; + const double dy = y - p.y; + const double sd = (dx * dx) + (dy * dy); + if (sd < radius2) { + height = std::min(height, static_cast(p.z)); + } + } + } else if (fit_target_ == "vector_map") { + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(vector_map_); + + geometry_msgs::msg::Pose pose; + pose.position.x = x; + pose.position.y = y; + pose.position.z = 0.0; + lanelet::ConstLanelet closest_lanelet; + const bool result = + lanelet::utils::query::getClosestLanelet(all_lanelets, pose, &closest_lanelet); + if (!result) { + RCLCPP_WARN_STREAM(logger, "failed to get closest lanelet"); + return point.z; } + height = closest_lanelet.centerline().back().z(); } - return std::isfinite(height) ? height : point.getZ(); + return std::isfinite(height) ? height : point.z; } std::optional MapHeightFitter::Impl::fit(const Point & position, const std::string & frame) { const auto logger = node_->get_logger(); - tf2::Vector3 point(position.x, position.y, position.z); + RCLCPP_INFO_STREAM(logger, "fit_target: " << fit_target_ << ", frame: " << frame); - RCLCPP_INFO(logger, "original point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ()); + Point point; + point.x = position.x; + point.y = position.y; + point.z = position.z; - if (cli_map_) { - if (!get_partial_point_cloud_map(position)) { + RCLCPP_DEBUG(logger, "original point: %.3f %.3f %.3f", point.x, point.y, point.z); + + // prepare data + if (fit_target_ == "pointcloud_map") { + if (cli_pcd_map_) { // if cli_pcd_map_ is available, prepare pointcloud map by partial loading + if (!get_partial_point_cloud_map(position)) { + RCLCPP_WARN_STREAM(logger, "failed to get partial point cloud map"); + return std::nullopt; + } + } // otherwise, pointcloud map should be already prepared by on_pcd_map + if (!map_cloud_) { + RCLCPP_WARN_STREAM(logger, "point cloud map is not ready"); + return std::nullopt; + } + } else if (fit_target_ == "vector_map") { + // vector_map_ should be already prepared by on_vector_map + if (!vector_map_) { + RCLCPP_WARN_STREAM(logger, "vector map is not ready"); return std::nullopt; } + } else { + throw std::runtime_error("invalid fit_target"); } - if (!map_cloud_) { - RCLCPP_WARN_STREAM(logger, "point cloud map is not ready"); + // transform frame to map_frame_ + try { + const auto stamped = tf2_buffer_.lookupTransform(frame, map_frame_, tf2::TimePointZero); + tf2::doTransform(point, point, stamped); + } catch (tf2::TransformException & exception) { + RCLCPP_WARN_STREAM(logger, "failed to lookup transform: " << exception.what()); return std::nullopt; } + // fit height on map_frame_ + point.z = get_ground_height(point); + + // transform map_frame_ to frame try { const auto stamped = tf2_buffer_.lookupTransform(map_frame_, frame, tf2::TimePointZero); - tf2::Transform transform{tf2::Quaternion{}, tf2::Vector3{}}; - tf2::fromMsg(stamped.transform, transform); - point = transform * point; - point.setZ(get_ground_height(point)); - point = transform.inverse() * point; + tf2::doTransform(point, point, stamped); } catch (tf2::TransformException & exception) { RCLCPP_WARN_STREAM(logger, "failed to lookup transform: " << exception.what()); return std::nullopt; } - RCLCPP_INFO(logger, "modified point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ()); + RCLCPP_DEBUG(logger, "modified point: %.3f %.3f %.3f", point.x, point.y, point.z); - Point result; - result.x = point.getX(); - result.y = point.getY(); - result.z = point.getZ(); - return result; + return point; } MapHeightFitter::MapHeightFitter(rclcpp::Node * node) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 65d9594cb7415..8a31ecee50be5 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -20,7 +20,7 @@ NOTE: **We strongly recommend to use divided maps when using large pointcloud ma #### Prerequisites on pointcloud map file(s) -You may provide either a single .pcd file or multiple .pcd files. If you are using multiple PCD data and either of `enable_partial_load`, `enable_differential_load` or `enable_selected_load` are set true, it MUST obey the following rules: +You may provide either a single .pcd file or multiple .pcd files. If you are using multiple PCD data, it MUST obey the following rules: 1. **The pointcloud map should be projected on the same coordinate defined in `map_projection_loader`**, in order to be consistent with the lanelet2 map and other packages that converts between local and geodetic coordinates. For more information, please refer to [the readme of `map_projection_loader`](https://github.com/autowarefoundation/autoware.universe/tree/main/map/map_projection_loader/README.md). 2. **It must be divided by straight lines parallel to the x-axis and y-axis**. The system does not support division by diagonal lines or curved lines. @@ -29,8 +29,6 @@ You may provide either a single .pcd file or multiple .pcd files. If you are usi 5. **All the split maps should not overlap with each other.** 6. **Metadata file should also be provided.** The metadata structure description is provided below. -Note that these rules are not applicable when `enable_partial_load`, `enable_differential_load` and `enable_selected_load` are all set false. In this case, however, you also need to disable dynamic map loading mode for other nodes as well ([ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) and [compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/compare_map_segmentation) as of June 2023). - #### Metadata structure The metadata should look like this: @@ -113,16 +111,7 @@ Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.co ### Parameters -| Name | Type | Description | Default value | -| :---------------------------- | :---------- | :-------------------------------------------------------------------------------- | :------------ | -| enable_whole_load | bool | A flag to enable raw pointcloud map publishing | true | -| enable_downsampled_whole_load | bool | A flag to enable downsampled pointcloud map publishing | false | -| enable_partial_load | bool | A flag to enable partial pointcloud map server | false | -| enable_differential_load | bool | A flag to enable differential pointcloud map server | false | -| enable_selected_load | bool | A flag to enable selected pointcloud map server | false | -| leaf_size | float | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0 | -| pcd_paths_or_directory | std::string | Path(s) to pointcloud map file or directory | | -| pcd_metadata_path | std::string | Path to pointcloud metadata file | | +{{ json_to_markdown("map/map_loader/schema/pointcloud_map_loader.schema.json") }} ### Interfaces @@ -159,10 +148,7 @@ Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tie ### Parameters -| Name | Type | Description | Default value | -| :--------------------- | :---------- | :----------------------------------------------- | :------------ | -| center_line_resolution | double | Define the resolution of the lanelet center line | 5.0 | -| lanelet2_map_path | std::string | The lanelet2 map path | None | +{{ json_to_markdown("map/map_loader/schema/lanelet2_map_loader.schema.json") }} --- diff --git a/map/map_loader/config/lanelet2_map_loader.param.yaml b/map/map_loader/config/lanelet2_map_loader.param.yaml index 24d2b0e8ed7a8..b830e038f1de2 100755 --- a/map/map_loader/config/lanelet2_map_loader.param.yaml +++ b/map/map_loader/config/lanelet2_map_loader.param.yaml @@ -1,3 +1,4 @@ /**: ros__parameters: center_line_resolution: 5.0 # [m] + lanelet2_map_path: $(var lanelet2_map_path) # The lanelet2 map path diff --git a/map/map_loader/config/pointcloud_map_loader.param.yaml b/map/map_loader/config/pointcloud_map_loader.param.yaml index ba4c032d3e514..180a3e6f9a2e5 100644 --- a/map/map_loader/config/pointcloud_map_loader.param.yaml +++ b/map/map_loader/config/pointcloud_map_loader.param.yaml @@ -3,8 +3,9 @@ enable_whole_load: true enable_downsampled_whole_load: false enable_partial_load: true - enable_differential_load: true enable_selected_load: false # only used when downsample_whole_load enabled leaf_size: 3.0 # downsample leaf size [m] + pcd_paths_or_directory: [$(var pcd_paths_or_directory)] # Path to the pointcloud map file or directory + pcd_metadata_path: $(var pcd_metadata_path) # Path to pointcloud metadata file diff --git a/map/map_loader/launch/lanelet2_map_loader.launch.xml b/map/map_loader/launch/lanelet2_map_loader.launch.xml index b24ddae3a53e5..ea0157620ce43 100644 --- a/map/map_loader/launch/lanelet2_map_loader.launch.xml +++ b/map/map_loader/launch/lanelet2_map_loader.launch.xml @@ -5,13 +5,12 @@ - + - - + diff --git a/map/map_loader/launch/pointcloud_map_loader.launch.xml b/map/map_loader/launch/pointcloud_map_loader.launch.xml index 9901d04df5645..3e447456bb623 100644 --- a/map/map_loader/launch/pointcloud_map_loader.launch.xml +++ b/map/map_loader/launch/pointcloud_map_loader.launch.xml @@ -7,8 +7,6 @@ - - - + diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index 5230d4bd03214..a9b657f843447 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -5,9 +5,14 @@ 0.1.0 The map_loader package Ryohsuke Mitsudome + Yamato Ando Ryu Yamamoto Koji Minoda Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda Apache License 2.0 diff --git a/map/map_loader/schema/lanelet2_map_loader.schema.json b/map/map_loader/schema/lanelet2_map_loader.schema.json new file mode 100644 index 0000000000000..fa2b4d363ff92 --- /dev/null +++ b/map/map_loader/schema/lanelet2_map_loader.schema.json @@ -0,0 +1,38 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for lanelet2 map loader Node", + "type": "object", + "definitions": { + "lanelet2_map_loader": { + "type": "object", + "properties": { + "center_line_resolution": { + "type": "number", + "description": "Resolution of the Lanelet center line [m]", + "default": "5.0" + }, + "lanelet2_map_path": { + "type": "string", + "description": "The lanelet2 map path pointing to the .osm file", + "default": "" + } + }, + "required": ["center_line_resolution", "lanelet2_map_path"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/lanelet2_map_loader" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/map/map_loader/schema/pointcloud_map_loader.schema.json b/map/map_loader/schema/pointcloud_map_loader.schema.json new file mode 100644 index 0000000000000..03cf10b9c5423 --- /dev/null +++ b/map/map_loader/schema/pointcloud_map_loader.schema.json @@ -0,0 +1,71 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for point cloud map loader Node", + "type": "object", + "definitions": { + "pointcloud_map_loader": { + "type": "object", + "properties": { + "enable_whole_load": { + "type": "boolean", + "description": "Enable raw pointcloud map publishing", + "default": true + }, + "enable_downsampled_whole_load": { + "type": "boolean", + "description": "Enable downsampled pointcloud map publishing", + "default": false + }, + "enable_partial_load": { + "type": "boolean", + "description": "Enable partial pointcloud map server", + "default": true + }, + "enable_selected_load": { + "type": "boolean", + "description": "Enable selected pointcloud map server", + "default": false + }, + "leaf_size": { + "type": "number", + "description": "Downsampling leaf size (only used when enable_downsampled_whole_load is set true)", + "default": 3.0 + }, + "pcd_paths_or_directory": { + "type": "array", + "description": "Path(s) to pointcloud map file or directory", + "default": [] + }, + "pcd_metadata_path": { + "type": "string", + "description": "Path to pointcloud metadata file", + "default": "" + } + }, + "required": [ + "enable_whole_load", + "enable_downsampled_whole_load", + "enable_partial_load", + "enable_selected_load", + "leaf_size", + "pcd_paths_or_directory", + "pcd_metadata_path" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/pointcloud_map_loader" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_local_projector.hpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_local_projector.hpp new file mode 100644 index 0000000000000..225445d17bfa1 --- /dev/null +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_local_projector.hpp @@ -0,0 +1,41 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LANELET2_MAP_LOADER__LANELET2_LOCAL_PROJECTOR_HPP_ +#define LANELET2_MAP_LOADER__LANELET2_LOCAL_PROJECTOR_HPP_ + +#include + +namespace lanelet::projection +{ + +class LocalProjector : public Projector +{ +public: + LocalProjector() : Projector(Origin(GPSPoint{})) {} + + BasicPoint3d forward(const GPSPoint & gps) const override // NOLINT + { + return BasicPoint3d{0.0, 0.0, gps.ele}; + } + + GPSPoint reverse(const BasicPoint3d & point) const override + { + return GPSPoint{0.0, 0.0, point.z()}; + } +}; + +} // namespace lanelet::projection + +#endif // LANELET2_MAP_LOADER__LANELET2_LOCAL_PROJECTOR_HPP_ diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index 259c168edcc5c..617f3dd503ce0 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -33,6 +33,8 @@ #include "map_loader/lanelet2_map_loader_node.hpp" +#include "lanelet2_local_projector.hpp" + #include #include #include @@ -59,8 +61,8 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options sub_map_projector_info_, [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { on_map_projector_info(msg); }); - declare_parameter("lanelet2_map_path", ""); - declare_parameter("center_line_resolution", 5.0); + declare_parameter("lanelet2_map_path"); + declare_parameter("center_line_resolution"); } void Lanelet2MapLoaderNode::on_map_projector_info( @@ -72,6 +74,7 @@ void Lanelet2MapLoaderNode::on_map_projector_info( // load map from file const auto map = load_map(lanelet2_filename, *msg); if (!map) { + RCLCPP_ERROR(get_logger(), "Failed to load lanelet2_map. Not published."); return; } @@ -85,6 +88,7 @@ void Lanelet2MapLoaderNode::on_map_projector_info( pub_map_bin_ = create_publisher("output/lanelet2_map", rclcpp::QoS{1}.transient_local()); pub_map_bin_->publish(map_bin_msg); + RCLCPP_INFO(get_logger(), "Succeeded to load lanelet2_map. Map is published."); } lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( @@ -100,10 +104,15 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( return map; } } else { - // Use MGRSProjector as parser - lanelet::projection::MGRSProjector projector{}; + const lanelet::projection::LocalProjector projector; const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); + if (!errors.empty()) { + for (const auto & error : errors) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error); + } + } + // overwrite local_x, local_y for (lanelet::Point3d point : map->pointLayer) { if (point.hasAttribute("local_x")) { diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index 082dc95d6500a..bdfbcf904cf36 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -71,7 +71,7 @@ Lanelet2MapVisualizationNode::Lanelet2MapVisualizationNode(const rclcpp::NodeOpt { using std::placeholders::_1; - viz_lanelets_centerline_ = this->declare_parameter("viz_lanelets_centerline", true); + viz_lanelets_centerline_ = true; sub_map_bin_ = this->create_subscription( "input/lanelet2_map", rclcpp::QoS{1}.transient_local(), @@ -96,8 +96,10 @@ void Lanelet2MapVisualizationNode::onMapBin( lanelet::ConstLanelets crosswalk_lanelets = lanelet::utils::query::crosswalkLanelets(all_lanelets); lanelet::ConstLineStrings3d partitions = lanelet::utils::query::getAllPartitions(viz_lanelet_map); - lanelet::ConstLineStrings3d pedestrian_markings = - lanelet::utils::query::getAllPedestrianMarkings(viz_lanelet_map); + lanelet::ConstLineStrings3d pedestrian_polygon_markings = + lanelet::utils::query::getAllPedestrianPolygonMarkings(viz_lanelet_map); + lanelet::ConstLineStrings3d pedestrian_line_markings = + lanelet::utils::query::getAllPedestrianLineMarkings(viz_lanelet_map); lanelet::ConstLanelets walkway_lanelets = lanelet::utils::query::walkwayLanelets(all_lanelets); std::vector stop_lines = lanelet::utils::query::stopLinesLanelets(road_lanelets); @@ -179,8 +181,12 @@ void Lanelet2MapVisualizationNode::onMapBin( &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( "crosswalk_lanelets", crosswalk_lanelets, cl_cross)); insertMarkerArray( - &map_marker_array, lanelet::visualization::pedestrianMarkingsAsMarkerArray( - pedestrian_markings, cl_pedestrian_markings)); + &map_marker_array, lanelet::visualization::pedestrianPolygonMarkingsAsMarkerArray( + pedestrian_polygon_markings, cl_pedestrian_markings)); + + insertMarkerArray( + &map_marker_array, lanelet::visualization::pedestrianLineMarkingsAsMarkerArray( + pedestrian_line_markings, cl_pedestrian_markings)); insertMarkerArray( &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( "walkway_lanelets", walkway_lanelets, cl_cross)); @@ -215,6 +221,12 @@ void Lanelet2MapVisualizationNode::onMapBin( insertMarkerArray( &map_marker_array, lanelet::visualization::autowareTrafficLightsAsMarkerArray(aw_tl_reg_elems, cl_trafficlights)); + insertMarkerArray( + &map_marker_array, lanelet::visualization::generateTrafficLightRegulatoryElementIdMaker( + road_lanelets, cl_trafficlights)); + insertMarkerArray( + &map_marker_array, lanelet::visualization::generateTrafficLightRegulatoryElementIdMaker( + crosswalk_lanelets, cl_trafficlights)); insertMarkerArray( &map_marker_array, lanelet::visualization::generateTrafficLightIdMaker(aw_tl_reg_elems, cl_trafficlights)); @@ -224,6 +236,9 @@ void Lanelet2MapVisualizationNode::onMapBin( insertMarkerArray( &map_marker_array, lanelet::visualization::generateLaneletIdMarker(road_lanelets, cl_lanelet_id)); + insertMarkerArray( + &map_marker_array, lanelet::visualization::generateLaneletIdMarker( + crosswalk_lanelets, cl_lanelet_id, "crosswalk_lanelet_id")); insertMarkerArray( &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( "shoulder_road_lanelets", shoulder_lanelets, cl_shoulder)); diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp index b4aa930831a04..a66f9ee99534c 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp @@ -74,7 +74,7 @@ sensor_msgs::msg::PointCloud2 PointcloudMapLoaderModule::loadPCDFiles( for (size_t i = 0; i < pcd_paths.size(); ++i) { auto & path = pcd_paths[i]; if (i % 50 == 0) { - RCLCPP_INFO_STREAM( + RCLCPP_DEBUG_STREAM( logger_, fmt::format("Load {} ({} out of {})", path, i + 1, pcd_paths.size())); } diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index a2d9307084545..bacbabe60a719 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -53,7 +53,6 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt bool enable_whole_load = declare_parameter("enable_whole_load"); bool enable_downsample_whole_load = declare_parameter("enable_downsampled_whole_load"); bool enable_partial_load = declare_parameter("enable_partial_load"); - bool enable_differential_load = declare_parameter("enable_differential_load"); bool enable_selected_load = declare_parameter("enable_selected_load"); if (enable_whole_load) { @@ -68,56 +67,48 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt std::make_unique(this, pcd_paths, publisher_name, true); } - if (enable_partial_load || enable_differential_load || enable_selected_load) { - std::map pcd_metadata_dict; - try { - pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths); - } catch (std::runtime_error & e) { - RCLCPP_ERROR_STREAM(get_logger(), e.what()); - } + std::map pcd_metadata_dict; + try { + pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths); + } catch (std::runtime_error & e) { + RCLCPP_ERROR_STREAM(get_logger(), e.what()); + } - if (enable_partial_load) { - partial_map_loader_ = std::make_unique(this, pcd_metadata_dict); - } + if (enable_partial_load) { + partial_map_loader_ = std::make_unique(this, pcd_metadata_dict); + } - if (enable_differential_load) { - differential_map_loader_ = - std::make_unique(this, pcd_metadata_dict); - } + differential_map_loader_ = std::make_unique(this, pcd_metadata_dict); - if (enable_selected_load) { - selected_map_loader_ = std::make_unique(this, pcd_metadata_dict); - } + if (enable_selected_load) { + selected_map_loader_ = std::make_unique(this, pcd_metadata_dict); } } std::map PointCloudMapLoaderNode::getPCDMetadata( const std::string & pcd_metadata_path, const std::vector & pcd_paths) const { - std::map pcd_metadata_dict; - if (pcd_paths.size() != 1) { - if (!fs::exists(pcd_metadata_path)) { - throw std::runtime_error("PCD metadata file not found: " + pcd_metadata_path); - } - - pcd_metadata_dict = loadPCDMetadata(pcd_metadata_path); + if (fs::exists(pcd_metadata_path)) { + auto pcd_metadata_dict = loadPCDMetadata(pcd_metadata_path); pcd_metadata_dict = replaceWithAbsolutePath(pcd_metadata_dict, pcd_paths); - RCLCPP_INFO_STREAM(get_logger(), "Loaded PCD metadata: " << pcd_metadata_path); - } else { + return pcd_metadata_dict; + } else if (pcd_paths.size() == 1) { // An exception when using a single file PCD map so that the users do not have to provide // a metadata file. // Note that this should ideally be avoided and thus eventually be removed by someone, until // Autoware users get used to handling the PCD file(s) with metadata. - RCLCPP_INFO_STREAM(get_logger(), "Create PCD metadata, as the pointcloud is a single file."); + RCLCPP_DEBUG_STREAM(get_logger(), "Create PCD metadata, as the pointcloud is a single file."); pcl::PointCloud single_pcd; - if (pcl::io::loadPCDFile(pcd_paths[0], single_pcd) == -1) { - throw std::runtime_error("PCD load failed: " + pcd_paths[0]); + const auto & pcd_path = pcd_paths.front(); + if (pcl::io::loadPCDFile(pcd_path, single_pcd) == -1) { + throw std::runtime_error("PCD load failed: " + pcd_path); } PCDFileMetadata metadata = {}; pcl::getMinMax3D(single_pcd, metadata.min, metadata.max); - pcd_metadata_dict[pcd_paths[0]] = metadata; + return std::map{{pcd_path, metadata}}; + } else { + throw std::runtime_error("PCD metadata file not found: " + pcd_metadata_path); } - return pcd_metadata_dict; } std::vector PointCloudMapLoaderNode::getPcdPaths( diff --git a/map/map_loader/test/lanelet2_map_loader_launch.test.py b/map/map_loader/test/lanelet2_map_loader_launch.test.py index 24758a46f75aa..f1aa9e0efe922 100644 --- a/map/map_loader/test/lanelet2_map_loader_launch.test.py +++ b/map/map_loader/test/lanelet2_map_loader_launch.test.py @@ -34,7 +34,7 @@ def generate_test_description(): lanelet2_map_loader = Node( package="map_loader", executable="lanelet2_map_loader", - parameters=[{"lanelet2_map_path": lanelet2_map_path}], + parameters=[{"lanelet2_map_path": lanelet2_map_path, "center_line_resolution": 5.0}], ) context = {} diff --git a/map/map_projection_loader/CMakeLists.txt b/map/map_projection_loader/CMakeLists.txt index 700d468ed4431..f6102a1efa795 100644 --- a/map/map_projection_loader/CMakeLists.txt +++ b/map/map_projection_loader/CMakeLists.txt @@ -59,4 +59,5 @@ endif() ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md index 94b142da3dcf9..848fcfba95f14 100644 --- a/map/map_projection_loader/README.md +++ b/map/map_projection_loader/README.md @@ -28,6 +28,15 @@ sample-map-rosbag projector_type: local ``` +#### Limitation + +The functionality that requires latitude and longitude will become unavailable. + +The currently identified unavailable functionalities are: + +- GNSS localization +- Sending the self-position in latitude and longitude using ADAPI + ### Using MGRS If you want to use MGRS, please specify the MGRS grid as well. @@ -39,6 +48,10 @@ vertical_datum: WGS84 mgrs_grid: 54SUE ``` +#### Limitation + +It cannot be used with maps that span across two or more MGRS grids. Please use it only when it falls within the scope of a single MGRS grid. + ### Using LocalCartesianUTM If you want to use local cartesian UTM, please specify the map origin as well. @@ -69,11 +82,10 @@ map_origin: ## Published Topics -- ~/map_projector_info (tier4_map_msgs/MapProjectorInfo) : Topic for defining map projector information +- `~/map_projector_info` (tier4_map_msgs/MapProjectorInfo) : This topic shows the definition of map projector information ## Parameters -| Name | Type | Description | -| :---------------------- | :---------- | :------------------------------------------------------------------------------- | -| map_projector_info_path | std::string | A path to map_projector_info.yaml (used by default) | -| lanelet2_map_path | std::string | A path to lanelet2 map (used only when `map_projector_info_path` does not exist) | +Note that these parameters are assumed to be passed from launch arguments, and it is not recommended to directly write them in `map_projection_loader.param.yaml`. + +{{ json_to_markdown("map/map_projection_loader/schema/map_projection_loader.schema.json") }} diff --git a/map/map_projection_loader/config/map_projection_loader.param.yaml b/map/map_projection_loader/config/map_projection_loader.param.yaml new file mode 100644 index 0000000000000..6ec300309a308 --- /dev/null +++ b/map/map_projection_loader/config/map_projection_loader.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + map_projector_info_path: $(var map_projector_info_path) + lanelet2_map_path: $(var lanelet2_map_path) diff --git a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp index 16442eb43c740..54e794e2742bf 100644 --- a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp +++ b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp @@ -23,6 +23,8 @@ #include tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & filename); +tier4_map_msgs::msg::MapProjectorInfo load_map_projector_info( + const std::string & yaml_filename, const std::string & lanelet2_map_filename); class MapProjectionLoader : public rclcpp::Node { diff --git a/map/map_projection_loader/launch/map_projection_loader.launch.xml b/map/map_projection_loader/launch/map_projection_loader.launch.xml index fc625e3162911..a6570b69d3498 100644 --- a/map/map_projection_loader/launch/map_projection_loader.launch.xml +++ b/map/map_projection_loader/launch/map_projection_loader.launch.xml @@ -1,9 +1,10 @@ + + - - + diff --git a/map/map_projection_loader/package.xml b/map/map_projection_loader/package.xml index b128c2cf15e15..475881577bd58 100644 --- a/map/map_projection_loader/package.xml +++ b/map/map_projection_loader/package.xml @@ -7,6 +7,11 @@ Koji Minoda Yamato Ando Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 ament_cmake_auto diff --git a/map/map_projection_loader/schema/map_projection_loader.schema.json b/map/map_projection_loader/schema/map_projection_loader.schema.json new file mode 100644 index 0000000000000..bb7fe5d2910ad --- /dev/null +++ b/map/map_projection_loader/schema/map_projection_loader.schema.json @@ -0,0 +1,38 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for map_projection_loader", + "type": "object", + "definitions": { + "map_projection_loader": { + "type": "object", + "properties": { + "map_projector_info_path": { + "type": "string", + "description": "The path where map_projector_info.yaml is located", + "default": "$(var map_projector_info_path)" + }, + "lanelet2_map_path": { + "type": "string", + "description": "The path where the lanelet2 map file (.osm) is located", + "default": "$(var lanelet2_map_path)" + } + }, + "required": ["map_projector_info_path", "lanelet2_map_path"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/map_projection_loader" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 9fdba288601cc..5966baaed8383 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -20,6 +20,7 @@ #include +#include #include tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & filename) @@ -55,28 +56,40 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & fi return msg; } -MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") +tier4_map_msgs::msg::MapProjectorInfo load_map_projector_info( + const std::string & yaml_filename, const std::string & lanelet2_map_filename) { - std::string yaml_filename = this->declare_parameter("map_projector_info_path"); - std::string lanelet2_map_filename = this->declare_parameter("lanelet2_map_path"); - std::ifstream file(yaml_filename); - tier4_map_msgs::msg::MapProjectorInfo msg; - bool use_yaml_file = file.is_open(); - if (use_yaml_file) { - RCLCPP_INFO(this->get_logger(), "Load %s", yaml_filename.c_str()); + if (std::filesystem::exists(yaml_filename)) { + std::cout << "Load " << yaml_filename << std::endl; msg = load_info_from_yaml(yaml_filename); - } else { - RCLCPP_INFO(this->get_logger(), "Load %s", lanelet2_map_filename.c_str()); - RCLCPP_WARN( - this->get_logger(), - "DEPRECATED WARNING: Loading map projection info from lanelet2 map may soon be deleted. " - "Please use map_projector_info.yaml instead. For more info, visit " - "https://github.com/autowarefoundation/autoware.universe/blob/main/map/map_projection_loader/" - "README.md"); + } else if (std::filesystem::exists(lanelet2_map_filename)) { + std::cout << "Load " << lanelet2_map_filename << std::endl; + std::cout + << "DEPRECATED WARNING: Loading map projection info from lanelet2 map may soon be deleted. " + "Please use map_projector_info.yaml instead. For more info, visit " + "https://github.com/autowarefoundation/autoware.universe/blob/main/map/" + "map_projection_loader/" + "README.md" + << std::endl; msg = load_info_from_lanelet2_map(lanelet2_map_filename); + } else { + throw std::runtime_error( + "No map projector info files found. Please provide either " + "map_projector_info.yaml or lanelet2_map.osm"); } + return msg; +} + +MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") +{ + const std::string yaml_filename = this->declare_parameter("map_projector_info_path"); + const std::string lanelet2_map_filename = + this->declare_parameter("lanelet2_map_path"); + + const tier4_map_msgs::msg::MapProjectorInfo msg = + load_map_projector_info(yaml_filename, lanelet2_map_filename); // Publish the message const auto adaptor = component_interface_utils::NodeAdaptor(this); diff --git a/map/map_tf_generator/CMakeLists.txt b/map/map_tf_generator/CMakeLists.txt index 4e6390e0cacb6..e20289766cdda 100644 --- a/map/map_tf_generator/CMakeLists.txt +++ b/map/map_tf_generator/CMakeLists.txt @@ -48,5 +48,6 @@ if(BUILD_TESTING) endif() ament_auto_package(INSTALL_TO_SHARE + config launch ) diff --git a/map/map_tf_generator/README.md b/map/map_tf_generator/README.md new file mode 100644 index 0000000000000..643f4233c06f0 --- /dev/null +++ b/map/map_tf_generator/README.md @@ -0,0 +1,50 @@ +# map_tf_generator + +## Purpose + +The nodes in this package broadcast the `viewer` frame for visualization of the map in RViz. + +Note that there is no module to need the `viewer` frame and this is used only for visualization. + +The following are the supported methods to calculate the position of the `viewer` frame: + +- `pcd_map_tf_generator_node` outputs the geometric center of all points in the PCD. +- `vector_map_tf_generator_node` outputs the geometric center of all points in the point layer. + +## Inner-workings / Algorithms + +## Inputs / Outputs + +### Input + +#### pcd_map_tf_generator + +| Name | Type | Description | +| --------------------- | ------------------------------- | ----------------------------------------------------------------- | +| `/map/pointcloud_map` | `sensor_msgs::msg::PointCloud2` | Subscribe pointcloud map to calculate position of `viewer` frames | + +#### vector_map_tf_generator + +| Name | Type | Description | +| ----------------- | -------------------------------------------- | ------------------------------------------------------------- | +| `/map/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Subscribe vector map to calculate position of `viewer` frames | + +### Output + +| Name | Type | Description | +| ------------ | ------------------------ | ------------------------- | +| `/tf_static` | `tf2_msgs/msg/TFMessage` | Broadcast `viewer` frames | + +## Parameters + +### Node Parameters + +None + +### Core Parameters + +{{ json_to_markdown("map/map_tf_generator/schema/map_tf_generator.schema.json") }} + +## Assumptions / Known limits + +TBD. diff --git a/map/map_tf_generator/Readme.md b/map/map_tf_generator/Readme.md deleted file mode 100644 index 1b858ec12c2c2..0000000000000 --- a/map/map_tf_generator/Readme.md +++ /dev/null @@ -1,53 +0,0 @@ -# map_tf_generator - -## Purpose - -The nodes in this package broadcast the `viewer` frame for visualization of the map in RViz. - -Note that there is no module to need the `viewer` frame and this is used only for visualization. - -The following are the supported methods to calculate the position of the `viewer` frame: - -- `pcd_map_tf_generator_node` outputs the geometric center of all points in the PCD. -- `vector_map_tf_generator_node` outputs the geometric center of all points in the point layer. - -## Inner-workings / Algorithms - -## Inputs / Outputs - -### Input - -#### pcd_map_tf_generator - -| Name | Type | Description | -| --------------------- | ------------------------------- | ----------------------------------------------------------------- | -| `/map/pointcloud_map` | `sensor_msgs::msg::PointCloud2` | Subscribe pointcloud map to calculate position of `viewer` frames | - -#### vector_map_tf_generator - -| Name | Type | Description | -| ----------------- | -------------------------------------------- | ------------------------------------------------------------- | -| `/map/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Subscribe vector map to calculate position of `viewer` frames | - -### Output - -| Name | Type | Description | -| ------------ | ------------------------ | ------------------------- | -| `/tf_static` | `tf2_msgs/msg/TFMessage` | Broadcast `viewer` frames | - -## Parameters - -### Node Parameters - -None - -### Core Parameters - -| Name | Type | Default Value | Explanation | -| -------------- | ------ | ------------- | ------------------------------------- | -| `viewer_frame` | string | viewer | Name of `viewer` frame | -| `map_frame` | string | map | The parent frame name of viewer frame | - -## Assumptions / Known limits - -TBD. diff --git a/map/map_tf_generator/config/map_tf_generator.param.yaml b/map/map_tf_generator/config/map_tf_generator.param.yaml new file mode 100644 index 0000000000000..90790808acace --- /dev/null +++ b/map/map_tf_generator/config/map_tf_generator.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + map_frame: map + viewer_frame: viewer diff --git a/map/map_tf_generator/launch/map_tf_generator.launch.xml b/map/map_tf_generator/launch/map_tf_generator.launch.xml index ab5b515f8e61d..43d487c2bc982 100644 --- a/map/map_tf_generator/launch/map_tf_generator.launch.xml +++ b/map/map_tf_generator/launch/map_tf_generator.launch.xml @@ -1,13 +1,11 @@ - + - - + - - + diff --git a/map/map_tf_generator/package.xml b/map/map_tf_generator/package.xml index 6dc68911699aa..6f08da169309e 100644 --- a/map/map_tf_generator/package.xml +++ b/map/map_tf_generator/package.xml @@ -4,7 +4,13 @@ map_tf_generator 0.1.0 map_tf_generator package as a ROS 2 node - azumi-suzuki + Yamato Ando + Kento Yabuuchi + Masahiro Sakamoto + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 ament_cmake_auto diff --git a/map/map_tf_generator/schema/map_tf_generator.schema.json b/map/map_tf_generator/schema/map_tf_generator.schema.json new file mode 100644 index 0000000000000..e501f7a9678f6 --- /dev/null +++ b/map/map_tf_generator/schema/map_tf_generator.schema.json @@ -0,0 +1,38 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Map Tf Generator", + "type": "object", + "definitions": { + "map_tf_generator": { + "type": "object", + "properties": { + "map_frame": { + "type": "string", + "description": "The parent frame name of viewer frame", + "default": "map" + }, + "viewer_frame": { + "type": "string", + "description": "Name of `viewer` frame", + "default": "viewer" + } + }, + "required": ["map_frame", "viewer_frame"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/map_tf_generator" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/map/map_tf_generator/src/pcd_map_tf_generator_node.cpp b/map/map_tf_generator/src/pcd_map_tf_generator_node.cpp index bc686b17fbcab..69a66cf52c865 100644 --- a/map/map_tf_generator/src/pcd_map_tf_generator_node.cpp +++ b/map/map_tf_generator/src/pcd_map_tf_generator_node.cpp @@ -36,8 +36,8 @@ class PcdMapTFGeneratorNode : public rclcpp::Node explicit PcdMapTFGeneratorNode(const rclcpp::NodeOptions & options) : Node("pcd_map_tf_generator", options) { - map_frame_ = declare_parameter("map_frame", "map"); - viewer_frame_ = declare_parameter("viewer_frame", "viewer"); + map_frame_ = declare_parameter("map_frame"); + viewer_frame_ = declare_parameter("viewer_frame"); sub_ = create_subscription( "pointcloud_map", rclcpp::QoS{1}.transient_local(), diff --git a/map/map_tf_generator/src/vector_map_tf_generator_node.cpp b/map/map_tf_generator/src/vector_map_tf_generator_node.cpp index 055d71cfb2b35..20ca1c037a578 100644 --- a/map/map_tf_generator/src/vector_map_tf_generator_node.cpp +++ b/map/map_tf_generator/src/vector_map_tf_generator_node.cpp @@ -31,8 +31,8 @@ class VectorMapTFGeneratorNode : public rclcpp::Node explicit VectorMapTFGeneratorNode(const rclcpp::NodeOptions & options) : Node("vector_map_tf_generator", options) { - map_frame_ = declare_parameter("map_frame", "map"); - viewer_frame_ = declare_parameter("viewer_frame", "viewer"); + map_frame_ = declare_parameter("map_frame"); + viewer_frame_ = declare_parameter("viewer_frame"); sub_ = create_subscription( "vector_map", rclcpp::QoS{1}.transient_local(), diff --git a/map/util/lanelet2_map_preprocessor/CMakeLists.txt b/map/util/lanelet2_map_preprocessor/CMakeLists.txt index 8a6b16c05011b..982a262455efa 100644 --- a/map/util/lanelet2_map_preprocessor/CMakeLists.txt +++ b/map/util/lanelet2_map_preprocessor/CMakeLists.txt @@ -29,5 +29,6 @@ if(BUILD_TESTING) endif() ament_auto_package(INSTALL_TO_SHARE + config launch ) diff --git a/map/util/lanelet2_map_preprocessor/config/fix_z_value_by_pcd.param.yaml b/map/util/lanelet2_map_preprocessor/config/fix_z_value_by_pcd.param.yaml new file mode 100644 index 0000000000000..de86ef7936d74 --- /dev/null +++ b/map/util/lanelet2_map_preprocessor/config/fix_z_value_by_pcd.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + llt_map_path: $(var llt_map_path) + pcd_map_path: $(var pcd_map_path) + llt_output_path: $(var llt_output_path) diff --git a/map/util/lanelet2_map_preprocessor/config/transform_maps.param.yaml b/map/util/lanelet2_map_preprocessor/config/transform_maps.param.yaml new file mode 100644 index 0000000000000..548c28055c834 --- /dev/null +++ b/map/util/lanelet2_map_preprocessor/config/transform_maps.param.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + llt_map_path: $(var llt_map_path) + pcd_map_path: $(var pcd_map_path) + llt_output_path: $(var llt_output_path) + pcd_output_path: $(var pcd_output_path) + x: 0.0 + y: 0.0 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 diff --git a/map/util/lanelet2_map_preprocessor/launch/fix_z_value_by_pcd.launch.xml b/map/util/lanelet2_map_preprocessor/launch/fix_z_value_by_pcd.launch.xml index 82021ff25d240..5b2aa2e425184 100644 --- a/map/util/lanelet2_map_preprocessor/launch/fix_z_value_by_pcd.launch.xml +++ b/map/util/lanelet2_map_preprocessor/launch/fix_z_value_by_pcd.launch.xml @@ -1,12 +1,6 @@ - - - - - - - + diff --git a/map/util/lanelet2_map_preprocessor/launch/transform_maps.launch.xml b/map/util/lanelet2_map_preprocessor/launch/transform_maps.launch.xml index 902ca982dace1..950593771878c 100644 --- a/map/util/lanelet2_map_preprocessor/launch/transform_maps.launch.xml +++ b/map/util/lanelet2_map_preprocessor/launch/transform_maps.launch.xml @@ -1,26 +1,6 @@ - - - - - - - - - - - - - - - - - - - - - + diff --git a/map/util/lanelet2_map_preprocessor/package.xml b/map/util/lanelet2_map_preprocessor/package.xml index 79fce94f87a95..421f04257de67 100644 --- a/map/util/lanelet2_map_preprocessor/package.xml +++ b/map/util/lanelet2_map_preprocessor/package.xml @@ -5,7 +5,15 @@ 0.1.0 The lanelet2_map_preprocessor package Ryohsuke Mitsudome + Yamato Ando + Kento Yabuuchi + Masahiro Sakamoto + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 + Ryohsuke Mitsudome ament_cmake_auto autoware_cmake diff --git a/map/util/lanelet2_map_preprocessor/schema/transform_maps.schema.json b/map/util/lanelet2_map_preprocessor/schema/transform_maps.schema.json new file mode 100644 index 0000000000000..78fb3952d44ce --- /dev/null +++ b/map/util/lanelet2_map_preprocessor/schema/transform_maps.schema.json @@ -0,0 +1,75 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Transforming Maps", + "type": "object", + "definitions": { + "transform_maps": { + "type": "object", + "properties": { + "llt_map_path": { + "type": "string", + "description": "Path pointing to the input lanelet2 file", + "default": "" + }, + "pcd_map_path": { + "type": "string", + "description": "Path pointing to the input point cloud file", + "default": "" + }, + "llt_output_path": { + "type": "string", + "description": "Path pointing to the output lanelet2 file", + "default": "" + }, + "pcd_output_path": { + "type": "string", + "description": "Path pointing to the output point cloud file", + "default": "" + }, + "x": { + "type": "number", + "default": 0.0, + "description": "x factor of Translation vector for transforming maps [m]" + }, + "y": { + "type": "number", + "default": 0.0, + "description": "y factor of Translation vector for transforming maps [m]" + }, + "z": { + "type": "number", + "default": 0.0, + "description": "z factor of Translation vector for transforming maps [m]" + }, + "roll": { + "type": "number", + "default": 0.0, + "description": "roll factor of Rotation vector for transforming maps [rad]" + }, + "pitch": { + "type": "number", + "default": 0.0, + "description": "pitch factor of Rotation vector for transforming maps [rad]" + }, + "yaw": { + "type": "number", + "default": 0.0, + "description": "yaw factor of Rotation vector for transforming maps [rad]" + } + }, + "required": ["x", "y", "z", "roll", "pitch", "yaw"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/transform_maps" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp b/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp index 987f559be1e8e..2317ddb7d9f95 100644 --- a/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp +++ b/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp @@ -107,12 +107,12 @@ int main(int argc, char * argv[]) const auto pcd_map_path = node->declare_parameter("pcd_map_path"); const auto llt_output_path = node->declare_parameter("llt_output_path"); const auto pcd_output_path = node->declare_parameter("pcd_output_path"); - const auto x = node->declare_parameter("x", 0.0); - const auto y = node->declare_parameter("y", 0.0); - const auto z = node->declare_parameter("z", 0.0); - const auto roll = node->declare_parameter("roll", 0.0); - const auto pitch = node->declare_parameter("pitch", 0.0); - const auto yaw = node->declare_parameter("yaw", 0.0); + const auto x = node->declare_parameter("x"); + const auto y = node->declare_parameter("y"); + const auto z = node->declare_parameter("z"); + const auto roll = node->declare_parameter("roll"); + const auto pitch = node->declare_parameter("pitch"); + const auto yaw = node->declare_parameter("yaw"); std::cout << "transforming maps with following parameters" << std::endl << "x " << x << std::endl diff --git a/mkdocs_macros.py b/mkdocs_macros.py index 56292a815d80d..97f76442be491 100644 --- a/mkdocs_macros.py +++ b/mkdocs_macros.py @@ -42,6 +42,8 @@ def format_param_range(param): def extract_parameter_info(parameters, namespace=""): params = [] for k, v in parameters.items(): + if "$ref" in v.keys(): + continue if v["type"] != "object": param = {} param["Name"] = namespace + k 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..90116c4dae7e5 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 aspect ratio 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/bytetrack.param.yaml b/perception/bytetrack/config/bytetrack.param.yaml new file mode 100644 index 0000000000000..e38dbdb4d3efa --- /dev/null +++ b/perception/bytetrack/config/bytetrack.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + track_buffer_length: 30 diff --git a/perception/bytetrack/config/bytetrack_visualizer.param.yaml b/perception/bytetrack/config/bytetrack_visualizer.param.yaml new file mode 100644 index 0000000000000..ad1f6fce301b3 --- /dev/null +++ b/perception/bytetrack/config/bytetrack_visualizer.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + use_raw: false diff --git a/perception/bytetrack/config/kalman_filter.param.yaml b/perception/bytetrack/config/kalman_filter.param.yaml new file mode 100644 index 0000000000000..7078f190c87e6 --- /dev/null +++ b/perception/bytetrack/config/kalman_filter.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/launch/bytetrack.launch.xml b/perception/bytetrack/launch/bytetrack.launch.xml index 25ed56b991c30..3a5a369637453 100644 --- a/perception/bytetrack/launch/bytetrack.launch.xml +++ b/perception/bytetrack/launch/bytetrack.launch.xml @@ -4,21 +4,21 @@ - - + + - + - + 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/data_type.h b/perception/bytetrack/lib/include/data_type.h index 896e021538cae..64e8acc6c1e50 100644 --- a/perception/bytetrack/lib/include/data_type.h +++ b/perception/bytetrack/lib/include/data_type.h @@ -50,7 +50,7 @@ typedef Eigen::Matrix FEATURE; typedef Eigen::Matrix FEATURESS; // typedef std::vector FEATURESS; -// Kalmanfilter +// Kalman Filter // typedef Eigen::Matrix KAL_FILTER; typedef Eigen::Matrix KAL_MEAN; typedef Eigen::Matrix KAL_COVA; 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..f2c5a40ad3c2e 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/kalman_filter.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