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 6b4daaa4b1773..d3d9149cd2c8d 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -4,8 +4,10 @@ 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 @@ -22,10 +24,10 @@ common/grid_map_utils/** maxime.clement@tier4.jp common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp common/motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -common/object_recognition_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yusuke.muramatsu@tier4.jp +common/object_recognition_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yoshi.ri@tier4.jp common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/path_distance_calculator/** isamu.takagi@tier4.jp -common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yusuke.muramatsu@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 @@ -33,14 +35,13 @@ common/signal_processing/** ali.boyali@tier4.jp fumiya.watanabe@tier4.jp kyoichi common/tensorrt_common/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato.hirabayashi@tier4.jp common/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp common/tier4_api_utils/** isamu.takagi@tier4.jp -common/tier4_automatic_goal_rviz_plugin/** dawid.moszynski@robotec.ai shumpei.wakabayashi@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 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 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 @@ -56,16 +57,16 @@ common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomo 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 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 +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 @@ -76,10 +77,11 @@ evaluator/diagnostic_converter/** kyoichi.sugahara@tier4.jp maxime.clement@tier4 evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp evaluator/localization_evaluator/** dominik.jargot@robotec.ai koji.minoda@tier4.jp evaluator/planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp +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/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -launch/tier4_map_launch/** kento.yabuuchi.2@tier4.jp 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 zhe.shen@tier4.jp zulfaqar.azmi@tier4.jp launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp @@ -95,6 +97,7 @@ localization/localization_error_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi. 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 @@ -106,10 +109,10 @@ localization/yabloc/yabloc_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tie localization/yabloc/yabloc_particle_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_pose_initializer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp map/map_height_fitter/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -map/map_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp +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/** ryohsuke.mitsudome@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 @@ -119,14 +122,12 @@ perception/detected_object_validation/** dai.nguyen@tier4.jp shunsuke.miura@tier perception/detection_by_tracker/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/elevation_map_loader/** kosuke.takeuchi@tier4.jp taichi.higashide@tier4.jp perception/euclidean_cluster/** yukihiro.saito@tier4.jp -perception/front_vehicle_velocity_estimator/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp perception/ground_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp -perception/heatmap_visualizer/** kotaro.uetake@tier4.jp -perception/image_projection_based_fusion/** dai.nguyen@tier4.jp kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp yoshi.ri@tier4.jp 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 perception/multi_object_tracker/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @@ -135,15 +136,14 @@ 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_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 yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp shunsuke.miura@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/traffic_light_arbiter/** kenzo.lobos@tier4.jp shunsuke.miura@tier4.jp @@ -152,18 +152,18 @@ perception/traffic_light_fine_detector/** shunsuke.miura@tier4.jp tao.zhong@tier perception/traffic_light_map_based_detector/** 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_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 +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/** kosuke.takeuchi@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_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/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_path_start_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_crosswalk_module/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yuki.takagi@tier4.jp planning/behavior_velocity_detection_area_module/** kyoichi.sugahara@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @@ -187,19 +187,18 @@ planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hir planning/freespace_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/freespace_planning_algorithms/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/mission_planner/** isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp ryohsuke.mitsudome@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp takamasa.horibe@tier4.jp +planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp planning/objects_of_interest_marker_interface/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp zulfaqar.azmi@tier4.jp planning/obstacle_avoidance_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/obstacle_cruise_planner/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp -planning/obstacle_stop_planner/** berkay@leodrive.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@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 zulfaqar.azmi@tier4.jp -planning/rtc_interface/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp taiki.tanaka@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 @@ -208,16 +207,16 @@ 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/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/** ryu.yamamoto@tier4.jp sensing/vehicle_velocity_converter/** ryu.yamamoto@tier4.jp simulator/dummy_perception_publisher/** yukihiro.saito@tier4.jp @@ -234,15 +233,14 @@ 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/emergency_handler/** makoto.kurihara@tier4.jp ryuta.kambe@tier4.jp tetsuhiro.kawaguchi@tier4.jp system/mrm_comfortable_stop_operator/** makoto.kurihara@tier4.jp tomohito.ando@tier4.jp system/mrm_emergency_stop_operator/** makoto.kurihara@tier4.jp tomohito.ando@tier4.jp system/system_error_monitor/** fumihito.ito@tier4.jp system/system_monitor/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp system/topic_state_monitor/** ryohsuke.mitsudome@tier4.jp system/velodyne_monitor/** fumihito.ito@tier4.jp -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 diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index e3711d413cb68..e9db0d140d947 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -16,7 +16,7 @@ jobs: build-and-test-differential: needs: prevent-no-label-execution if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }} - runs-on: ubuntu-latest + runs-on: [self-hosted, linux, X64] container: ${{ matrix.container }}${{ matrix.container-suffix }} strategy: fail-fast: false @@ -76,7 +76,7 @@ jobs: run: df -h clang-tidy-differential: - runs-on: ubuntu-latest + runs-on: [self-hosted, linux, X64] container: ghcr.io/autowarefoundation/autoware-universe:humble-latest-cuda needs: build-and-test-differential steps: diff --git a/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/CHANGELOG.rst b/common/autoware_overlay_rviz_plugin/autoware_overlay_msgs/CHANGELOG.rst similarity index 100% rename from common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/CHANGELOG.rst rename to common/autoware_overlay_rviz_plugin/autoware_overlay_msgs/CHANGELOG.rst diff --git a/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/CMakeLists.txt b/common/autoware_overlay_rviz_plugin/autoware_overlay_msgs/CMakeLists.txt similarity index 92% rename from common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/CMakeLists.txt rename to common/autoware_overlay_rviz_plugin/autoware_overlay_msgs/CMakeLists.txt index 9e9a8f277fd53..e23a4e755cbc4 100644 --- a/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/CMakeLists.txt +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_msgs/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(rviz_2d_overlay_msgs) +project(autoware_overlay_msgs) if (NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) diff --git a/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/msg/OverlayText.msg b/common/autoware_overlay_rviz_plugin/autoware_overlay_msgs/msg/OverlayText.msg similarity index 100% rename from common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/msg/OverlayText.msg rename to common/autoware_overlay_rviz_plugin/autoware_overlay_msgs/msg/OverlayText.msg diff --git a/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/package.xml b/common/autoware_overlay_rviz_plugin/autoware_overlay_msgs/package.xml similarity index 96% rename from common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/package.xml rename to common/autoware_overlay_rviz_plugin/autoware_overlay_msgs/package.xml index 53396c64aa156..4881b126ffffb 100644 --- a/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/package.xml +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_msgs/package.xml @@ -1,7 +1,7 @@ - rviz_2d_overlay_msgs + autoware_overlay_msgs 1.3.0 Messages describing 2D overlays for RVIZ, extracted/derived from the jsk_visualization package. Team Spatzenhirn diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/CMakeLists.txt b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt similarity index 94% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/CMakeLists.txt rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt index da67a6f63aeae..afb12bffeeaa7 100644 --- a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/CMakeLists.txt +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(awf_2d_overlay_vehicle) +project(autoware_overlay_rviz_plugin) # find dependencies find_package(ament_cmake_auto REQUIRED) @@ -9,7 +9,7 @@ find_package(tier4_planning_msgs REQUIRED) find_package(autoware_perception_msgs REQUIRED) ament_auto_find_build_dependencies() -find_package(rviz_2d_overlay_msgs REQUIRED) +find_package(autoware_overlay_msgs REQUIRED) find_package(rviz_common REQUIRED) find_package(rviz_rendering REQUIRED) @@ -90,7 +90,7 @@ ament_target_dependencies( PUBLIC rviz_common rviz_rendering - rviz_2d_overlay_msgs + autoware_overlay_msgs autoware_auto_vehicle_msgs tier4_planning_msgs autoware_perception_msgs @@ -136,5 +136,5 @@ install( add_definitions(-DQT_NO_KEYWORDS) ament_package( - CONFIG_EXTRAS "awf_2d_overlay_vehicle-extras.cmake" + CONFIG_EXTRAS "autoware_overlay_rviz_plugin-extras.cmake" ) diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/LICENSE b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/LICENSE similarity index 100% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/LICENSE rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/LICENSE 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/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/LICENSE b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/LICENSE similarity index 100% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/LICENSE rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/LICENSE diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Bold.ttf b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Bold.ttf similarity index 100% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Bold.ttf rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Bold.ttf diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Light.ttf b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Light.ttf similarity index 100% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Light.ttf rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Light.ttf diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Medium.ttf b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Medium.ttf similarity index 100% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Medium.ttf rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Medium.ttf diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Regular.ttf b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Regular.ttf similarity index 100% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Regular.ttf rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Regular.ttf diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-SemiBold.ttf b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-SemiBold.ttf similarity index 100% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-SemiBold.ttf rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-SemiBold.ttf diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/arrow.png b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/arrow.png similarity index 100% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/arrow.png rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/arrow.png diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_add.png b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/select_add.png similarity index 100% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_add.png rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/select_add.png 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/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/wheel.png b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/wheel.png similarity index 100% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/wheel.png rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/wheel.png diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/awf_2d_overlay_vehicle-extras.cmake b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin-extras.cmake similarity index 100% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/awf_2d_overlay_vehicle-extras.cmake rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin-extras.cmake diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/gear_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp similarity index 94% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/gear_display.hpp rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp index b289bfc541cee..3fe473d5d5123 100644 --- a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/gear_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp @@ -29,7 +29,7 @@ #include #include -namespace awf_2d_overlay_vehicle +namespace autoware_overlay_rviz_plugin { class GearDisplay @@ -44,6 +44,6 @@ class GearDisplay QColor gray = QColor(194, 194, 194); }; -} // namespace awf_2d_overlay_vehicle +} // namespace autoware_overlay_rviz_plugin #endif // GEAR_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/overlay_text_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/overlay_text_display.hpp similarity index 93% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/overlay_text_display.hpp rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/overlay_text_display.hpp index 9b3ecccf6301c..ccea51b9a5349 100644 --- a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/overlay_text_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/overlay_text_display.hpp @@ -50,7 +50,7 @@ #ifndef OVERLAY_TEXT_DISPLAY_HPP_ #define OVERLAY_TEXT_DISPLAY_HPP_ -#include "rviz_2d_overlay_msgs/msg/overlay_text.hpp" +#include "autoware_overlay_msgs/msg/overlay_text.hpp" #ifndef Q_MOC_RUN #include "overlay_utils.hpp" @@ -69,10 +69,10 @@ #include #endif -namespace awf_2d_overlay_vehicle +namespace autoware_overlay_rviz_plugin { class OverlayTextDisplay -: public rviz_common::RosTopicDisplay +: public rviz_common::RosTopicDisplay { Q_OBJECT public: @@ -80,7 +80,7 @@ class OverlayTextDisplay virtual ~OverlayTextDisplay(); protected: - awf_2d_overlay_vehicle::OverlayObject::SharedPtr overlay_; + autoware_overlay_rviz_plugin::OverlayObject::SharedPtr overlay_; int texture_width_; int texture_height_; @@ -150,8 +150,8 @@ protected Q_SLOTS: void updateLineWidth(); private: - void processMessage(rviz_2d_overlay_msgs::msg::OverlayText::ConstSharedPtr msg) override; + void processMessage(autoware_overlay_msgs::msg::OverlayText::ConstSharedPtr msg) override; }; -} // namespace awf_2d_overlay_vehicle +} // namespace autoware_overlay_rviz_plugin #endif // OVERLAY_TEXT_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/overlay_utils.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/overlay_utils.hpp similarity index 90% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/overlay_utils.hpp rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/overlay_utils.hpp index 1270f15ca80ae..8581628ef0bce 100644 --- a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/overlay_utils.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/overlay_utils.hpp @@ -54,7 +54,7 @@ #include #include -#include "rviz_2d_overlay_msgs/msg/overlay_text.hpp" +#include "autoware_overlay_msgs/msg/overlay_text.hpp" #include #include @@ -70,7 +70,7 @@ #include #include -namespace awf_2d_overlay_vehicle +namespace autoware_overlay_rviz_plugin { class OverlayObject; @@ -90,15 +90,15 @@ class ScopedPixelBuffer }; enum class VerticalAlignment : uint8_t { - CENTER = rviz_2d_overlay_msgs::msg::OverlayText::CENTER, - TOP = rviz_2d_overlay_msgs::msg::OverlayText::TOP, - BOTTOM = rviz_2d_overlay_msgs::msg::OverlayText::BOTTOM, + 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 = rviz_2d_overlay_msgs::msg::OverlayText::LEFT, - RIGHT = rviz_2d_overlay_msgs::msg::OverlayText::RIGHT, - CENTER = rviz_2d_overlay_msgs::msg::OverlayText::CENTER + LEFT = autoware_overlay_msgs::msg::OverlayText::LEFT, + RIGHT = autoware_overlay_msgs::msg::OverlayText::RIGHT, + CENTER = autoware_overlay_msgs::msg::OverlayText::CENTER }; /** @@ -136,6 +136,6 @@ class OverlayObject Ogre::MaterialPtr panel_material_; Ogre::TexturePtr texture_; }; -} // namespace awf_2d_overlay_vehicle +} // namespace autoware_overlay_rviz_plugin #endif // OVERLAY_UTILS_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/signal_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp similarity index 93% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/signal_display.hpp rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp index aff475aba2f33..4fa39f2c5a903 100644 --- a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/signal_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp @@ -39,7 +39,7 @@ #include #endif -namespace awf_2d_overlay_vehicle +namespace autoware_overlay_rviz_plugin { class SignalDisplay : public rviz_common::Display { @@ -70,7 +70,7 @@ private Q_SLOTS: private: std::mutex mutex_; - awf_2d_overlay_vehicle::OverlayObject::SharedPtr overlay_; + 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_; @@ -84,7 +84,8 @@ private Q_SLOTS: std::unique_ptr traffic_topic_property_; std::unique_ptr speed_limit_topic_property_; - void drawBackground(QPainter & painter, const QRectF & backgroundRect); + void drawHorizontalRoundedRectangle(QPainter & painter, const QRectF & backgroundRect); + void drawVerticalRoundedRectangle(QPainter & painter, const QRectF & backgroundRect); void setupRosSubscriptions(); std::unique_ptr steering_wheel_display_; @@ -119,6 +120,6 @@ private Q_SLOTS: const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); void drawWidget(QImage & hud); }; -} // namespace awf_2d_overlay_vehicle +} // namespace autoware_overlay_rviz_plugin #endif // SIGNAL_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/speed_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp similarity index 94% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/speed_display.hpp rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp index 317e45917f927..0669028dba3b2 100644 --- a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/speed_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp @@ -29,7 +29,7 @@ #include #include -namespace awf_2d_overlay_vehicle +namespace autoware_overlay_rviz_plugin { class SpeedDisplay @@ -44,6 +44,6 @@ class SpeedDisplay QColor gray = QColor(194, 194, 194); }; -} // namespace awf_2d_overlay_vehicle +} // namespace autoware_overlay_rviz_plugin #endif // SPEED_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/speed_limit_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp similarity index 78% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/speed_limit_display.hpp rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp index 00eae077ff2ac..b59f4acf380db 100644 --- a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/speed_limit_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp @@ -23,13 +23,14 @@ #include #include +#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" #include #include #include #include -namespace awf_2d_overlay_vehicle +namespace autoware_overlay_rviz_plugin { class SpeedLimitDisplay @@ -38,12 +39,14 @@ class SpeedLimitDisplay SpeedLimitDisplay(); void drawSpeedLimitIndicator(QPainter & painter, const QRectF & backgroundRect); void updateSpeedLimitData(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); + void updateSpeedData(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); private: - float current_limit; // Internal variable to store current gear + 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 awf_2d_overlay_vehicle +} // namespace autoware_overlay_rviz_plugin #endif // SPEED_LIMIT_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/steering_wheel_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp similarity index 94% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/steering_wheel_display.hpp rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp index a8291064c3807..121ee9a0a4d84 100644 --- a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/steering_wheel_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp @@ -29,7 +29,7 @@ #include #include -namespace awf_2d_overlay_vehicle +namespace autoware_overlay_rviz_plugin { class SteeringWheelDisplay @@ -49,6 +49,6 @@ class SteeringWheelDisplay QImage coloredImage(const QImage & source, const QColor & color); }; -} // namespace awf_2d_overlay_vehicle +} // namespace autoware_overlay_rviz_plugin #endif // STEERING_WHEEL_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/traffic_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp similarity index 84% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/traffic_display.hpp rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp index bf776d27dfa94..20410a78edc44 100644 --- a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/traffic_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp @@ -31,7 +31,7 @@ #include #include -namespace awf_2d_overlay_vehicle +namespace autoware_overlay_rviz_plugin { class TrafficDisplay @@ -45,18 +45,15 @@ class TrafficDisplay private: QImage traffic_light_image_; - // yellow #CFC353 - QColor yellow = QColor(207, 195, 83); - // red #CF5353 - QColor red = QColor(207, 83, 83); - // green #53CF5F - QColor green = QColor(83, 207, 95); - // gray #C2C2C2 - QColor gray = QColor(194, 194, 194); + + 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 awf_2d_overlay_vehicle +} // namespace autoware_overlay_rviz_plugin #endif // TRAFFIC_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/turn_signals_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/turn_signals_display.hpp similarity index 94% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/turn_signals_display.hpp rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/turn_signals_display.hpp index cd659883c9ca0..87f141353d5b2 100644 --- a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/turn_signals_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/turn_signals_display.hpp @@ -32,7 +32,7 @@ #include -namespace awf_2d_overlay_vehicle +namespace autoware_overlay_rviz_plugin { class TurnSignalsDisplay @@ -47,7 +47,7 @@ class TurnSignalsDisplay private: QImage arrowImage; - QColor gray = QColor(194, 194, 194); + 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 @@ -58,6 +58,6 @@ class TurnSignalsDisplay const std::chrono::milliseconds blink_interval_{500}; // Blink interval in milliseconds }; -} // namespace awf_2d_overlay_vehicle +} // namespace autoware_overlay_rviz_plugin #endif // TURN_SIGNALS_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/package.xml b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml similarity index 91% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/package.xml rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml index b19b384d020b6..cc0ada00fa41b 100644 --- a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/package.xml +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml @@ -1,7 +1,7 @@ - awf_2d_overlay_vehicle + autoware_overlay_rviz_plugin 0.0.1 RViz2 plugin for 2D overlays in the 3D view. Mainly a port of the JSK overlay plugin @@ -12,9 +12,9 @@ BSD-3-Clause autoware_auto_vehicle_msgs + autoware_overlay_msgs autoware_perception_msgs boost - rviz_2d_overlay_msgs rviz_common rviz_ogre_vendor rviz_rendering 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/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/gear_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp similarity index 92% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/gear_display.cpp rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp index 153e106f04264..8f05ce6fdce1c 100644 --- a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/gear_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp @@ -31,12 +31,13 @@ #include #include -namespace awf_2d_overlay_vehicle +namespace autoware_overlay_rviz_plugin { GearDisplay::GearDisplay() : current_gear_(0) { - std::string package_path = ament_index_cpp::get_package_share_directory("awf_2d_overlay_vehicle"); + 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( @@ -91,8 +92,8 @@ void GearDisplay::drawGearIndicator(QPainter & painter, const QRectF & backgroun int gearY = backgroundRect.height() - gearBoxSize - 20; QRect gearRect(gearX, gearY, gearBoxSize, gearBoxSize); painter.setBrush(QColor(0, 0, 0, 0)); - painter.drawRoundedRect(gearRect, 5, 5); + painter.drawRoundedRect(gearRect, 10, 10); painter.drawText(gearRect, Qt::AlignCenter, QString::fromStdString(gearString)); } -} // namespace awf_2d_overlay_vehicle +} // namespace autoware_overlay_rviz_plugin diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/overlay_text_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/overlay_text_display.cpp similarity index 93% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/overlay_text_display.cpp rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/overlay_text_display.cpp index 053d0f6e981c0..b853e14a5858d 100644 --- a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/overlay_text_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/overlay_text_display.cpp @@ -67,7 +67,7 @@ #include #include -namespace awf_2d_overlay_vehicle +namespace autoware_overlay_rviz_plugin { OverlayTextDisplay::OverlayTextDisplay() : texture_width_(0), @@ -105,15 +105,15 @@ OverlayTextDisplay::OverlayTextDisplay() hor_alignment_property_ = new rviz_common::properties::EnumProperty( "hor_alignment", "left", "horizontal alignment of the overlay", this, SLOT(updateHorizontalAlignment())); - hor_alignment_property_->addOption("left", rviz_2d_overlay_msgs::msg::OverlayText::LEFT); - hor_alignment_property_->addOption("center", rviz_2d_overlay_msgs::msg::OverlayText::CENTER); - hor_alignment_property_->addOption("right", rviz_2d_overlay_msgs::msg::OverlayText::RIGHT); + 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", rviz_2d_overlay_msgs::msg::OverlayText::TOP); - ver_alignment_property_->addOption("center", rviz_2d_overlay_msgs::msg::OverlayText::CENTER); - ver_alignment_property_->addOption("bottom", rviz_2d_overlay_msgs::msg::OverlayText::BOTTOM); + 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); @@ -212,7 +212,7 @@ void OverlayTextDisplay::update(float /*wall_dt*/, float /*ros_dt*/) overlay_->updateTextureSize(texture_width_, texture_height_); { - awf_2d_overlay_vehicle::ScopedPixelBuffer buffer = overlay_->getBuffer(); + autoware_overlay_rviz_plugin::ScopedPixelBuffer buffer = overlay_->getBuffer(); QImage Hud = buffer.getQImage(*overlay_, bg_color_); QPainter painter(&Hud); painter.setRenderHint(QPainter::Antialiasing, true); @@ -288,7 +288,7 @@ void OverlayTextDisplay::reset() } } -void OverlayTextDisplay::processMessage(rviz_2d_overlay_msgs::msg::OverlayText::ConstSharedPtr msg) +void OverlayTextDisplay::processMessage(autoware_overlay_msgs::msg::OverlayText::ConstSharedPtr msg) { if (!isEnabled()) { return; @@ -297,13 +297,13 @@ void OverlayTextDisplay::processMessage(rviz_2d_overlay_msgs::msg::OverlayText:: static int count = 0; std::stringstream ss; ss << "OverlayTextDisplayObject" << count++; - overlay_.reset(new awf_2d_overlay_vehicle::OverlayObject(ss.str())); + overlay_.reset(new autoware_overlay_rviz_plugin::OverlayObject(ss.str())); overlay_->show(); } if (overlay_) { - if (msg->action == rviz_2d_overlay_msgs::msg::OverlayText::DELETE) { + if (msg->action == autoware_overlay_msgs::msg::OverlayText::DELETE) { overlay_->hide(); - } else if (msg->action == rviz_2d_overlay_msgs::msg::OverlayText::ADD) { + } else if (msg->action == autoware_overlay_msgs::msg::OverlayText::ADD) { overlay_->show(); } } @@ -550,7 +550,7 @@ void OverlayTextDisplay::updateLineWidth() } } -} // namespace awf_2d_overlay_vehicle +} // namespace autoware_overlay_rviz_plugin #include -PLUGINLIB_EXPORT_CLASS(awf_2d_overlay_vehicle::OverlayTextDisplay, rviz_common::Display) +PLUGINLIB_EXPORT_CLASS(autoware_overlay_rviz_plugin::OverlayTextDisplay, rviz_common::Display) diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/overlay_utils.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/overlay_utils.cpp similarity index 99% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/overlay_utils.cpp rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/overlay_utils.cpp index e65a74415fb6a..d6f061e724369 100644 --- a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/overlay_utils.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/overlay_utils.cpp @@ -52,7 +52,7 @@ #include -namespace awf_2d_overlay_vehicle +namespace autoware_overlay_rviz_plugin { ScopedPixelBuffer::ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer) : pixel_buffer_(pixel_buffer) @@ -264,4 +264,4 @@ unsigned int OverlayObject::getTextureHeight() const return 0; } } -} // namespace awf_2d_overlay_vehicle +} // namespace autoware_overlay_rviz_plugin diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/signal_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp similarity index 92% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/signal_display.cpp rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp index f2a82dd386b37..b2995327eb472 100644 --- a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/signal_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp @@ -33,7 +33,7 @@ #include #include -namespace awf_2d_overlay_vehicle +namespace autoware_overlay_rviz_plugin { SignalDisplay::SignalDisplay() @@ -47,7 +47,7 @@ SignalDisplay::SignalDisplay() 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(94, 130, 255), "Color of the signal arrows", this, + "Signal Color", QColor(QString("#00E678")), "Color of the signal arrows", this, SLOT(updateOverlayColor())); // Initialize the component displays @@ -68,7 +68,7 @@ void SignalDisplay::onInitialize() static int count = 0; std::stringstream ss; ss << "SignalDisplayObject" << count++; - overlay_.reset(new awf_2d_overlay_vehicle::OverlayObject(ss.str())); + overlay_.reset(new autoware_overlay_rviz_plugin::OverlayObject(ss.str())); overlay_->show(); updateOverlaySize(); updateOverlayPosition(); @@ -206,7 +206,7 @@ void SignalDisplay::update(float /* wall_dt */, float /* ros_dt */) if (!overlay_) { return; } - awf_2d_overlay_vehicle::ScopedPixelBuffer buffer = overlay_->getBuffer(); + autoware_overlay_rviz_plugin::ScopedPixelBuffer buffer = overlay_->getBuffer(); QImage hud = buffer.getQImage(*overlay_); hud.fill(Qt::transparent); drawWidget(hud); @@ -297,6 +297,7 @@ void SignalDisplay::updateSpeedData( if (speed_display_) { speed_display_->updateSpeedData(msg); + speed_limit_display_->updateSpeedData(msg); queueRender(); } } @@ -324,7 +325,7 @@ void SignalDisplay::drawWidget(QImage & hud) painter.setRenderHint(QPainter::Antialiasing, true); QRectF backgroundRect(0, 0, 322, hud.height()); - drawBackground(painter, backgroundRect); + drawHorizontalRoundedRectangle(painter, backgroundRect); // Draw components if (steering_wheel_display_) { @@ -342,9 +343,9 @@ void SignalDisplay::drawWidget(QImage & hud) // a 27px space between the two halves of the HUD - QRectF smallerBackgroundRect(349, 0, 168, hud.height() / 2); + QRectF smallerBackgroundRect(340, 0, 190.0 / 2, hud.height()); - drawBackground(painter, smallerBackgroundRect); + drawVerticalRoundedRectangle(painter, smallerBackgroundRect); if (traffic_display_) { traffic_display_->drawTrafficLightIndicator(painter, smallerBackgroundRect); @@ -357,13 +358,33 @@ void SignalDisplay::drawWidget(QImage & hud) painter.end(); } -void SignalDisplay::drawBackground(QPainter & painter, const QRectF & backgroundRect) +void SignalDisplay::drawHorizontalRoundedRectangle( + QPainter & painter, const QRectF & backgroundRect) { - painter.setBrush(QColor(0, 0, 0, 255 * 0.2)); // Black background with opacity + 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() { @@ -495,7 +516,7 @@ void SignalDisplay::topic_updated_traffic() }); } -} // namespace awf_2d_overlay_vehicle +} // namespace autoware_overlay_rviz_plugin #include -PLUGINLIB_EXPORT_CLASS(awf_2d_overlay_vehicle::SignalDisplay, rviz_common::Display) +PLUGINLIB_EXPORT_CLASS(autoware_overlay_rviz_plugin::SignalDisplay, rviz_common::Display) diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/speed_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp similarity index 95% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/speed_display.cpp rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp index 8212758c09a9f..b1df14956d16c 100644 --- a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/speed_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp @@ -31,12 +31,13 @@ #include #include -namespace awf_2d_overlay_vehicle +namespace autoware_overlay_rviz_plugin { SpeedDisplay::SpeedDisplay() : current_speed_(0.0) { - std::string package_path = ament_index_cpp::get_package_share_directory("awf_2d_overlay_vehicle"); + 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( @@ -106,4 +107,4 @@ void SpeedDisplay::drawSpeedDisplay(QPainter & painter, const QRectF & backgroun painter.drawText(unitPos, speedUnit); } -} // namespace awf_2d_overlay_vehicle +} // namespace autoware_overlay_rviz_plugin diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/speed_limit_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp similarity index 54% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/speed_limit_display.cpp rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp index fcc1df998798b..6913ef6a48ecd 100644 --- a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/speed_limit_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp @@ -31,12 +31,13 @@ #include #include -namespace awf_2d_overlay_vehicle +namespace autoware_overlay_rviz_plugin { -SpeedLimitDisplay::SpeedLimitDisplay() : current_limit(0.0) +SpeedLimitDisplay::SpeedLimitDisplay() : current_limit(0.0), current_speed_(0.0) { - std::string package_path = ament_index_cpp::get_package_share_directory("awf_2d_overlay_vehicle"); + 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( @@ -54,45 +55,81 @@ void SpeedLimitDisplay::updateSpeedLimitData( 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); - // #C2C2C2 - painter.setPen(QPen(gray, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); - painter.setBrush(QBrush(gray, Qt::SolidPattern)); + 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() / 2 - 20); - outerCircleRect.setHeight(backgroundRect.height() - 20); - outerCircleRect.moveTopLeft(QPointF(backgroundRect.left() + 10, backgroundRect.top() + 10)); + 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.375); - innerCircleRect.setHeight(outerCircleRect.height() / 1.375); + innerCircleRect.setWidth(outerCircleRect.width() / 1.25); + innerCircleRect.setHeight(outerCircleRect.height() / 1.25); innerCircleRect.moveCenter(outerCircleRect.center()); - // Draw the outer circle - painter.drawEllipse(outerCircleRect); + painter.setRenderHint(QPainter::Antialiasing, true); + QColor colorFromHSV; + colorFromHSV.setHsv(0, 0, 0); // Hue, Saturation, Value - // Change the composition mode and draw the inner circle - painter.setCompositionMode(QPainter::CompositionMode_Clear); + painter.setBrush(colorFromHSV); painter.drawEllipse(innerCircleRect); - // Reset the composition mode - painter.setCompositionMode(QPainter::CompositionMode_SourceOver); - 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", 14, QFont::Bold); + QFont font = QFont("Quicksand", 24, QFont::Bold); painter.setFont(font); // #C2C2C2 @@ -102,4 +139,4 @@ void SpeedLimitDisplay::drawSpeedLimitIndicator(QPainter & painter, const QRectF painter.drawText(innerCircleRect, Qt::AlignCenter, text); } -} // namespace awf_2d_overlay_vehicle +} // namespace autoware_overlay_rviz_plugin diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/steering_wheel_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp similarity index 95% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/steering_wheel_display.cpp rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp index b4da8ff5f8ffb..5882d7780dc92 100644 --- a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/steering_wheel_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp @@ -31,13 +31,14 @@ #include #include -namespace awf_2d_overlay_vehicle +namespace autoware_overlay_rviz_plugin { SteeringWheelDisplay::SteeringWheelDisplay() { // Load the Quicksand font - std::string package_path = ament_index_cpp::get_package_share_directory("awf_2d_overlay_vehicle"); + 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( @@ -120,4 +121,4 @@ QImage SteeringWheelDisplay::coloredImage(const QImage & source, const QColor & return result; } -} // namespace awf_2d_overlay_vehicle +} // namespace autoware_overlay_rviz_plugin diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/traffic_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp similarity index 54% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/traffic_display.cpp rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp index 233da1f36b4a7..4fea8f78b80da 100644 --- a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/traffic_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp @@ -31,13 +31,18 @@ #include #include -namespace awf_2d_overlay_vehicle +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("awf_2d_overlay_vehicle"); + 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()); } @@ -54,50 +59,60 @@ void TrafficDisplay::drawTrafficLightIndicator(QPainter & painter, const QRectF 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() / 2 - 20); - circleRect.setHeight(backgroundRect.height() - 20); - circleRect.moveTopRight(QPointF(backgroundRect.right() - 10, backgroundRect.top() + 10)); - - painter.setBrush(QBrush(gray)); - painter.drawEllipse(circleRect.center(), 30, 30); - - // Define the area for the traffic light image (should be smaller or positioned within the circle) - QRectF imageRect = - circleRect.adjusted(15, 15, -15, -15); // Adjusting the rectangle to make the image smaller - - QImage scaled_traffic_image = traffic_light_image_.scaled( - imageRect.size().toSize(), Qt::KeepAspectRatio, Qt::SmoothTransformation); - - if (current_traffic_.signals.size() > 0) { + 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(red)); - painter.drawEllipse(circleRect.center(), 30, 30); + painter.setBrush(QBrush(tl_red_)); + painter.drawEllipse(circleRect); break; case 2: - painter.setBrush(QBrush(yellow)); - painter.drawEllipse(circleRect.center(), 30, 30); + painter.setBrush(QBrush(tl_yellow_)); + painter.drawEllipse(circleRect); break; case 3: - painter.setBrush(QBrush(green)); - painter.drawEllipse(circleRect.center(), 30, 30); + painter.setBrush(QBrush(tl_green_)); + painter.drawEllipse(circleRect); break; case 4: - painter.setBrush(QBrush(gray)); - painter.drawEllipse(circleRect.center(), 30, 30); + painter.setBrush(tl_gray_); + painter.drawEllipse(circleRect); break; default: - painter.setBrush(QBrush(gray)); - painter.drawEllipse(circleRect.center(), 30, 30); + painter.setBrush(tl_gray_); + painter.drawEllipse(circleRect); break; } } - // make the image thicker - painter.setPen(QPen(Qt::black, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); - painter.drawImage(imageRect, scaled_traffic_image); + // 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) @@ -110,4 +125,4 @@ QImage TrafficDisplay::coloredImage(const QImage & source, const QColor & color) return result; } -} // namespace awf_2d_overlay_vehicle +} // namespace autoware_overlay_rviz_plugin diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/turn_signals_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp similarity index 95% rename from common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/turn_signals_display.cpp rename to common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp index a9b780cb4eab2..b6d37062bf08b 100644 --- a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/turn_signals_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp @@ -31,7 +31,7 @@ #include #include -namespace awf_2d_overlay_vehicle +namespace autoware_overlay_rviz_plugin { TurnSignalsDisplay::TurnSignalsDisplay() : current_turn_signal_(0) @@ -39,7 +39,8 @@ 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("awf_2d_overlay_vehicle"); + 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()); } @@ -119,4 +120,4 @@ QImage TurnSignalsDisplay::coloredImage(const QImage & source, const QColor & co return result; } -} // namespace awf_2d_overlay_vehicle +} // namespace autoware_overlay_rviz_plugin diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/README.md b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/README.md deleted file mode 100644 index 6728f26878e10..0000000000000 --- a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/README.md +++ /dev/null @@ -1,54 +0,0 @@ -# awf_2d_overlay_vehicle - -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 twist | -| `/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 | - -## 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 rviz and select Add under the Displays panel. - - ![select_add](./assets/images/select_add.png) - -2. Select any one of the tier4_vehicle_rviz_plugin and press OK. - - ![select_vehicle_plugin](./assets/images/select_vehicle_plugin.png) - -3. Enter the name of the topic where you want to view the status. - - ![select_topic_name](./assets/images/select_topic_name.png) diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_topic_name.png b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_topic_name.png deleted file mode 100644 index 5466bcf0050df..0000000000000 Binary files a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_topic_name.png and /dev/null differ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_vehicle_plugin.png b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_vehicle_plugin.png deleted file mode 100644 index 863f63d728616..0000000000000 Binary files a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_vehicle_plugin.png and /dev/null differ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/traffic.png b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/traffic.png deleted file mode 100644 index 960985a3da5de..0000000000000 Binary files a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/traffic.png and /dev/null differ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/plugins_description.xml b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/plugins_description.xml deleted file mode 100644 index 8a5af0abcf0dd..0000000000000 --- a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/plugins_description.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - Signal overlay plugin for the 3D view. - - diff --git a/common/tensorrt_common/include/tensorrt_common/logger.hpp b/common/tensorrt_common/include/tensorrt_common/logger.hpp index 73190d13b9de3..355efe5167885 100644 --- a/common/tensorrt_common/include/tensorrt_common/logger.hpp +++ b/common/tensorrt_common/include/tensorrt_common/logger.hpp @@ -493,7 +493,7 @@ namespace //! inline LogStreamConsumer LOG_VERBOSE(const Logger & logger) { - return LogStreamConsumer(logger.getReportableSeverity(), Severity::kVERBOSE) << "[TRT] "; + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kVERBOSE); } //! @@ -505,7 +505,7 @@ inline LogStreamConsumer LOG_VERBOSE(const Logger & logger) //! inline LogStreamConsumer LOG_INFO(const Logger & logger) { - return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINFO) << "[TRT] "; + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINFO); } //! @@ -517,7 +517,7 @@ inline LogStreamConsumer LOG_INFO(const Logger & logger) //! inline LogStreamConsumer LOG_WARN(const Logger & logger) { - return LogStreamConsumer(logger.getReportableSeverity(), Severity::kWARNING) << "[TRT] "; + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kWARNING); } //! @@ -529,7 +529,7 @@ inline LogStreamConsumer LOG_WARN(const Logger & logger) //! inline LogStreamConsumer LOG_ERROR(const Logger & logger) { - return LogStreamConsumer(logger.getReportableSeverity(), Severity::kERROR) << "[TRT] "; + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kERROR); } //! @@ -543,7 +543,7 @@ inline LogStreamConsumer LOG_ERROR(const Logger & logger) //! inline LogStreamConsumer LOG_FATAL(const Logger & logger) { - return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINTERNAL_ERROR) << "[TRT] "; + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINTERNAL_ERROR); } } // anonymous namespace 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_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 index 6a67573717ae1..4f5b4961f2500 100644 Binary files a/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticCheckpointTool.png 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_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_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_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/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_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 6ce95acc4ef1d..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 @@ -17,9 +17,9 @@ #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 @@ -35,9 +35,9 @@ 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. diff --git a/common/traffic_light_utils/src/traffic_light_utils.cpp b/common/traffic_light_utils/src/traffic_light_utils.cpp index c8f2ca85b2089..0bd3d85a9b71f 100644 --- a/common/traffic_light_utils/src/traffic_light_utils.cpp +++ b/common/traffic_light_utils/src/traffic_light_utils.cpp @@ -33,14 +33,14 @@ 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; 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/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 960aadf226b70..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; @@ -384,9 +384,10 @@ bool AEB::hasCollision( 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; @@ -397,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; @@ -435,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{}; @@ -452,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; @@ -471,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_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/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/joy_controller/config/joy_controller.param.yaml b/control/joy_controller/config/joy_controller.param.yaml index 73a5d028985c5..c9fe348867e68 100644 --- a/control/joy_controller/config/joy_controller.param.yaml +++ b/control/joy_controller/config/joy_controller.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - joy_type: DS4 + joy_type: $(var joy_type) update_rate: 10.0 accel_ratio: 3.0 brake_ratio: 5.0 diff --git a/control/joy_controller/launch/joy_controller.launch.xml b/control/joy_controller/launch/joy_controller.launch.xml index d2804a7339046..c392ca100a9c0 100644 --- a/control/joy_controller/launch/joy_controller.launch.xml +++ b/control/joy_controller/launch/joy_controller.launch.xml @@ -30,8 +30,7 @@ - - + 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 cc1c9c08887ba..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 @@ -30,6 +30,7 @@ #include #include +#include namespace autoware::motion::control::pid_longitudinal_controller { @@ -62,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 @@ -82,7 +83,7 @@ Pose calcPoseAfterTimeDelay( * @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; @@ -101,6 +102,8 @@ 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.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( @@ -114,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); } /** @@ -138,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 b4ac9e1506eb7..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 @@ -66,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 @@ -184,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; @@ -276,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 @@ -309,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 @@ -341,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 @@ -350,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; @@ -359,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 @@ -383,12 +391,9 @@ 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(); }; 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 f1ddea7ebad88..e95428096c17c 100644 --- a/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml +++ b/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml @@ -69,8 +69,9 @@ max_jerk: 2.0 min_jerk: -5.0 - # pitch - use_trajectory_for_pitch_calculation: false + # slope compensation lpf_pitch_gain: 0.95 + slope_source: "raw_pitch" # raw_pitch, trajectory_pitch or trajectory_adaptive + adaptive_trajectory_velocity_th: 1.0 max_pitch_rad: 0.1 min_pitch_rad: -0.1 diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 33a54663ccaef..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( @@ -158,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 31c48fb5060a8..aab2ecf8f081e 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -168,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") @@ -355,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; @@ -401,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); @@ -428,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; @@ -449,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(); } @@ -460,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; @@ -539,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; @@ -555,7 +646,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d return; }; - const auto info_once = [this](const auto & s) { RCLCPP_INFO_ONCE(logger_, "%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; @@ -581,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; @@ -621,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_once("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_once("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."); } // --------------- @@ -662,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( @@ -722,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; } @@ -739,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()); } @@ -791,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; @@ -815,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); @@ -866,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; @@ -899,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 @@ -913,59 +997,69 @@ 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; @@ -978,11 +1072,12 @@ double PidLongitudinalController::applyVelocityFeedback( const bool enable_integration = (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. @@ -993,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; @@ -1020,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::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::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::ERROR_VEL, + control_data.interpolated_traj.points.at(control_data.nearest_idx).longitudinal_velocity_mps - + control_data.current_motion.vel); } void PidLongitudinalController::setupDiagnosticUpdater() 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 58aa867b2deab..02bcce8c91c4b 100644 --- a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp @@ -132,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) @@ -355,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) @@ -486,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/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/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/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/evaluator/tier4_metrics_rviz_plugin/package.xml b/evaluator/tier4_metrics_rviz_plugin/package.xml index 7bd930f4f3c4c..fe84a0ab81760 100644 --- a/evaluator/tier4_metrics_rviz_plugin/package.xml +++ b/evaluator/tier4_metrics_rviz_plugin/package.xml @@ -13,6 +13,7 @@ autoware_cmake diagnostic_msgs + libqt5-charts-dev libqt5-core libqt5-gui libqt5-widgets 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 f4d6679291849..9d3fba05b4a6e 100644 --- a/launch/tier4_localization_launch/launch/localization.launch.xml +++ b/launch/tier4_localization_launch/launch/localization.launch.xml @@ -1,18 +1,21 @@ - - + - - - - + + + + + + + + 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 ac7589ea2273b..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,13 +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 bee300c587816..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 @@ -12,7 +12,7 @@ - + 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 02c6da20e17da..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,152 +1,116 @@ + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - + + + - - - - - - - - - - - - - - - - - - - - + + + + + + + + - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + - - + + + - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + 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 22a45fe7b8530..0000000000000 --- a/launch/tier4_localization_launch/launch/util/util.launch.py +++ /dev/null @@ -1,136 +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.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, - ] - - load_composable_nodes = LoadComposableNodes( - composable_node_descriptions=composable_nodes, - target_container=LaunchConfiguration("lidar_container_name"), - ) - - 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( - "lidar_container_name", - "/sensing/lidar/top/pointcloud_preprocessor/pointcloud_container", - "container name of main lidar used for localization", - ) - - 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 9b4f727c9ce52..d0046acf294b5 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -11,6 +11,7 @@ NGUYEN Viet Anh Taiki Yamada Shintaro Sakoda + Masahiro Sakamoto Apache License 2.0 Yamato Ando @@ -27,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.xml b/launch/tier4_map_launch/launch/map.launch.xml index 56ad5b01c5024..132d1ec9be3ea 100644 --- a/launch/tier4_map_launch/launch/map.launch.xml +++ b/launch/tier4_map_launch/launch/map.launch.xml @@ -8,6 +8,8 @@ + + @@ -47,8 +49,7 @@ - - + @@ -59,6 +60,7 @@ + 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 50b73709cc902..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 @@ -7,7 +7,6 @@ - @@ -56,7 +55,6 @@ - @@ -130,7 +128,6 @@ - @@ -190,7 +187,6 @@ - @@ -219,7 +215,6 @@ - diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml index 2232feb6d7c67..ddcbbbaeae107 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml @@ -4,7 +4,6 @@ - @@ -38,7 +37,7 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml index 5b5fabd4dd886..f9b65fd5c30eb 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml @@ -3,7 +3,6 @@ - @@ -17,13 +16,12 @@ - - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml index cec0c3bc05aac..0fabd5e98e45f 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml @@ -3,7 +3,6 @@ - @@ -12,7 +11,6 @@ - @@ -27,7 +25,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml index 6ccff44933966..0ecc89c8743e8 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml @@ -45,6 +45,7 @@ + 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 1dcb9cfc90daf..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("individual_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("pointcloud_container_name"), - condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), ) - return [individual_container, pointcloud_container_loader] + return [pointcloud_container_loader] def generate_launch_description(): @@ -167,9 +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("pointcloud_container_name", "pointcloud_container") - add_launch_arg("individual_container_name", "pointcloud_map_filter_container") add_launch_arg("use_pointcloud_map", "true") set_container_executable = SetLaunchConfiguration( "container_executable", 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 85c6e87c0fd19..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 @@ -36,6 +36,7 @@ + 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 66a0bb4d0fb63..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 @@ -504,21 +503,11 @@ def launch_setup(context, *args, **kwargs): output_topic=pipeline.output_topic, ) ) - individual_container = ComposableNodeContainer( - name=LaunchConfiguration("individual_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("pointcloud_container_name"), - condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), ) - return [individual_container, pointcloud_container_loader] + return [pointcloud_container_loader] def generate_launch_description(): @@ -530,9 +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("pointcloud_container_name", "pointcloud_container") - add_launch_arg("individual_container_name", "ground_segmentation_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 244e0940acb70..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,7 +6,6 @@ - @@ -23,7 +22,7 @@ - + @@ -39,7 +38,7 @@ - + @@ -55,7 +54,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 0d517ced97d17..dbb74335f79d2 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -82,7 +82,6 @@ default="false" description="if use_empty_dynamic_object_publisher:=true, /perception/object_recognition/objects topic has an empty DynamicObjectArray" /> - @@ -131,7 +130,6 @@ - @@ -146,7 +144,6 @@ - @@ -197,7 +194,6 @@ - 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 7b81a42fdb79f..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 @@ -24,12 +24,9 @@ - - - - - - + + + @@ -49,10 +46,11 @@ - - + + + @@ -62,12 +60,9 @@ - - - - - - + + + @@ -106,10 +101,11 @@ - - + + + @@ -119,12 +115,9 @@ - - - - - - + + + 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 dc5a2443d3f45..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,83 +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", - ) - 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_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( - "car_classifier_model_path", - os.path.join(classifier_share_dir, "data", "traffic_light_classifier_efficientNet_b1.onnx"), - ) - add_launch_arg( - "pedestrian_classifier_model_path", - os.path.join( - classifier_share_dir, "data", "pedestrian_traffic_light_classifier_efficientNet_b1.onnx" - ), - ) - add_launch_arg( - "car_classifier_label_path", os.path.join(classifier_share_dir, "data", "lamp_labels.txt") - ) - add_launch_arg( - "pedestrian_classifier_label_path", - os.path.join(classifier_share_dir, "data", "lamp_labels_ped.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("backlight_threshold", "0.85") - - 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="", @@ -112,19 +59,7 @@ def create_parameter_dict(*args): plugin="traffic_light::TrafficLightClassifierNodelet", name="car_traffic_light_classifier", namespace="classification", - parameters=[ - { - "approximate_sync": LaunchConfiguration("approximate_sync"), - "classifier_type": LaunchConfiguration("classifier_type"), - "classify_traffic_light_type": 0, - "classifier_model_path": LaunchConfiguration("car_classifier_model_path"), - "classifier_label_path": LaunchConfiguration("car_classifier_label_path"), - "classifier_precision": LaunchConfiguration("classifier_precision"), - "classifier_mean": LaunchConfiguration("classifier_mean"), - "classifier_std": LaunchConfiguration("classifier_std"), - "backlight_threshold": LaunchConfiguration("backlight_threshold"), - } - ], + parameters=[car_traffic_light_classifier_model_param], remappings=[ ("~/input/image", LaunchConfiguration("input/image")), ("~/input/rois", LaunchConfiguration("output/rois")), @@ -139,23 +74,7 @@ def create_parameter_dict(*args): plugin="traffic_light::TrafficLightClassifierNodelet", name="pedestrian_traffic_light_classifier", namespace="classification", - parameters=[ - { - "approximate_sync": LaunchConfiguration("approximate_sync"), - "classifier_type": LaunchConfiguration("classifier_type"), - "classify_traffic_light_type": 1, - "classifier_model_path": LaunchConfiguration( - "pedestrian_classifier_model_path" - ), - "classifier_label_path": LaunchConfiguration( - "pedestrian_classifier_label_path" - ), - "classifier_precision": LaunchConfiguration("classifier_precision"), - "classifier_mean": LaunchConfiguration("classifier_mean"), - "classifier_std": LaunchConfiguration("classifier_std"), - "backlight_threshold": LaunchConfiguration("backlight_threshold"), - } - ], + parameters=[pedestrian_traffic_light_classifier_model_param], remappings=[ ("~/input/image", LaunchConfiguration("input/image")), ("~/input/rois", LaunchConfiguration("output/rois")), @@ -214,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( @@ -229,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"), @@ -245,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", @@ -262,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/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 a297697234ef2..90ebd23215be5 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 @@ -15,6 +15,7 @@ + @@ -58,6 +59,11 @@ value="$(eval "'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::StartPlannerModuleManager, '")" if="$(var launch_start_planner_module)" /> + + @@ -262,59 +269,28 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + - - - + + + + + + + + + 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/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md index 02f14a86cd7df..1b09420f5047d 100644 --- a/localization/ekf_localizer/README.md +++ b/localization/ekf_localizer/README.md @@ -78,65 +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 | -| publish_tf | bool | Whether to publish tf | true | -| 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 -| Name | Type | Description | Default value | -| :-------------------- | :----- | :---------------------------------------------- | :------------ | -| z_filter_proc_dev | double | Simple1DFilter - Z filter process deviation | 1.0 | -| roll_filter_proc_dev | double | Simple1DFilter - Roll filter process deviation | 0.01 | -| pitch_filter_proc_dev | double | Simple1DFilter - Pitch filter process deviation | 0.01 | +{{ 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 diff --git a/localization/ekf_localizer/config/ekf_localizer.param.yaml b/localization/ekf_localizer/config/ekf_localizer.param.yaml index 9de7f56f5c006..4cc71e0904ca8 100644 --- a/localization/ekf_localizer/config/ekf_localizer.param.yaml +++ b/localization/ekf_localizer/config/ekf_localizer.param.yaml @@ -1,38 +1,46 @@ /**: ros__parameters: - show_debug_info: false - enable_yaw_bias_estimation: true - predict_frequency: 50.0 - tf_rate: 50.0 - publish_tf: true - 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 - #Simple1DFilter parameters - z_filter_proc_dev: 1.0 - roll_filter_proc_dev: 0.01 - pitch_filter_proc_dev: 0.01 + 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 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 + 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 - # for velocity measurement limitation (Set 0.0 if you want to ignore) - threshold_observable_velocity_mps: 0.0 # [m/s] + 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/hyper_parameters.hpp b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp index 5139f900a339e..56a13d1ecaf55 100644 --- a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp @@ -24,36 +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)), - publish_tf_(node->declare_parameter("publish_tf", true)), - 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)), - z_filter_proc_dev(node->declare_parameter("z_filter_proc_dev", 1.0)), - roll_filter_proc_dev(node->declare_parameter("roll_filter_proc_dev", 0.01)), - pitch_filter_proc_dev(node->declare_parameter("pitch_filter_proc_dev", 0.01)), + 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")) { } 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/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/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 7956a663f92d5..74d49e13a4c21 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -56,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 @@ -153,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. @@ -206,11 +204,7 @@ Using the feature, `ndt_scan_matcher` can theoretically handle any large size ma ### Parameters -| Name | Type | Description | -| ------------------------------------- | ------ | ------------------------------------------------------------ | -| `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) | +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/dynamic_map_loading.json") }} ### Notes for dynamic map loading @@ -237,10 +231,7 @@ This is a function that uses no ground LiDAR scan to estimate the scan matching ### Parameters -| Name | Type | Description | -| ------------------------------------- | ------ | ----------------------------------------------------------------------------------- | -| `estimate_scores_by_no_ground_points` | bool | Flag for using scan matching score based on no ground 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 @@ -259,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/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index 157421fc3ccb1..38b5a932ae91d 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 @@ -36,29 +36,28 @@ #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, + rclcpp::Node * node, std::mutex * ndt_ptr_mutex, NdtPtrType & ndt_ptr, HyperParameters::DynamicMapLoading param); private: friend class NDTScanMatcher; - void update_ndt( - const std::vector & maps_to_add, - const std::vector & map_ids_to_remove); + // Update the specified NDT + void 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_; @@ -66,7 +65,7 @@ class MapUpdateModule rclcpp::Client::SharedPtr pcd_loader_client_; - std::shared_ptr ndt_ptr_; + NdtPtrType & ndt_ptr_; std::mutex * ndt_ptr_mutex_; rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; @@ -74,6 +73,10 @@ class MapUpdateModule std::optional last_update_position_ = std::nullopt; 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/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/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 402ec9da32782..17bafcfedfe80 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -15,9 +15,9 @@ #include "ndt_scan_matcher/map_update_module.hpp" MapUpdateModule::MapUpdateModule( - rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr, HyperParameters::DynamicMapLoading param) -: 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), logger_(node->get_logger()), clock_(node->get_clock()), @@ -28,29 +28,90 @@ MapUpdateModule::MapUpdateModule( pcd_loader_client_ = node->create_client("pcd_loader_service"); + + 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; } -bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & position) const +bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & position) { if (last_update_position_ == std::nullopt) { return false; } + 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::update_map(const geometry_msgs::msg::Point & position) +{ + // 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(); + + ndt_ptr_.reset(new NdtType); + + ndt_ptr_->setParams(param); + + update_ndt(position, *ndt_ptr_); + 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. + update_ndt(position, *secondary_ndt_ptr_); + + ndt_ptr_mutex_->lock(); + auto input_source = ndt_ptr_->getInputSource(); + ndt_ptr_ = secondary_ndt_ptr_; + ndt_ptr_->setInputSource(input_source); + ndt_ptr_mutex_->unlock(); + } + + 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_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(param_.map_radius); - request->cached_ids = ndt_ptr_->getCurrentMapIDs(); + 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 the pointcloud_map_loader."); @@ -69,60 +130,49 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) } 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; } - const auto exe_start_time = std::chrono::system_clock::now(); + 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); + 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]); } - (*ndt_ptr_mutex_).lock(); - // Add pcd for (size_t i = 0; i < add_size; i++) { - ndt_ptr_->addTarget(points_pcl[i], maps_to_add[i].cell_id); + 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) { - ndt_ptr_->removeTarget(map_id_to_remove); + ndt.removeTarget(map_id_to_remove); } - ndt_ptr_->createVoxelKdtree(); - - (*ndt_ptr_mutex_).unlock(); + 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); - - publish_partial_pcd_map(); } 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/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..3a32d5883642e --- /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 | ndt | true | true | /sensing/gnss/pose_with_covariance | +| ndt, eagleye | ndt | ndt | false | true | /sensing/gnss/pose_with_covariance | +| ndt, artag | ndt | ndt | 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 | ndt | true | true | /sensing/gnss/pose_with_covariance | +| ndt, eagleye, artag | ndt | ndt | false | true | /sensing/gnss/pose_with_covariance | +| yabloc, eagleye, artag | yabloc | ndt | true | true | /sensing/gnss/pose_with_covariance | +| ndt, yabloc, eagleye, artag | ndt | ndt | 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/perception/traffic_light_ssd_fine_detector/lib/include/gather_topk_kernel.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp similarity index 53% rename from perception/traffic_light_ssd_fine_detector/lib/include/gather_topk_kernel.hpp rename to localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp index d9cc9fa533df6..e48507948520b 100644 --- a/perception/traffic_light_ssd_fine_detector/lib/include/gather_topk_kernel.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 TIER IV, Inc. +// 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. @@ -12,19 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -// Copyright (c) OpenMMLab. All rights reserved. +#include "pose_estimator_arbiter/pose_estimator_arbiter.hpp" -#ifndef GATHER_TOPK_KERNEL_HPP_ -#define GATHER_TOPK_KERNEL_HPP_ +#include -#include - -namespace ssd +int main(int argc, char * argv[]) { -template -void gather_topk_impl( - const scalar_t * input, const int * indices, const int * dims, int nbDims, - const int * indices_dims, int indices_nbDims, scalar_t * output, cudaStream_t stream); -} // namespace ssd + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); -#endif // GATHER_TOPK_KERNEL_HPP_ + 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/perception/traffic_light_ssd_fine_detector/lib/include/grid_priors_kernel.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_type.hpp similarity index 55% rename from perception/traffic_light_ssd_fine_detector/lib/include/grid_priors_kernel.hpp rename to localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_type.hpp index 04b4d0142d93b..edf12537198a3 100644 --- a/perception/traffic_light_ssd_fine_detector/lib/include/grid_priors_kernel.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_type.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 TIER IV, Inc. +// 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. @@ -12,19 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -// Copyright (c) OpenMMLab. All rights reserved. +#ifndef POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_TYPE_HPP_ +#define POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_TYPE_HPP_ -#ifndef GRID_PRIORS_KERNEL_HPP_ -#define GRID_PRIORS_KERNEL_HPP_ - -#include - -namespace ssd +namespace pose_estimator_arbiter { -template -void grid_priors_impl( - const scalar_t * base_anchor, scalar_t * output, int num_base_anchors, int feat_w, int feat_h, - int stride_w, int stride_h, cudaStream_t stream); -} // namespace ssd +enum class PoseEstimatorType : int { ndt = 1, yabloc = 2, eagleye = 4, artag = 8 }; +} -#endif // GRID_PRIORS_KERNEL_HPP_ +#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..3063e13c10226 100644 --- a/localization/pose_initializer/README.md +++ b/localization/pose_initializer/README.md @@ -22,6 +22,8 @@ This node depends on the map height fitter library. | `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 | Name | Type | Description | 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/schema/pose_initializer.schema.json b/localization/pose_initializer/schema/pose_initializer.schema.json new file mode 100644 index 0000000000000..14a286f059030 --- /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,\n 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,\n 0.0, 0.0, 0.01, 0.0, 0.0, 0.0,\n 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,\n 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,\n 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,\n 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,\n 0.0, 0.0, 0.01, 0.0, 0.0, 0.0,\n 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,\n 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,\n 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/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/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 85258a8e54b22..7f384aa43ec7b 100644 --- a/map/map_height_fitter/package.xml +++ b/map/map_height_fitter/package.xml @@ -18,8 +18,10 @@ 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 a05c63d44ebce..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; } @@ -102,7 +130,7 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point) req->area.radius = 50; RCLCPP_DEBUG(logger, "Send request to map_loader"); - auto future = cli_map_->async_send_request(req); + 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_DEBUG(logger, "waiting response"); @@ -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_DEBUG(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_DEBUG(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 fbe019096a3e7..8a31ecee50be5 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -111,15 +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_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 @@ -156,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 b4efbec9706b4..180a3e6f9a2e5 100644 --- a/map/map_loader/config/pointcloud_map_loader.param.yaml +++ b/map/map_loader/config/pointcloud_map_loader.param.yaml @@ -7,3 +7,5 @@ # 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/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_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index 5db00fe0ad2f5..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 @@ -61,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( @@ -74,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; } @@ -87,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( @@ -105,6 +107,12 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( 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 a0c57759d51a6..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)); 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 d7bd75a1e9f90..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 @@ -88,31 +88,27 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt 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) { - while (!fs::exists(pcd_metadata_path)) { - RCLCPP_ERROR_STREAM(get_logger(), "PCD metadata file not found: " << pcd_metadata_path); - std::this_thread::sleep_for(std::chrono::seconds(1)); - } - - 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 1887a1cd8934f..848fcfba95f14 100644 --- a/map/map_projection_loader/README.md +++ b/map/map_projection_loader/README.md @@ -82,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/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/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_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 similarity index 83% rename from map/map_tf_generator/Readme.md rename to map/map_tf_generator/README.md index 1b858ec12c2c2..643f4233c06f0 100644 --- a/map/map_tf_generator/Readme.md +++ b/map/map_tf_generator/README.md @@ -43,10 +43,7 @@ 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 | +{{ json_to_markdown("map/map_tf_generator/schema/map_tf_generator.schema.json") }} ## Assumptions / Known limits 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/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/perception/bytetrack/README.md b/perception/bytetrack/README.md index cabbdd2f05836..90116c4dae7e5 100644 --- a/perception/bytetrack/README.md +++ b/perception/bytetrack/README.md @@ -25,7 +25,7 @@ the number of false negatives is expected to decrease by using it. The paper just says that the 2d tracking algorithm is a simple Kalman filter. Original codes use the `top-left-corner` and `aspect ratio` and `size` as the state vector. -This is sometimes unstable because the aspectratio can be changed by the occlusion. +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`. 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/bytetracker.param.yaml b/perception/bytetrack/config/kalman_filter.param.yaml similarity index 100% rename from perception/bytetrack/config/bytetracker.param.yaml rename to perception/bytetrack/config/kalman_filter.param.yaml 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/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/src/strack.cpp b/perception/bytetrack/lib/src/strack.cpp index eda11509d2403..f2c5a40ad3c2e 100644 --- a/perception/bytetrack/lib/src/strack.cpp +++ b/perception/bytetrack/lib/src/strack.cpp @@ -72,7 +72,7 @@ STrack::STrack(std::vector input_tlwh, float score, int label) const std::string package_share_directory = ament_index_cpp::get_package_share_directory("bytetrack"); const std::string default_config_path = - package_share_directory + "/config/bytetracker.param.yaml"; + package_share_directory + "/config/kalman_filter.param.yaml"; if (!_parameters_loaded) { load_parameters(default_config_path); _parameters_loaded = true; diff --git a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp index 06bd89af38259..ef3727019c0f4 100644 --- a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp +++ b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp @@ -241,8 +241,10 @@ bool VoxelGridMapLoader::is_in_voxel( const double dist_x = map->points.at(voxel_index).x - target_point.x; const double dist_y = map->points.at(voxel_index).y - target_point.y; const double dist_z = map->points.at(voxel_index).z - target_point.z; - const double sqr_distance = dist_x * dist_x + dist_y * dist_y + dist_z * dist_z; - if (sqr_distance < distance_threshold * distance_threshold * downsize_ratio_z_axis_) { + // check if the point is inside the distance threshold voxel + if ( + std::abs(dist_x) < distance_threshold && std::abs(dist_y) < distance_threshold && + std::abs(dist_z) < distance_threshold * downsize_ratio_z_axis_) { return true; } } diff --git a/perception/crosswalk_traffic_light_estimator/README.md b/perception/crosswalk_traffic_light_estimator/README.md index 73c7151997c54..412a93ae6dd7a 100644 --- a/perception/crosswalk_traffic_light_estimator/README.md +++ b/perception/crosswalk_traffic_light_estimator/README.md @@ -8,17 +8,17 @@ ### Input -| Name | Type | Description | -| ------------------------------------ | ------------------------------------------------ | ------------------ | -| `~/input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map | -| `~/input/route` | `autoware_planning_msgs::msg::LaneletRoute` | route | -| `~/input/classified/traffic_signals` | `tier4_perception_msgs::msg::TrafficSignalArray` | classified signals | +| Name | Type | Description | +| ------------------------------------ | --------------------------------------------------- | ------------------ | +| `~/input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map | +| `~/input/route` | `autoware_planning_msgs::msg::LaneletRoute` | route | +| `~/input/classified/traffic_signals` | `autoware_perception_msgs::msg::TrafficSignalArray` | classified signals | ### Output -| Name | Type | Description | -| -------------------------- | ------------------------------------------------ | --------------------------------------------------------- | -| `~/output/traffic_signals` | `tier4_perception_msgs::msg::TrafficSignalArray` | output that contains estimated pedestrian traffic signals | +| Name | Type | Description | +| -------------------------- | --------------------------------------------------- | --------------------------------------------------------- | +| `~/output/traffic_signals` | `autoware_perception_msgs::msg::TrafficSignalArray` | output that contains estimated pedestrian traffic signals | ## Parameters diff --git a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp index 0578d671d27b9..ccffde4fb3a11 100644 --- a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp +++ b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp @@ -23,9 +23,6 @@ #include #include #include -#include -#include -#include #include #include diff --git a/perception/crosswalk_traffic_light_estimator/src/node.cpp b/perception/crosswalk_traffic_light_estimator/src/node.cpp index 904a365786755..e29a80de283ce 100644 --- a/perception/crosswalk_traffic_light_estimator/src/node.cpp +++ b/perception/crosswalk_traffic_light_estimator/src/node.cpp @@ -102,7 +102,7 @@ CrosswalkTrafficLightEstimatorNode::CrosswalkTrafficLightEstimatorNode( void CrosswalkTrafficLightEstimatorNode::onMap(const HADMapBin::ConstSharedPtr msg) { - RCLCPP_INFO(get_logger(), "[CrosswalkTrafficLightEstimatorNode]: Start loading lanelet"); + RCLCPP_DEBUG(get_logger(), "[CrosswalkTrafficLightEstimatorNode]: Start loading lanelet"); lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); @@ -117,7 +117,7 @@ void CrosswalkTrafficLightEstimatorNode::onMap(const HADMapBin::ConstSharedPtr m lanelet::routing::RoutingGraphContainer overall_graphs({vehicle_graph, pedestrian_graph}); overall_graphs_ptr_ = std::make_shared(overall_graphs); - RCLCPP_INFO(get_logger(), "[CrosswalkTrafficLightEstimatorNode]: Map is loaded"); + RCLCPP_DEBUG(get_logger(), "[CrosswalkTrafficLightEstimatorNode]: Map is loaded"); } void CrosswalkTrafficLightEstimatorNode::onRoute(const LaneletRoute::ConstSharedPtr msg) diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp index 4331b5c19d3f1..e3c3263466033 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -27,6 +28,7 @@ #include #include +#include #include namespace object_lanelet_filter @@ -48,6 +50,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node rclcpp::Publisher::SharedPtr object_pub_; rclcpp::Subscription::SharedPtr map_sub_; rclcpp::Subscription::SharedPtr object_sub_; + std::unique_ptr debug_publisher_{nullptr}; lanelet::LaneletMapPtr lanelet_map_ptr_; lanelet::ConstLanelets road_lanelets_; diff --git a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp index 2d70247445043..eb6da6bc45b24 100644 --- a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp @@ -20,6 +20,7 @@ #include "detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp" #include +#include #include #include @@ -133,6 +134,7 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node rclcpp::Publisher::SharedPtr objects_pub_; message_filters::Subscriber objects_sub_; message_filters::Subscriber obstacle_pointcloud_sub_; + std::unique_ptr debug_publisher_{nullptr}; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index cd16d414425cb..74000a91e60fc 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -52,6 +52,9 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod "input/object", rclcpp::QoS{1}, std::bind(&ObjectLaneletFilterNode::objectCallback, this, _1)); object_pub_ = this->create_publisher( "output/object", rclcpp::QoS{1}); + + debug_publisher_ = + std::make_unique(this, "object_lanelet_filter"); } void ObjectLaneletFilterNode::mapCallback( @@ -116,6 +119,15 @@ void ObjectLaneletFilterNode::objectCallback( ++index; } object_pub_->publish(output_object_msg); + + // Publish debug info + const double pipeline_latency = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - output_object_msg.header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency); } geometry_msgs::msg::Polygon ObjectLaneletFilterNode::setFootprint( diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp index d903a9ca3bb41..f3f56652792e9 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -304,6 +304,8 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( objects_pub_ = create_publisher( "~/output/objects", rclcpp::QoS{1}); + debug_publisher_ = std::make_unique( + this, "obstacle_pointcloud_based_validator"); const bool enable_debugger = declare_parameter("enable_debugger", false); if (enable_debugger) debugger_ = std::make_shared(this); @@ -350,6 +352,14 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( debugger_->publishNeighborPointcloud(input_obstacle_pointcloud->header); debugger_->publishPointcloudWithinPolygon(input_obstacle_pointcloud->header); } + + // Publish processing time info + const double pipeline_latency = + std::chrono::duration( + std::chrono::nanoseconds((this->get_clock()->now() - output.header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency); } } // namespace obstacle_pointcloud_based_validator diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp index a890c19e25d06..746ef1bafb583 100644 --- a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp @@ -64,10 +64,7 @@ void EuclideanClusterNode::onPointCloud( cluster_pub_->publish(output); // build debug msg - if (debug_pub_->get_subscription_count() < 1) { - return; - } - { + if (debug_pub_->get_subscription_count() >= 1) { sensor_msgs::msg::PointCloud2 debug; convertObjectMsg2SensorMsg(output, debug); debug_pub_->publish(debug); @@ -75,10 +72,16 @@ void EuclideanClusterNode::onPointCloud( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + const double pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds((this->get_clock()->now() - output.header.stamp).nanoseconds())) + .count(); debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp index a28791e5d4057..f58d9ac6dcc48 100644 --- a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp @@ -76,10 +76,7 @@ void VoxelGridBasedEuclideanClusterNode::onPointCloud( cluster_pub_->publish(output); // build debug msg - if (debug_pub_->get_subscription_count() < 1) { - return; - } - { + if (debug_pub_->get_subscription_count() >= 1) { sensor_msgs::msg::PointCloud2 debug; convertObjectMsg2SensorMsg(output, debug); debug_pub_->publish(debug); @@ -87,10 +84,16 @@ void VoxelGridBasedEuclideanClusterNode::onPointCloud( if (debug_publisher_) { const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds((this->get_clock()->now() - output.header.stamp).nanoseconds())) + .count(); debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/perception/front_vehicle_velocity_estimator/CMakeLists.txt b/perception/front_vehicle_velocity_estimator/CMakeLists.txt deleted file mode 100644 index 98094d029a316..0000000000000 --- a/perception/front_vehicle_velocity_estimator/CMakeLists.txt +++ /dev/null @@ -1,30 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(front_vehicle_velocity_estimator) - -# Dependencies -find_package(autoware_cmake REQUIRED) -autoware_package() - -# Targets -ament_auto_add_library(front_vehicle_velocity_estimator_node_component SHARED - src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator_node.cpp - src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator.cpp -) - -rclcpp_components_register_node(front_vehicle_velocity_estimator_node_component - PLUGIN "front_vehicle_velocity_estimator::FrontVehicleVelocityEstimatorNode" - EXECUTABLE front_vehicle_velocity_estimator_node -) - -# Tests -if(BUILD_TESTING) - list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -# Package -ament_auto_package( - INSTALL_TO_SHARE - launch -) diff --git a/perception/front_vehicle_velocity_estimator/README.md b/perception/front_vehicle_velocity_estimator/README.md deleted file mode 100644 index 2225880db101c..0000000000000 --- a/perception/front_vehicle_velocity_estimator/README.md +++ /dev/null @@ -1,45 +0,0 @@ -# front_vehicle_velocity_estimator - -This package contains a front vehicle velocity estimation for offline perception module analysis. -This package can: - -- Attach velocity to 3D detections from velocity estimation with LiDAR pointcloud. - -## Algorithm - -- Processing flow - 1. Choose front vehicle from front area objects. - 2. Choose nearest neighbor point within front vehicle. - 3. Estimate velocity of front vehicle by using the differentiated value from time series of nearest neighbor point positions. - 4. Compensate ego vehicle twist - -## Input - -| Name | Type | Description | -| -------------------- | ---------------------------------------------------- | -------------------- | -| `~/input/objects` | autoware_auto_perception_msgs/msg/DetectedObject.msg | 3D detected objects. | -| `~/input/pointcloud` | sensor_msgs/msg/PointCloud2.msg | LiDAR pointcloud. | -| `~/input/odometry` | nav_msgs::msg::Odometry.msg | Odometry data. | - -## Output - -| Name | Type | Description | -| ------------------------------------- | ----------------------------------------------------- | --------------------------------------------- | -| `~/output/objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | 3D detected object with twist. | -| `~/debug/nearest_neighbor_pointcloud` | sensor_msgs/msg/PointCloud2.msg | The pointcloud msg of nearest neighbor point. | - -## Node parameter - -| Name | Type | Description | Default value | -| :--------------- | :----- | :-------------------- | :------------ | -| `update_rate_hz` | double | The update rate [hz]. | 10.0 | - -## Core parameter - -| Name | Type | Description | Default value | -| :---------------------------- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| `moving_average_num` | int | The moving average number for velocity estimation. | 1 | -| `threshold_pointcloud_z_high` | float | The threshold for z position value of point when choosing nearest neighbor point within front vehicle [m]. If z > `threshold_pointcloud_z_high`, the point is considered to noise. | 1.0f | -| `threshold_pointcloud_z_low` | float | The threshold for z position value of point when choosing nearest neighbor point within front vehicle [m]. If z < `threshold_pointcloud_z_low`, the point is considered to noise like ground. | 0.6f | -| `threshold_relative_velocity` | double | The threshold for min and max of estimated relative velocity ($v_{re}$) [m/s]. If $v_{re}$ < - `threshold_relative_velocity` , then $v_{re}$ = - `threshold_relative_velocity`. If $v_{re}$ > `threshold_relative_velocity`, then $v_{re}$ = `threshold_relative_velocity`. | 10.0 | -| `threshold_absolute_velocity` | double | The threshold for max of estimated absolute velocity ($v_{ae}$) [m/s]. If $v_{ae}$ > `threshold_absolute_velocity`, then $v_{ae}$ = `threshold_absolute_velocity`. | 20.0 | diff --git a/perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp b/perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp deleted file mode 100644 index 01f738fc6beaa..0000000000000 --- a/perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp +++ /dev/null @@ -1,107 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef FRONT_VEHICLE_VELOCITY_ESTIMATOR__FRONT_VEHICLE_VELOCITY_ESTIMATOR_HPP_ -#define FRONT_VEHICLE_VELOCITY_ESTIMATOR__FRONT_VEHICLE_VELOCITY_ESTIMATOR_HPP_ - -#include "pcl/point_cloud.h" -#include "pcl/point_types.h" -#include "pcl_conversions/pcl_conversions.h" -#include "rclcpp/logger.hpp" -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" - -#include "autoware_auto_perception_msgs/msg/detected_objects.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "sensor_msgs/msg/point_cloud2.hpp" - -#include -#include -#include - -namespace front_vehicle_velocity_estimator -{ -using autoware_auto_perception_msgs::msg::DetectedObject; -using autoware_auto_perception_msgs::msg::DetectedObjectKinematics; -using autoware_auto_perception_msgs::msg::DetectedObjects; -using autoware_auto_perception_msgs::msg::ObjectClassification; -using nav_msgs::msg::Odometry; -using sensor_msgs::msg::PointCloud2; -using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::Point2d; - -class FrontVehicleVelocityEstimator -{ -public: - explicit FrontVehicleVelocityEstimator(const rclcpp::Logger & logger) : logger_(logger) {} - - struct Input - { - PointCloud2::ConstSharedPtr pointcloud{}; - DetectedObjects::ConstSharedPtr objects{}; - Odometry::ConstSharedPtr odometry{}; - }; - - struct Output - { - DetectedObjects objects{}; - PointCloud2 nearest_neighbor_pointcloud{}; - }; - - struct Param - { - int moving_average_num{}; - float threshold_pointcloud_z_high{}; - float threshold_pointcloud_z_low{}; - double threshold_relative_velocity{}; - double threshold_absolute_velocity{}; - }; - - void setParam(const Param & param) { param_ = param; } - Output update(const Input & input); - -private: - struct ObjectsWithFrontVehicle - { - DetectedObjects::SharedPtr objects_without_front_vehicle{}; - DetectedObject front_vehicle{}; - bool is_front_vehicle = false; - }; - - rclcpp::Logger logger_; - - // Buffer data - Param param_{}; - std::deque velocity_queue_{}; - rclcpp::Time prev_time_{}; - pcl::PointXYZ prev_point_{}; - - // Function - LinearRing2d createBoxArea(const Point2d size); - LinearRing2d createObjectArea(const DetectedObject & object); - ObjectsWithFrontVehicle filterFrontVehicle( - DetectedObjects::ConstSharedPtr objects, const LinearRing2d & front_area); - pcl::PointXYZ getNearestNeighborPoint( - const DetectedObject & object, PointCloud2::ConstSharedPtr pointcloud, - const Point2d & front_size); - double estimateRelativeVelocity(const pcl::PointXYZ & point, const rclcpp::Time & header_time); - double estimateAbsoluteVelocity( - const double relative_velocity, Odometry::ConstSharedPtr odometry); - bool isFrontVehicle(const DetectedObject & object, const LinearRing2d & front_area); - bool isWithinVehicle( - const DetectedObject & object, const pcl::PointXYZ & point, const Point2d & front_size); -}; - -} // namespace front_vehicle_velocity_estimator - -#endif // FRONT_VEHICLE_VELOCITY_ESTIMATOR__FRONT_VEHICLE_VELOCITY_ESTIMATOR_HPP_ diff --git a/perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator_node.hpp b/perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator_node.hpp deleted file mode 100644 index 8b42b61606677..0000000000000 --- a/perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator_node.hpp +++ /dev/null @@ -1,98 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef FRONT_VEHICLE_VELOCITY_ESTIMATOR__FRONT_VEHICLE_VELOCITY_ESTIMATOR_NODE_HPP_ -#define FRONT_VEHICLE_VELOCITY_ESTIMATOR__FRONT_VEHICLE_VELOCITY_ESTIMATOR_NODE_HPP_ - -#include "front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp" -#include "rclcpp/rclcpp.hpp" - -#include "autoware_auto_perception_msgs/msg/detected_objects.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "sensor_msgs/msg/point_cloud2.hpp" - -#include -#include -#include - -#include -#include -#include -#include - -namespace front_vehicle_velocity_estimator -{ -using autoware_auto_perception_msgs::msg::DetectedObject; -using autoware_auto_perception_msgs::msg::DetectedObjects; -using nav_msgs::msg::Odometry; -using sensor_msgs::msg::PointCloud2; -using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::Point2d; - -class FrontVehicleVelocityEstimatorNode : public rclcpp::Node -{ -public: - explicit FrontVehicleVelocityEstimatorNode(const rclcpp::NodeOptions & node_options); - - struct NodeParam - { - double update_rate_hz{}; - }; - -private: - // Subscriber - message_filters::Subscriber sub_pointcloud_{}; - message_filters::Subscriber sub_objects_{}; - message_filters::Subscriber sub_odometry_{}; - - using SyncPolicy = - message_filters::sync_policies::ApproximateTime; - using Sync = message_filters::Synchronizer; - typename std::shared_ptr sync_ptr_; - - // Callback - void onData( - const PointCloud2::ConstSharedPtr pointcloud_msg, - const DetectedObjects::ConstSharedPtr object_msg, const Odometry::ConstSharedPtr odometry_msg); - - // Data Buffer - PointCloud2::ConstSharedPtr pointcloud_data_{}; - DetectedObjects::ConstSharedPtr objects_data_{}; - Odometry::ConstSharedPtr odometry_data_{}; - - // Publisher - rclcpp::Publisher::SharedPtr pub_objects_{}; - rclcpp::Publisher::SharedPtr pub_nearest_neighbor_pointcloud_{}; - - // Timer - bool isDataReady(); - - // Parameter Server - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - rcl_interfaces::msg::SetParametersResult onSetParam( - const std::vector & params); - - // Parameter - NodeParam node_param_{}; - - // Core - FrontVehicleVelocityEstimator::Input input_{}; - FrontVehicleVelocityEstimator::Output output_{}; - FrontVehicleVelocityEstimator::Param core_param_{}; - std::unique_ptr front_vehicle_velocity_estimator_{}; -}; - -} // namespace front_vehicle_velocity_estimator - -#endif // FRONT_VEHICLE_VELOCITY_ESTIMATOR__FRONT_VEHICLE_VELOCITY_ESTIMATOR_NODE_HPP_ diff --git a/perception/front_vehicle_velocity_estimator/launch/front_vehicle_velocity_estimator.launch.xml b/perception/front_vehicle_velocity_estimator/launch/front_vehicle_velocity_estimator.launch.xml deleted file mode 100644 index 518791821a0ac..0000000000000 --- a/perception/front_vehicle_velocity_estimator/launch/front_vehicle_velocity_estimator.launch.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/perception/front_vehicle_velocity_estimator/package.xml b/perception/front_vehicle_velocity_estimator/package.xml deleted file mode 100644 index 3b1af8cf544ac..0000000000000 --- a/perception/front_vehicle_velocity_estimator/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - front_vehicle_velocity_estimator - 0.1.0 - front_vehicle_velocity_estimator - Satoshi Tanaka - Shunsuke Miura - Apache License 2.0 - Satoshi Tanaka - - ament_cmake_auto - autoware_cmake - - autoware_auto_perception_msgs - geometry_msgs - message_filters - nav_msgs - pcl_conversions - pcl_ros - rclcpp - rclcpp_components - sensor_msgs - tier4_autoware_utils - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/perception/front_vehicle_velocity_estimator/src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator.cpp b/perception/front_vehicle_velocity_estimator/src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator.cpp deleted file mode 100644 index 6f59823615c04..0000000000000 --- a/perception/front_vehicle_velocity_estimator/src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator.cpp +++ /dev/null @@ -1,248 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp" - -#include "tier4_autoware_utils/geometry/geometry.hpp" - -#include - -#include -#include -#include - -namespace front_vehicle_velocity_estimator -{ -FrontVehicleVelocityEstimator::Output FrontVehicleVelocityEstimator::update( - const FrontVehicleVelocityEstimator::Input & input) -{ - Output output_{}; - ObjectsWithFrontVehicle objects_with_front_vehicle{}; - - // Filter front vehicle - const Point2d front_size{100.0, 3.0}; - LinearRing2d front_area = createBoxArea(front_size); - objects_with_front_vehicle = filterFrontVehicle(input.objects, front_area); - - // Get nearest neighbor pointcloud - pcl::PointXYZ nearest_neighbor_point = - getNearestNeighborPoint(objects_with_front_vehicle.front_vehicle, input.pointcloud, front_size); - - // Set objects output - output_.objects = *(objects_with_front_vehicle.objects_without_front_vehicle); - - // If there is no front vehicle, return output - if (!objects_with_front_vehicle.is_front_vehicle) { - return output_; - } - - // Estimate relative velocity - double now_relative_velocity = - estimateRelativeVelocity(nearest_neighbor_point, input.pointcloud->header.stamp); - - // Estimate absolute velocity - double now_absolute_velocity = estimateAbsoluteVelocity(now_relative_velocity, input.odometry); - - // Validate velocity - if ( - !isfinite(now_relative_velocity) || - now_relative_velocity > param_.threshold_relative_velocity || - now_relative_velocity < -param_.threshold_relative_velocity) { - output_.objects.objects.emplace_back(objects_with_front_vehicle.front_vehicle); - return output_; - } - now_absolute_velocity = std::min(param_.threshold_absolute_velocity, now_absolute_velocity); - - // Set queue of nearest_neighbor_point - if (static_cast(velocity_queue_.size()) >= param_.moving_average_num) { - velocity_queue_.pop_front(); - } - - // Estimate average velocity - velocity_queue_.push_back(now_absolute_velocity); - double velocity = std::accumulate(std::begin(velocity_queue_), std::end(velocity_queue_), 0.0) / - velocity_queue_.size(); - // RCLCPP_INFO( - // rclcpp::get_logger("front_vehicle_velocity_estimator"), "x=%f, v=%f km/h, v_a=%f km/h", - // objects_with_front_vehicle.front_vehicle.kinematics.pose_with_covariance.pose.position.x, - // now_absolute_velocity * 3.6, velocity * 3.6); - - // Set kinematics information for front vehicle - objects_with_front_vehicle.front_vehicle.kinematics.has_twist = true; - objects_with_front_vehicle.front_vehicle.kinematics.twist_with_covariance.twist.linear.x = - velocity; - objects_with_front_vehicle.front_vehicle.kinematics.has_twist_covariance = true; - objects_with_front_vehicle.front_vehicle.kinematics.twist_with_covariance.covariance.at(0) = 1.0; - output_.objects.objects.emplace_back(objects_with_front_vehicle.front_vehicle); - - // Set previous data buffer - prev_time_ = input.pointcloud->header.stamp; - prev_point_ = nearest_neighbor_point; - - // Set nearest_neighbor_pointcloud output for debug - pcl::PointCloud output_pointcloud; - output_pointcloud.points.push_back(nearest_neighbor_point); - pcl::toROSMsg(output_pointcloud, output_.nearest_neighbor_pointcloud); - output_.nearest_neighbor_pointcloud.header = input.pointcloud->header; - - return output_; -} - -// Create front area x = 0 ~ size.x, y = -size.y ~ size.y -LinearRing2d FrontVehicleVelocityEstimator::createBoxArea(const Point2d size) -{ - LinearRing2d box{}; - box.push_back(Point2d{0.0, size.y()}); - box.push_back(Point2d{size.x(), size.y()}); - box.push_back(Point2d{size.x(), -size.y()}); - box.push_back(Point2d{0.0, -size.y()}); - box.push_back(Point2d{0.0, size.y()}); - return box; -} - -// Create object area -LinearRing2d FrontVehicleVelocityEstimator::createObjectArea(const DetectedObject & object) -{ - Point2d object_size{object.shape.dimensions.x, object.shape.dimensions.y}; - const double x_front = object_size.x() / 2.0; - const double x_rear = -object_size.x() / 2.0; - const double y_left = object_size.y() / 2.0; - const double y_right = -object_size.y() / 2.0; - - LinearRing2d box{}; - box.push_back(Point2d{x_front, y_left}); - box.push_back(Point2d{x_front, y_right}); - box.push_back(Point2d{x_rear, y_right}); - box.push_back(Point2d{x_rear, y_left}); - box.push_back(Point2d{x_front, y_left}); - - LinearRing2d transformed_box = tier4_autoware_utils::transformVector( - box, tier4_autoware_utils::pose2transform(object.kinematics.pose_with_covariance.pose)); - - return transformed_box; -} - -// Filter for a front vehicle. -FrontVehicleVelocityEstimator::ObjectsWithFrontVehicle -FrontVehicleVelocityEstimator::filterFrontVehicle( - DetectedObjects::ConstSharedPtr objects, const LinearRing2d & front_area) -{ - // Initialize output - ObjectsWithFrontVehicle output{}; - output.is_front_vehicle = false; - - DetectedObjects output_objects_{}; - output_objects_.header = objects->header; - - for (const auto & object : objects->objects) { - if (!isFrontVehicle(object, front_area)) { - output_objects_.objects.emplace_back(object); - } else if (!output.is_front_vehicle) { - output.front_vehicle = object; - output.is_front_vehicle = true; - } else { - auto front_vehicle_position = - output.front_vehicle.kinematics.pose_with_covariance.pose.position; - if (object.kinematics.pose_with_covariance.pose.position.x < front_vehicle_position.x) { - output_objects_.objects.emplace_back(output.front_vehicle); - output.front_vehicle = object; - } else { - output_objects_.objects.emplace_back(object); - } - } - } - output.objects_without_front_vehicle = std::make_shared(output_objects_); - return output; -} - -pcl::PointXYZ FrontVehicleVelocityEstimator::getNearestNeighborPoint( - const DetectedObject & object, PointCloud2::ConstSharedPtr pointcloud, const Point2d & front_size) -{ - pcl::PointCloud::Ptr pcl_msg(new pcl::PointCloud); - pcl::fromROSMsg(*pointcloud, *pcl_msg); - - // Initialize - pcl::PointXYZ nearest_neighbor_point; - bool is_initialized = false; - - for (const auto & point : *pcl_msg) { - if (isWithinVehicle(object, point, front_size)) { - if (!is_initialized) { - nearest_neighbor_point = point; - } else if (point.x < nearest_neighbor_point.x) { - nearest_neighbor_point = point; - } - } - } - return nearest_neighbor_point; -} - -bool FrontVehicleVelocityEstimator::isWithinVehicle( - const DetectedObject & object, const pcl::PointXYZ & point, const Point2d & front_size) -{ - LinearRing2d object_ring_2d = createObjectArea(object); - Point2d point_{point.x, point.y}; - if (point.z < param_.threshold_pointcloud_z_low) { - return false; - } else if (point.z > param_.threshold_pointcloud_z_high) { - return false; - } else if (point.x < 0) { - return false; - } else if (point.x > front_size.x()) { - return false; - } else if (point.y < -front_size.y()) { - return false; - } else if (point.y > front_size.y()) { - return false; - } else if (!boost::geometry::within(point_, object_ring_2d)) { - return false; - } - return true; -} - -double FrontVehicleVelocityEstimator::estimateRelativeVelocity( - const pcl::PointXYZ & point, const rclcpp::Time & header_time) -{ - if (velocity_queue_.size() == 0) { - return 0.0; - } - const double dt = (header_time - prev_time_).seconds(); - const double relative_velocity = (point.x - prev_point_.x) / dt; - return relative_velocity; -} - -double FrontVehicleVelocityEstimator::estimateAbsoluteVelocity( - const double relative_velocity, Odometry::ConstSharedPtr odometry) -{ - const double velocity = relative_velocity + odometry->twist.twist.linear.x; - return velocity; -} - -bool FrontVehicleVelocityEstimator::isFrontVehicle( - const DetectedObject & object, const LinearRing2d & front_area) -{ - auto position = object.kinematics.pose_with_covariance.pose.position; - auto label = object.classification.at(0).label; - Point2d object_point{position.x, position.y}; - if ( - !(label == ObjectClassification::UNKNOWN) && !(label == ObjectClassification::PEDESTRIAN) && - !(label == ObjectClassification::BICYCLE) && !(label == ObjectClassification::MOTORCYCLE) && - boost::geometry::within(object_point, front_area)) { - return true; - } else { - return false; - } -} - -} // namespace front_vehicle_velocity_estimator diff --git a/perception/front_vehicle_velocity_estimator/src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator_node.cpp b/perception/front_vehicle_velocity_estimator/src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator_node.cpp deleted file mode 100644 index d79164bf54e0d..0000000000000 --- a/perception/front_vehicle_velocity_estimator/src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator_node.cpp +++ /dev/null @@ -1,185 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "front_vehicle_velocity_estimator/front_vehicle_velocity_estimator_node.hpp" - -#include "rclcpp/rclcpp.hpp" - -#include -#include -#include - -using namespace std::literals; -using std::chrono::duration; -using std::chrono::duration_cast; -using std::chrono::nanoseconds; - -namespace -{ -template -bool update_param( - const std::vector & params, const std::string & name, T & value) -{ - const auto itr = std::find_if( - params.cbegin(), params.cend(), - [&name](const rclcpp::Parameter & p) { return p.get_name() == name; }); - - // Not found - if (itr == params.cend()) { - return false; - } - - value = itr->template get_value(); - return true; -} -} // namespace - -namespace front_vehicle_velocity_estimator -{ - -FrontVehicleVelocityEstimatorNode::FrontVehicleVelocityEstimatorNode( - const rclcpp::NodeOptions & node_options) -: Node("front_vehicle_velocity_estimator", node_options) -{ - // Parameter Server - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&FrontVehicleVelocityEstimatorNode::onSetParam, this, std::placeholders::_1)); - - // Node Parameter - node_param_.update_rate_hz = declare_parameter("node_params.update_rate_hz", 10.0); - - // Core Parameter - core_param_.moving_average_num = declare_parameter("core_params.moving_average_num", 1); - core_param_.threshold_pointcloud_z_high = - declare_parameter("core_params.threshold_pointcloud_z_high", 1.0f); - core_param_.threshold_pointcloud_z_low = - declare_parameter("core_params.threshold_pointcloud_z_low", 0.6f); - core_param_.threshold_relative_velocity = - declare_parameter("core_params.threshold_relative_velocity", 10.0); - core_param_.threshold_absolute_velocity = - declare_parameter("core_params.threshold_absolute_velocity", 20.0); - - // Core - front_vehicle_velocity_estimator_ = std::make_unique(get_logger()); - front_vehicle_velocity_estimator_->setParam(core_param_); - - // Subscriber - sub_pointcloud_.subscribe( - this, "~/input/pointcloud", rclcpp::SensorDataQoS().get_rmw_qos_profile()); - sub_objects_.subscribe(this, "~/input/objects", rclcpp::QoS{1}.get_rmw_qos_profile()); - sub_odometry_.subscribe(this, "~/input/odometry", rclcpp::QoS{1}.get_rmw_qos_profile()); - - using std::placeholders::_1; - using std::placeholders::_2; - using std::placeholders::_3; - sync_ptr_ = std::make_shared(SyncPolicy(20), sub_pointcloud_, sub_objects_, sub_odometry_); - sync_ptr_->registerCallback( - std::bind(&FrontVehicleVelocityEstimatorNode::onData, this, _1, _2, _3)); - - // Publisher - pub_objects_ = create_publisher("~/output/objects", 1); - pub_nearest_neighbor_pointcloud_ = - create_publisher("~/debug/nearest_neighbor_pointcloud", 1); -} - -void FrontVehicleVelocityEstimatorNode::onData( - const PointCloud2::ConstSharedPtr pointcloud_msg, - const DetectedObjects::ConstSharedPtr object_msg, const Odometry::ConstSharedPtr odometry_msg) -{ - pointcloud_data_ = pointcloud_msg; - objects_data_ = object_msg; - odometry_data_ = odometry_msg; - - if (!isDataReady()) { - return; - } - - if (!odometry_data_) { - // If odometry data does not come, publish original objects - pub_objects_->publish(*objects_data_); - } else { - // Set input data - input_.objects = objects_data_; - input_.pointcloud = pointcloud_data_; - input_.odometry = odometry_data_; - - // Update - output_ = front_vehicle_velocity_estimator_->update(input_); - - // Publish - pub_objects_->publish(output_.objects); - pub_nearest_neighbor_pointcloud_->publish(output_.nearest_neighbor_pointcloud); - } -} - -rcl_interfaces::msg::SetParametersResult FrontVehicleVelocityEstimatorNode::onSetParam( - const std::vector & params) -{ - rcl_interfaces::msg::SetParametersResult result; - - try { - // Node Parameter - { - auto & p = node_param_; - - // Update params - update_param(params, "node_params.update_rate_hz", p.update_rate_hz); - } - - // Core Parameter - { - auto & p = core_param_; - - // Update params - update_param(params, "core_params.moving_average_num", p.moving_average_num); - update_param( - params, "core_params.threshold_pointcloud_z_high", p.threshold_pointcloud_z_high); - update_param(params, "core_params.threshold_pointcloud_z_low", p.threshold_pointcloud_z_low); - update_param( - params, "core_params.threshold_relative_velocity", p.threshold_relative_velocity); - update_param( - params, "core_params.threshold_absolute_velocity", p.threshold_absolute_velocity); - - // Set parameter to instance - if (front_vehicle_velocity_estimator_) { - front_vehicle_velocity_estimator_->setParam(core_param_); - } - } - } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { - result.successful = false; - result.reason = e.what(); - return result; - } - - result.successful = true; - result.reason = "success"; - return result; -} - -bool FrontVehicleVelocityEstimatorNode::isDataReady() -{ - if (!objects_data_) { - RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, "waiting for objects msg..."); - return false; - } - if (!pointcloud_data_) { - RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, "waiting for pointcloud msg..."); - return false; - } - return true; -} -} // namespace front_vehicle_velocity_estimator - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(front_vehicle_velocity_estimator::FrontVehicleVelocityEstimatorNode) diff --git a/perception/heatmap_visualizer/CMakeLists.txt b/perception/heatmap_visualizer/CMakeLists.txt deleted file mode 100644 index 75fa02a1cdcf7..0000000000000 --- a/perception/heatmap_visualizer/CMakeLists.txt +++ /dev/null @@ -1,20 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(heatmap_visualizer) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(heatmap_visualizer_component SHARED - src/heatmap_visualizer_node.cpp - src/utils.cpp -) - -rclcpp_components_register_node(heatmap_visualizer_component - PLUGIN "heatmap_visualizer::HeatmapVisualizerNode" - EXECUTABLE heatmap_visualizer -) - -ament_auto_package(INSTALL_TO_SHARE - launch - config -) diff --git a/perception/heatmap_visualizer/README.md b/perception/heatmap_visualizer/README.md deleted file mode 100644 index 747a158b3d487..0000000000000 --- a/perception/heatmap_visualizer/README.md +++ /dev/null @@ -1,92 +0,0 @@ -# heatmap_visualizer - -## Purpose - -heatmap_visualizer is a package for visualizing heatmap of detected 3D objects' positions on the BEV space. - -This package is used for qualitative evaluation and trend analysis of the detector, it means, for instance, the heatmap shows "This detector performs good for near around of our vehicle, but far is bad". - -### How to run - -```shell -ros2 launch heatmap_visualizer heatmap_visualizer.launch.xml input/objects:= -``` - -## Inner-workings / Algorithms - -In this implementation, create heatmap of the center position of detected objects for each classes, for instance, CAR, PEDESTRIAN, etc, and publish them as occupancy grid maps. - -![heatmap_visualizer_sample](./image/heatmap_sample.png) - -In the above figure, the pink represents high detection frequency area and blue one is low, or black represents there is no detection. - -![heatmap_visualizer](./image/heatmap_visualizer.png) - -As inner-workings, add center positions of detected objects to index of each corresponding grid map cell in a buffer. -The created heatmap will be published by each specific frame, which can be specified with `frame_count`. Note that the buffer to be add the positions is not reset per publishing. -When publishing, firstly these values are normalized to [0, 1] using maximum and minimum values in the buffer. Secondly, they are scaled to integer in [0, 100] because `nav_msgs::msg::OccupancyGrid` only allow the value in [0, 100]. - -## Inputs / Outputs - -### Input - -| Name | Type | Description | -| ----------------- | ----------------------------------------------------- | ---------------- | -| `~/input/objects` | `autoware_auto_perception_msgs::msg::DetectedObjects` | detected objects | - -### Output - -| Name | Type | Description | -| ------------------------------- | ------------------------------ | ------------------ | -| `~/output/objects/` | `nav_msgs::msg::OccupancyGrid` | visualized heatmap | - -## Parameters - -### Core Parameters - -| Name | Type | Default Value | Description | -| --------------------- | ------------- | ------------------------------------------------------------------------------------- | ----------------------------------------------- | -| `publish_frame_count` | int | `50` | The number of frames to publish heatmap | -| `heatmap_frame_id` | string | `base_link` | The frame ID of heatmap to be respected | -| `heatmap_length` | float | `200.0` | A length of map in meter | -| `heatmap_resolution` | float | `0.8` | A resolution of map | -| `use_confidence` | bool | `false` | A flag if use confidence score as heatmap value | -| `class_names` | array | `["UNKNOWN", "CAR", "TRUCK", "BUS", "TRAILER", "BICYCLE", "MOTORBIKE", "PEDESTRIAN"]` | An array of class names to be published | -| `rename_to_car` | bool | `true` | A flag if rename car like vehicle to car | - -## Assumptions / Known limits - -The heatmap depends on the data to be used, so if the objects in data are sparse the heatmap will be sparse. - -## (Optional) Error detection and handling - - - -## (Optional) Performance characterization - - - -## References/External links - -## (Optional) Future extensions / Unimplemented parts - - diff --git a/perception/heatmap_visualizer/config/heatmap_visualizer.param.yaml b/perception/heatmap_visualizer/config/heatmap_visualizer.param.yaml deleted file mode 100644 index 2da863ea5e37a..0000000000000 --- a/perception/heatmap_visualizer/config/heatmap_visualizer.param.yaml +++ /dev/null @@ -1,33 +0,0 @@ -# Copyright 2023 TIER IV, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -/**: - ros__parameters: - publish_frame_count: 50 - heatmap_frame_id: "base_link" - heatmap_length: 200.0 - heatmap_resolution: 0.8 - use_confidence: false - class_names: - [ - "UNKNOWN", - "CAR", - "TRUCK", - "BUS", - "TRAILER", - "BICYCLE", - "MOTORBIKE", - "PEDESTRIAN", - ] - rename_to_car: false diff --git a/perception/heatmap_visualizer/image/heatmap_sample.png b/perception/heatmap_visualizer/image/heatmap_sample.png deleted file mode 100644 index 42a0c8eab4a98..0000000000000 Binary files a/perception/heatmap_visualizer/image/heatmap_sample.png and /dev/null differ diff --git a/perception/heatmap_visualizer/image/heatmap_visualizer.png b/perception/heatmap_visualizer/image/heatmap_visualizer.png deleted file mode 100644 index cf43ddedeb6ce..0000000000000 Binary files a/perception/heatmap_visualizer/image/heatmap_visualizer.png and /dev/null differ diff --git a/perception/heatmap_visualizer/include/heatmap_visualizer/heatmap_visualizer_node.hpp b/perception/heatmap_visualizer/include/heatmap_visualizer/heatmap_visualizer_node.hpp deleted file mode 100644 index 508ce238b51f1..0000000000000 --- a/perception/heatmap_visualizer/include/heatmap_visualizer/heatmap_visualizer_node.hpp +++ /dev/null @@ -1,79 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef HEATMAP_VISUALIZER__HEATMAP_VISUALIZER_NODE_HPP_ -#define HEATMAP_VISUALIZER__HEATMAP_VISUALIZER_NODE_HPP_ - -#include - -#include -#include -#include - -#include -#include -#include - -namespace heatmap_visualizer -{ -class HeatmapVisualizerNode : public rclcpp::Node -{ -public: - /** - * @brief Construct a new Heatmap Visualizer Node object - * - * @param node_options - */ - explicit HeatmapVisualizerNode(const rclcpp::NodeOptions & node_options); - -private: - /** - * @brief Callback function - * - * @param msg - */ - void detectedObjectsCallback( - const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg); - - // Subscriber - rclcpp::Subscription::SharedPtr objects_sub_; - - // Publishers - std::map::SharedPtr> heatmap_pubs_; - - std::map heatmaps_; - - uint32_t frame_count_; - - // ROS params - uint32_t total_frame_; - std::string target_frame_; - std::string src_frame_; - std::string map_frame_; - float map_length_; - float map_resolution_; - bool use_confidence_; - std::vector class_names_{"UNKNOWN", "CAR", "TRUCK", "BUS", - "TRAILER", "BICYCLE", "MOTORBIKE", "PEDESTRIAN"}; - bool rename_car_to_truck_and_bus_; - - // Number of width and height cells - uint32_t width_; - uint32_t height_; - std::map> data_buffers_; -}; // class HeatmapVisualizerNode - -} // namespace heatmap_visualizer - -#endif // HEATMAP_VISUALIZER__HEATMAP_VISUALIZER_NODE_HPP_ diff --git a/perception/heatmap_visualizer/include/heatmap_visualizer/utils.hpp b/perception/heatmap_visualizer/include/heatmap_visualizer/utils.hpp deleted file mode 100644 index cd0c833e2dea8..0000000000000 --- a/perception/heatmap_visualizer/include/heatmap_visualizer/utils.hpp +++ /dev/null @@ -1,109 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef HEATMAP_VISUALIZER__UTILS_HPP_ -#define HEATMAP_VISUALIZER__UTILS_HPP_ - -#include -#include -#include -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -#include -#include - -namespace heatmap_visualizer -{ -using Label = autoware_auto_perception_msgs::msg::ObjectClassification; - -/** - * @brief IndexXYT object - * - */ -struct IndexXYT -{ - int x; - int y; - int theta; -}; // struct IndexXYT - -/** - * @brief - * - * @param theta - * @param theta_size - * @return int - */ -int discretizeAngle(const double theta, const int theta_size); - -/** - * @brief - * - * @param costmap - * @param pose_local - * @param theta_size - * @return IndexXYT - */ -IndexXYT pose2index( - const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_local, - const int theta_size); - -/** - * @brief Set confidence score or 1 to box position to data buffer. - * - * @param obj - * @param heatmap - * @param data_buffer - * @param use_confidence - */ -void setHeatmapToBuffer( - const autoware_auto_perception_msgs::msg::DetectedObject & obj, - const nav_msgs::msg::OccupancyGrid & heatmap, std::vector * data_buffer, - const bool use_confidence); - -/** - * @brief Set the Heatmap To Occupancy Grid object - * - * @param data_buffer - * @param heatmap - */ -void setHeatmapToOccupancyGrid( - const std::vector & data_buffer, nav_msgs::msg::OccupancyGrid * heatmap); - -/** - * @brief Get the Semantic Type object - * - * @param class_name - * @return uint8_t - */ -uint8_t getSemanticType(const std::string & class_name); - -/** - * @brief Get the Class Name object - * - * @param label - * @return std::string - */ -std::string getClassName(const uint8_t label); - -} // namespace heatmap_visualizer - -#endif // HEATMAP_VISUALIZER__UTILS_HPP_ diff --git a/perception/heatmap_visualizer/launch/heatmap_visualizer.launch.xml b/perception/heatmap_visualizer/launch/heatmap_visualizer.launch.xml deleted file mode 100644 index 7251d776b6110..0000000000000 --- a/perception/heatmap_visualizer/launch/heatmap_visualizer.launch.xml +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/perception/heatmap_visualizer/package.xml b/perception/heatmap_visualizer/package.xml deleted file mode 100644 index ab75738cbd008..0000000000000 --- a/perception/heatmap_visualizer/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - heatmap_visualizer - 1.0.0 - The heatmap_visualizer package - Kotaro Uetake - Apache License 2.0 - - Kotaro Uetake - - ament_cmake_auto - autoware_cmake - - autoware_auto_perception_msgs - geometry_msgs - nav_msgs - object_recognition_utils - rclcpp - rclcpp_components - tf2 - tf2_geometry_msgs - tier4_autoware_utils - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/perception/heatmap_visualizer/schema/heatmap_visualizer.schema.json b/perception/heatmap_visualizer/schema/heatmap_visualizer.schema.json deleted file mode 100644 index cb67ae383c35a..0000000000000 --- a/perception/heatmap_visualizer/schema/heatmap_visualizer.schema.json +++ /dev/null @@ -1,81 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "Parameters for Heatmap Visualizer Node", - "type": "object", - "definitions": { - "heatmap_visualizer": { - "type": "object", - "properties": { - "publish_frame_count": { - "type": "integer", - "description": "The number of frames to be published at once.", - "default": 50, - "minimum": 1 - }, - "heatmap_frame_id": { - "type": "string", - "description": "A frame ID in which visualized heatmap is with respect to.", - "default": "base_link" - }, - "heatmap_length": { - "type": "number", - "description": "A size length of heatmap [m].", - "default": 200.0, - "exclusiveMinimum": 0.0 - }, - "heatmap_resolution": { - "type": "number", - "description": "A cell resolution of heatmap [m].", - "default": 0.8, - "exclusiveMinimum": 0.0 - }, - "use_confidence": { - "type": "boolean", - "description": "Indicates whether to use object existence probability as score. It this parameter is false, 1.0 is set to score.", - "default": false - }, - "class_names": { - "type": "array", - "description": "An array of object class names.", - "default": [ - "UNKNOWN", - "CAR", - "TRUCK", - "BUS", - "TRAILER", - "BICYCLE", - "MOTORBIKE", - "PEDESTRIAN" - ], - "uniqueItems": true - }, - "rename_to_car": { - "type": "boolean", - "description": "Indicates whether to rename car like vehicle into car.", - "default": false - } - }, - "required": [ - "publish_frame_count", - "heatmap_frame_id", - "heatmap_length", - "heatmap_resolution", - "use_confidence", - "class_names", - "rename_to_car" - ] - } - }, - "properties": { - "/**": { - "type": "object", - "properties": { - "ros__parameters": { - "$ref": "#/definitions/heatmap_visualizer" - } - }, - "required": ["ros__parameters"] - } - }, - "required": ["/**"] -} diff --git a/perception/heatmap_visualizer/src/heatmap_visualizer_node.cpp b/perception/heatmap_visualizer/src/heatmap_visualizer_node.cpp deleted file mode 100644 index d9337e0c9f61d..0000000000000 --- a/perception/heatmap_visualizer/src/heatmap_visualizer_node.cpp +++ /dev/null @@ -1,103 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "heatmap_visualizer/heatmap_visualizer_node.hpp" - -#include "heatmap_visualizer/utils.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" - -#include - -#include -#include - -namespace heatmap_visualizer -{ -HeatmapVisualizerNode::HeatmapVisualizerNode(const rclcpp::NodeOptions & node_options) -: Node("heatmap_visualizer", node_options), frame_count_(0) -{ - total_frame_ = static_cast(declare_parameter("publish_frame_count")); - map_frame_ = declare_parameter("heatmap_frame_id"); - map_length_ = static_cast(declare_parameter("heatmap_length")); - map_resolution_ = static_cast(declare_parameter("heatmap_resolution")); - use_confidence_ = declare_parameter("use_confidence"); - class_names_ = declare_parameter>("class_names"); - rename_car_to_truck_and_bus_ = declare_parameter("rename_to_car"); - - width_ = static_cast(map_length_ / map_resolution_); - height_ = static_cast(map_length_ / map_resolution_); - - objects_sub_ = create_subscription( - "/heatmap_visualizer/input/objects", 1, - std::bind(&HeatmapVisualizerNode::detectedObjectsCallback, this, std::placeholders::_1)); - - for (std::string & key : class_names_) { - nav_msgs::msg::OccupancyGrid heatmap; - heatmap.header.frame_id = map_frame_; - heatmap.info.width = width_; - heatmap.info.height = height_; - heatmap.info.resolution = map_resolution_; - heatmap.info.origin.position.x = -map_length_ / 2; - heatmap.info.origin.position.y = -map_length_ / 2; - heatmap.data.resize(width_ * height_, 0); - - std::vector buffer; - buffer.resize(width_ * height_, 0); - - uint8_t label = getSemanticType(key); - bool is_car_like_vehicle = object_recognition_utils::isCarLikeVehicle(label); - if ((!rename_car_to_truck_and_bus_) && (is_car_like_vehicle)) { - uint8_t car_label = getSemanticType("CAR"); - heatmaps_.insert(std::make_pair(car_label, heatmap)); - data_buffers_.insert(std::make_pair(car_label, buffer)); - - heatmap_pubs_.insert(std::make_pair( - car_label, create_publisher("~/output/heatmap/CAR", 10.0))); - } else { - heatmaps_.insert(std::make_pair(label, heatmap)); - data_buffers_.insert(std::make_pair(label, buffer)); - - heatmap_pubs_.insert(std::make_pair( - label, create_publisher("~/output/heatmap/" + key, 10.0))); - } - } -} - -void HeatmapVisualizerNode::detectedObjectsCallback( - const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg) -{ - frame_count_ += 1; - for (const auto & obj : msg->objects) { - uint8_t label = obj.classification[0].label; - bool is_car_like_vehicle = object_recognition_utils::isCarLikeVehicle(label); - if ((!rename_car_to_truck_and_bus_) && (is_car_like_vehicle)) { - label = getSemanticType("CAR"); - } - // Set value to data buffer - setHeatmapToBuffer(obj, heatmaps_.at(label), &data_buffers_.at(label), use_confidence_); - } - // Publish messages if frame_count_ == total_frame_ - if (frame_count_ == total_frame_) { - for (auto & map : heatmaps_) { - setHeatmapToOccupancyGrid(data_buffers_.at(map.first), &map.second); - heatmap_pubs_.at(map.first)->publish(map.second); - } - // Reset frame count - frame_count_ = 0; - } -} - -} // namespace heatmap_visualizer - -RCLCPP_COMPONENTS_REGISTER_NODE(heatmap_visualizer::HeatmapVisualizerNode) diff --git a/perception/heatmap_visualizer/src/utils.cpp b/perception/heatmap_visualizer/src/utils.cpp deleted file mode 100644 index c4b1c3d6a22c6..0000000000000 --- a/perception/heatmap_visualizer/src/utils.cpp +++ /dev/null @@ -1,131 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "heatmap_visualizer/utils.hpp" - -#include -#include -#include - -#include - -#include -#include - -namespace heatmap_visualizer -{ -using tier4_autoware_utils::normalizeRadian; - -int discretizeAngle(const double theta, const int theta_size) -{ - const double one_angle_range = 2.0 * M_PI / theta_size; - return static_cast(std::rint(normalizeRadian(theta, 0.0) / one_angle_range)) % theta_size; -} - -IndexXYT pose2index( - const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_local, - const int theta_size) -{ - const int index_x = pose_local.position.x / costmap.info.resolution; - const int index_y = pose_local.position.y / costmap.info.resolution; - const double theta = tf2::getYaw(pose_local.orientation); - const int index_theta = discretizeAngle(theta, theta_size); - return {index_x, index_y, index_theta}; -} - -void setHeatmapToBuffer( - const autoware_auto_perception_msgs::msg::DetectedObject & obj, - const nav_msgs::msg::OccupancyGrid & heatmap, std::vector * data_buffer, - const bool use_confidence) -{ - int theta_size = 48; - IndexXYT indexPose{pose2index(heatmap, obj.kinematics.pose_with_covariance.pose, theta_size)}; - int mapWidth = heatmap.info.width; - int mapHeight = heatmap.info.height; - int indexX = indexPose.x + mapWidth / 2; - int indexY = indexPose.y + mapHeight / 2; - - float score; - if (use_confidence) { - score = obj.existence_probability; - } else { - score = 1.0; - } - - try { - data_buffer->at(indexY * mapWidth + indexX) += score; - } catch (const std::out_of_range & e) { - RCLCPP_ERROR(rclcpp::get_logger("setHeatmapToBuffer"), e.what()); - } -} - -void setHeatmapToOccupancyGrid( - const std::vector & data_buffer, nav_msgs::msg::OccupancyGrid * heatmap) -{ - float max_value = *std::max_element(data_buffer.begin(), data_buffer.end()); - float min_value = *std::min_element(data_buffer.begin(), data_buffer.end()); - if (max_value == min_value) { - return; - } - - for (size_t i = 0; i < heatmap->data.size(); ++i) { - heatmap->data[i] = - static_cast(100 * (data_buffer[i] - min_value) / (max_value - min_value)); - } - return; -} - -uint8_t getSemanticType(const std::string & class_name) -{ - if (class_name == "CAR") { - return Label::CAR; - } else if (class_name == "TRUCK") { - return Label::TRUCK; - } else if (class_name == "BUS") { - return Label::BUS; - } else if (class_name == "TRAILER") { - return Label::TRAILER; - } else if (class_name == "BICYCLE") { - return Label::BICYCLE; - } else if (class_name == "MOTORBIKE") { - return Label::MOTORCYCLE; - } else if (class_name == "PEDESTRIAN") { - return Label::PEDESTRIAN; - } else { - return Label::UNKNOWN; - } -} - -std::string getClassName(const uint8_t label) -{ - if (label == Label::CAR) { - return "CAR"; - } else if (label == Label::TRUCK) { - return "TRUCK"; - } else if (label == Label::BUS) { - return "BUS"; - } else if (label == Label::TRAILER) { - return "TRAILER"; - } else if (label == Label::BICYCLE) { - return "BICYCLE"; - } else if (label == Label::MOTORCYCLE) { - return "MOTORBIKE"; - } else if (label == Label::PEDESTRIAN) { - return "PEDESTRIAN"; - } else { - return "UNKNOWN"; - } -} - -} // namespace heatmap_visualizer diff --git a/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml index afb2de29deb7a..917473333183c 100644 --- a/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml +++ b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml @@ -32,10 +32,10 @@ # fusion interface common params # debug debug_mode: false - filter_scope_minx: -100 - filter_scope_maxx: 100 - filter_scope_miny: -100 - filter_scope_maxy: 100 - filter_scope_minz: -100 - filter_scope_maxz: 100 + filter_scope_min_x: -100 + filter_scope_max_x: 100 + filter_scope_min_y: -100 + filter_scope_max_y: 100 + filter_scope_min_z: -100 + filter_scope_max_z: 100 image_buffer_size: 15 diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp index fe8acb6746dba..814fc5eca087e 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp @@ -44,8 +44,6 @@ #include #include -// cspell: ignore minx, maxx, miny, maxy, minz, maxz - namespace image_projection_based_fusion { using autoware_auto_perception_msgs::msg::DetectedObject; @@ -131,13 +129,12 @@ class FusionNode : public rclcpp::Node // debugger std::shared_ptr debugger_; virtual bool out_of_scope(const ObjType & obj) = 0; - // cspell: ignore minx, maxx, miny, maxy, minz, maxz - float filter_scope_minx_; - float filter_scope_maxx_; - float filter_scope_miny_; - float filter_scope_maxy_; - float filter_scope_minz_; - float filter_scope_maxz_; + float filter_scope_min_x_; + float filter_scope_max_x_; + float filter_scope_min_y_; + float filter_scope_max_y_; + float filter_scope_min_z_; + float filter_scope_max_z_; /** \brief processing time publisher. **/ std::unique_ptr> stop_watch_ptr_; diff --git a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml index f336eb16e94a1..9236d7d789ed1 100644 --- a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml @@ -52,7 +52,7 @@ - + @@ -93,7 +93,7 @@ - + diff --git a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml index 52dd71e9579c1..8df1a374b00b6 100644 --- a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml @@ -22,7 +22,6 @@ - diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml index 1322d053b358a..4457cf3d3992d 100644 --- a/perception/image_projection_based_fusion/package.xml +++ b/perception/image_projection_based_fusion/package.xml @@ -10,6 +10,7 @@ badai nguyen Kotaro Uetake Tao Zhong + Koji Minoda Apache License 2.0 ament_cmake_auto diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index 6bced39bc61ef..febec4dbc20b8 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -132,19 +132,17 @@ FusionNode::FusionNode( using tier4_autoware_utils::DebugPublisher; using tier4_autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); - debug_publisher_ = std::make_unique(this, "image_projection_based_fusion"); + debug_publisher_ = std::make_unique(this, get_name()); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } - // cspell: ignore minx, maxx, miny, maxy, minz, maxz - // FIXME: use min_x instead of minx - filter_scope_minx_ = declare_parameter("filter_scope_min_x"); - filter_scope_maxx_ = declare_parameter("filter_scope_max_x"); - filter_scope_miny_ = declare_parameter("filter_scope_min_y"); - filter_scope_maxy_ = declare_parameter("filter_scope_max_y"); - filter_scope_minz_ = declare_parameter("filter_scope_min_z"); - filter_scope_maxz_ = declare_parameter("filter_scope_max_z"); + filter_scope_min_x_ = declare_parameter("filter_scope_min_x"); + filter_scope_max_x_ = declare_parameter("filter_scope_max_x"); + filter_scope_min_y_ = declare_parameter("filter_scope_min_y"); + filter_scope_max_y_ = declare_parameter("filter_scope_max_y"); + filter_scope_min_z_ = declare_parameter("filter_scope_min_z"); + filter_scope_max_z_ = declare_parameter("filter_scope_max_z"); } template @@ -169,19 +167,27 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr timer_->cancel(); postprocess(*(cached_msg_.second)); publish(*(cached_msg_.second)); - cached_msg_.second = nullptr; std::fill(is_fused_.begin(), is_fused_.end(), false); // add processing time for debug if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - cached_msg_.second->header.stamp).nanoseconds())) + .count(); debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); processing_time_ms = 0; } + + cached_msg_.second = nullptr; } std::lock_guard lock(mutex_cached_msgs_); diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 1c064e4d0f060..feaf1ad495ce8 100644 --- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -28,8 +28,6 @@ #include #endif -// cspell: ignore minx, maxx, miny, maxy, minz, maxz - namespace image_projection_based_fusion { @@ -253,17 +251,17 @@ bool RoiClusterFusionNode::out_of_scope(const DetectedObjectWithFeature & obj) for (sensor_msgs::PointCloud2ConstIterator iter_x(cluster, "x"), iter_y(cluster, "y"), iter_z(cluster, "z"); iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { - if (!valid_point(*iter_x, filter_scope_minx_, filter_scope_maxx_)) { + if (!valid_point(*iter_x, filter_scope_min_x_, filter_scope_max_x_)) { is_out = true; break; } - if (!valid_point(*iter_y, filter_scope_miny_, filter_scope_maxy_)) { + if (!valid_point(*iter_y, filter_scope_min_y_, filter_scope_max_y_)) { is_out = true; break; } - if (!valid_point(*iter_z, filter_scope_minz_, filter_scope_maxz_)) { + if (!valid_point(*iter_z, filter_scope_min_z_, filter_scope_max_z_)) { is_out = true; break; } diff --git a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index 19defc0a1cab0..762d3e0ee51b1 100644 --- a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -19,8 +19,6 @@ #include #include -// cspell: ignore minx, maxx, miny, maxy, minz, maxz - namespace image_projection_based_fusion { @@ -268,13 +266,13 @@ bool RoiDetectedObjectFusionNode::out_of_scope(const DetectedObject & obj) return (p > min_num) && (p < max_num); }; - if (!valid_point(pose.position.x, filter_scope_minx_, filter_scope_maxx_)) { + if (!valid_point(pose.position.x, filter_scope_min_x_, filter_scope_max_x_)) { return is_out; } - if (!valid_point(pose.position.y, filter_scope_miny_, filter_scope_maxy_)) { + if (!valid_point(pose.position.y, filter_scope_min_y_, filter_scope_max_y_)) { return is_out; } - if (!valid_point(pose.position.z, filter_scope_minz_, filter_scope_maxz_)) { + if (!valid_point(pose.position.z, filter_scope_min_z_, filter_scope_max_z_)) { return is_out; } diff --git a/perception/lidar_apollo_instance_segmentation/README.md b/perception/lidar_apollo_instance_segmentation/README.md index 2d86b695d2474..f9dc6b3f44a23 100644 --- a/perception/lidar_apollo_instance_segmentation/README.md +++ b/perception/lidar_apollo_instance_segmentation/README.md @@ -47,6 +47,7 @@ None | `use_constant_feature` | bool | false | The flag to use direction and distance feature of pointcloud. | | `target_frame` | string | "base_link" | Pointcloud data is transformed into this frame. | | `z_offset` | int | 2 | z offset from target frame. [m] | +| `build_only` | bool | `false` | shutdown the node after TensorRT engine file is built | ## Assumptions / Known limits diff --git a/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml b/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml index 7f5024929b531..207dac45671dd 100644 --- a/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml +++ b/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml @@ -2,6 +2,7 @@ + @@ -24,5 +25,8 @@ + + + diff --git a/perception/lidar_apollo_instance_segmentation/src/detector.cpp b/perception/lidar_apollo_instance_segmentation/src/detector.cpp index 5da7825f96681..236f0998ab21f 100644 --- a/perception/lidar_apollo_instance_segmentation/src/detector.cpp +++ b/perception/lidar_apollo_instance_segmentation/src/detector.cpp @@ -48,6 +48,11 @@ LidarApolloInstanceSegmentation::LidarApolloInstanceSegmentation(rclcpp::Node * return; } + if (node_->declare_parameter("build_only", false)) { + RCLCPP_INFO(node_->get_logger(), "TensorRT engine is built and shutdown node."); + rclcpp::shutdown(); + } + // GPU memory allocation const auto input_dims = trt_common_->getBindingDimensions(0); const auto input_size = diff --git a/perception/lidar_centerpoint/README.md b/perception/lidar_centerpoint/README.md index f5a15916f31a9..2a9adedad9074 100644 --- a/perception/lidar_centerpoint/README.md +++ b/perception/lidar_centerpoint/README.md @@ -45,6 +45,15 @@ We trained the models using . | `nms_iou_threshold` | double | - | IoU threshold for the IoU-based Non Maximum Suppression | | `build_only` | bool | `false` | shutdown the node after TensorRT engine file is built | +### The `build_only` option + +The `lidar_centerpoint` node has `build_only` option to build the TensorRT engine file from the ONNX file. +Although it is preferred to move all the ROS parameters in `.param.yaml` file in Autoware Universe, the `build_only` option is not moved to the `.param.yaml` file for now, because it may be used as a flag to execute the build as a pre-task. You can execute with the following command: + +```bash +ros2 launch lidar_centerpoint lidar_centerpoint.launch.xml model_name:=centerpoint_tiny model_path:=/home/autoware/autoware_data/lidar_centerpoint model_param_path:=$(ros2 pkg prefix lidar_centerpoint --share)/config/centerpoint_tiny.param.yaml build_only:=true +``` + ## Assumptions / Known limits - The `object.existence_probability` is stored the value of classification confidence of a DNN, not probability. diff --git a/perception/lidar_centerpoint/config/centerpoint.param.yaml b/perception/lidar_centerpoint/config/centerpoint.param.yaml index 0b29a87965b42..a9c3174846d0d 100644 --- a/perception/lidar_centerpoint/config/centerpoint.param.yaml +++ b/perception/lidar_centerpoint/config/centerpoint.param.yaml @@ -13,3 +13,14 @@ iou_nms_search_distance_2d: 10.0 iou_nms_threshold: 0.1 yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] + score_threshold: 0.35 + has_twist: false + trt_precision: fp16 + densification_num_past_frames: 1 + densification_world_frame_id: map + + # weight files + encoder_onnx_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).onnx" + encoder_engine_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).engine" + head_onnx_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx" + head_engine_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).engine" diff --git a/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml b/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml index 8252fde8273d8..474d0e2b695ac 100644 --- a/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml +++ b/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml @@ -13,3 +13,14 @@ iou_nms_search_distance_2d: 10.0 iou_nms_threshold: 0.1 yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] + score_threshold: 0.35 + has_twist: false + trt_precision: fp16 + densification_num_past_frames: 1 + densification_world_frame_id: map + + # weight files + encoder_onnx_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).onnx" + encoder_engine_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).engine" + head_onnx_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx" + head_engine_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).engine" diff --git a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml index 1210875770510..5489af2c8fb60 100644 --- a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml +++ b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml @@ -7,8 +7,6 @@ - - @@ -19,18 +17,11 @@ - - - - - - - - - - - + + + + @@ -38,18 +29,11 @@ - - - - - - - - - - - + + + + diff --git a/perception/lidar_centerpoint/package.xml b/perception/lidar_centerpoint/package.xml index fee97a84be917..674325cccc677 100644 --- a/perception/lidar_centerpoint/package.xml +++ b/perception/lidar_centerpoint/package.xml @@ -5,6 +5,7 @@ 1.0.0 The lidar_centerpoint package Kenzo Lobos-Tsunekawa + Koji Minoda Apache License 2.0 ament_cmake_auto diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index 39683755bcaf0..d370a60a26aa5 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -169,10 +169,17 @@ void LidarCenterPointNode::pointCloudCallback( if (debug_publisher_ptr_ && stop_watch_ptr_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + const double pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - output_msg.header.stamp).nanoseconds())) + .count(); debug_publisher_ptr_->publish( "debug/cyclic_time_ms", cyclic_time_ms); debug_publisher_ptr_->publish( "debug/processing_time_ms", processing_time_ms); + debug_publisher_ptr_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md index 2419cdb010799..a58e19db70304 100644 --- a/perception/map_based_prediction/README.md +++ b/perception/map_based_prediction/README.md @@ -191,6 +191,11 @@ This module treats **Pedestrians** and **Bicycles** as objects using the crosswa If there are a reachable crosswalk entry points within the `prediction_time_horizon` and the objects satisfies above condition, this module outputs additional predicted path to cross the opposite side via the crosswalk entry point. +This module takes into account the corresponding traffic light information. +When RED signal is indicated, we assume the target object will not walk across. +In addition, if the target object is stopping (not moving) against GREEN signal, we assume the target object will not walk across either. +This prediction comes from the assumption that the object should move if the traffic light is green and the object is intended to cross. +
@@ -205,10 +210,11 @@ If the target object is inside the road or crosswalk, this module outputs one or ### Input -| Name | Type | Description | -| -------------------------------------------------- | ---------------------------------------------------- | ---------------------------------------- | -| `~/perception/object_recognition/tracking/objects` | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking objects without predicted path. | -| `~/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | binary data of Lanelet2 Map. | +| Name | Type | Description | +| -------------------------------------------------------- | ---------------------------------------------------- | ---------------------------------------------------------- | +| `~/perception/object_recognition/tracking/objects` | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking objects without predicted path. | +| `~/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | binary data of Lanelet2 Map. | +| '~/perception/traffic_light_recognition/traffic_signals' | 'autoware_perception_msgs::msg::TrafficSignalArray;' | rearranged information on the corresponding traffic lights | ### Output diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml index fdd64174de9be..f8ad371ab92a6 100644 --- a/perception/map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml @@ -19,6 +19,12 @@ use_vehicle_acceleration: false # whether to consider current vehicle acceleration when predicting paths or not speed_limit_multiplier: 1.5 # When using vehicle acceleration. Set vehicle's maximum predicted speed as the legal speed limit in that lanelet times this value acceleration_exponential_half_life: 2.5 # [s] When using vehicle acceleration. The decaying acceleration model considers that the current vehicle acceleration will be halved after this many seconds + crosswalk_with_signal: + use_crosswalk_signal: true + threshold_velocity_assumed_as_stopping: 0.25 # [m/s] velocity threshold for the module to judge whether the objects is stopped + # If the pedestrian does not move for X seconds against green signal, the module judge that the pedestrian has no intention to walk. + distance_set_for_no_intention_to_walk: [1.0, 5.0] # [m] ascending order, keys of map + timeout_set_for_no_intention_to_walk: [1.0, 0.0] # [s] values of map # parameter for shoulder lane prediction prediction_time_horizon_rate_for_validate_shoulder_lane_length: 0.8 diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index f61dc75ffb85b..321342c3d8367 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -41,7 +42,9 @@ #include #include +#include #include +#include #include #include #include @@ -107,6 +110,9 @@ using autoware_auto_perception_msgs::msg::TrackedObject; using autoware_auto_perception_msgs::msg::TrackedObjectKinematics; using autoware_auto_perception_msgs::msg::TrackedObjects; using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_perception_msgs::msg::TrafficSignal; +using autoware_perception_msgs::msg::TrafficSignalArray; +using autoware_perception_msgs::msg::TrafficSignalElement; using tier4_autoware_utils::StopWatch; using tier4_debug_msgs::msg::StringStamped; using TrajectoryPoints = std::vector; @@ -122,15 +128,19 @@ class MapBasedPredictionNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_calculation_time_; rclcpp::Subscription::SharedPtr sub_objects_; rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Subscription::SharedPtr sub_traffic_signals_; // Object History std::unordered_map> objects_history_; + std::map, rclcpp::Time> stopped_times_against_green_; // Lanelet Map Pointers std::shared_ptr lanelet_map_ptr_; std::shared_ptr routing_graph_ptr_; std::shared_ptr traffic_rules_ptr_; + std::unordered_map traffic_signal_id_map_; + // parameter update OnSetParametersCallbackHandle::SharedPtr set_param_res_; rcl_interfaces::msg::SetParametersResult onParam( @@ -181,11 +191,17 @@ class MapBasedPredictionNode : public rclcpp::Node double speed_limit_multiplier_; double acceleration_exponential_half_life_; + bool use_crosswalk_signal_; + double threshold_velocity_assumed_as_stopping_; + std::vector distance_set_for_no_intention_to_walk_; + std::vector timeout_set_for_no_intention_to_walk_; + // Stop watch StopWatch stop_watch_; // Member Functions void mapCallback(const HADMapBin::ConstSharedPtr msg); + void trafficSignalsCallback(const TrafficSignalArray::ConstSharedPtr msg); void objectsCallback(const TrackedObjects::ConstSharedPtr in_objects); bool doesPathCrossAnyFence(const PredictedPath & predicted_path); @@ -203,7 +219,8 @@ class MapBasedPredictionNode : public rclcpp::Node PredictedObject getPredictedObjectAsCrosswalkUser(const TrackedObject & object); - void removeOldObjectsHistory(const double current_time); + void removeOldObjectsHistory( + const double current_time, const TrackedObjects::ConstSharedPtr in_objects); LaneletsData getCurrentLanelets(const TrackedObject & object); bool checkCloseLaneletCondition( @@ -249,6 +266,11 @@ class MapBasedPredictionNode : public rclcpp::Node const LaneletsData & lanelets_data); bool isDuplicated( const PredictedPath & predicted_path, const std::vector & predicted_paths); + std::optional getTrafficSignalId(const lanelet::ConstLanelet & way_lanelet); + std::optional getTrafficSignalElement(const lanelet::Id & id); + bool calcIntentionToCrossWithTrafficSignal( + const TrackedObject & object, const lanelet::ConstLanelet & crosswalk, + const lanelet::Id & signal_id); visualization_msgs::msg::Marker getDebugMarker( const TrackedObject & object, const Maneuver & maneuver, const size_t obj_num); diff --git a/perception/map_based_prediction/launch/map_based_prediction.launch.xml b/perception/map_based_prediction/launch/map_based_prediction.launch.xml index 619c0c9507e0b..7f81971b2f8d7 100644 --- a/perception/map_based_prediction/launch/map_based_prediction.launch.xml +++ b/perception/map_based_prediction/launch/map_based_prediction.launch.xml @@ -3,12 +3,14 @@ + + diff --git a/perception/map_based_prediction/package.xml b/perception/map_based_prediction/package.xml index b07d9855f9821..8efea9efa1844 100644 --- a/perception/map_based_prediction/package.xml +++ b/perception/map_based_prediction/package.xml @@ -16,6 +16,7 @@ autoware_cmake autoware_auto_perception_msgs + autoware_perception_msgs interpolation lanelet2_extension libgoogle-glog-dev diff --git a/perception/map_based_prediction/schema/map_based_pediction.schema.json b/perception/map_based_prediction/schema/map_based_prediction.schema.json similarity index 100% rename from perception/map_based_prediction/schema/map_based_pediction.schema.json rename to perception/map_based_prediction/schema/map_based_prediction.schema.json diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index c949eae21aeff..21697bb1b5f1a 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -788,6 +788,14 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ acceleration_exponential_half_life_ = declare_parameter("acceleration_exponential_half_life"); + use_crosswalk_signal_ = declare_parameter("crosswalk_with_signal.use_crosswalk_signal"); + threshold_velocity_assumed_as_stopping_ = + declare_parameter("crosswalk_with_signal.threshold_velocity_assumed_as_stopping"); + distance_set_for_no_intention_to_walk_ = declare_parameter>( + "crosswalk_with_signal.distance_set_for_no_intention_to_walk"); + timeout_set_for_no_intention_to_walk_ = declare_parameter>( + "crosswalk_with_signal.timeout_set_for_no_intention_to_walk"); + path_generator_ = std::make_shared( prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_, min_crosswalk_user_velocity_); @@ -801,6 +809,9 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ sub_map_ = this->create_subscription( "/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&MapBasedPredictionNode::mapCallback, this, std::placeholders::_1)); + sub_traffic_signals_ = this->create_subscription( + "/traffic_signals", 1, + std::bind(&MapBasedPredictionNode::trafficSignalsCallback, this, std::placeholders::_1)); pub_objects_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); pub_debug_markers_ = @@ -859,11 +870,11 @@ PredictedObject MapBasedPredictionNode::convertToPredictedObject( void MapBasedPredictionNode::mapCallback(const HADMapBin::ConstSharedPtr msg) { - RCLCPP_INFO(get_logger(), "[Map Based Prediction]: Start loading lanelet"); + RCLCPP_DEBUG(get_logger(), "[Map Based Prediction]: Start loading lanelet"); lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); - RCLCPP_INFO(get_logger(), "[Map Based Prediction]: Map is loaded"); + RCLCPP_DEBUG(get_logger(), "[Map Based Prediction]: Map is loaded"); const auto all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); const auto crosswalks = lanelet::utils::query::crosswalkLanelets(all_lanelets); @@ -872,6 +883,14 @@ void MapBasedPredictionNode::mapCallback(const HADMapBin::ConstSharedPtr msg) crosswalks_.insert(crosswalks_.end(), walkways.begin(), walkways.end()); } +void MapBasedPredictionNode::trafficSignalsCallback(const TrafficSignalArray::ConstSharedPtr msg) +{ + traffic_signal_id_map_.clear(); + for (const auto & signal : msg->signals) { + traffic_signal_id_map_[signal.traffic_signal_id] = signal; + } +} + void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects) { stop_watch_.tic(); @@ -899,7 +918,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt // Remove old objects information in object history const double objects_detected_time = rclcpp::Time(in_objects->header.stamp).seconds(); - removeOldObjectsHistory(objects_detected_time); + removeOldObjectsHistory(objects_detected_time, in_objects); // result output PredictedObjects output; @@ -1216,8 +1235,17 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( } } } + // try to find the edge points for all crosswalks and generate path to the crosswalk edge for (const auto & crosswalk : crosswalks_) { + const auto crosswalk_signal_id_opt = getTrafficSignalId(crosswalk); + if (crosswalk_signal_id_opt.has_value() && use_crosswalk_signal_) { + if (!calcIntentionToCrossWithTrafficSignal( + object, crosswalk, crosswalk_signal_id_opt.value())) { + continue; + } + } + const auto edge_points = getCrosswalkEdgePoints(crosswalk); const auto reachable_first = hasPotentialToReach( @@ -1281,6 +1309,13 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object) // assumption: the object vx is much larger than vy if (object.kinematics.twist_with_covariance.twist.linear.x >= 0.0) return; + // calculate absolute velocity and do nothing if it is too slow + const double abs_object_speed = std::hypot( + object.kinematics.twist_with_covariance.twist.linear.x, + object.kinematics.twist_with_covariance.twist.linear.y); + constexpr double min_abs_speed = 1e-1; // 0.1 m/s + if (abs_object_speed < min_abs_speed) return; + switch (object.kinematics.orientation_availability) { case autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN: { const auto original_yaw = @@ -1305,7 +1340,8 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object) return; } -void MapBasedPredictionNode::removeOldObjectsHistory(const double current_time) +void MapBasedPredictionNode::removeOldObjectsHistory( + const double current_time, const TrackedObjects::ConstSharedPtr in_objects) { std::vector invalid_object_id; for (auto iter = objects_history_.begin(); iter != objects_history_.end(); ++iter) { @@ -1346,6 +1382,19 @@ void MapBasedPredictionNode::removeOldObjectsHistory(const double current_time) for (const auto & key : invalid_object_id) { objects_history_.erase(key); } + + for (auto it = stopped_times_against_green_.begin(); it != stopped_times_against_green_.end();) { + const bool isDisappeared = std::none_of( + in_objects->objects.begin(), in_objects->objects.end(), + [&it](autoware_auto_perception_msgs::msg::TrackedObject obj) { + return tier4_autoware_utils::toHexString(obj.object_id) == it->first.first; + }); + if (isDisappeared) { + it = stopped_times_against_green_.erase(it); + } else { + ++it; + } + } } LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & object) @@ -2211,6 +2260,101 @@ bool MapBasedPredictionNode::isDuplicated( return false; } + +std::optional MapBasedPredictionNode::getTrafficSignalId( + const lanelet::ConstLanelet & way_lanelet) +{ + const auto traffic_light_reg_elems = + way_lanelet.regulatoryElementsAs(); + if (traffic_light_reg_elems.empty()) { + return std::nullopt; + } else if (traffic_light_reg_elems.size() > 1) { + RCLCPP_ERROR( + get_logger(), + "[Map Based Prediction]: " + "Multiple regulatory elements as TrafficLight are defined to one lanelet object."); + } + return traffic_light_reg_elems.front()->id(); +} + +std::optional MapBasedPredictionNode::getTrafficSignalElement( + const lanelet::Id & id) +{ + if (traffic_signal_id_map_.count(id) != 0) { + const auto & signal_elements = traffic_signal_id_map_.at(id).elements; + if (signal_elements.size() > 1) { + RCLCPP_ERROR( + get_logger(), "[Map Based Prediction]: Multiple TrafficSignalElement_ are received."); + } else if (!signal_elements.empty()) { + return signal_elements.front(); + } + } + return std::nullopt; +} + +bool MapBasedPredictionNode::calcIntentionToCrossWithTrafficSignal( + const TrackedObject & object, const lanelet::ConstLanelet & crosswalk, + const lanelet::Id & signal_id) +{ + const auto signal_color = [&] { + const auto elem_opt = getTrafficSignalElement(signal_id); + return elem_opt ? elem_opt.value().color : TrafficSignalElement::UNKNOWN; + }(); + + const auto key = std::make_pair(tier4_autoware_utils::toHexString(object.object_id), signal_id); + if ( + signal_color == TrafficSignalElement::GREEN && + tier4_autoware_utils::calcNorm(object.kinematics.twist_with_covariance.twist.linear) < + threshold_velocity_assumed_as_stopping_) { + stopped_times_against_green_.try_emplace(key, this->get_clock()->now()); + + const auto timeout_no_intention_to_walk = [&]() { + auto InterpolateMap = []( + const std::vector & key_set, + const std::vector & value_set, const double query) { + if (query <= key_set.front()) { + return value_set.front(); + } else if (query >= key_set.back()) { + return value_set.back(); + } + for (size_t i = 0; i < key_set.size() - 1; ++i) { + if (key_set.at(i) <= query && query <= key_set.at(i + 1)) { + auto ratio = + (query - key_set.at(i)) / std::max(key_set.at(i + 1) - key_set.at(i), 1.0e-5); + ratio = std::clamp(ratio, 0.0, 1.0); + return value_set.at(i) + ratio * (value_set.at(i + 1) - value_set.at(i)); + } + } + return value_set.back(); + }; + + const auto obj_position = object.kinematics.pose_with_covariance.pose.position; + const double distance_to_crosswalk = boost::geometry::distance( + crosswalk.polygon2d().basicPolygon(), + lanelet::BasicPoint2d(obj_position.x, obj_position.y)); + return InterpolateMap( + distance_set_for_no_intention_to_walk_, timeout_set_for_no_intention_to_walk_, + distance_to_crosswalk); + }(); + + if ( + (this->get_clock()->now() - stopped_times_against_green_.at(key)).seconds() > + timeout_no_intention_to_walk) { + return false; + } + + } else { + stopped_times_against_green_.erase(key); + // If the pedestrian disappears, another function erases the old data. + } + + if (signal_color == TrafficSignalElement::RED) { + return false; + } + + return true; +} + } // namespace map_based_prediction #include diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp index ad268283d5890..ba50933eddaec 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -32,16 +32,18 @@ class BicycleTracker : public Tracker private: KalmanFilter ekf_; rclcpp::Time last_update_time_; - enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 }; + enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; struct EkfParams { char dim_x = 5; - float q_cov_x; - float q_cov_y; - float q_cov_yaw; - float q_cov_slip; - float q_cov_vx; - float p0_cov_vx; + float q_stddev_acc_long; + float q_stddev_acc_lat; + float q_stddev_yaw_rate_min; + float q_stddev_yaw_rate_max; + float q_cov_slip_rate_min; + float q_cov_slip_rate_max; + float q_max_slip_angle; + float p0_cov_vel; float p0_cov_slip; float r_cov_x; float r_cov_y; @@ -51,7 +53,7 @@ class BicycleTracker : public Tracker float p0_cov_yaw; } ekf_params_; - double max_vx_; + double max_vel_; double max_slip_; double lf_; double lr_; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index 8a9b6adfc9cd5..e19b6382ad952 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -32,26 +32,28 @@ class BigVehicleTracker : public Tracker private: KalmanFilter ekf_; rclcpp::Time last_update_time_; - enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 }; + enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; struct EkfParams { char dim_x = 5; - float q_cov_x; - float q_cov_y; - float q_cov_yaw; - float q_cov_slip; - float q_cov_vx; - float p0_cov_vx; + float q_stddev_acc_long; + float q_stddev_acc_lat; + float q_stddev_yaw_rate_min; + float q_stddev_yaw_rate_max; + float q_cov_slip_rate_min; + float q_cov_slip_rate_max; + float q_max_slip_angle; + float p0_cov_vel; float p0_cov_slip; float r_cov_x; float r_cov_y; float r_cov_yaw; - float r_cov_vx; + float r_cov_vel; float p0_cov_x; float p0_cov_y; float p0_cov_yaw; } ekf_params_; - double max_vx_; + double max_vel_; double max_slip_; double lf_; double lr_; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 79e9a20a10421..05fa0a5ef01ba 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -33,28 +33,30 @@ class NormalVehicleTracker : public Tracker private: KalmanFilter ekf_; rclcpp::Time last_update_time_; - enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 }; + enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; struct EkfParams { char dim_x = 5; - float q_cov_x; - float q_cov_y; - float q_cov_yaw; - float q_cov_slip; - float q_cov_vx; - float p0_cov_vx; + float q_stddev_acc_long; + float q_stddev_acc_lat; + float q_stddev_yaw_rate_min; + float q_stddev_yaw_rate_max; + float q_cov_slip_rate_min; + float q_cov_slip_rate_max; + float q_max_slip_angle; + float p0_cov_vel; float p0_cov_slip; float r_cov_x; float r_cov_y; float r_cov_yaw; - float r_cov_vx; + float r_cov_vel; float p0_cov_x; float p0_cov_y; float p0_cov_yaw; } ekf_params_; - double max_vx_; + double max_vel_; double max_slip_; double lf_; double lr_; @@ -88,6 +90,7 @@ class NormalVehicleTracker : public Tracker const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const override; void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform); + double getMeasurementYaw(const autoware_auto_perception_msgs::msg::DetectedObject & object); virtual ~NormalVehicleTracker() {} }; diff --git a/perception/multi_object_tracker/models.md b/perception/multi_object_tracker/models.md index f74967943be32..f1f53c7398c0a 100644 --- a/perception/multi_object_tracker/models.md +++ b/perception/multi_object_tracker/models.md @@ -11,21 +11,21 @@ CTRV model is a model that assumes constant turn rate and velocity magnitude. - state transition equation $$ -\begin{align*} -x_{k+1} &= x_k + v_{x_k} \cos(\psi_k) \cdot dt \\ -y_{k+1} &= y_k + v_{x_k} \sin(\psi_k) \cdot dt \\ -\psi_{k+1} &= \psi_k + \dot{\psi}_k \cdot dt \\ -v_{x_{k+1}} &= v_{x_k} \\ -\dot{\psi}_{k+1} &= \dot{\psi}_k \\ -\end{align*} +\begin{aligned} +x_{k+1} & = x_{k} + v_{k} \cos(\psi_k) \cdot {d t} \\ +y_{k+1} & = y_{k} + v_{k} \sin(\psi_k) \cdot {d t} \\ +\psi_{k+1} & = \psi_k + \dot\psi_{k} \cdot {d t} \\ +v_{k+1} & = v_{k} \\ +\dot\psi_{k+1} & = \dot\psi_{k} \\ +\end{aligned} $$ - jacobian $$ A = \begin{bmatrix} -1 & 0 & -v_x \sin(\psi) \cdot dt & \cos(\psi) \cdot dt & 0 \\ -0 & 1 & v_x \cos(\psi) \cdot dt & \sin(\psi) \cdot dt & 0 \\ +1 & 0 & -v \sin(\psi) \cdot dt & \cos(\psi) \cdot dt & 0 \\ +0 & 1 & v \cos(\psi) \cdot dt & \sin(\psi) \cdot dt & 0 \\ 0 & 0 & 1 & 0 & dt \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \\ @@ -40,17 +40,20 @@ The merit of using this model is that it can prevent unintended yaw rotation whe ![kinematic_bicycle_model](image/kinematic_bicycle_model.png) - **state variable** - - pose( $x,y$ ), velocity( $v$ ), yaw( $\psi$ ), and slip angle ( $\beta$ ) - - $[x_{k} ,y_{k} , v_{k} , \psi_{k} , \beta_{k} ]^\mathrm{T}$ + - pose( $x,y$ ), yaw( $\psi$ ), velocity( $v$ ), and slip angle ( $\beta$ ) + - $[x_{k}, y_{k}, \psi_{k}, v_{k}, \beta_{k} ]^\mathrm{T}$ - **Prediction Equation** - $dt$: sampling time + - $w_{k} = \dot\psi_{k} = \frac{ v_{k} \sin \left( \beta_{k} \right) }{ l_r }$ : angular velocity $$ \begin{aligned} -x_{k+1} & =x_{k}+v_{k} \cos \left(\psi_{k}+\beta_{k}\right) d t \\ -y_{k+1} & =y_{k}+v_{k} \sin \left(\psi_{k}+\beta_{k}\right) d t \\ -v_{k+1} & =v_{k} \\ -\psi_{k+1} & =\psi_{k}+\frac{v_{k}}{l_{r}} \sin \beta_{k} d t \\ +x_{k+1} & = x_{k} + v_{k} \cos \left( \psi_{k}+\beta_{k} \right) {d t} + -\frac{1}{2} \left\lbrace w_k v_k \sin \left(\psi_{k}+\beta_{k} \right) \right\rbrace {d t}^2\\ +y_{k+1} & = y_{k} + v_{k} \sin \left( \psi_{k}+\beta_{k} \right) {d t} + +\frac{1}{2} \left\lbrace w_k v_k \cos \left(\psi_{k}+\beta_{k} \right) \right\rbrace {d t}^2\\ +\psi_{k+1} & =\psi_{k} + w_k {d t} \\ +v_{k+1} & = v_{k} \\ \beta_{k+1} & =\beta_{k} \end{aligned} $$ @@ -59,9 +62,15 @@ $$ $$ \frac{\partial f}{\partial \mathrm x}=\left[\begin{array}{ccccc} -1 & 0 & -v \sin (\psi+\beta) d t & v \cos (\psi+\beta) & -v \sin (\psi+\beta) d t \\ -0 & 1 & v \cos (\psi+\beta) d t & v \sin (\psi+\beta) & v \cos (\psi+\beta) d t \\ -0 & 0 & 1 & \frac{1}{l_r} \sin \beta d t & \frac{v}{l_r} \cos \beta d t \\ +1 & 0 + & v \cos (\psi+\beta) {d t} - \frac{1}{2} \left\lbrace w v \cos \left( \psi+\beta \right) \right\rbrace {d t}^2 + & \sin (\psi+\beta) {d t} - \left\lbrace w \sin \left( \psi+\beta \right) \right\rbrace {d t}^2 + & -v \sin (\psi+\beta) {d t} - \frac{v^2}{2l_r} \left\lbrace \cos(\beta)\sin(\psi+\beta)+\sin(\beta)\cos(\psi+\beta) \right\rbrace {d t}^2 \\ +0 & 1 + & v \sin (\psi+\beta) {d t} - \frac{1}{2} \left\lbrace w v \sin \left( \psi+\beta \right) \right\rbrace {d t}^2 + & \cos (\psi+\beta) {d t} + \left\lbrace w \cos \left( \psi+\beta \right) \right\rbrace {d t}^2 + & v \cos (\psi+\beta) {d t} + \frac{v^2}{2l_r} \left\lbrace \cos(\beta)\cos(\psi+\beta)-\sin(\beta)\sin(\psi+\beta) \right\rbrace {d t}^2 \\ +0 & 0 & 1 & \frac{1}{l_r} \sin \beta {d t} & \frac{v}{l_r} \cos \beta d t \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \end{array}\right] diff --git a/perception/multi_object_tracker/src/data_association/data_association.cpp b/perception/multi_object_tracker/src/data_association/data_association.cpp index 5e7cadc423216..f367e19c11f2a 100644 --- a/perception/multi_object_tracker/src/data_association/data_association.cpp +++ b/perception/multi_object_tracker/src/data_association/data_association.cpp @@ -201,7 +201,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( measurement_object.kinematics.pose_with_covariance.pose.position, tracked_object.kinematics.pose_with_covariance.pose.position, getXYCovariance(tracked_object.kinematics.pose_with_covariance)); - if (2.448 /*95%*/ <= mahalanobis_dist) passed_gate = false; + if (3.035 /*99%*/ <= mahalanobis_dist) passed_gate = false; } // 2d iou gate if (passed_gate) { diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 6571e70b8c123..8a3a5629ec277 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -52,51 +52,57 @@ BicycleTracker::BicycleTracker( { object_ = object; - // initialize params - float q_stddev_x = 0.6; // [m/s] - float q_stddev_y = 0.6; // [m/s] - float q_stddev_yaw = tier4_autoware_utils::deg2rad(10); // [rad/s] - float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] - float q_stddev_slip = tier4_autoware_utils::deg2rad(15); // [rad/(s*s)] - float r_stddev_x = 0.6; // [m] - float r_stddev_y = 0.4; // [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] - float p0_stddev_x = 0.8; // [m/s] - float p0_stddev_y = 0.5; // [m/s] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000); // [rad/s] - float p0_stddev_vx = tier4_autoware_utils::kmph2mps(1000); // [m/(s*s)] - float p0_stddev_slip = tier4_autoware_utils::deg2rad(10); // [rad/(s*s)] - ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); - ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); - ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); - ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - ekf_params_.q_cov_slip = std::pow(q_stddev_slip, 2.0); + // Initialize parameters + // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty + float r_stddev_x = 0.5; // in vehicle coordinate [m] + float r_stddev_y = 0.4; // in vehicle coordinate [m] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); + // initial state covariance + float p0_stddev_x = 0.8; // [m] + float p0_stddev_y = 0.5; // [m] + float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); - ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); + ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s] - - // initialize X matrix + // process noise covariance + ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + ekf_params_.q_stddev_yaw_rate_min = + tier4_autoware_utils::deg2rad(5.0); // [rad/s] uncertain yaw change rate + ekf_params_.q_stddev_yaw_rate_max = + tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate + float q_stddev_slip_rate_min = + tier4_autoware_utils::deg2rad(1); // [rad/s] uncertain slip angle change rate + float q_stddev_slip_rate_max = + tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate + ekf_params_.q_cov_slip_rate_min = std::pow(q_stddev_slip_rate_min, 2.0); + ekf_params_.q_cov_slip_rate_max = std::pow(q_stddev_slip_rate_max, 2.0); + ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle + // limitations + max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] + max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s] + + // initialize state vector X Eigen::MatrixXd X(ekf_params_.dim_x, 1); X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + X(IDX::SLIP) = 0.0; if (object.kinematics.has_twist) { - X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x; + X(IDX::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; } else { - X(IDX::VX) = 0.0; + X(IDX::VEL) = 0.0; } - X(IDX::SLIP) = 0.0; - // initialize P matrix + // initialize state covariance matrix P Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(X(IDX::YAW)); const double sin_yaw = std::sin(X(IDX::YAW)); @@ -110,7 +116,7 @@ BicycleTracker::BicycleTracker( ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } else { P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; @@ -120,10 +126,10 @@ BicycleTracker::BicycleTracker( P(IDX::YAW, IDX::YAW) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; if (object.kinematics.has_twist_covariance) { - P(IDX::VX, IDX::VX) = + P(IDX::VEL, IDX::VEL) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } else { - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; } P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } @@ -137,15 +143,28 @@ BicycleTracker::BicycleTracker( ekf_.init(X, P); // Set lf, lr - double point_ratio = 0.2; // under steered if smaller than 0.5 - lf_ = bounding_box_.length * point_ratio; - lr_ = bounding_box_.length * (1.0 - point_ratio); + lf_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% front from the center, minimum of 0.3m + lr_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% rear from the center, minimum of 0.3m } bool BicycleTracker::predict(const rclcpp::Time & time) { const double dt = (time - last_update_time_).seconds(); - bool ret = predict(dt, ekf_); + if (dt < 0.0) { + RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); + return false; + } + // if dt is too large, shorten dt and repeat prediction + const double dt_max = 0.11; + const uint32_t repeat = std::ceil(dt / dt_max); + const double dt_ = dt / repeat; + bool ret = false; + for (uint32_t i = 0; i < repeat; ++i) { + ret = predict(dt_, ekf_); + if (!ret) { + return false; + } + } if (ret) { last_update_time_ = time; } @@ -154,69 +173,116 @@ bool BicycleTracker::predict(const rclcpp::Time & time) bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const { - /* == Nonlinear model == static bicycle model + /* Motion model: static bicycle model (constant slip angle, constant velocity) * - * x_{k+1} = x_k + vx_k * cos(yaw_k + slip_k) * dt - * y_{k+1} = y_k + vx_k * sin(yaw_k + slip_k) * dt - * yaw_{k+1} = yaw_k + vx_k / l_r * sin(slip_k) * dt - * vx_{k+1} = vx_k + * w_k = vel_k * sin(slip_k) / l_r + * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt + * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt + * yaw_{k+1} = yaw_k + w_k * dt + * vel_{k+1} = vel_k * slip_{k+1} = slip_k * */ - /* == Linearized model == + /* Jacobian Matrix + * + * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, + * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, + * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] + * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, + * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, + * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] + * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/l_r*cos(slip)*dt] + * [ 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 1] * - * A = [ 1, 0, -vx*sin(yaw+slip)*dt, cos(yaw+slip)*dt, -vx*sin(yaw+slip)*dt] - * [ 0, 1, vx*cos(yaw+slip)*dt, sin(yaw+slip)*dt, vx*cos(yaw+slip)*dt] - * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vx/l_r*cos(slip)*dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] */ - // X t + // Current state vector X t Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf.getX(X_t); const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double vel = X_t(IDX::VEL); const double cos_slip = std::cos(X_t(IDX::SLIP)); const double sin_slip = std::sin(X_t(IDX::SLIP)); - const double vx = X_t(IDX::VX); - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - // X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = X_t(IDX::X) + vx * cos_yaw * dt; // dx = v * cos(yaw) - X_next_t(IDX::Y) = X_t(IDX::Y) + vx * sin_yaw * dt; // dy = v * sin(yaw) - X_next_t(IDX::YAW) = X_t(IDX::YAW) + vx / lr_ * sin_slip * dt; // dyaw = omega - X_next_t(IDX::VX) = X_t(IDX::VX); - X_next_t(IDX::SLIP) = X_t(IDX::SLIP); - - // A + double w = vel * sin_slip / lr_; + const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); + const double w_dtdt = w * dt * dt; + const double vv_dtdt__lr = vel * vel * dt * dt / lr_; + + // Predict state vector X t+1 + Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state + X_next_t(IDX::X) = + X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt + X_next_t(IDX::Y) = + X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt + X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt + X_next_t(IDX::VEL) = X_t(IDX::VEL); + X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) + + // State transition matrix A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -vx * sin_yaw * dt; - A(IDX::X, IDX::VX) = cos_yaw * dt; - A(IDX::X, IDX::SLIP) = -vx * sin_yaw * dt; - A(IDX::Y, IDX::YAW) = vx * cos_yaw * dt; - A(IDX::Y, IDX::VX) = sin_yaw * dt; - A(IDX::Y, IDX::SLIP) = vx * cos_yaw * dt; - A(IDX::YAW, IDX::VX) = 1.0 / lr_ * sin_slip * dt; - A(IDX::YAW, IDX::SLIP) = vx / lr_ * cos_slip * dt; - - // Q + A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; + A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; + A(IDX::X, IDX::SLIP) = + -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; + A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; + A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; + A(IDX::Y, IDX::SLIP) = + vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; + A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; + A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; + + // Process noise covariance Q + float q_stddev_yaw_rate{0.0}; + if (vel <= 0.01) { + q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min; + } else { + // uncertainty of the yaw rate is limited by + // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v + // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r + q_stddev_yaw_rate = std::min( + ekf_params_.q_stddev_acc_lat / vel, + vel * std::sin(ekf_params_.q_max_slip_angle) / lr_); // [rad/s] + q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max); + q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min); + } + float q_cov_slip_rate{0.0f}; + if (vel <= 0.01) { + q_cov_slip_rate = ekf_params_.q_cov_slip_rate_min; + } else { + // The slip angle rate uncertainty is modeled as follows: + // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt + // where sin(slip) = w * l_r / v + // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) + // d(v)/dt and d(w)/t are considered to be uncorrelated + q_cov_slip_rate = + std::pow(ekf_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); + q_cov_slip_rate = std::min(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_max); + q_cov_slip_rate = std::max(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_min); + } + const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2); + const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2); + const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); + const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2); + const float q_cov_slip = q_cov_slip_rate * dt * dt; + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); // Rotate the covariance matrix according to the vehicle yaw // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = - (ekf_params_.q_cov_x * cos_yaw * cos_yaw + ekf_params_.q_cov_y * sin_yaw * sin_yaw) * dt * dt; - Q(IDX::X, IDX::Y) = (0.5f * (ekf_params_.q_cov_x - ekf_params_.q_cov_y) * sin_2yaw) * dt * dt; - Q(IDX::Y, IDX::Y) = - (ekf_params_.q_cov_x * sin_yaw * sin_yaw + ekf_params_.q_cov_y * cos_yaw * cos_yaw) * dt * dt; + Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); + Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); + Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw * dt * dt; - Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt; - Q(IDX::SLIP, IDX::SLIP) = ekf_params_.q_cov_slip * dt * dt; - Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); + Q(IDX::YAW, IDX::YAW) = q_cov_yaw; + Q(IDX::VEL, IDX::VEL) = q_cov_vel; + Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; + + // control-input model B and control-input u are not used + // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); + // Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); if (!ekf.predict(X_next_t, A, Q)) { RCLCPP_WARN(logger_, "Cannot predict"); @@ -225,12 +291,6 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const return true; } -/** - * @brief measurement update with ekf on receiving detected_object - * - * @param object : Detected object - * @return bool : measurement is updatable - */ bool BicycleTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { @@ -267,19 +327,16 @@ bool BicycleTracker::measureWithPose( const int dim_y = use_orientation_information ? 3 : 2; // pos x, pos y, (pos yaw) depending on Pose output - /* Set measurement matrix */ + // Set measurement matrix C and observation vector Y Eigen::MatrixXd Y(dim_y, 1); + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); Y(IDX::X, 0) = object.kinematics.pose_with_covariance.pose.position.x; Y(IDX::Y, 0) = object.kinematics.pose_with_covariance.pose.position.y; - - /* Set measurement matrix */ - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); C(0, IDX::X) = 1.0; // for pos x C(1, IDX::Y) = 1.0; // for pos y - /* Set measurement noise covariance */ + // Set noise covariance matrix R Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - if (!object.kinematics.has_position_covariance) { R(0, 0) = ekf_params_.r_cov_x; // x - x R(0, 1) = 0.0; // x - y @@ -310,20 +367,20 @@ bool BicycleTracker::measureWithPose( } } - // update with ekf + // ekf update if (!ekf_.update(Y, C, R)) { RCLCPP_WARN(logger_, "Cannot update"); } - // normalize yaw and limit vx, wz + // normalize yaw and limit vel, slip { Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); ekf_.getX(X_t); ekf_.getP(P_t); X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { - X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; + if (!(-max_vel_ <= X_t(IDX::VEL) && X_t(IDX::VEL) <= max_vel_)) { + X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -max_vel_ : max_vel_; } if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; @@ -341,14 +398,26 @@ bool BicycleTracker::measureWithPose( bool BicycleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { + // if the input shape is convex type, convert it to bbox type + autoware_auto_perception_msgs::msg::DetectedObject bbox_object; if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - return false; + utils::convertConvexHullToBoundingBox(object, bbox_object); + } else { + bbox_object = object; } - constexpr float gain = 0.9; - bounding_box_.length = gain * bounding_box_.length + (1.0 - gain) * object.shape.dimensions.x; - bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * object.shape.dimensions.y; - bounding_box_.height = gain * bounding_box_.height + (1.0 - gain) * object.shape.dimensions.z; + constexpr float gain = 0.9; + bounding_box_.length = + gain * bounding_box_.length + (1.0 - gain) * bbox_object.shape.dimensions.x; + bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * bbox_object.shape.dimensions.y; + bounding_box_.height = + gain * bounding_box_.height + (1.0 - gain) * bbox_object.shape.dimensions.z; + last_input_bounding_box_ = { + bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + + // update lf, lr + lf_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% front from the center, minimum of 0.3m + lr_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% rear from the center, minimum of 0.3m return true; } @@ -382,7 +451,7 @@ bool BicycleTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict kinematics + // predict state KalmanFilter tmp_ekf_for_no_update = ekf_; const double dt = (time - last_update_time_).seconds(); if (0.001 /*1msec*/ < dt) { @@ -393,6 +462,7 @@ bool BicycleTracker::getTrackedObject( tmp_ekf_for_no_update.getX(X_t); tmp_ekf_for_no_update.getP(P); + /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; @@ -429,23 +499,44 @@ bool BicycleTracker::getTrackedObject( pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); // twist - twist_with_cov.twist.linear.x = X_t(IDX::VX) * std::cos(X_t(IDX::SLIP)); - twist_with_cov.twist.linear.y = X_t(IDX::VX) * std::sin(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); twist_with_cov.twist.angular.z = - X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k) - // twist covariance - constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r + /* twist covariance + * convert covariance from velocity, slip angle to vx, vy, and yaw angle + * + * vx = vel * cos(slip) + * vy = vel * sin(slip) + * wz = vel * sin(slip) / l_r = vy / l_r + * + */ + constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov; + + Eigen::MatrixXd cov_jacob(3, 2); + cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_; + Eigen::MatrixXd cov_twist(2, 2); + cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), + P(IDX::SLIP, IDX::SLIP); + Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); + + twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::SLIP); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::SLIP, IDX::VX); twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::SLIP, IDX::SLIP); // set shape object.shape.dimensions.x = bounding_box_.length; diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 97d6d48c35d1b..80c0df7e5ffb1 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -16,6 +16,14 @@ // Author: v1.0 Yukihiro Saito // +#include "multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" + +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include +#include + #include #include #include @@ -26,17 +34,11 @@ #else #include #endif - -#define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" +#define EIGEN_MPL2_ONLY #include #include -#include -#include -#include using Label = autoware_auto_perception_msgs::msg::ObjectClassification; @@ -51,53 +53,59 @@ BigVehicleTracker::BigVehicleTracker( { object_ = object; - // initialize params - float q_stddev_x = 1.5; // [m/s] - float q_stddev_y = 1.5; // [m/s] - float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] - float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] - float q_stddev_slip = tier4_autoware_utils::deg2rad(5); // [rad/(s*s)] - float r_stddev_x = 1.5; // [m] - float r_stddev_y = 0.5; // [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] - float r_stddev_vx = 1.0; // [m/s] - float p0_stddev_x = 1.5; // [m] - float p0_stddev_y = 0.5; // [m] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] - float p0_stddev_vx = tier4_autoware_utils::kmph2mps(1000); // [m/s] - float p0_stddev_slip = tier4_autoware_utils::deg2rad(10); // [rad/s] - ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); - ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); - ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); - ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - ekf_params_.q_cov_slip = std::pow(q_stddev_slip, 2.0); + // Initialize parameters + // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty + float r_stddev_x = 0.5; // in vehicle coordinate [m] + float r_stddev_y = 0.4; // in vehicle coordinate [m] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad] + float r_stddev_vel = 1.0; // [m/s] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); - ekf_params_.r_cov_vx = std::pow(r_stddev_vx, 2.0); + ekf_params_.r_cov_vel = std::pow(r_stddev_vel, 2.0); + // initial state covariance + float p0_stddev_x = 1.5; // [m] + float p0_stddev_y = 0.5; // [m] + float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); - ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); + ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s] + // process noise covariance + ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + ekf_params_.q_stddev_yaw_rate_min = + tier4_autoware_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate + ekf_params_.q_stddev_yaw_rate_max = + tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate + float q_stddev_slip_rate_min = + tier4_autoware_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate + float q_stddev_slip_rate_max = + tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate + ekf_params_.q_cov_slip_rate_min = std::pow(q_stddev_slip_rate_min, 2.0); + ekf_params_.q_cov_slip_rate_max = std::pow(q_stddev_slip_rate_max, 2.0); + ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle + // limitations + max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad] velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] - // initialize X matrix + // initialize state vector X Eigen::MatrixXd X(ekf_params_.dim_x, 1); X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); X(IDX::SLIP) = 0.0; if (object.kinematics.has_twist) { - X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x; - // X(IDX::YAW) = object.kinematics.twist_with_covariance.twist.angular.z; + X(IDX::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; } else { - X(IDX::VX) = 0.0; + X(IDX::VEL) = 0.0; } - // initialize P matrix + // initialize state covariance matrix P Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(X(IDX::YAW)); @@ -112,7 +120,7 @@ BigVehicleTracker::BigVehicleTracker( ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } else { P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; @@ -122,10 +130,10 @@ BigVehicleTracker::BigVehicleTracker( P(IDX::YAW, IDX::YAW) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; if (object.kinematics.has_twist_covariance) { - P(IDX::VX, IDX::VX) = + P(IDX::VEL, IDX::VEL) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } else { - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; } P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } @@ -142,9 +150,7 @@ BigVehicleTracker::BigVehicleTracker( bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; - last_input_bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, - bbox_object.shape.dimensions.z}; + last_input_bounding_box_ = bounding_box_; } ekf_.init(X, P); @@ -152,15 +158,28 @@ BigVehicleTracker::BigVehicleTracker( setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step // Set lf, lr - double point_ratio = 0.2; // under steered if smaller than 0.5 - lf_ = bounding_box_.length * point_ratio; - lr_ = bounding_box_.length * (1.0 - point_ratio); + lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m + lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center, minimum of 1.5m } bool BigVehicleTracker::predict(const rclcpp::Time & time) { const double dt = (time - last_update_time_).seconds(); - bool ret = predict(dt, ekf_); + if (dt < 0.0) { + RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); + return false; + } + // if dt is too large, shorten dt and repeat prediction + const double dt_max = 0.11; + const uint32_t repeat = std::ceil(dt / dt_max); + const double dt_ = dt / repeat; + bool ret = false; + for (uint32_t i = 0; i < repeat; ++i) { + ret = predict(dt_, ekf_); + if (!ret) { + return false; + } + } if (ret) { last_update_time_ = time; } @@ -169,69 +188,116 @@ bool BigVehicleTracker::predict(const rclcpp::Time & time) bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const { - /* == Nonlinear model == static bicycle model + /* Motion model: static bicycle model (constant slip angle, constant velocity) * - * x_{k+1} = x_k + vx_k * cos(yaw_k + slip_k) * dt - * y_{k+1} = y_k + vx_k * sin(yaw_k + slip_k) * dt - * yaw_{k+1} = yaw_k + vx_k / l_r * sin(slip_k) * dt - * vx_{k+1} = vx_k + * w_k = vel_k * sin(slip_k) / l_r + * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt + * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt + * yaw_{k+1} = yaw_k + w_k * dt + * vel_{k+1} = vel_k * slip_{k+1} = slip_k * */ - /* == Linearized model == + /* Jacobian Matrix + * + * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, + * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, + * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] + * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, + * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, + * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] + * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/l_r*cos(slip)*dt] + * [ 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 1] * - * A = [ 1, 0, -vx*sin(yaw+slip)*dt, cos(yaw+slip)*dt, -vx*sin(yaw+slip)*dt] - * [ 0, 1, vx*cos(yaw+slip)*dt, sin(yaw+slip)*dt, vx*cos(yaw+slip)*dt] - * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vx/l_r*cos(slip)*dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] */ - // X t + // Current state vector X t Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf.getX(X_t); const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double vel = X_t(IDX::VEL); const double cos_slip = std::cos(X_t(IDX::SLIP)); const double sin_slip = std::sin(X_t(IDX::SLIP)); - const double vx = X_t(IDX::VX); - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - // X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = X_t(IDX::X) + vx * cos_yaw * dt; // dx = v * cos(yaw) - X_next_t(IDX::Y) = X_t(IDX::Y) + vx * sin_yaw * dt; // dy = v * sin(yaw) - X_next_t(IDX::YAW) = X_t(IDX::YAW) + vx / lr_ * sin_slip * dt; // dyaw = omega - X_next_t(IDX::VX) = X_t(IDX::VX); - X_next_t(IDX::SLIP) = X_t(IDX::SLIP); - - // A + double w = vel * sin_slip / lr_; + const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); + const double w_dtdt = w * dt * dt; + const double vv_dtdt__lr = vel * vel * dt * dt / lr_; + + // Predict state vector X t+1 + Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state + X_next_t(IDX::X) = + X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt + X_next_t(IDX::Y) = + X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt + X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt + X_next_t(IDX::VEL) = X_t(IDX::VEL); + X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) + + // State transition matrix A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -vx * sin_yaw * dt; - A(IDX::X, IDX::VX) = cos_yaw * dt; - A(IDX::X, IDX::SLIP) = -vx * sin_yaw * dt; - A(IDX::Y, IDX::YAW) = vx * cos_yaw * dt; - A(IDX::Y, IDX::VX) = sin_yaw * dt; - A(IDX::Y, IDX::SLIP) = vx * cos_yaw * dt; - A(IDX::YAW, IDX::VX) = 1.0 / lr_ * sin_slip * dt; - A(IDX::YAW, IDX::SLIP) = vx / lr_ * cos_slip * dt; - - // Q + A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; + A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; + A(IDX::X, IDX::SLIP) = + -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; + A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; + A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; + A(IDX::Y, IDX::SLIP) = + vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; + A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; + A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; + + // Process noise covariance Q + float q_stddev_yaw_rate{0.0}; + if (vel <= 0.01) { + q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min; + } else { + // uncertainty of the yaw rate is limited by + // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v + // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r + q_stddev_yaw_rate = std::min( + ekf_params_.q_stddev_acc_lat / vel, + vel * std::sin(ekf_params_.q_max_slip_angle) / lr_); // [rad/s] + q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max); + q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min); + } + float q_cov_slip_rate{0.0f}; + if (vel <= 0.01) { + q_cov_slip_rate = ekf_params_.q_cov_slip_rate_min; + } else { + // The slip angle rate uncertainty is modeled as follows: + // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt + // where sin(slip) = w * l_r / v + // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) + // d(v)/dt and d(w)/t are considered to be uncorrelated + q_cov_slip_rate = + std::pow(ekf_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); + q_cov_slip_rate = std::min(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_max); + q_cov_slip_rate = std::max(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_min); + } + const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2); + const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2); + const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); + const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2); + const float q_cov_slip = q_cov_slip_rate * dt * dt; + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); // Rotate the covariance matrix according to the vehicle yaw // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = - (ekf_params_.q_cov_x * cos_yaw * cos_yaw + ekf_params_.q_cov_y * sin_yaw * sin_yaw) * dt * dt; - Q(IDX::X, IDX::Y) = (0.5f * (ekf_params_.q_cov_x - ekf_params_.q_cov_y) * sin_2yaw) * dt * dt; - Q(IDX::Y, IDX::Y) = - (ekf_params_.q_cov_x * sin_yaw * sin_yaw + ekf_params_.q_cov_y * cos_yaw * cos_yaw) * dt * dt; + Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); + Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); + Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw * dt * dt; - Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt; - Q(IDX::SLIP, IDX::SLIP) = ekf_params_.q_cov_slip * dt * dt; - Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); + Q(IDX::YAW, IDX::YAW) = q_cov_yaw; + Q(IDX::VEL, IDX::VEL) = q_cov_vel; + Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; + + // control-input model B and control-input u are not used + // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); + // Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); if (!ekf.predict(X_next_t, A, Q)) { RCLCPP_WARN(logger_, "Cannot predict"); @@ -249,14 +315,14 @@ bool BigVehicleTracker::measureWithPose( float r_cov_y; const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - if (label == Label::CAR) { - constexpr float r_stddev_x = 8.0; // [m] - constexpr float r_stddev_y = 0.8; // [m] - r_cov_x = std::pow(r_stddev_x, 2.0); - r_cov_y = std::pow(r_stddev_y, 2.0); - } else if (utils::isLargeVehicleLabel(label)) { + if (utils::isLargeVehicleLabel(label)) { r_cov_x = ekf_params_.r_cov_x; r_cov_y = ekf_params_.r_cov_y; + } else if (label == Label::CAR) { + constexpr float r_stddev_x = 2.0; // [m] + constexpr float r_stddev_y = 2.0; // [m] + r_cov_x = std::pow(r_stddev_x, 2.0); + r_cov_y = std::pow(r_stddev_y, 2.0); } else { r_cov_x = ekf_params_.r_cov_x; r_cov_y = ekf_params_.r_cov_y; @@ -267,16 +333,16 @@ bool BigVehicleTracker::measureWithPose( if (object.kinematics.has_twist) { Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf_.getX(X_t); - const double predicted_vx = X_t(IDX::VX); - const double observed_vx = object.kinematics.twist_with_covariance.twist.linear.x; + const double predicted_vel = X_t(IDX::VEL); + const double observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; - if (std::fabs(predicted_vx - observed_vx) < velocity_deviation_threshold_) { + if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { // Velocity deviation is small enable_velocity_measurement = true; } } - // pos x, pos y, yaw, vx depending on pose measurement + // pos x, pos y, yaw, vel depending on pose measurement const int dim_y = enable_velocity_measurement ? 4 : 3; double measurement_yaw = getMeasurementYaw(object); // get sign-solved yaw angle Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state @@ -296,11 +362,9 @@ bool BigVehicleTracker::measureWithPose( last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_, bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_); - /* Set measurement matrix */ + // Set measurement matrix C and observation vector Y Eigen::MatrixXd Y(dim_y, 1); Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - Y(IDX::X, 0) = offset_object.kinematics.pose_with_covariance.pose.position.x; Y(IDX::Y, 0) = offset_object.kinematics.pose_with_covariance.pose.position.y; Y(IDX::YAW, 0) = measurement_yaw; @@ -308,7 +372,8 @@ bool BigVehicleTracker::measureWithPose( C(1, IDX::Y) = 1.0; // for pos y C(2, IDX::YAW) = 1.0; // for yaw - /* Set measurement noise covariance */ + // Set noise covariance matrix R + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(measurement_yaw); const double sin_yaw = std::sin(measurement_yaw); @@ -330,31 +395,32 @@ bool BigVehicleTracker::measureWithPose( R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; } + // Update the velocity when necessary if (dim_y == 4) { - Y(IDX::VX, 0) = object.kinematics.twist_with_covariance.twist.linear.x; - C(3, IDX::VX) = 1.0; // for vx + Y(IDX::VEL, 0) = object.kinematics.twist_with_covariance.twist.linear.x; + C(3, IDX::VEL) = 1.0; // for vel if (!object.kinematics.has_twist_covariance) { - R(3, 3) = ekf_params_.r_cov_vx; // vx -vx + R(3, 3) = ekf_params_.r_cov_vel; // vel -vel } else { R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } } - /* ekf tracks constant tracking point */ + // ekf update if (!ekf_.update(Y, C, R)) { RCLCPP_WARN(logger_, "Cannot update"); } - // normalize yaw and limit vx, slip + // normalize yaw and limit vel, slip { Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); ekf_.getX(X_t); ekf_.getP(P_t); X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { - X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; + if (!(-max_vel_ <= X_t(IDX::VEL) && X_t(IDX::VEL) <= max_vel_)) { + X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -max_vel_ : max_vel_; } if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; @@ -372,9 +438,8 @@ bool BigVehicleTracker::measureWithPose( bool BigVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { + // if the input shape is convex type, convert it to bbox type autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - - // if input is convex shape convert it to bbox shape if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { utils::convertConvexHullToBoundingBox(object, bbox_object); } else { @@ -389,6 +454,11 @@ bool BigVehicleTracker::measureWithShape( gain * bounding_box_.height + (1.0 - gain) * bbox_object.shape.dimensions.z; last_input_bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + + // update lf, lr + lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m + lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center, minimum of 1.5m + return true; } @@ -480,23 +550,44 @@ bool BigVehicleTracker::getTrackedObject( pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); // twist - twist_with_cov.twist.linear.x = X_t(IDX::VX) * std::cos(X_t(IDX::SLIP)); - twist_with_cov.twist.linear.y = X_t(IDX::VX) * std::sin(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); twist_with_cov.twist.angular.z = - X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k) - // twist covariance - constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r + /* twist covariance + * convert covariance from velocity, slip angle to vx, vy, and yaw angle + * + * vx = vel * cos(slip) + * vy = vel * sin(slip) + * wz = vel * sin(slip) / l_r = vy / l_r + * + */ + constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov; + + Eigen::MatrixXd cov_jacob(3, 2); + cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_; + Eigen::MatrixXd cov_twist(2, 2); + cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), + P(IDX::SLIP, IDX::SLIP); + Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); + + twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::SLIP); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::SLIP, IDX::VX); twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::SLIP, IDX::SLIP); // set shape object.shape.dimensions.x = bounding_box_.length; diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index aa3f7b1c30d01..7714c381894f0 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -16,6 +16,14 @@ // Author: v1.0 Yukihiro Saito // +#include "multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" + +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include +#include + #include #include #include @@ -26,17 +34,11 @@ #else #include #endif - -#define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" +#define EIGEN_MPL2_ONLY #include #include -#include -#include -#include using Label = autoware_auto_perception_msgs::msg::ObjectClassification; @@ -51,52 +53,59 @@ NormalVehicleTracker::NormalVehicleTracker( { object_ = object; - // initialize params - float q_stddev_x = 1.0; // object coordinate [m/s] - float q_stddev_y = 1.0; // object coordinate [m/s] - float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // map coordinate[rad/s] - float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // object coordinate [m/(s*s)] - float q_stddev_slip = tier4_autoware_utils::deg2rad(5); // object coordinate [rad/(s*s)] - float r_stddev_x = 1.0; // object coordinate [m] - float r_stddev_y = 0.3; // object coordinate [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // map coordinate [rad] - float r_stddev_vx = 1.0; // object coordinate [m/s] - float p0_stddev_x = 1.0; // object coordinate [m/s] - float p0_stddev_y = 0.3; // object coordinate [m/s] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(30); // map coordinate [rad] - float p0_stddev_vx = tier4_autoware_utils::kmph2mps(1000); // object coordinate [m/s] - float p0_stddev_slip = tier4_autoware_utils::deg2rad(10); // object coordinate [rad/s] - ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); - ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); - ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); - ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - ekf_params_.q_cov_slip = std::pow(q_stddev_slip, 2.0); + // Initialize parameters + // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty + float r_stddev_x = 0.5; // in vehicle coordinate [m] + float r_stddev_y = 0.4; // in vehicle coordinate [m] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // in map coordinate [rad] + float r_stddev_vel = 1.0; // in object coordinate [m/s] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); - ekf_params_.r_cov_vx = std::pow(r_stddev_vx, 2.0); + ekf_params_.r_cov_vel = std::pow(r_stddev_vel, 2.0); + // initial state covariance + float p0_stddev_x = 1.0; // in object coordinate [m] + float p0_stddev_y = 0.3; // in object coordinate [m] + float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); - ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); + ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s] + // process noise covariance + ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + ekf_params_.q_stddev_yaw_rate_min = + tier4_autoware_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate + ekf_params_.q_stddev_yaw_rate_max = + tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate + float q_stddev_slip_rate_min = + tier4_autoware_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate + float q_stddev_slip_rate_max = + tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate + ekf_params_.q_cov_slip_rate_min = std::pow(q_stddev_slip_rate_min, 2.0); + ekf_params_.q_cov_slip_rate_max = std::pow(q_stddev_slip_rate_max, 2.0); + ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle + // limitations + max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] + max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad] velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] - // initialize X matrix + // initialize state vector X Eigen::MatrixXd X(ekf_params_.dim_x, 1); X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + X(IDX::SLIP) = 0.0; if (object.kinematics.has_twist) { - X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x; + X(IDX::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; } else { - X(IDX::VX) = 0.0; + X(IDX::VEL) = 0.0; } - X(IDX::SLIP) = 0.0; - // initialize P matrix + // initialize state covariance matrix P Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(X(IDX::YAW)); @@ -111,7 +120,7 @@ NormalVehicleTracker::NormalVehicleTracker( ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } else { P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; @@ -121,10 +130,10 @@ NormalVehicleTracker::NormalVehicleTracker( P(IDX::YAW, IDX::YAW) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; if (object.kinematics.has_twist_covariance) { - P(IDX::VX, IDX::VX) = + P(IDX::VEL, IDX::VEL) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } else { - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; } P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } @@ -132,8 +141,7 @@ NormalVehicleTracker::NormalVehicleTracker( if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; - last_input_bounding_box_ = { - object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; + last_input_bounding_box_ = bounding_box_; } else { // past default value // bounding_box_ = {4.0, 1.7, 2.0}; @@ -142,9 +150,7 @@ NormalVehicleTracker::NormalVehicleTracker( bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; - last_input_bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, - bbox_object.shape.dimensions.z}; + last_input_bounding_box_ = bounding_box_; } ekf_.init(X, P); @@ -152,15 +158,28 @@ NormalVehicleTracker::NormalVehicleTracker( setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step // Set lf, lr - double point_ratio = 0.2; // under steered if smaller than 0.5 - lf_ = bounding_box_.length * point_ratio; - lr_ = bounding_box_.length * (1.0 - point_ratio); + lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m + lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center, minimum of 1.0m } bool NormalVehicleTracker::predict(const rclcpp::Time & time) { const double dt = (time - last_update_time_).seconds(); - bool ret = predict(dt, ekf_); + if (dt < 0.0) { + RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); + return false; + } + // if dt is too large, shorten dt and repeat prediction + const double dt_max = 0.11; + const uint32_t repeat = std::ceil(dt / dt_max); + const double dt_ = dt / repeat; + bool ret = false; + for (uint32_t i = 0; i < repeat; ++i) { + ret = predict(dt_, ekf_); + if (!ret) { + return false; + } + } if (ret) { last_update_time_ = time; } @@ -169,69 +188,116 @@ bool NormalVehicleTracker::predict(const rclcpp::Time & time) bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const { - /* == Nonlinear model == static bicycle model + /* Motion model: static bicycle model (constant slip angle, constant velocity) * - * x_{k+1} = x_k + vx_k * cos(yaw_k + slip_k) * dt - * y_{k+1} = y_k + vx_k * sin(yaw_k + slip_k) * dt - * yaw_{k+1} = yaw_k + vx_k / l_r * sin(slip_k) * dt - * vx_{k+1} = vx_k + * w_k = vel_k * sin(slip_k) / l_r + * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt + * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt + * yaw_{k+1} = yaw_k + w_k * dt + * vel_{k+1} = vel_k * slip_{k+1} = slip_k * */ - /* == Linearized model == + /* Jacobian Matrix + * + * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, + * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, + * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] + * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, + * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, + * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] + * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/l_r*cos(slip)*dt] + * [ 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 1] * - * A = [ 1, 0, -vx*sin(yaw+slip)*dt, cos(yaw+slip)*dt, -vx*sin(yaw+slip)*dt] - * [ 0, 1, vx*cos(yaw+slip)*dt, sin(yaw+slip)*dt, vx*cos(yaw+slip)*dt] - * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vx/l_r*cos(slip)*dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] */ - // X t + // Current state vector X t Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf.getX(X_t); const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double vel = X_t(IDX::VEL); const double cos_slip = std::cos(X_t(IDX::SLIP)); const double sin_slip = std::sin(X_t(IDX::SLIP)); - const double vx = X_t(IDX::VX); - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - // X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = X_t(IDX::X) + vx * cos_yaw * dt; // dx = v * cos(yaw) - X_next_t(IDX::Y) = X_t(IDX::Y) + vx * sin_yaw * dt; // dy = v * sin(yaw) - X_next_t(IDX::YAW) = X_t(IDX::YAW) + vx / lr_ * sin_slip * dt; // dyaw = omega - X_next_t(IDX::VX) = X_t(IDX::VX); - X_next_t(IDX::SLIP) = X_t(IDX::SLIP); - - // A + double w = vel * sin_slip / lr_; + const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); + const double w_dtdt = w * dt * dt; + const double vv_dtdt__lr = vel * vel * dt * dt / lr_; + + // Predict state vector X t+1 + Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state + X_next_t(IDX::X) = + X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt + X_next_t(IDX::Y) = + X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt + X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt + X_next_t(IDX::VEL) = X_t(IDX::VEL); + X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) + + // State transition matrix A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -vx * sin_yaw * dt; - A(IDX::X, IDX::VX) = cos_yaw * dt; - A(IDX::X, IDX::SLIP) = -vx * sin_yaw * dt; - A(IDX::Y, IDX::YAW) = vx * cos_yaw * dt; - A(IDX::Y, IDX::VX) = sin_yaw * dt; - A(IDX::Y, IDX::SLIP) = vx * cos_yaw * dt; - A(IDX::YAW, IDX::VX) = 1.0 / lr_ * sin_slip * dt; - A(IDX::YAW, IDX::SLIP) = vx / lr_ * cos_slip * dt; - - // Q + A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; + A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; + A(IDX::X, IDX::SLIP) = + -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; + A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; + A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; + A(IDX::Y, IDX::SLIP) = + vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; + A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; + A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; + + // Process noise covariance Q + float q_stddev_yaw_rate{0.0}; + if (vel <= 0.01) { + q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min; + } else { + // uncertainty of the yaw rate is limited by + // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v + // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r + q_stddev_yaw_rate = std::min( + ekf_params_.q_stddev_acc_lat / vel, + vel * std::sin(ekf_params_.q_max_slip_angle) / lr_); // [rad/s] + q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max); + q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min); + } + float q_cov_slip_rate{0.0f}; + if (vel <= 0.01) { + q_cov_slip_rate = ekf_params_.q_cov_slip_rate_min; + } else { + // The slip angle rate uncertainty is modeled as follows: + // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt + // where sin(slip) = w * l_r / v + // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) + // d(v)/dt and d(w)/t are considered to be uncorrelated + q_cov_slip_rate = + std::pow(ekf_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); + q_cov_slip_rate = std::min(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_max); + q_cov_slip_rate = std::max(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_min); + } + const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2); + const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2); + const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); + const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2); + const float q_cov_slip = q_cov_slip_rate * dt * dt; + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); // Rotate the covariance matrix according to the vehicle yaw // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = - (ekf_params_.q_cov_x * cos_yaw * cos_yaw + ekf_params_.q_cov_y * sin_yaw * sin_yaw) * dt * dt; - Q(IDX::X, IDX::Y) = (0.5f * (ekf_params_.q_cov_x - ekf_params_.q_cov_y) * sin_2yaw) * dt * dt; - Q(IDX::Y, IDX::Y) = - (ekf_params_.q_cov_x * sin_yaw * sin_yaw + ekf_params_.q_cov_y * cos_yaw * cos_yaw) * dt * dt; + Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); + Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); + Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw * dt * dt; - Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt; - Q(IDX::SLIP, IDX::SLIP) = ekf_params_.q_cov_slip * dt * dt; - Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); + Q(IDX::YAW, IDX::YAW) = q_cov_yaw; + Q(IDX::VEL, IDX::VEL) = q_cov_vel; + Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; + + // control-input model B and control-input u are not used + // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); + // Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); if (!ekf.predict(X_next_t, A, Q)) { RCLCPP_WARN(logger_, "Cannot predict"); @@ -253,8 +319,8 @@ bool NormalVehicleTracker::measureWithPose( r_cov_x = ekf_params_.r_cov_x; r_cov_y = ekf_params_.r_cov_y; } else if (utils::isLargeVehicleLabel(label)) { - constexpr float r_stddev_x = 8.0; // [m] - constexpr float r_stddev_y = 0.8; // [m] + constexpr float r_stddev_x = 2.0; // [m] + constexpr float r_stddev_y = 2.0; // [m] r_cov_x = std::pow(r_stddev_x, 2.0); r_cov_y = std::pow(r_stddev_y, 2.0); } else { @@ -262,35 +328,25 @@ bool NormalVehicleTracker::measureWithPose( r_cov_y = ekf_params_.r_cov_y; } - // extract current state - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf_.getX(X_t); - // Decide dimension of measurement vector bool enable_velocity_measurement = false; if (object.kinematics.has_twist) { - const double predicted_vx = X_t(IDX::VX); - const double observed_vx = object.kinematics.twist_with_covariance.twist.linear.x; + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state + ekf_.getX(X_t); + const double predicted_vel = X_t(IDX::VEL); + const double observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; - if (std::fabs(predicted_vx - observed_vx) < velocity_deviation_threshold_) { + if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { // Velocity deviation is small enable_velocity_measurement = true; } } - // pos x, pos y, yaw, vx depending on pose output + // pos x, pos y, yaw, vel depending on pose measurement const int dim_y = enable_velocity_measurement ? 4 : 3; - double measurement_yaw = tier4_autoware_utils::normalizeRadian( - tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); - { - // Fixed measurement_yaw to be in the range of +-90 degrees of X_t(IDX::YAW) - while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { - measurement_yaw = measurement_yaw + M_PI; - } - while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { - measurement_yaw = measurement_yaw - M_PI; - } - } + double measurement_yaw = getMeasurementYaw(object); // get sign-solved yaw angle + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state + ekf_.getX(X_t); // convert to boundingbox if input is convex shape autoware_auto_perception_msgs::msg::DetectedObject bbox_object; @@ -306,11 +362,9 @@ bool NormalVehicleTracker::measureWithPose( last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_, bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_); - /* Set measurement matrix and noise covariance*/ + // Set measurement matrix C and observation vector Y Eigen::MatrixXd Y(dim_y, 1); Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - Y(IDX::X, 0) = offset_object.kinematics.pose_with_covariance.pose.position.x; Y(IDX::Y, 0) = offset_object.kinematics.pose_with_covariance.pose.position.y; Y(IDX::YAW, 0) = measurement_yaw; @@ -318,7 +372,8 @@ bool NormalVehicleTracker::measureWithPose( C(1, IDX::Y) = 1.0; // for pos y C(2, IDX::YAW) = 1.0; // for yaw - /* Set measurement noise covariance */ + // Set noise covariance matrix R + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(measurement_yaw); const double sin_yaw = std::sin(measurement_yaw); @@ -342,30 +397,30 @@ bool NormalVehicleTracker::measureWithPose( // Update the velocity when necessary if (dim_y == 4) { - Y(IDX::VX, 0) = object.kinematics.twist_with_covariance.twist.linear.x; - C(3, IDX::VX) = 1.0; // for vx + Y(IDX::VEL, 0) = object.kinematics.twist_with_covariance.twist.linear.x; + C(3, IDX::VEL) = 1.0; // for vel if (!object.kinematics.has_twist_covariance) { - R(3, 3) = ekf_params_.r_cov_vx; // vx -vx + R(3, 3) = ekf_params_.r_cov_vel; // vel -vel } else { R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } } - // ekf update: this tracks tracking point + // ekf update if (!ekf_.update(Y, C, R)) { RCLCPP_WARN(logger_, "Cannot update"); } - // normalize yaw and limit vx, wz + // normalize yaw and limit vel, slip { Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); ekf_.getX(X_t); ekf_.getP(P_t); X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { - X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; + if (!(-max_vel_ <= X_t(IDX::VEL) && X_t(IDX::VEL) <= max_vel_)) { + X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -max_vel_ : max_vel_; } if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; @@ -383,9 +438,8 @@ bool NormalVehicleTracker::measureWithPose( bool NormalVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { + // if the input shape is convex type, convert it to bbox type autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - - // if input is convex shape convert it to bbox shape if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { utils::convertConvexHullToBoundingBox(object, bbox_object); } else { @@ -400,6 +454,11 @@ bool NormalVehicleTracker::measureWithShape( gain * bounding_box_.height + (1.0 - gain) * bbox_object.shape.dimensions.z; last_input_bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + + // update lf, lr + lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m + lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center, minimum of 1.0m + return true; } @@ -422,12 +481,6 @@ bool NormalVehicleTracker::measure( measureWithPose(object); measureWithShape(object); - // refinement - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); - ekf_.getX(X_t); - ekf_.getP(P_t); - /* calc nearest corner index*/ setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step @@ -441,7 +494,7 @@ bool NormalVehicleTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict kinematics + // predict state KalmanFilter tmp_ekf_for_no_update = ekf_; const double dt = (time - last_update_time_).seconds(); if (0.001 /*1msec*/ < dt) { @@ -452,6 +505,7 @@ bool NormalVehicleTracker::getTrackedObject( tmp_ekf_for_no_update.getX(X_t); tmp_ekf_for_no_update.getP(P); + /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; @@ -496,23 +550,44 @@ bool NormalVehicleTracker::getTrackedObject( pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); // twist - twist_with_cov.twist.linear.x = X_t(IDX::VX) * std::cos(X_t(IDX::SLIP)); - twist_with_cov.twist.linear.y = X_t(IDX::VX) * std::sin(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); twist_with_cov.twist.angular.z = - X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k) - // twist covariance - constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r + /* twist covariance + * convert covariance from velocity, slip angle to vx, vy, and yaw angle + * + * vx = vel * cos(slip) + * vy = vel * sin(slip) + * wz = vel * sin(slip) / l_r = vy / l_r + * + */ + constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov; + + Eigen::MatrixXd cov_jacob(3, 2); + cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_; + Eigen::MatrixXd cov_twist(2, 2); + cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), + P(IDX::SLIP, IDX::SLIP); + Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); + + twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::SLIP); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::SLIP, IDX::VX); twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::SLIP, IDX::SLIP); // set shape object.shape.dimensions.x = bounding_box_.length; @@ -522,6 +597,7 @@ bool NormalVehicleTracker::getTrackedObject( const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + return true; } @@ -534,3 +610,22 @@ void NormalVehicleTracker::setNearestCornerOrSurfaceIndex( X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, self_transform); } + +double NormalVehicleTracker::getMeasurementYaw( + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + double measurement_yaw = tier4_autoware_utils::normalizeRadian( + tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); + { + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); + ekf_.getX(X_t); + // Fixed measurement_yaw to be in the range of +-90 degrees of X_t(IDX::YAW) + while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { + measurement_yaw = measurement_yaw + M_PI; + } + while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { + measurement_yaw = measurement_yaw - M_PI; + } + } + return measurement_yaw; +} diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index b168717042db3..57c2e8f899951 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -19,7 +19,6 @@ #include "multi_object_tracker/tracker/model/pedestrian_tracker.hpp" #include "multi_object_tracker/utils/utils.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -35,6 +34,7 @@ #else #include #endif +#include "object_recognition_utils/object_recognition_utils.hpp" #define EIGEN_MPL2_ONLY #include @@ -52,37 +52,41 @@ PedestrianTracker::PedestrianTracker( { object_ = object; - // initialize params - float q_stddev_x = 0.4; // [m/s] - float q_stddev_y = 0.4; // [m/s] - float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] - float q_stddev_vx = tier4_autoware_utils::kmph2mps(5); // [m/(s*s)] - float q_stddev_wz = tier4_autoware_utils::deg2rad(20); // [rad/(s*s)] - float r_stddev_x = 0.4; // [m] - float r_stddev_y = 0.4; // [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] - float p0_stddev_x = 1.0; // [m/s] - float p0_stddev_y = 1.0; // [m/s] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000); // [rad/s] - float p0_stddev_vx = tier4_autoware_utils::kmph2mps(5); // [m/(s*s)] - float p0_stddev_wz = tier4_autoware_utils::deg2rad(10); // [rad/(s*s)] - ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); - ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); - ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); - ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - ekf_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0); + // Initialize parameters + // measurement noise covariance + float r_stddev_x = 0.4; // [m] + float r_stddev_y = 0.4; // [m] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); + // initial state covariance + float p0_stddev_x = 2.0; // [m] + float p0_stddev_y = 2.0; // [m] + float p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000); // [rad] + float p0_stddev_vx = tier4_autoware_utils::kmph2mps(120); // [m/s] + float p0_stddev_wz = tier4_autoware_utils::deg2rad(360); // [rad/s] ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); ekf_params_.p0_cov_wz = std::pow(p0_stddev_wz, 2.0); + // process noise covariance + float q_stddev_x = 0.4; // [m/s] + float q_stddev_y = 0.4; // [m/s] + float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] + float q_stddev_vx = tier4_autoware_utils::kmph2mps(5); // [m/(s*s)] + float q_stddev_wz = tier4_autoware_utils::deg2rad(30); // [rad/(s*s)] + ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); + ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); + ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); + ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); + ekf_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0); + // limitations max_vx_ = tier4_autoware_utils::kmph2mps(10); // [m/s] max_wz_ = tier4_autoware_utils::deg2rad(30); // [rad/s] - // initialize X matrix + // initialize state vector X Eigen::MatrixXd X(ekf_params_.dim_x, 1); X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; @@ -95,7 +99,7 @@ PedestrianTracker::PedestrianTracker( X(IDX::WZ) = 0.0; } - // initialize P matrix + // initialize state covariance matrix P Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(X(IDX::YAW)); @@ -154,7 +158,7 @@ bool PedestrianTracker::predict(const rclcpp::Time & time) bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const { - /* == Nonlinear model == + /* Motion model: Constant turn-rate motion model (CTRV) * * x_{k+1} = x_k + vx_k * cos(yaw_k) * dt * y_{k+1} = y_k + vx_k * sin(yaw_k) * dt @@ -164,7 +168,7 @@ bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const * */ - /* == Linearized model == + /* Jacobian Matrix * * A = [ 1, 0, -vx*sin(yaw)*dt, cos(yaw)*dt, 0] * [ 0, 1, vx*cos(yaw)*dt, sin(yaw)*dt, 0] @@ -173,14 +177,14 @@ bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const * [ 0, 0, 0, 0, 1] */ - // X t + // Current state vector X t Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf.getX(X_t); const double cos_yaw = std::cos(X_t(IDX::YAW)); const double sin_yaw = std::sin(X_t(IDX::YAW)); const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - // X t+1 + // Predict state vector X t+1 Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VX) * cos_yaw * dt; // dx = v * cos(yaw) X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VX) * sin_yaw * dt; // dy = v * sin(yaw) @@ -188,7 +192,7 @@ bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const X_next_t(IDX::VX) = X_t(IDX::VX); X_next_t(IDX::WZ) = X_t(IDX::WZ); - // A + // State transition matrix A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); A(IDX::X, IDX::YAW) = -X_t(IDX::VX) * sin_yaw * dt; A(IDX::X, IDX::VX) = cos_yaw * dt; @@ -196,7 +200,7 @@ bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const A(IDX::Y, IDX::VX) = sin_yaw * dt; A(IDX::YAW, IDX::WZ) = dt; - // Q + // Process noise covariance Q Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); // Rotate the covariance matrix according to the vehicle yaw // because q_cov_x and y are in the vehicle coordinate system. @@ -240,18 +244,16 @@ bool PedestrianTracker::measureWithPose( // if (tier4_autoware_utils::deg2rad(60) < std::fabs(theta)) return false; // } - /* Set measurement matrix */ + // Set measurement matrix C and observation vector Y Eigen::MatrixXd Y(dim_y, 1); + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); Y << object.kinematics.pose_with_covariance.pose.position.x, object.kinematics.pose_with_covariance.pose.position.y; - - /* Set measurement matrix */ - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); C(0, IDX::X) = 1.0; // for pos x C(1, IDX::Y) = 1.0; // for pos y // C(2, IDX::YAW) = 1.0; // for yaw - /* Set measurement noise covariance */ + // Set noise covariance matrix R Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); if (!object.kinematics.has_position_covariance) { R(0, 0) = ekf_params_.r_cov_x; // x - x @@ -270,6 +272,8 @@ bool PedestrianTracker::measureWithPose( // R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; // R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; } + + // ekf update if (!ekf_.update(Y, C, R)) { RCLCPP_WARN(logger_, "Cannot update"); } @@ -345,7 +349,7 @@ bool PedestrianTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict kinematics + // predict state KalmanFilter tmp_ekf_for_no_update = ekf_; const double dt = (time - last_update_time_).seconds(); if (0.001 /*1msec*/ < dt) { @@ -356,6 +360,7 @@ bool PedestrianTracker::getTrackedObject( tmp_ekf_for_no_update.getX(X_t); tmp_ekf_for_no_update.getP(P); + /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; @@ -399,6 +404,7 @@ bool PedestrianTracker::getTrackedObject( constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; diff --git a/perception/object_merger/config/object_association_merger.param.yaml b/perception/object_merger/config/object_association_merger.param.yaml new file mode 100644 index 0000000000000..5e5dafad5d573 --- /dev/null +++ b/perception/object_merger/config/object_association_merger.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + sync_queue_size: 20 + precision_threshold_to_judge_overlapped: 0.4 + remove_overlapped_unknown_objects: true diff --git a/perception/object_merger/launch/object_association_merger.launch.xml b/perception/object_merger/launch/object_association_merger.launch.xml index 5754c256fef3c..b26788bb04667 100644 --- a/perception/object_merger/launch/object_association_merger.launch.xml +++ b/perception/object_merger/launch/object_association_merger.launch.xml @@ -6,7 +6,7 @@ - + @@ -14,9 +14,9 @@ + + + - - - diff --git a/perception/object_range_splitter/src/node.cpp b/perception/object_range_splitter/src/node.cpp index 52383ef922621..4c4f824feb6fb 100644 --- a/perception/object_range_splitter/src/node.cpp +++ b/perception/object_range_splitter/src/node.cpp @@ -20,7 +20,7 @@ ObjectRangeSplitterNode::ObjectRangeSplitterNode(const rclcpp::NodeOptions & nod : Node("object_range_splitter_node", node_options) { using std::placeholders::_1; - spilt_range_ = declare_parameter("split_range", 30.0); + spilt_range_ = declare_parameter("split_range"); sub_ = this->create_subscription( "input/object", rclcpp::QoS{1}, std::bind(&ObjectRangeSplitterNode::objectCallback, this, _1)); long_range_object_pub_ = diff --git a/perception/object_velocity_splitter/CMakeLists.txt b/perception/object_velocity_splitter/CMakeLists.txt index 0bf6de39f8f3c..68e87a3355f86 100644 --- a/perception/object_velocity_splitter/CMakeLists.txt +++ b/perception/object_velocity_splitter/CMakeLists.txt @@ -1,17 +1,6 @@ cmake_minimum_required(VERSION 3.5) project(object_velocity_splitter) -# Compile options -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() - # Dependencies find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/perception/object_velocity_splitter/README.md b/perception/object_velocity_splitter/README.md index 9dcd535a5a10f..57d2834f3f442 100644 --- a/perception/object_velocity_splitter/README.md +++ b/perception/object_velocity_splitter/README.md @@ -3,21 +3,23 @@ This package contains a object filter module for [autoware_auto_perception_msgs/msg/DetectedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/DetectedObject.idl). This package can split DetectedObjects into two messages by object's speed. -## Input +## Interface -| Name | Type | Description | -| ----------------- | ----------------------------------------------------- | -------------------- | -| `~/input/objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | 3D detected objects. | +### Input -## Output +- `~/input/objects` (`autoware_auto_perception_msgs/msg/DetectedObjects.msg`) + - 3D detected objects -| Name | Type | Description | -| ----------------------------- | ----------------------------------------------------- | ----------------------- | -| `~/output/low_speed_objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | Objects with low speed | -| `~/output/high_speed_objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | Objects with high speed | +### Output -## Parameters +- `~/output/low_speed_objects` (`autoware_auto_perception_msgs/msg/DetectedObjects.msg`) + - Objects with low speed +- `~/output/high_speed_objects` (`autoware_auto_perception_msgs/msg/DetectedObjects.msg`) + - Objects with high speed -| Name | Type | Description | Default value | -| :------------------- | :----- | :-------------------------------------------------- | :------------ | -| `velocity_threshold` | double | Velocity threshold parameter to split objects [m/s] | 3.0 | +### Parameters + +- `velocity_threshold` (double) [m/s] + - Default parameter is 3.0 + +This parameter is velocity threshold to split objects diff --git a/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp b/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp index a5d54b63b3311..0dfe6c6034948 100644 --- a/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp +++ b/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp @@ -53,7 +53,7 @@ ObjectVelocitySplitterNode::ObjectVelocitySplitterNode(const rclcpp::NodeOptions std::bind(&ObjectVelocitySplitterNode::onSetParam, this, std::placeholders::_1)); // Node Parameter - node_param_.velocity_threshold = declare_parameter("velocity_threshold", 3.0); + node_param_.velocity_threshold = declare_parameter("velocity_threshold"); // Subscriber sub_objects_ = create_subscription( diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index 82da92da7f146..63b6715948f93 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -206,10 +206,17 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( if (debug_publisher_ptr_ && stop_watch_ptr_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + const double pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - input_raw_msg->header.stamp).nanoseconds())) + .count(); debug_publisher_ptr_->publish( "debug/cyclic_time_ms", cyclic_time_ms); debug_publisher_ptr_->publish( "debug/processing_time_ms", processing_time_ms); + debug_publisher_ptr_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/perception/probabilistic_occupancy_grid_map/test/test_synchronized_grid_map_fusion_node.py b/perception/probabilistic_occupancy_grid_map/test/test_synchronized_grid_map_fusion_node.py index c15daaf886b6f..e73aea581433e 100644 --- a/perception/probabilistic_occupancy_grid_map/test/test_synchronized_grid_map_fusion_node.py +++ b/perception/probabilistic_occupancy_grid_map/test/test_synchronized_grid_map_fusion_node.py @@ -57,7 +57,7 @@ def generate_test_description(): FindPackageShare(PACKAGE_NAME).find(PACKAGE_NAME) + "/launch/synchronized_occupancy_grid_map_fusion.launch.xml" ) - # use default launch arguments and parms + # use default launch arguments and params launch_args = [] # action to include launch file test_launch_file = launch.actions.IncludeLaunchDescription( diff --git a/perception/radar_crossing_objects_noise_filter/CMakeLists.txt b/perception/radar_crossing_objects_noise_filter/CMakeLists.txt index 6f1270c656bde..dd179e99829a3 100644 --- a/perception/radar_crossing_objects_noise_filter/CMakeLists.txt +++ b/perception/radar_crossing_objects_noise_filter/CMakeLists.txt @@ -1,17 +1,6 @@ cmake_minimum_required(VERSION 3.5) project(radar_crossing_objects_noise_filter) -# Compile options -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() - # Dependencies find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/perception/radar_crossing_objects_noise_filter/README.md b/perception/radar_crossing_objects_noise_filter/README.md index dc1283798fed9..d6d6005a5178e 100644 --- a/perception/radar_crossing_objects_noise_filter/README.md +++ b/perception/radar_crossing_objects_noise_filter/README.md @@ -3,7 +3,7 @@ This package contains a radar noise filter module for [autoware_auto_perception_msgs/msg/DetectedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/DetectedObject.idl). This package can filter the noise objects which cross to the ego vehicle. -## Algorithm +## Design ### Background @@ -32,7 +32,7 @@ When the ego vehicle turn right, the surrounding objects have left circular moti ![turning_around](docs/turning_around.png) -### Detail Algorithm +### Algorithm To filter the objects crossing to ego vehicle, this package filter the objects as below algorithm. @@ -51,22 +51,30 @@ To filter the objects crossing to ego vehicle, this package filter the objects a } ``` -## Input +## Interface -| Name | Type | Description | -| ----------------- | ----------------------------------------------------- | -------------- | -| `~/input/objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | Radar objects. | +### Input -## Output +- `~/input/objects` (`autoware_auto_perception_msgs/msg/DetectedObjects.msg`) + - Input radar objects -| Name | Type | Description | -| --------------------------- | ----------------------------------------------------- | ---------------- | -| `~/output/noise_objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | Noise objects | -| `~/output/filtered_objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | Filtered objects | +### Output -## Parameters +- `~/output/noise_objects` (`autoware_auto_perception_msgs/msg/DetectedObjects.msg`) + - Noise objects +- `~/output/filtered_objects` (`autoware_auto_perception_msgs/msg/DetectedObjects.msg`) + - Filtered objects -| Name | Type | Description | Default value | -| :------------------- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| `angle_threshold` | double | The angle threshold parameter to filter [rad]. This parameter has condition that 0 < `angle_threshold` < pi / 2. See algorithm chapter for details. | 1.0472 | -| `velocity_threshold` | double | The velocity threshold parameter to filter [m/s]. See algorithm chapter for details. | 3.0 | +### Parameters + +- `angle_threshold` (double) [rad] + - Default parameter is 1.0472. + +This parameter is the angle threshold to filter. It has condition that 0 < `angle_threshold` < pi / 2. If the crossing angle is larger than this parameter, it can be a candidate for noise object. In other words, if it is smaller than this parameter, it is a filtered object. +If this parameter is set smaller, more objects are considered noise. In detail, see algorithm chapter. + +- `velocity_threshold` (double) [m/s] + - Default parameter is 3.0. + +This parameter is the velocity threshold to filter. If velocity of an object is larger than this parameter, it can be a candidate for noise object. In other words, if velocity of an object is smaller than this parameter, it is a filtered object. +If this parameter is set smaller, more objects are considered noise. In detail, see algorithm chapter. diff --git a/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp b/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp index 947856e5d2a4d..5b69ba65f3964 100644 --- a/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp +++ b/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp @@ -64,8 +64,8 @@ RadarCrossingObjectsNoiseFilterNode::RadarCrossingObjectsNoiseFilterNode( std::bind(&RadarCrossingObjectsNoiseFilterNode::onSetParam, this, std::placeholders::_1)); // Node Parameter - node_param_.angle_threshold = declare_parameter("angle_threshold", 1.0472); - node_param_.velocity_threshold = declare_parameter("velocity_threshold", 3.0); + node_param_.angle_threshold = declare_parameter("angle_threshold"); + node_param_.velocity_threshold = declare_parameter("velocity_threshold"); // Subscriber sub_objects_ = create_subscription( diff --git a/perception/radar_fusion_to_detected_object/README.md b/perception/radar_fusion_to_detected_object/README.md index defc716a744d9..9ea8e5d188f71 100644 --- a/perception/radar_fusion_to_detected_object/README.md +++ b/perception/radar_fusion_to_detected_object/README.md @@ -1,46 +1,100 @@ # radar_fusion_to_detected_object -This package contains a sensor fusion module for radar-detected objects and 3D detected objects. The fusion node can: +This package contains a sensor fusion module for radar-detected objects and 3D detected objects. + +The fusion node can: - Attach velocity to 3D detections when successfully matching radar data. The tracking modules use the velocity information to enhance the tracking results while planning modules use it to execute actions like adaptive cruise control. - Improve the low confidence 3D detections when corresponding radar detections are found. ![process_low_confidence](docs/radar_fusion_to_detected_object_6.drawio.svg) -## Core algorithm +## Design + +### Background + +This package is the fusion with LiDAR-based 3D detection output and radar data. +LiDAR based 3D detection can estimate position and size of objects with high precision, but it cannot estimate velocity of objects. +Radar data can estimate doppler velocity of objects, but it cannot estimate position and size of objects with high precision +This fusion package is aim to fuse these characteristic data, and to estimate position, size, velocity of objects with high precision. + +### Algorithm The document of core algorithm is [here](docs/algorithm.md) +## Interface for core algorithm + +The parameters for core algorithm can be set as `core_params`. + ### Parameters for sensor fusion -| Name | Type | Description | Default value | -| :----------------------- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| bounding_box_margin | double | The distance to extend the 2D bird's-eye view Bounding Box on each side. This distance is used as a threshold to find radar centroids falling inside the extended box. [m] | 2.0 | -| split_threshold_velocity | double | The object's velocity threshold to decide to split for two objects from radar information (currently not implemented) [m/s] | 5.0 | -| threshold_yaw_diff | double | The yaw orientation threshold. If ∣ θ_ob − θ_ra ∣ < threshold × yaw_diff attached to radar information include estimated velocity, where*θob*is yaw angle from 3d detected object,\*θ_ra is yaw angle from radar object. [rad] | 0.35 | +- `bounding_box_margin` (double) [m] + - Default parameter is 2.0. + +This parameter is the distance to extend the 2D bird's-eye view bounding box on each side. +This parameter is used as a threshold to find radar centroids falling inside the extended box. + +- `split_threshold_velocity` (double) [m/s] + - Default parameter is 5.0. + +This parameter is the object's velocity threshold to decide to split for two objects from radar information. +Note that this feature is not currently implemented. + +- `threshold_yaw_diff` (double) [rad] + - Default parameter is 0.35. + +This parameter is the yaw orientation threshold. +If the difference of yaw degree between from a LiDAR-based detection object and radar velocity, radar information is attached to output objects. ### Weight parameters for velocity estimation To tune these weight parameters, please see [document](docs/algorithm.md) in detail. -| Name | Type | Description | Default value | -| :----------------------------------- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| velocity_weight_average | double | The twist coefficient of average twist of radar data in velocity estimation. | 0.0 | -| velocity_weight_median | double | The twist coefficient of median twist of radar data in velocity estimation. | 0.0 | -| velocity_weight_min_distance | double | The twist coefficient of radar data nearest to the center of bounding box in velocity estimation. | 1.0 | -| velocity_weight_target_value_average | double | The twist coefficient of target value weighted average in velocity estimation. Target value is amplitude if using radar pointcloud. Target value is probability if using radar objects. | -| 0.0 | -| velocity_weight_target_value_top | double | The twist coefficient of top target value radar data in velocity estimation. Target value is amplitude if using radar pointcloud. Target value is probability if using radar objects. | 0.0 | +- `velocity_weight_average` (double) +- Default parameter is 0.0. + +This parameter is the twist coefficient of average twist of radar data in velocity estimation. + +- `velocity_weight_median` (double) +- Default parameter is 0.0. + +This parameter is the twist coefficient of median twist of radar data in velocity estimation. + +- `velocity_weight_min_distance` (double) +- Default parameter is 1.0. + +This parameter is the twist coefficient of radar data nearest to the center of bounding box in velocity estimation. + +- `velocity_weight_target_value_average` (double) +- Default parameter is 0.0. + +This parameter is the twist coefficient of target value weighted average in velocity estimation. Target value is amplitude if using radar pointcloud. Target value is probability if using radar objects. + +- `velocity_weight_target_value_top` (double) +- Default parameter is 0.0. + +This parameter is the twist coefficient of top target value radar data in velocity estimation. Target value is amplitude if using radar pointcloud. Target value is probability if using radar objects. ### Parameters for fixed object information -| Name | Type | Description | Default value | -| :----------------------- | :---- | :----------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| convert_doppler_to_twist | bool | Convert doppler velocity to twist using the yaw information of a detected object. | false | -| threshold_probability | float | If the probability of an output object is lower than this parameter, and the output object does not have radar points/objects, then delete the object. | 0.4 | -| compensate_probability | bool | If this parameter is true, compensate probability of objects to threshold probability. | false | +- `convert_doppler_to_twist` (bool) + - Default parameter is false. + +This parameter is the flag whether convert doppler velocity to twist using the yaw information of a detected object. -## radar_object_fusion_to_detected_object +- `threshold_probability` (float) + - Default parameter is 0.4. + +This parameter is the threshold to filter output objects. +If the probability of an output object is lower than this parameter, and the output object does not have radar points/objects, then delete the object. + +- `compensate_probability` (bool) + - Default parameter is false. + +This parameter is the flag to use probability compensation. +If this parameter is true, compensate probability of objects to threshold probability. + +## Interface for radar_object_fusion_to_detected_object Sensor fusion with radar objects and a detected object. @@ -56,24 +110,28 @@ ros2 launch radar_fusion_to_detected_object radar_object_to_detected_object.laun ### Input -| Name | Type | Description | -| ----------------------- | ----------------------------------------------------- | ---------------------------------------------------------------------- | -| `~/input/objects` | autoware_auto_perception_msgs/msg/DetectedObject.msg | 3D detected objects. | -| `~/input/radar_objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | Radar objects. Note that frame_id need to be same as `~/input/objects` | +- `~/input/objects` (`autoware_auto_perception_msgs/msg/DetectedObjects.msg`) + - 3D detected objects. +- `~/input/radar_objects` (`autoware_auto_perception_msgs/msg/DetectedObjects.msg`) + - Radar objects. Note that frame_id need to be same as `~/input/objects` ### Output -| Name | Type | Description | -| -------------------------------- | ----------------------------------------------------- | -------------------------------------------------------------------------------------- | -| `~/output/objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | 3D detected object with twist. | -| `~/debug/low_confidence_objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | 3D detected object that doesn't output as `~/output/objects` because of low confidence | +- `~/output/objects` (`autoware_auto_perception_msgs/msg/DetectedObjects.msg`) + - 3D detected object with twist. +- `~/debug/low_confidence_objects` (`autoware_auto_perception_msgs/msg/DetectedObjects.msg`) + - 3D detected object that doesn't output as `~/output/objects` because of low confidence ### Parameters -| Name | Type | Description | Default value | -| :------------- | :----- | :-------------------- | :------------ | -| update_rate_hz | double | The update rate [hz]. | 20.0 | +The parameters for core algorithm can be set as `node_params`. + +- `update_rate_hz` (double) [hz] + - Default parameter is 20.0 + +This parameter is update rate for the `onTimer` function. +This parameter should be same as the frame rate of input topics. -## radar_scan_fusion_to_detected_object (TBD) +## Interface for radar_scan_fusion_to_detected_object (TBD) -TBD +Under implement diff --git a/perception/radar_object_clustering/CMakeLists.txt b/perception/radar_object_clustering/CMakeLists.txt index ba911c3035765..ea25353d3e1d7 100644 --- a/perception/radar_object_clustering/CMakeLists.txt +++ b/perception/radar_object_clustering/CMakeLists.txt @@ -1,17 +1,6 @@ cmake_minimum_required(VERSION 3.5) project(radar_object_clustering) -# Compile options -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() - # Dependencies find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/perception/radar_object_clustering/README.md b/perception/radar_object_clustering/README.md index 6463dd7157dea..0b2dd07f9e59f 100644 --- a/perception/radar_object_clustering/README.md +++ b/perception/radar_object_clustering/README.md @@ -7,7 +7,7 @@ In other word, this package can combine multiple radar detections from one objec ![radar_clustering](docs/radar_clustering.drawio.svg) -## Algorithm +## Design ### Background @@ -15,14 +15,14 @@ In radars with object output, there are cases that multiple detection results ar Its multiple detection results cause separation of objects in tracking module. Therefore, by this package the multiple detection results are clustered into one object in advance. -### Detail Algorithm +### Algorithm -- Sort by distance from `base_link` +- 1. Sort by distance from `base_link` At first, to prevent changing the result from depending on the order of objects in DetectedObjects, input objects are sorted by distance from `base_link`. In addition, to apply matching in closeness order considering occlusion, objects are sorted in order of short distance in advance. -- Clustering +- 2. Clustering If two radar objects are near, and yaw angle direction and velocity between two radar objects is similar (the degree of these is defined by parameters), then these are clustered. Note that radar characteristic affect parameters for this matching. @@ -32,13 +32,13 @@ For example, if resolution of range distance or angle is low and accuracy of vel After grouping for all radar objects, if multiple radar objects are grouping, the kinematics of the new clustered object is calculated from average of that and label and shape of the new clustered object is calculated from top confidence in radar objects. -- Fixed label correction +- 3. Fixed label correction When the label information from radar outputs lack accuracy, `is_fixed_label` parameter is recommended to set `true`. If the parameter is true, the label of a clustered object is overwritten by the label set by `fixed_label` parameter. If this package use for faraway dynamic object detection with radar, the parameter is recommended to set to `VEHICLE`. -- Fixed size correction +- 4. Fixed size correction When the size information from radar outputs lack accuracy, `is_fixed_size` parameter is recommended to set `true`. If the parameter is true, the size of a clustered object is overwritten by the label set by `size_x`, `size_y`, and `size_z` parameters. @@ -51,28 +51,75 @@ Note that to use for [multi_objects_tracker](https://github.com/autowarefoundati For now, size estimation for clustered object is not implemented. So `is_fixed_size` parameter is recommended to set `true`, and size parameters is recommended to set to value near to average size of vehicles. -## Input - -| Name | Type | Description | -| ----------------- | ----------------------------------------------------- | -------------- | -| `~/input/objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | Radar objects. | - -## Output - -| Name | Type | Description | -| ------------------ | ----------------------------------------------------- | -------------- | -| `~/output/objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | Output objects | - -## Parameters - -| Name | Type | Description | Default value | -| :------------------- | :----- | :---------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| `angle_threshold` | double | Angle threshold to judge whether radar detections come from one object. [rad] | 0.174 | -| `distance_threshold` | double | Distance threshold to judge whether radar detections come from one object. [m] | 4.0 | -| `velocity_threshold` | double | Velocity threshold to judge whether radar detections come from one object. [m/s] | 2.0 | -| `is_fixed_label` | bool | If this parameter is true, the label of a clustered object is overwritten by the label set by `fixed_label` parameter. | false | -| `fixed_label` | string | If `is_fixed_label` is true, the label of a clustered object is overwritten by this parameter. | "UNKNOWN" | -| `is_fixed_size` | bool | If this parameter is true, the size of a clustered object is overwritten by the label set by `size_x`, `size_y`, and `size_z` parameters. | false | -| `size_x` | double | If `is_fixed_size` is true, the x-axis size of a clustered object is overwritten by this parameter. [m] | 4.0 | -| `size_y` | double | If `is_fixed_size` is true, the y-axis size of a clustered object is overwritten by this parameter. [m] | 1.5 | -| `size_z` | double | If `is_fixed_size` is true, the z-axis size of a clustered object is overwritten by this parameter. [m] | 1.5 | +## Interface + +### Input + +- `~/input/objects` (`autoware_auto_perception_msgs/msg/DetectedObjects.msg`) + - Radar objects + +### Output + +- `~/output/objects` (`autoware_auto_perception_msgs/msg/DetectedObjects.msg`) + - Output objects + +### Parameter + +- `angle_threshold` (double) [rad] + - Default parameter is 0.174. +- `distance_threshold` (double) [m] + - Default parameter is 4.0. +- `velocity_threshold` (double) [m/s] + - Default parameter is 2.0. + +These parameter are thresholds for angle, distance, and velocity to judge whether radar detections come from one object in "clustering" processing, which is written in detail at algorithm section. +If all of the difference in angle/distance/velocity from two objects is less than the thresholds, then the two objects are merged to one clustered object. +If these parameter is larger, more objects are merged to one clustered object. + +These are used in `isSameObject` function as below. + +```cpp + +bool RadarObjectClusteringNode::isSameObject( + const DetectedObject & object_1, const DetectedObject & object_2) +{ + const double angle_diff = std::abs(tier4_autoware_utils::normalizeRadian( + tf2::getYaw(object_1.kinematics.pose_with_covariance.pose.orientation) - + tf2::getYaw(object_2.kinematics.pose_with_covariance.pose.orientation))); + const double velocity_diff = std::abs( + object_1.kinematics.twist_with_covariance.twist.linear.x - + object_2.kinematics.twist_with_covariance.twist.linear.x); + const double distance = tier4_autoware_utils::calcDistance2d( + object_1.kinematics.pose_with_covariance.pose.position, + object_2.kinematics.pose_with_covariance.pose.position); + + if ( + distance < node_param_.distance_threshold && angle_diff < node_param_.angle_threshold && + velocity_diff < node_param_.velocity_threshold) { + return true; + } else { + return false; + } +} +``` + +- `is_fixed_label` (bool) + - Default parameter is false. +- `fixed_label` (string) + - Default parameter is "UNKNOWN". + +`is_fixed_label` is the flag to use fixed label. +If it is true, the label of a clustered object is overwritten by the label set by `fixed_label` parameter. +If the radar objects do not have label information, then it is recommended to use fixed label. + +- `is_fixed_size` (bool) + - Default parameter is false. +- `size_x` (double) [m] + - Default parameter is 4.0. +- `size_y` (double) [m] + - Default parameter is 1.5. +- `size_z` (double) [m] + - Default parameter is 1.5. + +`is_fixed_size` is the flag to use fixed size parameters. +If it is true, the size of a clustered object is overwritten by the label set by `size_x`, `size_y`, and `size_z` parameters. diff --git a/perception/radar_object_tracker/config/radar_object_tracker.param.yaml b/perception/radar_object_tracker/config/radar_object_tracker.param.yaml index d2c0841cf372e..2fe006ffe8394 100644 --- a/perception/radar_object_tracker/config/radar_object_tracker.param.yaml +++ b/perception/radar_object_tracker/config/radar_object_tracker.param.yaml @@ -24,3 +24,6 @@ max_distance_from_lane: 5.0 # [m] max_angle_diff_from_lane: 0.785398 # [rad] (45 deg) max_lateral_velocity: 7.0 # [m/s] + + # tracking model parameters + tracking_config_directory: $(find-pkg-share radar_object_tracker)/config/tracking/ diff --git a/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml b/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml index 60ed2efb5767b..dfebf334456ea 100644 --- a/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml +++ b/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml @@ -3,7 +3,6 @@ - @@ -14,7 +13,6 @@ - - + diff --git a/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp b/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp index 5815fe34a68a9..62d3ff532da4d 100644 --- a/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp +++ b/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp @@ -387,11 +387,11 @@ bool ConstantTurnRateMotionTracker::measureWithPose( const bool enable_yaw_measurement = trust_yaw_input_ && object_has_orientation; if (enable_yaw_measurement) { - Eigen::MatrixXd Cyaw = Eigen::MatrixXd::Zero(1, ekf_params_.dim_x); - Cyaw(0, IDX::YAW) = 1; - C_list.push_back(Cyaw); + Eigen::MatrixXd C_yaw = Eigen::MatrixXd::Zero(1, ekf_params_.dim_x); + C_yaw(0, IDX::YAW) = 1; + C_list.push_back(C_yaw); - Eigen::MatrixXd Yyaw = Eigen::MatrixXd::Zero(1, 1); + Eigen::MatrixXd Y_yaw = Eigen::MatrixXd::Zero(1, 1); const auto yaw = [&] { auto obj_yaw = tier4_autoware_utils::normalizeRadian( tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); @@ -404,12 +404,12 @@ bool ConstantTurnRateMotionTracker::measureWithPose( return obj_yaw; }(); - Yyaw << yaw; - Y_list.push_back(Yyaw); + Y_yaw << yaw; + Y_list.push_back(Y_yaw); - Eigen::MatrixXd Ryaw = Eigen::MatrixXd::Zero(1, 1); - Ryaw << ekf_params_.r_cov_yaw; - R_block_list.push_back(Ryaw); + Eigen::MatrixXd R_yaw = Eigen::MatrixXd::Zero(1, 1); + R_yaw << ekf_params_.r_cov_yaw; + R_block_list.push_back(R_yaw); } // 3. add linear velocity measurement diff --git a/perception/radar_tracks_msgs_converter/config/radar_tracks_msgs_converter.param.yaml b/perception/radar_tracks_msgs_converter/config/radar_tracks_msgs_converter.param.yaml index 5a30a6bb4031d..7b344aaa57762 100644 --- a/perception/radar_tracks_msgs_converter/config/radar_tracks_msgs_converter.param.yaml +++ b/perception/radar_tracks_msgs_converter/config/radar_tracks_msgs_converter.param.yaml @@ -1,6 +1,7 @@ /**: ros__parameters: update_rate_hz: 20.0 + new_frame_id: "base_link" use_twist_compensation: true use_twist_yaw_compensation: false static_object_speed_threshold: 1.0 diff --git a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp index 323e4bf7788c5..c7f2bb5bdc60d 100644 --- a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp +++ b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp @@ -78,13 +78,12 @@ RadarTracksMsgsConverterNode::RadarTracksMsgsConverterNode(const rclcpp::NodeOpt std::bind(&RadarTracksMsgsConverterNode::onSetParam, this, _1)); // Node Parameter - node_param_.update_rate_hz = declare_parameter("update_rate_hz", 20.0); - node_param_.new_frame_id = declare_parameter("new_frame_id", "base_link"); - node_param_.use_twist_compensation = declare_parameter("use_twist_compensation", false); - node_param_.use_twist_yaw_compensation = - declare_parameter("use_twist_yaw_compensation", false); + node_param_.update_rate_hz = declare_parameter("update_rate_hz"); + node_param_.new_frame_id = declare_parameter("new_frame_id"); + node_param_.use_twist_compensation = declare_parameter("use_twist_compensation"); + node_param_.use_twist_yaw_compensation = declare_parameter("use_twist_yaw_compensation"); node_param_.static_object_speed_threshold = - declare_parameter("static_object_speed_threshold", 1.0); + declare_parameter("static_object_speed_threshold"); // Subscriber sub_radar_ = create_subscription( diff --git a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp index 683c6921eef5b..56250af766421 100644 --- a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp +++ b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp @@ -80,9 +80,9 @@ SimpleObjectMergerNode::SimpleObjectMergerNode(const rclcpp::NodeOptions & node_ std::bind(&SimpleObjectMergerNode::onSetParam, this, std::placeholders::_1)); // Node Parameter - node_param_.update_rate_hz = declare_parameter("update_rate_hz", 20.0); - node_param_.new_frame_id = declare_parameter("new_frame_id", "base_link"); - node_param_.timeout_threshold = declare_parameter("timeout_threshold", 0.1); + node_param_.update_rate_hz = declare_parameter("update_rate_hz"); + node_param_.new_frame_id = declare_parameter("new_frame_id"); + node_param_.timeout_threshold = declare_parameter("timeout_threshold"); declare_parameter("input_topics", std::vector()); node_param_.topic_names = get_parameter("input_topics").as_string_array(); diff --git a/perception/tensorrt_yolo/.gitignore b/perception/tensorrt_yolo/.gitignore deleted file mode 100755 index 160e7b321280b..0000000000000 --- a/perception/tensorrt_yolo/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -data/ -calib_image/ diff --git a/perception/tensorrt_yolo/CMakeLists.txt b/perception/tensorrt_yolo/CMakeLists.txt deleted file mode 100755 index b059ab44d8ece..0000000000000 --- a/perception/tensorrt_yolo/CMakeLists.txt +++ /dev/null @@ -1,162 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(tensorrt_yolo) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -# TODO(tensorrt_yolo): Remove once upgrading to TensorRT 8.5 is complete -add_compile_options(-Wno-deprecated-declarations) - -option(CUDA_VERBOSE "Verbose output of CUDA modules" OFF) - -find_package(OpenCV REQUIRED) - -# set flags for CUDA availability -option(CUDA_AVAIL "CUDA available" OFF) -find_package(CUDA) -if(CUDA_FOUND) - find_library(CUBLAS_LIBRARIES cublas HINTS - ${CUDA_TOOLKIT_ROOT_DIR}/lib64 - ${CUDA_TOOLKIT_ROOT_DIR}/lib - ) - if(CUDA_VERBOSE) - message(STATUS "CUDA is available!") - message(STATUS "CUDA Libs: ${CUDA_LIBRARIES}") - message(STATUS "CUDA Headers: ${CUDA_INCLUDE_DIRS}") - endif() - set(CUDA_AVAIL ON) -else() - message("CUDA NOT FOUND") - set(CUDA_AVAIL OFF) -endif() - -# set flags for TensorRT availability -option(TRT_AVAIL "TensorRT available" OFF) -# try to find the tensorRT modules -find_library(NVINFER NAMES nvinfer) -find_library(NVONNXPARSER nvonnxparser) -find_library(NVINFER_PLUGIN NAMES nvinfer_plugin) -if(NVINFER AND NVONNXPARSER AND NVINFER_PLUGIN) - if(CUDA_VERBOSE) - message(STATUS "TensorRT is available!") - message(STATUS "NVINFER: ${NVINFER}") - message(STATUS "NVPARSERS: ${NVPARSERS}") - message(STATUS "NVINFER_PLUGIN: ${NVINFER_PLUGIN}") - message(STATUS "NVONNXPARSER: ${NVONNXPARSER}") - endif() - set(TRT_AVAIL ON) -else() - message("TensorRT is NOT Available") - set(TRT_AVAIL OFF) -endif() - -# set flags for CUDNN availability -option(CUDNN_AVAIL "CUDNN available" OFF) -# try to find the CUDNN module -find_library(CUDNN_LIBRARY -NAMES libcudnn.so${__cudnn_ver_suffix} libcudnn${__cudnn_ver_suffix}.dylib ${__cudnn_lib_win_name} -PATHS $ENV{LD_LIBRARY_PATH} ${__libpath_cudart} ${CUDNN_ROOT_DIR} ${PC_CUDNN_LIBRARY_DIRS} ${CMAKE_INSTALL_PREFIX} -PATH_SUFFIXES lib lib64 bin -DOC "CUDNN library." -) -if(CUDNN_LIBRARY) - if(CUDA_VERBOSE) - message(STATUS "CUDNN is available!") - message(STATUS "CUDNN_LIBRARY: ${CUDNN_LIBRARY}") - endif() - set(CUDNN_AVAIL ON) -else() - message("CUDNN is NOT Available") - set(CUDNN_AVAIL OFF) -endif() - -# create calib image directory for int8 calibration -set(CALIB_IMAGE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/calib_image") -if(NOT EXISTS "${CALIB_IMAGE_PATH}") - execute_process(COMMAND mkdir -p ${CALIB_IMAGE_PATH}) -endif() - -if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) - include_directories( - include - lib/include - ${OpenCV_INCLUDE_DIRS} - ${CUDA_INCLUDE_DIRS} - ) - - - ### yolo ### - cuda_add_library(mish_plugin SHARED - lib/src/plugins/mish.cu - lib/src/plugins/mish_plugin.cpp - ) - - cuda_add_library(yolo_layer_plugin SHARED - lib/src/plugins/yolo_layer.cu - lib/src/plugins/yolo_layer_plugin.cpp - ) - - cuda_add_library(nms_plugin SHARED - lib/src/plugins/nms.cu - lib/src/plugins/nms_plugin.cpp - ) - - ament_auto_add_library(yolo SHARED - lib/src/trt_yolo.cpp - ) - - target_include_directories(yolo PUBLIC - lib/include - ) - - target_link_libraries(yolo - ${NVINFER} - ${NVONNXPARSER} - ${NVINFER_PLUGIN} - ${CUDA_LIBRARIES} - ${CUBLAS_LIBRARIES} - ${CUDNN_LIBRARY} - mish_plugin - yolo_layer_plugin - nms_plugin - ) - - ament_auto_add_library(tensorrt_yolo_nodelet SHARED - src/nodelet.cpp - ) - - target_link_libraries(tensorrt_yolo_nodelet - ${OpenCV_LIBS} - yolo - mish_plugin - yolo_layer_plugin - nms_plugin - ) - - rclcpp_components_register_node(tensorrt_yolo_nodelet - PLUGIN "object_recognition::TensorrtYoloNodelet" - EXECUTABLE tensorrt_yolo_node - ) - - ament_auto_package(INSTALL_TO_SHARE - config - launch - ) - - install( - TARGETS - mish_plugin - yolo_layer_plugin - nms_plugin - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - ) - -else() - message("TensorrtYolo won't be built, CUDA and/or TensorRT were not found.") - ament_auto_package(INSTALL_TO_SHARE - config - launch - ) -endif() diff --git a/perception/tensorrt_yolo/README.md b/perception/tensorrt_yolo/README.md deleted file mode 100644 index 58d4af0dfa83d..0000000000000 --- a/perception/tensorrt_yolo/README.md +++ /dev/null @@ -1,117 +0,0 @@ -# tensorrt_yolo - -## Purpose - -This package detects 2D bounding boxes for target objects e.g., cars, trucks, bicycles, and pedestrians on a image based on YOLO(You only look once) model. - -## Inner-workings / Algorithms - -### Cite - - - -yolov3 - -Redmon, J., & Farhadi, A. (2018). Yolov3: An incremental improvement. arXiv preprint arXiv:1804.02767. - -yolov4 - -Bochkovskiy, A., Wang, C. Y., & Liao, H. Y. M. (2020). Yolov4: Optimal speed and accuracy of object detection. arXiv preprint arXiv:2004.10934. - -yolov5 - -Jocher, G., et al. (2021). ultralytics/yolov5: v6.0 - YOLOv5n 'Nano' models, Roboflow integration, TensorFlow export, OpenCV DNN support (v6.0). Zenodo. - -## Inputs / Outputs - -### Input - -| Name | Type | Description | -| ---------- | ------------------- | --------------- | -| `in/image` | `sensor_msgs/Image` | The input image | - -### Output - -| Name | Type | Description | -| ------------- | -------------------------------------------------- | -------------------------------------------------- | -| `out/objects` | `tier4_perception_msgs/DetectedObjectsWithFeature` | The detected objects with 2D bounding boxes | -| `out/image` | `sensor_msgs/Image` | The image with 2D bounding boxes for visualization | - -## Parameters - -### Core Parameters - -| Name | Type | Default Value | Description | -| ------------------- | ------------ | ------------------------------------------------------------------------------------------------------------------ | ------------------------------------------------------------------------------------- | -| `anchors` | double array | [10.0, 13.0, 16.0, 30.0, 33.0, 23.0, 30.0, 61.0, 62.0, 45.0, 59.0, 119.0, 116.0, 90.0, 156.0, 198.0, 373.0, 326.0] | The anchors to create bounding box candidates | -| `scale_x_y` | double array | [1.0, 1.0, 1.0] | The scale parameter to eliminate grid sensitivity | -| `score_thresh` | double | 0.1 | If the objectness score is less than this value, the object is ignored in yolo layer. | -| `iou_thresh` | double | 0.45 | The iou threshold for NMS method | -| `detections_per_im` | int | 100 | The maximum detection number for one frame | -| `use_darknet_layer` | bool | true | The flag to use yolo layer in darknet | -| `ignore_thresh` | double | 0.5 | If the output score is less than this value, ths object is ignored. | - -### Node Parameters - -| Name | Type | Default Value | Description | -| ----------------------- | ------ | ------------- | ------------------------------------------------------------------ | -| `data_path` | string | "" | Packages data and artifacts directory path | -| `onnx_file` | string | "" | The onnx file name for yolo model | -| `engine_file` | string | "" | The tensorrt engine file name for yolo model | -| `label_file` | string | "" | The label file with label names for detected objects written on it | -| `calib_image_directory` | string | "" | The directory name including calibration images for int8 inference | -| `calib_cache_file` | string | "" | The calibration cache file for int8 inference | -| `mode` | string | "FP32" | The inference mode: "FP32", "FP16", "INT8" | -| `gpu_id` | int | 0 | GPU device ID that runs the model | - -## Assumptions / Known limits - -This package includes multiple licenses. - -## Onnx model - -All YOLO ONNX models are converted from the officially trained model. If you need information about training datasets and conditions, please refer to the official repositories. - -All models are downloaded during env preparation by ansible (as mention in [installation](https://autowarefoundation.github.io/autoware-documentation/main/installation/autoware/source-installation/)). It is also possible to download them manually, see [Manual downloading of artifacts](https://github.com/autowarefoundation/autoware/tree/main/ansible/roles/artifacts) . When launching the node with a model for the first time, the model is automatically converted to TensorRT, although this may take some time. - -### YOLOv3 - -[YOLOv3](https://awf.ml.dev.web.auto/perception/models/yolov3.onnx "YOLOv3"): Converted from darknet [weight file](https://pjreddie.com/media/files/yolov3.weights "weight file") and [conf file](https://github.com/pjreddie/darknet/blob/master/cfg/yolov3.cfg "conf file"). - -- [This code](https://github.com/wep21/yolo_onnx_converter) is used for converting darknet weight file and conf file to onnx. - -### YOLOv4 - -[YOLOv4](https://awf.ml.dev.web.auto/perception/models/yolov4.onnx "YOLOv4"): Converted from darknet [weight file](https://github.com/AlexeyAB/darknet/releases/download/darknet_yolo_v3_optimal/yolov4.weights "weight file") and [conf file](https://github.com/AlexeyAB/darknet/blob/master/cfg/yolov4.cfg "conf file"). - -[YOLOv4-tiny](https://awf.ml.dev.web.auto/perception/models/yolov4-tiny.onnx "YOLOv4-tiny"): Converted from darknet [weight file](https://github.com/AlexeyAB/darknet/releases/download/darknet_yolo_v4_pre/yolov4-tiny.weights "weight file") and [conf file](https://github.com/AlexeyAB/darknet/blob/master/cfg/yolov4-tiny.cfg "conf file"). - -- [This code](https://github.com/wep21/yolo_onnx_converter) is used for converting darknet weight file and conf file to onnx. - -### YOLOv5 - -Refer to [this guide](https://github.com/ultralytics/yolov5/issues/251 "guide") - -- [YOLOv5s](https://awf.ml.dev.web.auto/perception/models/yolov5s.onnx "YOLOv5s") - -- [YOLOv5m](https://awf.ml.dev.web.auto/perception/models/yolov5m.onnx "YOLOv5m") - -- [YOLOv5l](https://awf.ml.dev.web.auto/perception/models/yolov5l.onnx "YOLOv5l") - -- [YOLOv5x](https://awf.ml.dev.web.auto/perception/models/yolov5x.onnx "YOLOv5x") - -## Limitations - -- If you want to run multiple instances of this node for multiple cameras using "yolo.launch.xml", first of all, create a TensorRT engine by running the "tensorrt_yolo.launch.xml" launch file separately for each GPU. Otherwise, multiple instances of the node trying to create the same TensorRT engine can cause potential problems. - -## Reference repositories - -- - -- - -- - -- - -- diff --git a/perception/tensorrt_yolo/config/yolov3.param.yaml b/perception/tensorrt_yolo/config/yolov3.param.yaml deleted file mode 100644 index d97505dae5020..0000000000000 --- a/perception/tensorrt_yolo/config/yolov3.param.yaml +++ /dev/null @@ -1,10 +0,0 @@ -/**: - ros__parameters: - num_anchors: 3 - anchors: [116.0, 90.0, 156.0, 198.0, 373.0, 326.0, 30.0, 61.0, 62.0, 45.0, 59.0, 119.0, 10.0, 13.0, 16.0, 30.0, 33.0, 23.0] - scale_x_y: [1.0, 1.0, 1.0] - score_thresh: 0.1 - iou_thresh: 0.45 - detections_per_im: 100 - use_darknet_layer: true - ignore_thresh: 0.5 diff --git a/perception/tensorrt_yolo/config/yolov4-tiny.param.yaml b/perception/tensorrt_yolo/config/yolov4-tiny.param.yaml deleted file mode 100644 index 8071da9dcac06..0000000000000 --- a/perception/tensorrt_yolo/config/yolov4-tiny.param.yaml +++ /dev/null @@ -1,10 +0,0 @@ -/**: - ros__parameters: - num_anchors: 3 - anchors: [81.0, 82.0, 135.0, 169.0, 344.0, 319.0, 23.0, 27.0, 37.0, 58.0, 81.0, 82.0] - scale_x_y: [1.05, 1.05] - score_thresh: 0.1 - iou_thresh: 0.45 - detections_per_im: 100 - use_darknet_layer: true - ignore_thresh: 0.5 diff --git a/perception/tensorrt_yolo/config/yolov4.param.yaml b/perception/tensorrt_yolo/config/yolov4.param.yaml deleted file mode 100644 index 9edc0fc6ce708..0000000000000 --- a/perception/tensorrt_yolo/config/yolov4.param.yaml +++ /dev/null @@ -1,10 +0,0 @@ -/**: - ros__parameters: - num_anchors: 3 - anchors: [12.0, 16.0, 19.0, 36.0, 40.0, 28.0, 36.0, 75.0, 76.0, 55.0, 72.0, 146.0, 142.0, 110.0, 192.0, 243.0, 459.0, 401.0] - scale_x_y: [1.2, 1.1, 1.05] - score_thresh: 0.1 - iou_thresh: 0.45 - detections_per_im: 100 - use_darknet_layer: true - ignore_thresh: 0.5 diff --git a/perception/tensorrt_yolo/config/yolov5l.param.yaml b/perception/tensorrt_yolo/config/yolov5l.param.yaml deleted file mode 100644 index 67ae862fc33af..0000000000000 --- a/perception/tensorrt_yolo/config/yolov5l.param.yaml +++ /dev/null @@ -1,10 +0,0 @@ -/**: - ros__parameters: - num_anchors: 3 - anchors: [10.0, 13.0, 16.0, 30.0, 33.0, 23.0, 30.0, 61.0, 62.0, 45.0, 59.0, 119.0, 116.0, 90.0, 156.0, 198.0, 373.0, 326.0] - scale_x_y: [1.0, 1.0, 1.0] - score_thresh: 0.1 - iou_thresh: 0.45 - detections_per_im: 100 - use_darknet_layer: false - ignore_thresh: 0.5 diff --git a/perception/tensorrt_yolo/config/yolov5m.param.yaml b/perception/tensorrt_yolo/config/yolov5m.param.yaml deleted file mode 100644 index 67ae862fc33af..0000000000000 --- a/perception/tensorrt_yolo/config/yolov5m.param.yaml +++ /dev/null @@ -1,10 +0,0 @@ -/**: - ros__parameters: - num_anchors: 3 - anchors: [10.0, 13.0, 16.0, 30.0, 33.0, 23.0, 30.0, 61.0, 62.0, 45.0, 59.0, 119.0, 116.0, 90.0, 156.0, 198.0, 373.0, 326.0] - scale_x_y: [1.0, 1.0, 1.0] - score_thresh: 0.1 - iou_thresh: 0.45 - detections_per_im: 100 - use_darknet_layer: false - ignore_thresh: 0.5 diff --git a/perception/tensorrt_yolo/config/yolov5s.param.yaml b/perception/tensorrt_yolo/config/yolov5s.param.yaml deleted file mode 100644 index 67ae862fc33af..0000000000000 --- a/perception/tensorrt_yolo/config/yolov5s.param.yaml +++ /dev/null @@ -1,10 +0,0 @@ -/**: - ros__parameters: - num_anchors: 3 - anchors: [10.0, 13.0, 16.0, 30.0, 33.0, 23.0, 30.0, 61.0, 62.0, 45.0, 59.0, 119.0, 116.0, 90.0, 156.0, 198.0, 373.0, 326.0] - scale_x_y: [1.0, 1.0, 1.0] - score_thresh: 0.1 - iou_thresh: 0.45 - detections_per_im: 100 - use_darknet_layer: false - ignore_thresh: 0.5 diff --git a/perception/tensorrt_yolo/config/yolov5x.param.yaml b/perception/tensorrt_yolo/config/yolov5x.param.yaml deleted file mode 100644 index 67ae862fc33af..0000000000000 --- a/perception/tensorrt_yolo/config/yolov5x.param.yaml +++ /dev/null @@ -1,10 +0,0 @@ -/**: - ros__parameters: - num_anchors: 3 - anchors: [10.0, 13.0, 16.0, 30.0, 33.0, 23.0, 30.0, 61.0, 62.0, 45.0, 59.0, 119.0, 116.0, 90.0, 156.0, 198.0, 373.0, 326.0] - scale_x_y: [1.0, 1.0, 1.0] - score_thresh: 0.1 - iou_thresh: 0.45 - detections_per_im: 100 - use_darknet_layer: false - ignore_thresh: 0.5 diff --git a/perception/tensorrt_yolo/include/tensorrt_yolo/nodelet.hpp b/perception/tensorrt_yolo/include/tensorrt_yolo/nodelet.hpp deleted file mode 100644 index aea3142b1f35e..0000000000000 --- a/perception/tensorrt_yolo/include/tensorrt_yolo/nodelet.hpp +++ /dev/null @@ -1,76 +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 TENSORRT_YOLO__NODELET_HPP_ -#define TENSORRT_YOLO__NODELET_HPP_ - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#if __has_include() -#include -#else -#include -#endif - -#include -#include -#include -#include -#include -#include - -namespace object_recognition -{ -class TensorrtYoloNodelet : public rclcpp::Node -{ -public: - explicit TensorrtYoloNodelet(const rclcpp::NodeOptions & options); - void connectCb(); - void callback(const sensor_msgs::msg::Image::ConstSharedPtr image_msg); - bool readLabelFile(const std::string & filepath, std::vector * labels); - -private: - std::mutex connect_mutex_; - - image_transport::Publisher image_pub_; - rclcpp::Publisher::SharedPtr objects_pub_; - - image_transport::Subscriber image_sub_; - - rclcpp::TimerBase::SharedPtr timer_; - - yolo::Config yolo_config_; - - int gpu_device_id_; - - std::vector labels_; - std::unique_ptr out_scores_; - std::unique_ptr out_boxes_; - std::unique_ptr out_classes_; - std::unique_ptr net_ptr_; -}; - -} // namespace object_recognition - -#endif // TENSORRT_YOLO__NODELET_HPP_ diff --git a/perception/tensorrt_yolo/launch/tensorrt_yolo.launch.xml b/perception/tensorrt_yolo/launch/tensorrt_yolo.launch.xml deleted file mode 100755 index b2656de0ab72e..0000000000000 --- a/perception/tensorrt_yolo/launch/tensorrt_yolo.launch.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/perception/tensorrt_yolo/lib/include/calibrator.hpp b/perception/tensorrt_yolo/lib/include/calibrator.hpp deleted file mode 100644 index edfbc126d645a..0000000000000 --- a/perception/tensorrt_yolo/lib/include/calibrator.hpp +++ /dev/null @@ -1,200 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* - * Copyright (c) 2020, NVIDIA CORPORATION. All rights reserved. - * - * Permission is hereby granted, free of charge, to any person obtaining a - * copy of this software and associated documentation files (the "Software"), - * to deal in the Software without restriction, including without limitation - * the rights to use, copy, modify, merge, publish, distribute, sublicense, - * and/or sell copies of the Software, and to permit persons to whom the - * Software is furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL - * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING - * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER - * DEALINGS IN THE SOFTWARE. - */ - -#ifndef CALIBRATOR_HPP_ -#define CALIBRATOR_HPP_ - -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include - -namespace yolo -{ -class ImageStream -{ -public: - ImageStream( - int batch_size, nvinfer1::Dims input_dims, const std::vector calibration_images) - : batch_size_(batch_size), - calibration_images_(calibration_images), - current_batch_(0), - max_batches_(calibration_images.size() / batch_size_), - input_dims_(input_dims) - { - batch_.resize(batch_size_ * input_dims_.d[1] * input_dims_.d[2] * input_dims_.d[3]); - } - - int getBatchSize() const { return batch_size_; } - - int getMaxBatches() const { return max_batches_; } - - float * getBatch() { return &batch_[0]; } - - nvinfer1::Dims getInputDims() { return input_dims_; } - - std::vector preprocess(const cv::Mat & in_img, const int c, const int w, const int h) const - { - cv::Mat rgb; - cv::cvtColor(in_img, rgb, cv::COLOR_BGR2RGB); - - cv::resize(rgb, rgb, cv::Size(w, h)); - - cv::Mat img_float; - rgb.convertTo(img_float, CV_32FC3, 1 / 255.0); - - // HWC TO CHW - std::vector input_channels(c); - cv::split(img_float, input_channels); - - std::vector result(h * w * c); - auto data = result.data(); - int channel_length = h * w; - for (int i = 0; i < c; ++i) { - memcpy(data, input_channels[i].data, channel_length * sizeof(float)); - data += channel_length; - } - - return result; - } - - bool next() - { - if (current_batch_ == max_batches_) { - return false; - } - - for (int i = 0; i < batch_size_; ++i) { - auto image = - cv::imread(calibration_images_[batch_size_ * current_batch_ + i].c_str(), cv::IMREAD_COLOR); - auto input = preprocess(image, input_dims_.d[1], input_dims_.d[3], input_dims_.d[2]); - batch_.insert( - batch_.begin() + i * input_dims_.d[1] * input_dims_.d[2] * input_dims_.d[3], input.begin(), - input.end()); - } - - ++current_batch_; - return true; - } - - void reset() { current_batch_ = 0; } - -private: - int batch_size_; - std::vector calibration_images_; - int current_batch_; - int max_batches_; - nvinfer1::Dims input_dims_; - - std::vector batch_; -}; - -class Int8EntropyCalibrator : public nvinfer1::IInt8EntropyCalibrator2 -{ -public: - Int8EntropyCalibrator( - ImageStream & stream, const std::string calibration_cache_file, bool read_cache = true) - : stream_(stream), calibration_cache_file_(calibration_cache_file), read_cache_(read_cache) - { - auto d = stream_.getInputDims(); - input_count_ = stream_.getBatchSize() * d.d[1] * d.d[2] * d.d[3]; - CHECK_CUDA_ERROR(cudaMalloc(&device_input_, input_count_ * sizeof(float))); - } - - int getBatchSize() const noexcept override { return stream_.getBatchSize(); } - - virtual ~Int8EntropyCalibrator() { CHECK_CUDA_ERROR(cudaFree(device_input_)); } - - bool getBatch(void * bindings[], const char * names[], int nb_bindings) noexcept override - { - (void)names; - (void)nb_bindings; - - if (!stream_.next()) { - return false; - } - - try { - CHECK_CUDA_ERROR(cudaMemcpy( - device_input_, stream_.getBatch(), input_count_ * sizeof(float), cudaMemcpyHostToDevice)); - } catch (const std::exception & e) { - // Do nothing - } - bindings[0] = device_input_; - return true; - } - - const void * readCalibrationCache(size_t & length) noexcept override - { - calib_cache_.clear(); - std::ifstream input(calibration_cache_file_, std::ios::binary); - input >> std::noskipws; - if (read_cache_ && input.good()) { - std::copy( - std::istream_iterator(input), std::istream_iterator(), - std::back_inserter(calib_cache_)); - } - - length = calib_cache_.size(); - return length ? &calib_cache_[0] : nullptr; - } - - void writeCalibrationCache(const void * cache, size_t length) noexcept override - { - std::ofstream output(calibration_cache_file_, std::ios::binary); - output.write(reinterpret_cast(cache), length); - } - -private: - ImageStream stream_; - const std::string calibration_cache_file_; - bool read_cache_{true}; - size_t input_count_; - void * device_input_{nullptr}; - std::vector calib_cache_; -}; -} // namespace yolo - -#endif // CALIBRATOR_HPP_ diff --git a/perception/tensorrt_yolo/lib/include/cuda_utils.hpp b/perception/tensorrt_yolo/lib/include/cuda_utils.hpp deleted file mode 100644 index 8a8a21d99e2cb..0000000000000 --- a/perception/tensorrt_yolo/lib/include/cuda_utils.hpp +++ /dev/null @@ -1,120 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* - * Copyright (c) 2020, NVIDIA CORPORATION. All rights reserved. - * - * 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. - */ - -/* - * This code is licensed under CC0 1.0 Universal (Public Domain). - * You can use this without any limitation. - * https://creativecommons.org/publicdomain/zero/1.0/deed.en - */ - -#ifndef CUDA_UTILS_HPP_ -#define CUDA_UTILS_HPP_ - -#include - -#include -#include -#include -#include - -#define CHECK_CUDA_ERROR(e) (cuda::check_error(e, __FILE__, __LINE__)) - -namespace cuda -{ -void check_error(const ::cudaError_t e, const char * f, int n) -{ - if (e != ::cudaSuccess) { - ::std::stringstream s; - s << ::cudaGetErrorName(e) << " (" << e << ")@" << f << "#L" << n << ": " - << ::cudaGetErrorString(e); - throw ::std::runtime_error{s.str()}; - } -} - -struct deleter -{ - void operator()(void * p) const { CHECK_CUDA_ERROR(::cudaFree(p)); } -}; -template -using unique_ptr = ::std::unique_ptr; - -template -typename ::std::enable_if<::std::is_array::value, cuda::unique_ptr>::type make_unique( - const ::std::size_t n) -{ - using U = typename ::std::remove_extent::type; - U * p; - CHECK_CUDA_ERROR(::cudaMalloc(reinterpret_cast(&p), sizeof(U) * n)); - return cuda::unique_ptr{p}; -} - -template -cuda::unique_ptr make_unique() -{ - T * p; - CHECK_CUDA_ERROR(::cudaMalloc(reinterpret_cast(&p), sizeof(T))); - return cuda::unique_ptr{p}; -} - -constexpr size_t CUDA_ALIGN = 256; - -template -inline size_t get_size_aligned(size_t num_elem) -{ - size_t size = num_elem * sizeof(T); - size_t extra_align = 0; - if (size % CUDA_ALIGN != 0) { - extra_align = CUDA_ALIGN - size % CUDA_ALIGN; - } - return size + extra_align; -} - -template -inline T * get_next_ptr(size_t num_elem, void *& workspace, size_t & workspace_size) -{ - size_t size = get_size_aligned(num_elem); - if (size > workspace_size) { - throw ::std::runtime_error("Workspace is too small!"); - } - workspace_size -= size; - T * ptr = reinterpret_cast(workspace); - workspace = reinterpret_cast(reinterpret_cast(workspace) + size); - return ptr; -} - -} // namespace cuda - -#endif // CUDA_UTILS_HPP_ diff --git a/perception/tensorrt_yolo/lib/include/mish.hpp b/perception/tensorrt_yolo/lib/include/mish.hpp deleted file mode 100644 index 566cd0203ff41..0000000000000 --- a/perception/tensorrt_yolo/lib/include/mish.hpp +++ /dev/null @@ -1,51 +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. - -/* - * MIT License - - * Copyright (c) 2019-2020 Wang Xinyu - - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#ifndef MISH_HPP_ -#define MISH_HPP_ - -#include - -#include - -namespace yolo -{ -int mish(cudaStream_t stream, const float * input, float * output, int n); -} // namespace yolo - -#endif // MISH_HPP_ diff --git a/perception/tensorrt_yolo/lib/include/mish_plugin.hpp b/perception/tensorrt_yolo/lib/include/mish_plugin.hpp deleted file mode 100644 index e49d2d2ba5ac9..0000000000000 --- a/perception/tensorrt_yolo/lib/include/mish_plugin.hpp +++ /dev/null @@ -1,131 +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. - -/* - * MIT License - - * Copyright (c) 2019-2020 Wang Xinyu - - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#ifndef MISH_PLUGIN_HPP_ -#define MISH_PLUGIN_HPP_ - -#include - -#include -#include -#include - -namespace yolo -{ -class MishPlugin : public nvinfer1::IPluginV2DynamicExt -{ -public: - MishPlugin(); - MishPlugin(const void * data, size_t length); - - // IPluginV2 methods - const char * getPluginType() const noexcept override; - const char * getPluginVersion() const noexcept override; - int getNbOutputs() const noexcept override; - int initialize() noexcept override; - void terminate() noexcept override; - size_t getSerializationSize() const noexcept override; - void serialize(void * buffer) const noexcept override; - void destroy() noexcept override; - void setPluginNamespace(const char * libNamespace) noexcept override; - const char * getPluginNamespace() const noexcept override; - - // IPluginV2Ext methods - nvinfer1::DataType getOutputDataType( - int index, const nvinfer1::DataType * inputType, int nbInputs) const noexcept override; - - // IPluginV2DynamicExt methods - nvinfer1::IPluginV2DynamicExt * clone() const noexcept override; - nvinfer1::DimsExprs getOutputDimensions( - int outputIndex, const nvinfer1::DimsExprs * inputs, int nbInputs, - nvinfer1::IExprBuilder & exprBuilder) noexcept override; - bool supportsFormatCombination( - int pos, const nvinfer1::PluginTensorDesc * inOut, int nbInputs, - int nbOutputs) noexcept override; - void configurePlugin( - const nvinfer1::DynamicPluginTensorDesc * in, int nbInputs, - const nvinfer1::DynamicPluginTensorDesc * out, int nbOutputs) noexcept override; - size_t getWorkspaceSize( - const nvinfer1::PluginTensorDesc * inputs, int nbInputs, - const nvinfer1::PluginTensorDesc * outputs, int nbOutputs) const noexcept override; - int enqueue( - const nvinfer1::PluginTensorDesc * inputDesc, const nvinfer1::PluginTensorDesc * outputDesc, - const void * const * inputs, void * const * outputs, void * workspace, - cudaStream_t stream) noexcept override; - -private: - const char * mPluginNamespace; -}; - -class MishPluginCreator : public nvinfer1::IPluginCreator -{ -public: - MishPluginCreator(); - - ~MishPluginCreator() override = default; - - const char * getPluginName() const noexcept override; - - const char * getPluginVersion() const noexcept override; - - const nvinfer1::PluginFieldCollection * getFieldNames() noexcept override; - - nvinfer1::IPluginV2DynamicExt * createPlugin( - const char * name, const nvinfer1::PluginFieldCollection * fc) noexcept override; - - nvinfer1::IPluginV2DynamicExt * deserializePlugin( - const char * name, const void * serialData, size_t serialLength) noexcept override; - - void setPluginNamespace(const char * libNamespace) noexcept override - { - mNamespace = libNamespace; - } - - const char * getPluginNamespace() const noexcept override { return mNamespace.c_str(); } - -private: - std::string mNamespace; - static nvinfer1::PluginFieldCollection mFC; - static std::vector mPluginAttributes; -}; - -REGISTER_TENSORRT_PLUGIN(MishPluginCreator); - -} // namespace yolo - -#endif // MISH_PLUGIN_HPP_ diff --git a/perception/tensorrt_yolo/lib/include/nms.hpp b/perception/tensorrt_yolo/lib/include/nms.hpp deleted file mode 100644 index 1070c50f35807..0000000000000 --- a/perception/tensorrt_yolo/lib/include/nms.hpp +++ /dev/null @@ -1,52 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* - * Copyright (c) 2020, NVIDIA CORPORATION. All rights reserved. - * - * Permission is hereby granted, free of charge, to any person obtaining a - * copy of this software and associated documentation files (the "Software"), - * to deal in the Software without restriction, including without limitation - * the rights to use, copy, modify, merge, publish, distribute, sublicense, - * and/or sell copies of the Software, and to permit persons to whom the - * Software is furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL - * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING - * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER - * DEALINGS IN THE SOFTWARE. - */ - -#ifndef NMS_HPP_ -#define NMS_HPP_ - -#include - -#include - -namespace yolo -{ -int nms( - int batchSize, const void * const * inputs, void * const * outputs, size_t count, - int detections_per_im, float nms_thresh, void * workspace, size_t workspace_size, - cudaStream_t stream); -} - -#endif // NMS_HPP_ diff --git a/perception/tensorrt_yolo/lib/include/nms_plugin.hpp b/perception/tensorrt_yolo/lib/include/nms_plugin.hpp deleted file mode 100644 index 58bc49e57adf4..0000000000000 --- a/perception/tensorrt_yolo/lib/include/nms_plugin.hpp +++ /dev/null @@ -1,123 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* - * Copyright (c) 2020, NVIDIA CORPORATION. All rights reserved. - * - * Permission is hereby granted, free of charge, to any person obtaining a - * copy of this software and associated documentation files (the "Software"), - * to deal in the Software without restriction, including without limitation - * the rights to use, copy, modify, merge, publish, distribute, sublicense, - * and/or sell copies of the Software, and to permit persons to whom the - * Software is furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL - * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING - * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER - * DEALINGS IN THE SOFTWARE. - */ - -#ifndef NMS_PLUGIN_HPP_ -#define NMS_PLUGIN_HPP_ - -#include - -#include -#include - -namespace yolo -{ -class NMSPlugin : public nvinfer1::IPluginV2DynamicExt -{ -public: - NMSPlugin(float nms_thresh, int detections_per_im); - NMSPlugin(float nms_thresh, int detections_per_im, size_t count); - NMSPlugin(const void * data, size_t length); - - // IPluginV2 methods - const char * getPluginType() const noexcept override; - const char * getPluginVersion() const noexcept override; - int getNbOutputs() const noexcept override; - int initialize() noexcept override; - void terminate() noexcept override; - size_t getSerializationSize() const noexcept override; - void serialize(void * buffer) const noexcept override; - void destroy() noexcept override; - void setPluginNamespace(const char * libNamespace) noexcept override; - const char * getPluginNamespace() const noexcept override; - - // IPluginV2Ext methods - nvinfer1::DataType getOutputDataType( - int index, const nvinfer1::DataType * inputType, int nbInputs) const noexcept override; - - // IPluginV2DynamicExt methods - nvinfer1::IPluginV2DynamicExt * clone() const noexcept override; - nvinfer1::DimsExprs getOutputDimensions( - int outputIndex, const nvinfer1::DimsExprs * inputs, int nbInputs, - nvinfer1::IExprBuilder & exprBuilder) noexcept override; - bool supportsFormatCombination( - int pos, const nvinfer1::PluginTensorDesc * inOut, int nbInputs, - int nbOutputs) noexcept override; - void configurePlugin( - const nvinfer1::DynamicPluginTensorDesc * in, int nbInputs, - const nvinfer1::DynamicPluginTensorDesc * out, int nbOutputs) noexcept override; - size_t getWorkspaceSize( - const nvinfer1::PluginTensorDesc * inputs, int nbInputs, - const nvinfer1::PluginTensorDesc * outputs, int nbOutputs) const noexcept override; - int enqueue( - const nvinfer1::PluginTensorDesc * inputDesc, const nvinfer1::PluginTensorDesc * outputDesc, - const void * const * inputs, void * const * outputs, void * workspace, - cudaStream_t stream) noexcept override; - -private: - float nms_thresh_; - int detections_per_im_; - - size_t count_; - mutable int size = -1; -}; - -class NMSPluginCreator : public nvinfer1::IPluginCreator -{ -public: - NMSPluginCreator(); - - const char * getPluginName() const noexcept override; - - const char * getPluginVersion() const noexcept override; - - const nvinfer1::PluginFieldCollection * getFieldNames() noexcept override; - - nvinfer1::IPluginV2DynamicExt * createPlugin( - const char * name, const nvinfer1::PluginFieldCollection * fc) noexcept override; - - nvinfer1::IPluginV2DynamicExt * deserializePlugin( - const char * name, const void * serialData, size_t serialLength) noexcept override; - - void setPluginNamespace(const char * libNamespace) noexcept override; - - const char * getPluginNamespace() const noexcept override; -}; - -REGISTER_TENSORRT_PLUGIN(NMSPluginCreator); - -} // namespace yolo - -#endif // NMS_PLUGIN_HPP_ diff --git a/perception/tensorrt_yolo/lib/include/trt_yolo.hpp b/perception/tensorrt_yolo/lib/include/trt_yolo.hpp deleted file mode 100644 index a64c94c008526..0000000000000 --- a/perception/tensorrt_yolo/lib/include/trt_yolo.hpp +++ /dev/null @@ -1,148 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* - * Copyright (c) 2020, NVIDIA CORPORATION. All rights reserved. - * - * Permission is hereby granted, free of charge, to any person obtaining a - * copy of this software and associated documentation files (the "Software"), - * to deal in the Software without restriction, including without limitation - * the rights to use, copy, modify, merge, publish, distribute, sublicense, - * and/or sell copies of the Software, and to permit persons to whom the - * Software is furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL - * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING - * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER - * DEALINGS IN THE SOFTWARE. - */ - -#ifndef TRT_YOLO_HPP_ -#define TRT_YOLO_HPP_ - -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include - -namespace yolo -{ -struct Deleter -{ - template - void operator()(T * obj) const - { - if (obj) { - delete obj; - } - } -}; - -template -using unique_ptr = std::unique_ptr; - -struct Config -{ - int num_anchors; - std::vector anchors; - std::vector scale_x_y; - float score_thresh; - float iou_thresh; - int detections_per_im; - bool use_darknet_layer; - float ignore_thresh; -}; - -class Net -{ -public: - // Create engine from engine path - explicit Net(const std::string & engine_path, bool verbose = false); - - // Create engine from serialized onnx model - Net( - const std::string & onnx_file_path, const std::string & precision, const int max_batch_size, - const Config & yolo_config, const std::vector & calibration_images, - const std::string & calibration_table, bool verbose = true, - size_t workspace_size = (1ULL << 30)); - - ~Net(); - - // Save model to path - void save(const std::string & path) const; - - bool detect(const cv::Mat & in_img, float * out_scores, float * out_boxes, float * out_classes); - - // Get (c, h, w) size of the fixed input - std::vector getInputDims() const; - - std::vector getOutputScoreSize() const; - - // Get max allowed batch size - int getMaxBatchSize() const; - - // Get max number of detections - int getMaxDetections() const; - - int getInputSize() const; - -private: - unique_ptr runtime_ = nullptr; - unique_ptr plan_ = nullptr; - unique_ptr engine_ = nullptr; - unique_ptr context_ = nullptr; - cudaStream_t stream_ = nullptr; - cuda::unique_ptr input_d_ = nullptr; - cuda::unique_ptr out_scores_d_ = nullptr; - cuda::unique_ptr out_boxes_d_ = nullptr; - cuda::unique_ptr out_classes_d_ = nullptr; - tensorrt_common::Logger logger_; - - void load(const std::string & path); - bool prepare(); - std::vector preprocess( - const cv::Mat & in_img, const int c, const int h, const int w) const; - // Infer using pre-allocated GPU buffers {data, scores, boxes} - void infer(std::vector & buffers, const int batch_size); -}; - -bool set_cuda_device(int gpu_id) -{ - cudaError_t status = cudaSetDevice(gpu_id); - if (status != cudaSuccess) { - return false; - } else { - return true; - } -} - -} // namespace yolo - -#endif // TRT_YOLO_HPP_ diff --git a/perception/tensorrt_yolo/lib/include/yolo_layer.hpp b/perception/tensorrt_yolo/lib/include/yolo_layer.hpp deleted file mode 100644 index 62dc601185140..0000000000000 --- a/perception/tensorrt_yolo/lib/include/yolo_layer.hpp +++ /dev/null @@ -1,77 +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. - -/* - * MIT License - - * Copyright (c) 2019-2020 Wang Xinyu - - * 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 (c) 2020, NVIDIA CORPORATION. All rights reserved. - * - * Permission is hereby granted, free of charge, to any person obtaining a - * copy of this software and associated documentation files (the "Software"), - * to deal in the Software without restriction, including without limitation - * the rights to use, copy, modify, merge, publish, distribute, sublicense, - * and/or sell copies of the Software, and to permit persons to whom the - * Software is furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL - * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING - * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER - * DEALINGS IN THE SOFTWARE. - */ - -#ifndef YOLO_LAYER_HPP_ -#define YOLO_LAYER_HPP_ - -#include - -#include - -namespace yolo -{ -int yoloLayer( - int batch_size, const void * const * inputs, void * const * outputs, int grid_width, - int grid_height, int num_classes, int num_anchors, const std::vector & anchors, - int input_width, int input_height, float scale_xy, float score_thresh, bool use_darknet_layer, - void * workspace, size_t workspace_size, cudaStream_t stream); -} // namespace yolo - -#endif // YOLO_LAYER_HPP_ diff --git a/perception/tensorrt_yolo/lib/include/yolo_layer_plugin.hpp b/perception/tensorrt_yolo/lib/include/yolo_layer_plugin.hpp deleted file mode 100644 index a7e10e8a6351d..0000000000000 --- a/perception/tensorrt_yolo/lib/include/yolo_layer_plugin.hpp +++ /dev/null @@ -1,155 +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. - -/* - * MIT License - - * Copyright (c) 2019-2020 Wang Xinyu - - * 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 (c) 2020, NVIDIA CORPORATION. All rights reserved. - * - * Permission is hereby granted, free of charge, to any person obtaining a - * copy of this software and associated documentation files (the "Software"), - * to deal in the Software without restriction, including without limitation - * the rights to use, copy, modify, merge, publish, distribute, sublicense, - * and/or sell copies of the Software, and to permit persons to whom the - * Software is furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL - * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING - * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER - * DEALINGS IN THE SOFTWARE. - */ - -#ifndef YOLO_LAYER_PLUGIN_HPP_ -#define YOLO_LAYER_PLUGIN_HPP_ - -#include - -#include - -#include -#include -#include -#include - -namespace yolo -{ -class YoloLayerPlugin : public nvinfer1::IPluginV2DynamicExt -{ -public: - explicit YoloLayerPlugin( - int width, int height, int num_classes, std::vector & anchors, float scale_xy, - float score_thresh, bool use_darknet_layer); - YoloLayerPlugin(const void * data, size_t length); - - // IPluginV2 methods - const char * getPluginType() const noexcept override; - const char * getPluginVersion() const noexcept override; - int getNbOutputs() const noexcept override; - int initialize() noexcept override; - void terminate() noexcept override; - size_t getSerializationSize() const noexcept override; - void serialize(void * buffer) const noexcept override; - void destroy() noexcept override; - void setPluginNamespace(const char * libNamespace) noexcept override; - const char * getPluginNamespace() const noexcept override; - - // IPluginV2Ext methods - nvinfer1::DataType getOutputDataType( - int index, const nvinfer1::DataType * inputType, int nbInputs) const noexcept override; - - // IPluginV2DynamicExt methods - nvinfer1::IPluginV2DynamicExt * clone() const noexcept override; - nvinfer1::DimsExprs getOutputDimensions( - int outputIndex, const nvinfer1::DimsExprs * inputs, int nbInputs, - nvinfer1::IExprBuilder & exprBuilder) noexcept override; - bool supportsFormatCombination( - int pos, const nvinfer1::PluginTensorDesc * inOut, int nbInputs, - int nbOutputs) noexcept override; - void configurePlugin( - const nvinfer1::DynamicPluginTensorDesc * in, int nbInputs, - const nvinfer1::DynamicPluginTensorDesc * out, int nbOutputs) noexcept override; - size_t getWorkspaceSize( - const nvinfer1::PluginTensorDesc * inputs, int nbInputs, - const nvinfer1::PluginTensorDesc * outputs, int nbOutputs) const noexcept override; - int enqueue( - const nvinfer1::PluginTensorDesc * inputDesc, const nvinfer1::PluginTensorDesc * outputDesc, - const void * const * inputs, void * const * outputs, void * workspace, - cudaStream_t stream) noexcept override; - -private: - int width_; - int height_; - int num_anchors_; - std::vector anchors_; - float scale_x_y_; - float score_thresh_; - bool use_darknet_layer_; - mutable int size = -1; -}; - -class YoloLayerPluginCreator : public nvinfer1::IPluginCreator -{ -public: - YoloLayerPluginCreator(); - - const char * getPluginName() const noexcept override; - - const char * getPluginVersion() const noexcept override; - - const nvinfer1::PluginFieldCollection * getFieldNames() noexcept override; - - nvinfer1::IPluginV2DynamicExt * createPlugin( - const char * name, const nvinfer1::PluginFieldCollection * fc) noexcept override; - - nvinfer1::IPluginV2DynamicExt * deserializePlugin( - const char * name, const void * serialData, size_t serialLength) noexcept override; - - void setPluginNamespace(const char * libNamespace) noexcept override; - - const char * getPluginNamespace() const noexcept override; -}; - -REGISTER_TENSORRT_PLUGIN(YoloLayerPluginCreator); - -} // namespace yolo - -#endif // YOLO_LAYER_PLUGIN_HPP_ diff --git a/perception/tensorrt_yolo/lib/src/plugins/mish.cu b/perception/tensorrt_yolo/lib/src/plugins/mish.cu deleted file mode 100644 index 3f2eda9a984cf..0000000000000 --- a/perception/tensorrt_yolo/lib/src/plugins/mish.cu +++ /dev/null @@ -1,70 +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. - -/* - * MIT License - - * Copyright (c) 2019-2020 Wang Xinyu - - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include - -#include -#include - -namespace yolo -{ -__device__ float mish(float x) -{ - float e = __expf(x); - float n = e * e + 2 * e; - if (x <= -0.6f) return x * __fdividef(n, n + 2); - - return x - 2 * __fdividef(x, n + 2); -} -template -__global__ void mishKernel(const T * input, T * output, int num_elem) -{ - int idx = threadIdx.x + TPB * blockIdx.x; - if (idx >= num_elem) return; - output[idx] = mish(input[idx]); -} - -int mish(cudaStream_t stream, const float * input, float * output, int n) -{ - constexpr int blockSize = 256; - const int gridSize = (n + blockSize - 1) / blockSize; - mishKernel<<>>(input, output, n); - return 0; -} - -} // namespace yolo diff --git a/perception/tensorrt_yolo/lib/src/plugins/mish_plugin.cpp b/perception/tensorrt_yolo/lib/src/plugins/mish_plugin.cpp deleted file mode 100644 index fe8688ed946e8..0000000000000 --- a/perception/tensorrt_yolo/lib/src/plugins/mish_plugin.cpp +++ /dev/null @@ -1,272 +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. - -/* - * MIT License - - * Copyright (c) 2019-2020 Wang Xinyu - - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include -#include - -#include - -#include -#include -#include -#include - -using nvinfer1::DataType; -using nvinfer1::Dims; -using nvinfer1::DimsExprs; -using nvinfer1::DynamicPluginTensorDesc; -using nvinfer1::IExprBuilder; -using nvinfer1::IPluginV2DynamicExt; -using nvinfer1::PluginField; -using nvinfer1::PluginFieldCollection; -using nvinfer1::PluginFormat; -using nvinfer1::PluginTensorDesc; - -namespace -{ -const char * MISH_PLUGIN_VERSION{"1"}; -const char * MISH_PLUGIN_NAME{"Mish_TRT"}; - -inline int64_t volume(const Dims & d) -{ - int64_t v = 1; - for (int64_t i = 0; i < d.nbDims; i++) { - v *= d.d[i]; - } - return v; -} -} // namespace - -namespace yolo -{ -MishPlugin::MishPlugin() -{ -} - -// create the plugin at runtime from a byte stream -MishPlugin::MishPlugin(const void * data, size_t length) -{ - (void)data; - (void)length; -} - -// IPluginV2 Methods - -const char * MishPlugin::getPluginType() const noexcept -{ - return MISH_PLUGIN_NAME; -} - -const char * MishPlugin::getPluginVersion() const noexcept -{ - return MISH_PLUGIN_VERSION; -} - -int MishPlugin::getNbOutputs() const noexcept -{ - return 1; -} - -int MishPlugin::initialize() noexcept -{ - return 0; -} - -void MishPlugin::terminate() noexcept -{ -} - -size_t MishPlugin::getSerializationSize() const noexcept -{ - return 0; -} - -void MishPlugin::serialize(void * buffer) const noexcept -{ - (void)buffer; -} - -void MishPlugin::destroy() noexcept -{ - delete this; -} - -void MishPlugin::setPluginNamespace(const char * pluginNamespace) noexcept -{ - mPluginNamespace = pluginNamespace; -} - -const char * MishPlugin::getPluginNamespace() const noexcept -{ - return mPluginNamespace; -} - -// IPluginV2Ext Methods - -DataType MishPlugin::getOutputDataType( - int index, const DataType * inputTypes, int nbInputs) const noexcept -{ - (void)index; - (void)inputTypes; - (void)nbInputs; - - assert(inputTypes[0] == DataType::kFLOAT); - return inputTypes[0]; -} - -// IPluginV2DynamicExt Methods - -IPluginV2DynamicExt * MishPlugin::clone() const noexcept -{ - auto plugin = new MishPlugin(*this); - plugin->setPluginNamespace(mPluginNamespace); - return plugin; -} - -DimsExprs MishPlugin::getOutputDimensions( - int outputIndex, const DimsExprs * inputs, int nbInputs, IExprBuilder & exprBuilder) noexcept -{ - (void)outputIndex; - (void)nbInputs; - (void)exprBuilder; - - return inputs[0]; -} - -bool MishPlugin::supportsFormatCombination( - int pos, const PluginTensorDesc * inOut, int nbInputs, int nbOutputs) noexcept -{ - (void)nbInputs; - (void)nbOutputs; - - return inOut[pos].type == DataType::kFLOAT && inOut[pos].format == PluginFormat::kLINEAR; -} - -void MishPlugin::configurePlugin( - const DynamicPluginTensorDesc * in, int nbInput, const DynamicPluginTensorDesc * out, - int nbOutput) noexcept -{ - (void)in; - (void)nbInput; - (void)out; - (void)nbOutput; - - assert(nbInput == 1); - assert(nbOutput == 1); -} - -size_t MishPlugin::getWorkspaceSize( - const PluginTensorDesc * inputs, int nbInputs, const PluginTensorDesc * outputs, - int nbOutputs) const noexcept -{ - (void)inputs; - (void)nbInputs; - (void)outputs; - (void)nbOutputs; - - return 0; -} - -int MishPlugin::enqueue( - const PluginTensorDesc * inputDesc, const PluginTensorDesc * outputDesc, - const void * const * inputs, void * const * outputs, void * workspace, - cudaStream_t stream) noexcept -{ - (void)inputDesc; - (void)outputDesc; - (void)workspace; - - const int input_volume = volume(inputDesc[0].dims); - - int status = -1; - - const float * input = static_cast(inputs[0]); - float * output = static_cast(outputs[0]); - status = mish(stream, input, output, input_volume); - return status; -} - -PluginFieldCollection MishPluginCreator::mFC{}; -std::vector MishPluginCreator::mPluginAttributes; - -MishPluginCreator::MishPluginCreator() -{ - mPluginAttributes.clear(); - - mFC.nbFields = mPluginAttributes.size(); - mFC.fields = mPluginAttributes.data(); -} - -const char * MishPluginCreator::getPluginName() const noexcept -{ - return MISH_PLUGIN_NAME; -} - -const char * MishPluginCreator::getPluginVersion() const noexcept -{ - return MISH_PLUGIN_VERSION; -} - -const PluginFieldCollection * MishPluginCreator::getFieldNames() noexcept -{ - return &mFC; -} - -IPluginV2DynamicExt * MishPluginCreator::createPlugin( - const char * name, const PluginFieldCollection * fc) noexcept -{ - (void)name; - (void)fc; - - MishPlugin * obj = new MishPlugin(); - obj->setPluginNamespace(mNamespace.c_str()); - return obj; -} - -IPluginV2DynamicExt * MishPluginCreator::deserializePlugin( - const char * name, const void * serialData, size_t serialLength) noexcept -{ - (void)name; - - // This object will be deleted when the network is destroyed, which will - // call MishPlugin::destroy() - MishPlugin * obj = new MishPlugin(serialData, serialLength); - obj->setPluginNamespace(mNamespace.c_str()); - return obj; -} -} // namespace yolo diff --git a/perception/tensorrt_yolo/lib/src/plugins/nms.cu b/perception/tensorrt_yolo/lib/src/plugins/nms.cu deleted file mode 100644 index d73ab59ff4a7d..0000000000000 --- a/perception/tensorrt_yolo/lib/src/plugins/nms.cu +++ /dev/null @@ -1,188 +0,0 @@ -/// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* - * Copyright (c) 2020, NVIDIA CORPORATION. All rights reserved. - * - * Permission is hereby granted, free of charge, to any person obtaining a - * copy of this software and associated documentation files (the "Software"), - * to deal in the Software without restriction, including without limitation - * the rights to use, copy, modify, merge, publish, distribute, sublicense, - * and/or sell copies of the Software, and to permit persons to whom the - * Software is furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL - * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING - * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER - * DEALINGS IN THE SOFTWARE. - */ - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#if CUDART_VERSION >= 11000 -#include -#include -#else -#include -#include -using namespace thrust::cuda_cub; -#endif - -#include -#include - -namespace yolo -{ -__global__ void nms_kernel( - const int num_per_thread, const float threshold, const int num_detections, const int * indices, - float * scores, const float * classes, const float4 * boxes) -{ - // Go through detections by descending score - for (int m = 0; m < num_detections; m++) { - for (int n = 0; n < num_per_thread; n++) { - int i = threadIdx.x * num_per_thread + n; - if (i < num_detections && m < i && scores[m] > 0.0f) { - int idx = indices[i]; - int max_idx = indices[m]; - int icls = classes[idx]; - int mcls = classes[max_idx]; - if (mcls == icls) { - float4 ibox = boxes[idx]; - float4 mbox = boxes[max_idx]; - float x1 = max(ibox.x, mbox.x); - float y1 = max(ibox.y, mbox.y); - float x2 = min(ibox.x + ibox.z, mbox.x + mbox.z); - float y2 = min(ibox.y + ibox.w, ibox.y + mbox.w); - float w = max(0.0f, x2 - x1); - float h = max(0.0f, y2 - y1); - float iarea = ibox.z * ibox.w; - float marea = mbox.z * mbox.w; - float inter = w * h; - float overlap = inter / (iarea + marea - inter); - if (overlap > threshold) { - scores[i] = 0.0f; - } - } - } - } - - // Sync discarded detections - __syncthreads(); - } -} - -int nms( - int batch_size, const void * const * inputs, void * const * outputs, size_t count, - int detections_per_im, float nms_thresh, void * workspace, size_t workspace_size, - cudaStream_t stream) -{ - if (!workspace || !workspace_size) { - // Return required scratch space size cub style - workspace_size = cuda::get_size_aligned(count); // flags - workspace_size += cuda::get_size_aligned(count); // indices - workspace_size += cuda::get_size_aligned(count); // indices_sorted - workspace_size += cuda::get_size_aligned(count); // scores - workspace_size += cuda::get_size_aligned(count); // scores_sorted - - size_t temp_size_flag = 0; - cub::DeviceSelect::Flagged( - (void *)nullptr, temp_size_flag, cub::CountingInputIterator(count), (bool *)nullptr, - (int *)nullptr, (int *)nullptr, count); - size_t temp_size_sort = 0; - cub::DeviceRadixSort::SortPairsDescending( - (void *)nullptr, temp_size_sort, (float *)nullptr, (float *)nullptr, (int *)nullptr, - (int *)nullptr, count); - workspace_size += std::max(temp_size_flag, temp_size_sort); - - return workspace_size; - } - - auto on_stream = thrust::cuda::par.on(stream); - - auto flags = cuda::get_next_ptr(count, workspace, workspace_size); - auto indices = cuda::get_next_ptr(count, workspace, workspace_size); - auto indices_sorted = cuda::get_next_ptr(count, workspace, workspace_size); - auto scores = cuda::get_next_ptr(count, workspace, workspace_size); - auto scores_sorted = cuda::get_next_ptr(count, workspace, workspace_size); - - for (int batch = 0; batch < batch_size; batch++) { - auto in_scores = static_cast(inputs[0]) + batch * count; - auto in_boxes = static_cast(inputs[1]) + batch * count; - auto in_classes = static_cast(inputs[2]) + batch * count; - - auto out_scores = static_cast(outputs[0]) + batch * detections_per_im; - auto out_boxes = static_cast(outputs[1]) + batch * detections_per_im; - auto out_classes = static_cast(outputs[2]) + batch * detections_per_im; - - // Discard null scores - thrust::transform( - on_stream, in_scores, in_scores + count, flags, thrust::placeholders::_1 > 0.0f); - - int * num_selected = reinterpret_cast(indices_sorted); - cub::DeviceSelect::Flagged( - workspace, workspace_size, cub::CountingInputIterator(0), flags, indices, num_selected, - count, stream); - cudaStreamSynchronize(stream); - int num_detections = *thrust::device_pointer_cast(num_selected); - - // Sort scores and corresponding indices - thrust::gather(on_stream, indices, indices + num_detections, in_scores, scores); - cub::DeviceRadixSort::SortPairsDescending( - workspace, workspace_size, scores, scores_sorted, indices, indices_sorted, num_detections, 0, - sizeof(*scores) * 8, stream); - - // Launch actual NMS kernel - 1 block with each thread handling n detections - const int max_threads = 1024; - int num_per_thread = ceil((float)num_detections / max_threads); - nms_kernel<<<1, max_threads, 0, stream>>>( - num_per_thread, nms_thresh, num_detections, indices_sorted, scores_sorted, in_classes, - in_boxes); - - // Re-sort with updated scores - cub::DeviceRadixSort::SortPairsDescending( - workspace, workspace_size, scores_sorted, scores, indices_sorted, indices, num_detections, 0, - sizeof(*scores) * 8, stream); - - // Gather filtered scores, boxes, classes - num_detections = min(detections_per_im, num_detections); - cudaMemcpyAsync( - out_scores, scores, num_detections * sizeof *scores, cudaMemcpyDeviceToDevice, stream); - if (num_detections < detections_per_im) { - thrust::fill_n(on_stream, out_scores + num_detections, detections_per_im - num_detections, 0); - } - thrust::gather(on_stream, indices, indices + num_detections, in_boxes, out_boxes); - thrust::gather(on_stream, indices, indices + num_detections, in_classes, out_classes); - } - - return 0; -} - -} // namespace yolo diff --git a/perception/tensorrt_yolo/lib/src/plugins/nms_plugin.cpp b/perception/tensorrt_yolo/lib/src/plugins/nms_plugin.cpp deleted file mode 100644 index b486595a2cb7b..0000000000000 --- a/perception/tensorrt_yolo/lib/src/plugins/nms_plugin.cpp +++ /dev/null @@ -1,285 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* - * Copyright (c) 2020, NVIDIA CORPORATION. All rights reserved. - * - * Permission is hereby granted, free of charge, to any person obtaining a - * copy of this software and associated documentation files (the "Software"), - * to deal in the Software without restriction, including without limitation - * the rights to use, copy, modify, merge, publish, distribute, sublicense, - * and/or sell copies of the Software, and to permit persons to whom the - * Software is furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL - * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING - * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER - * DEALINGS IN THE SOFTWARE. - */ - -#include -#include - -#include -#include - -#include -#include -#include - -using nvinfer1::DataType; -using nvinfer1::DimsExprs; -using nvinfer1::DynamicPluginTensorDesc; -using nvinfer1::IExprBuilder; -using nvinfer1::IPluginV2DynamicExt; -using nvinfer1::PluginFieldCollection; -using nvinfer1::PluginFormat; -using nvinfer1::PluginTensorDesc; - -namespace -{ -const char * NMS_PLUGIN_VERSION{"1"}; -const char * NMS_PLUGIN_NAME{"YOLO_NMS_TRT"}; -const char * NMS_PLUGIN_NAMESPACE{""}; - -template -void write(char *& buffer, const T & val) -{ - std::memcpy(buffer, &val, sizeof(T)); - buffer += sizeof(T); -} - -template -void read(const char *& buffer, T & val) -{ - std::memcpy(&val, buffer, sizeof(T)); - buffer += sizeof(T); -} - -} // namespace - -namespace yolo -{ -NMSPlugin::NMSPlugin(float nms_thresh, int detections_per_im) -: nms_thresh_(nms_thresh), detections_per_im_(detections_per_im) -{ - assert(nms_thresh > 0); - assert(detections_per_im > 0); -} - -NMSPlugin::NMSPlugin(float nms_thresh, int detections_per_im, size_t count) -: nms_thresh_(nms_thresh), detections_per_im_(detections_per_im), count_(count) -{ - assert(nms_thresh > 0); - assert(detections_per_im > 0); - assert(count > 0); -} - -NMSPlugin::NMSPlugin(void const * data, size_t length) -{ - (void)length; - - const char * d = static_cast(data); - read(d, nms_thresh_); - read(d, detections_per_im_); - read(d, count_); -} - -const char * NMSPlugin::getPluginType() const noexcept -{ - return NMS_PLUGIN_NAME; -} - -const char * NMSPlugin::getPluginVersion() const noexcept -{ - return NMS_PLUGIN_VERSION; -} - -int NMSPlugin::getNbOutputs() const noexcept -{ - return 3; -} - -int NMSPlugin::initialize() noexcept -{ - return 0; -} - -void NMSPlugin::terminate() noexcept -{ -} - -size_t NMSPlugin::getSerializationSize() const noexcept -{ - return sizeof(nms_thresh_) + sizeof(detections_per_im_) + sizeof(count_); -} - -void NMSPlugin::serialize(void * buffer) const noexcept -{ - char * d = static_cast(buffer); - write(d, nms_thresh_); - write(d, detections_per_im_); - write(d, count_); -} - -void NMSPlugin::destroy() noexcept -{ - delete this; -} - -void NMSPlugin::setPluginNamespace(const char * N) noexcept -{ - (void)N; -} - -const char * NMSPlugin::getPluginNamespace() const noexcept -{ - return NMS_PLUGIN_NAMESPACE; -} - -// IPluginV2Ext Methods - -DataType NMSPlugin::getOutputDataType( - int index, const DataType * inputTypes, int nbInputs) const noexcept -{ - (void)index; - (void)inputTypes; - (void)nbInputs; - - assert(index < 3); - return DataType::kFLOAT; -} - -// IPluginV2DynamicExt Methods - -IPluginV2DynamicExt * NMSPlugin::clone() const noexcept -{ - return new NMSPlugin(nms_thresh_, detections_per_im_, count_); -} - -DimsExprs NMSPlugin::getOutputDimensions( - int outputIndex, const DimsExprs * inputs, int nbInputs, IExprBuilder & exprBuilder) noexcept -{ - (void)nbInputs; - - DimsExprs output(inputs[0]); - output.d[1] = exprBuilder.constant(detections_per_im_ * (outputIndex == 1 ? 4 : 1)); - output.d[2] = exprBuilder.constant(1); - output.d[3] = exprBuilder.constant(1); - return output; -} - -bool NMSPlugin::supportsFormatCombination( - int pos, const PluginTensorDesc * inOut, int nbInputs, int nbOutputs) noexcept -{ - (void)nbInputs; - (void)nbOutputs; - - assert(nbInputs == 3); - assert(nbOutputs == 3); - assert(pos < 6); - return inOut[pos].type == DataType::kFLOAT && inOut[pos].format == PluginFormat::kLINEAR; -} - -void NMSPlugin::configurePlugin( - const DynamicPluginTensorDesc * in, int nbInputs, const DynamicPluginTensorDesc * out, - int nbOutputs) noexcept -{ - (void)nbInputs; - (void)nbOutputs; - (void)out; - (void)nbOutputs; - - assert(nbInputs == 3); - assert(in[0].desc.dims.d[1] == in[2].desc.dims.d[1]); - assert(in[1].desc.dims.d[1] == in[2].desc.dims.d[1] * 4); - count_ = in[0].desc.dims.d[1]; -} - -size_t NMSPlugin::getWorkspaceSize( - const PluginTensorDesc * inputs, int nbInputs, const PluginTensorDesc * outputs, - int nbOutputs) const noexcept -{ - (void)nbInputs; - (void)outputs; - (void)nbOutputs; - - if (size < 0) { - size = nms( - inputs->dims.d[0], nullptr, nullptr, count_, detections_per_im_, nms_thresh_, nullptr, 0, - nullptr); - } - return size; -} - -int NMSPlugin::enqueue( - const PluginTensorDesc * inputDesc, const PluginTensorDesc * outputDesc, - const void * const * inputs, void * const * outputs, void * workspace, - cudaStream_t stream) noexcept -{ - return nms( - inputDesc->dims.d[0], inputs, outputs, count_, detections_per_im_, nms_thresh_, workspace, - getWorkspaceSize(inputDesc, 3, outputDesc, 3), stream); -} - -NMSPluginCreator::NMSPluginCreator() -{ -} - -const char * NMSPluginCreator::getPluginName() const noexcept -{ - return NMS_PLUGIN_NAME; -} - -const char * NMSPluginCreator::getPluginVersion() const noexcept -{ - return NMS_PLUGIN_VERSION; -} - -const char * NMSPluginCreator::getPluginNamespace() const noexcept -{ - return NMS_PLUGIN_NAMESPACE; -} - -void NMSPluginCreator::setPluginNamespace(const char * N) noexcept -{ - (void)N; -} -const PluginFieldCollection * NMSPluginCreator::getFieldNames() noexcept -{ - return nullptr; -} -IPluginV2DynamicExt * NMSPluginCreator::createPlugin( - const char * name, const PluginFieldCollection * fc) noexcept -{ - (void)name; - (void)fc; - return nullptr; -} - -IPluginV2DynamicExt * NMSPluginCreator::deserializePlugin( - const char * name, const void * serialData, size_t serialLength) noexcept -{ - (void)name; - - return new NMSPlugin(serialData, serialLength); -} - -} // namespace yolo diff --git a/perception/tensorrt_yolo/lib/src/plugins/yolo_layer.cu b/perception/tensorrt_yolo/lib/src/plugins/yolo_layer.cu deleted file mode 100644 index 0da2c342faaa9..0000000000000 --- a/perception/tensorrt_yolo/lib/src/plugins/yolo_layer.cu +++ /dev/null @@ -1,169 +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. - -/* - * MIT License - - * Copyright (c) 2019-2020 Wang Xinyu - - * 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 (c) 2020, NVIDIA CORPORATION. All rights reserved. - * - * Permission is hereby granted, free of charge, to any person obtaining a - * copy of this software and associated documentation files (the "Software"), - * to deal in the Software without restriction, including without limitation - * the rights to use, copy, modify, merge, publish, distribute, sublicense, - * and/or sell copies of the Software, and to permit persons to whom the - * Software is furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL - * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING - * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER - * DEALINGS IN THE SOFTWARE. - */ - -#include -#include - -#include -#include - -#include - -namespace yolo -{ -inline __device__ float sigmoid(float x) -{ - return 1.0f / (1.0f + __expf(-x)); -} - -inline __device__ float scaleSigmoid(float x, float scale) -{ - return scale * sigmoid(x) - (scale - 1.0f) * 0.5f; -} - -template -__global__ void yoloLayerKernel( - const float * input, float * out_scores, float4 * out_boxes, float * out_classes, int grid_width, - int grid_height, int num_classes, int num_anchors, const float * anchors, int input_width, - int input_height, float scale_x_y, float score_thresh, bool use_darknet_layer) -{ - int idx = threadIdx.x + TPB * blockIdx.x; - int total_grids = grid_width * grid_height; - if (idx >= total_grids * num_anchors) return; - auto out_score = (out_scores) + idx; - auto out_box = (out_boxes) + idx; - auto out_class = (out_classes) + idx; - - int anchor_idx = idx / total_grids; - idx = idx - total_grids * anchor_idx; - int info_len = 5 + num_classes; - auto cur_input = static_cast(input) + anchor_idx * (info_len * total_grids); - - int class_id; - float max_cls_logit = -CUDART_INF_F; // minus infinity - for (int i = 5; i < info_len; ++i) { - float l = cur_input[idx + i * total_grids]; - if (l > max_cls_logit) { - max_cls_logit = l; - class_id = i - 5; - } - } - float max_cls_prob = sigmoid(max_cls_logit); - float objectness = sigmoid(cur_input[idx + 4 * total_grids]); - - int row = idx / grid_width; - int col = idx % grid_width; - float x = 0, y = 0, w = 0, h = 0; - - if (use_darknet_layer) { - x = (col + scaleSigmoid(cur_input[idx + 0 * total_grids], scale_x_y)) / grid_width; // [0, 1] - y = (row + scaleSigmoid(cur_input[idx + 1 * total_grids], scale_x_y)) / grid_height; // [0, 1] - w = __expf(cur_input[idx + 2 * total_grids]) * anchors[2 * anchor_idx] / input_width; // [0, 1] - h = __expf(cur_input[idx + 3 * total_grids]) * anchors[2 * anchor_idx + 1] / - input_height; // [0, 1] - } else { - x = (col + sigmoid(cur_input[idx + 0 * total_grids]) * 2 - 0.5) / grid_width; // [0, 1] - y = (row + sigmoid(cur_input[idx + 1 * total_grids]) * 2 - 0.5) / grid_height; // [0, 1] - w = (sigmoid(cur_input[idx + 2 * total_grids]) * 2) * - (sigmoid(cur_input[idx + 2 * total_grids]) * 2) * anchors[2 * anchor_idx] / - input_width; // [0, 1] - h = (sigmoid(cur_input[idx + 3 * total_grids]) * 2) * - (sigmoid(cur_input[idx + 3 * total_grids]) * 2) * anchors[2 * anchor_idx + 1] / - input_height; // [0, 1] - } - x -= w / 2; // shift from center to top-left - y -= h / 2; - *out_box = make_float4(x, y, w, h); - *out_class = class_id; - *out_score = objectness < score_thresh ? 0.0 : max_cls_prob * objectness; -} - -int yoloLayer( - int batch_size, const void * const * inputs, void * const * outputs, int grid_width, - int grid_height, int num_classes, int num_anchors, const std::vector & anchors, - int input_width, int input_height, float scale_x_y, float score_thresh, bool use_darknet_layer, - void * workspace, size_t workspace_size, cudaStream_t stream) -{ - if (!workspace || !workspace_size) { - workspace_size = cuda::get_size_aligned(anchors.size()); - return workspace_size; - } - - auto anchors_d = cuda::get_next_ptr(anchors.size(), workspace, workspace_size); - cudaMemcpyAsync( - anchors_d, anchors.data(), anchors.size() * sizeof *anchors_d, cudaMemcpyHostToDevice, stream); - - int num_elements = num_anchors * grid_width * grid_height; - constexpr int block_size = 256; - const int grid_size = (num_elements + block_size - 1) / block_size; - for (int batch = 0; batch < batch_size; ++batch) { - auto input = static_cast(inputs[0]) + - batch * num_anchors * (num_classes + 5) * grid_width * grid_height; - auto out_scores = static_cast(outputs[0]) + batch * num_elements; - auto out_boxes = static_cast(outputs[1]) + batch * num_elements; - auto out_classes = static_cast(outputs[2]) + batch * num_elements; - yoloLayerKernel<<>>( - input, out_scores, out_boxes, out_classes, grid_width, grid_height, num_classes, num_anchors, - anchors_d, input_width, input_height, scale_x_y, score_thresh, use_darknet_layer); - } - return 0; -} - -} // namespace yolo diff --git a/perception/tensorrt_yolo/lib/src/plugins/yolo_layer_plugin.cpp b/perception/tensorrt_yolo/lib/src/plugins/yolo_layer_plugin.cpp deleted file mode 100644 index dc12921dc1e96..0000000000000 --- a/perception/tensorrt_yolo/lib/src/plugins/yolo_layer_plugin.cpp +++ /dev/null @@ -1,339 +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. - -/* - * MIT License - - * Copyright (c) 2019-2020 Wang Xinyu - - * 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 (c) 2020, NVIDIA CORPORATION. All rights reserved. - * - * Permission is hereby granted, free of charge, to any person obtaining a - * copy of this software and associated documentation files (the "Software"), - * to deal in the Software without restriction, including without limitation - * the rights to use, copy, modify, merge, publish, distribute, sublicense, - * and/or sell copies of the Software, and to permit persons to whom the - * Software is furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL - * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING - * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER - * DEALINGS IN THE SOFTWARE. - */ - -#include -#include - -#include -#include - -#include -#include -#include -#include - -using nvinfer1::DataType; -using nvinfer1::DimsExprs; -using nvinfer1::DynamicPluginTensorDesc; -using nvinfer1::IExprBuilder; -using nvinfer1::IPluginV2DynamicExt; -using nvinfer1::PluginFieldCollection; -using nvinfer1::PluginFormat; -using nvinfer1::PluginTensorDesc; - -namespace -{ -const char * YOLO_LAYER_PLUGIN_VERSION{"1"}; -const char * YOLO_LAYER_PLUGIN_NAME{"YOLO_Layer_TRT"}; -const char * YOLO_LAYER_PLUGIN_NAMESPACE{""}; - -template -void write(char *& buffer, const T & val) -{ - std::memcpy(buffer, &val, sizeof(T)); - buffer += sizeof(T); -} - -template -void read(const char *& buffer, T & val) -{ - std::memcpy(&val, buffer, sizeof(T)); - buffer += sizeof(T); -} -} // namespace - -namespace yolo -{ -YoloLayerPlugin::YoloLayerPlugin( - int width, int height, int num_anchors, std::vector & anchors, float scale_x_y, - float score_thresh, bool use_darknet_layer) -: width_(width), - height_(height), - num_anchors_(num_anchors), - anchors_(anchors), - scale_x_y_(scale_x_y), - score_thresh_(score_thresh), - use_darknet_layer_(use_darknet_layer) -{ -} - -// create the plugin at runtime from a byte stream -YoloLayerPlugin::YoloLayerPlugin(const void * data, size_t length) -{ - (void)length; - - const char * d = static_cast(data); - read(d, width_); - read(d, height_); - read(d, num_anchors_); - int anchor_size = num_anchors_ * 2; - while (anchor_size--) { - float val; - read(d, val); - anchors_.push_back(val); - } - read(d, scale_x_y_); - read(d, score_thresh_); - read(d, use_darknet_layer_); -} -// IPluginV2 Methods - -const char * YoloLayerPlugin::getPluginType() const noexcept -{ - return YOLO_LAYER_PLUGIN_NAME; -} - -const char * YoloLayerPlugin::getPluginVersion() const noexcept -{ - return YOLO_LAYER_PLUGIN_VERSION; -} - -int YoloLayerPlugin::getNbOutputs() const noexcept -{ - return 3; -} - -int YoloLayerPlugin::initialize() noexcept -{ - return 0; -} - -void YoloLayerPlugin::terminate() noexcept -{ -} - -size_t YoloLayerPlugin::getSerializationSize() const noexcept -{ - return sizeof(width_) + sizeof(height_) + sizeof(num_anchors_) + - sizeof(float) * num_anchors_ * 2 + sizeof(scale_x_y_) + sizeof(score_thresh_) + - sizeof(use_darknet_layer_); -} - -void YoloLayerPlugin::serialize(void * buffer) const noexcept -{ - char * d = reinterpret_cast(buffer); - write(d, width_); - write(d, height_); - write(d, num_anchors_); - for (int i = 0; i < num_anchors_ * 2; ++i) { - write(d, anchors_[i]); - } - write(d, scale_x_y_); - write(d, score_thresh_); - write(d, use_darknet_layer_); -} - -void YoloLayerPlugin::destroy() noexcept -{ - delete this; -} - -void YoloLayerPlugin::setPluginNamespace(const char * N) noexcept -{ - (void)N; -} - -const char * YoloLayerPlugin::getPluginNamespace() const noexcept -{ - return YOLO_LAYER_PLUGIN_NAMESPACE; -} - -// IPluginV2Ext Methods - -DataType YoloLayerPlugin::getOutputDataType( - int index, const DataType * inputTypes, int nbInputs) const noexcept -{ - (void)index; - (void)inputTypes; - (void)nbInputs; - - return DataType::kFLOAT; -} - -// IPluginV2DynamicExt Methods - -IPluginV2DynamicExt * YoloLayerPlugin::clone() const noexcept -{ - return new YoloLayerPlugin(*this); -} - -DimsExprs YoloLayerPlugin::getOutputDimensions( - int outputIndex, const DimsExprs * inputs, int nbInputs, IExprBuilder & exprBuilder) noexcept -{ - (void)nbInputs; - - DimsExprs ret = inputs[0]; - ret.nbDims = 3; - const auto total_count = - ret.d[2]->getConstantValue() * ret.d[3]->getConstantValue() * num_anchors_; - ret.d[1] = exprBuilder.constant(total_count * (outputIndex == 1 ? 4 : 1)); - ret.d[2] = exprBuilder.constant(1); - ret.d[3] = exprBuilder.constant(1); - return ret; -} - -bool YoloLayerPlugin::supportsFormatCombination( - int pos, const PluginTensorDesc * inOut, int nbInputs, int nbOutputs) noexcept -{ - (void)nbInputs; - (void)nbOutputs; - - assert(nbInputs == 1); - assert(nbOutputs == 3); - assert(pos < 4); - return inOut[pos].type == DataType::kFLOAT && inOut[pos].format == PluginFormat::kLINEAR; -} - -void YoloLayerPlugin::configurePlugin( - const DynamicPluginTensorDesc * in, int nbInput, const DynamicPluginTensorDesc * out, - int nbOutput) noexcept -{ - (void)in; - (void)nbInput; - (void)out; - (void)nbOutput; - - assert(nbInput == 1); - assert(nbOutput == 3); -} - -size_t YoloLayerPlugin::getWorkspaceSize( - const PluginTensorDesc * inputs, int nbInputs, const PluginTensorDesc * outputs, - int nbOutputs) const noexcept -{ - (void)nbInputs; - (void)outputs; - (void)nbOutputs; - - if (size < 0) { - const int batch_size = inputs[0].dims.d[0]; - const int grid_width = inputs[0].dims.d[2]; - const int grid_height = inputs[0].dims.d[3]; - const int num_classes = inputs[0].dims.d[1] / num_anchors_ - 5; - size = yoloLayer( - batch_size, nullptr, nullptr, grid_width, grid_height, num_classes, num_anchors_, anchors_, - width_, height_, scale_x_y_, score_thresh_, use_darknet_layer_, nullptr, 0, nullptr); - } - return size; -} - -int YoloLayerPlugin::enqueue( - const PluginTensorDesc * inputDesc, const PluginTensorDesc * outputDesc, - const void * const * inputs, void * const * outputs, void * workspace, - cudaStream_t stream) noexcept -{ - const int batch_size = inputDesc[0].dims.d[0]; - const int grid_width = inputDesc[0].dims.d[2]; - const int grid_height = inputDesc[0].dims.d[3]; - const int num_classes = inputDesc[0].dims.d[1] / num_anchors_ - 5; - - int status = -1; - status = yoloLayer( - batch_size, inputs, outputs, grid_width, grid_height, num_classes, num_anchors_, anchors_, - width_, height_, scale_x_y_, score_thresh_, use_darknet_layer_, workspace, - getWorkspaceSize(inputDesc, 1, outputDesc, 3), stream); - return status; -} - -YoloLayerPluginCreator::YoloLayerPluginCreator() -{ -} - -const char * YoloLayerPluginCreator::getPluginName() const noexcept -{ - return YOLO_LAYER_PLUGIN_NAME; -} - -const char * YoloLayerPluginCreator::getPluginVersion() const noexcept -{ - return YOLO_LAYER_PLUGIN_VERSION; -} - -const char * YoloLayerPluginCreator::getPluginNamespace() const noexcept -{ - return YOLO_LAYER_PLUGIN_NAMESPACE; -} - -void YoloLayerPluginCreator::setPluginNamespace(const char * N) noexcept -{ - (void)N; -} -const PluginFieldCollection * YoloLayerPluginCreator::getFieldNames() noexcept -{ - return nullptr; -} - -IPluginV2DynamicExt * YoloLayerPluginCreator::createPlugin( - const char * name, const PluginFieldCollection * fc) noexcept -{ - (void)name; - (void)fc; - - return nullptr; -} - -IPluginV2DynamicExt * YoloLayerPluginCreator::deserializePlugin( - const char * name, const void * serialData, size_t serialLength) noexcept -{ - (void)name; - - return new YoloLayerPlugin(serialData, serialLength); -} -} // namespace yolo diff --git a/perception/tensorrt_yolo/lib/src/trt_yolo.cpp b/perception/tensorrt_yolo/lib/src/trt_yolo.cpp deleted file mode 100644 index c8793aa4c8512..0000000000000 --- a/perception/tensorrt_yolo/lib/src/trt_yolo.cpp +++ /dev/null @@ -1,346 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* - * Copyright (c) 2020, NVIDIA CORPORATION. All rights reserved. - * - * Permission is hereby granted, free of charge, to any person obtaining a - * copy of this software and associated documentation files (the "Software"), - * to deal in the Software without restriction, including without limitation - * the rights to use, copy, modify, merge, publish, distribute, sublicense, - * and/or sell copies of the Software, and to permit persons to whom the - * Software is furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL - * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING - * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER - * DEALINGS IN THE SOFTWARE. - */ - -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace yolo -{ -void Net::load(const std::string & path) -{ - std::ifstream file(path, std::ios::in | std::ios::binary); - file.seekg(0, file.end); - size_t size = file.tellg(); - file.seekg(0, file.beg); - - char * buffer = new char[size]; - file.read(buffer, size); - file.close(); - if (runtime_) { - engine_ = unique_ptr(runtime_->deserializeCudaEngine(buffer, size)); - } - delete[] buffer; -} - -bool Net::prepare() -{ - if (!engine_) { - return false; - } - context_ = unique_ptr(engine_->createExecutionContext()); - if (!context_) { - return false; - } - input_d_ = cuda::make_unique(getMaxBatchSize() * getInputSize()); - out_scores_d_ = cuda::make_unique(getMaxBatchSize() * getMaxDetections()); - out_boxes_d_ = cuda::make_unique(getMaxBatchSize() * getMaxDetections() * 4); - out_classes_d_ = cuda::make_unique(getMaxBatchSize() * getMaxDetections()); - cudaStreamCreate(&stream_); - return true; -} - -std::vector Net::preprocess( - const cv::Mat & in_img, const int c, const int w, const int h) const -{ - cv::Mat rgb; - cv::cvtColor(in_img, rgb, cv::COLOR_BGR2RGB); - - cv::resize(rgb, rgb, cv::Size(w, h)); - - cv::Mat img_float; - rgb.convertTo(img_float, CV_32FC3, 1 / 255.0); - - // HWC TO CHW - std::vector input_channels(c); - cv::split(img_float, input_channels); - - std::vector result(h * w * c); - auto data = result.data(); - int channel_length = h * w; - for (int i = 0; i < c; ++i) { - memcpy(data, input_channels[i].data, channel_length * sizeof(float)); - data += channel_length; - } - - return result; -} - -Net::Net(const std::string & path, bool verbose) : logger_(verbose) -{ - runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger_)); - load(path); - if (!prepare()) { - std::cout << "Fail to prepare engine" << std::endl; - return; - } -} - -Net::~Net() -{ - if (stream_) { - cudaStreamSynchronize(stream_); - cudaStreamDestroy(stream_); - } -} - -Net::Net( - const std::string & onnx_file_path, const std::string & precision, const int max_batch_size, - const Config & yolo_config, const std::vector & calibration_images, - const std::string & calibration_table, bool verbose, size_t workspace_size) -: logger_(verbose) -{ - runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger_)); - if (!runtime_) { - std::cout << "Fail to create runtime" << std::endl; - return; - } - bool fp16 = precision.compare("FP16") == 0; - bool int8 = precision.compare("INT8") == 0; - - // Create builder - auto builder = unique_ptr(nvinfer1::createInferBuilder(logger_)); - if (!builder) { - std::cout << "Fail to create builder" << std::endl; - return; - } - auto config = unique_ptr(builder->createBuilderConfig()); - if (!config) { - std::cout << "Fail to create builder config" << std::endl; - return; - } - // Allow use of FP16 layers when running in INT8 - if (fp16 || int8) { - config->setFlag(nvinfer1::BuilderFlag::kFP16); - } -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400 - config->setMemoryPoolLimit(nvinfer1::MemoryPoolType::kWORKSPACE, workspace_size); -#else - config->setMaxWorkspaceSize(workspace_size); -#endif - - // Parse ONNX FCN - tensorrt_common::LOG_INFO(logger_) << "Building " << precision << " core model..." << std::endl; - const auto flag = - 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); - auto network = unique_ptr(builder->createNetworkV2(flag)); - if (!network) { - tensorrt_common::LOG_ERROR(logger_) << "Fail to create network" << std::endl; - return; - } - auto parser = unique_ptr(nvonnxparser::createParser(*network, logger_)); - if (!parser) { - tensorrt_common::LOG_ERROR(logger_) << "Fail to create parser" << std::endl; - return; - } - - auto log_thread = logger_.log_throttle( - nvinfer1::ILogger::Severity::kINFO, - "Applying optimizations and building TRT CUDA engine. Please wait for a few minutes...", 5); - parser->parseFromFile( - onnx_file_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR)); - std::vector scores, boxes, classes; - const auto input = network->getInput(0); - const auto num_outputs = network->getNbOutputs(); - const auto input_dims = input->getDimensions(); - const auto input_channel = input_dims.d[1]; - const auto input_height = input_dims.d[2]; - const auto input_width = input_dims.d[3]; - for (int i = 0; i < num_outputs; ++i) { - auto output = network->getOutput(i); - std::vector anchor( - yolo_config.anchors.begin() + i * yolo_config.num_anchors * 2, - yolo_config.anchors.begin() + (i + 1) * yolo_config.num_anchors * 2); - auto yoloLayerPlugin = yolo::YoloLayerPlugin( - input_width, input_height, 3, anchor, yolo_config.scale_x_y[i], yolo_config.score_thresh, - yolo_config.use_darknet_layer); - std::vector inputs = {output}; - auto layer = network->addPluginV2(inputs.data(), inputs.size(), yoloLayerPlugin); - scores.push_back(layer->getOutput(0)); - boxes.push_back(layer->getOutput(1)); - classes.push_back(layer->getOutput(2)); - } - - // Cleanup outputs - for (int i = 0; i < num_outputs; i++) { - auto output = network->getOutput(0); - network->unmarkOutput(*output); - } - - // Concat tensors from each feature map - std::vector concat; - for (auto tensors : {scores, boxes, classes}) { - auto layer = network->addConcatenation(tensors.data(), tensors.size()); - layer->setAxis(1); - auto output = layer->getOutput(0); - concat.push_back(output); - } - - // Add NMS plugin - auto nmsPlugin = yolo::NMSPlugin(yolo_config.iou_thresh, yolo_config.detections_per_im); - auto layer = network->addPluginV2(concat.data(), concat.size(), nmsPlugin); - for (int i = 0; i < layer->getNbOutputs(); i++) { - auto output = layer->getOutput(i); - network->markOutput(*output); - } - - // create profile - auto profile = builder->createOptimizationProfile(); - profile->setDimensions( - network->getInput(0)->getName(), nvinfer1::OptProfileSelector::kMIN, - nvinfer1::Dims4{max_batch_size, input_channel, input_height, input_width}); - profile->setDimensions( - network->getInput(0)->getName(), nvinfer1::OptProfileSelector::kOPT, - nvinfer1::Dims4{max_batch_size, input_channel, input_height, input_width}); - profile->setDimensions( - network->getInput(0)->getName(), nvinfer1::OptProfileSelector::kMAX, - nvinfer1::Dims4{max_batch_size, input_channel, input_height, input_width}); - config->addOptimizationProfile(profile); - - std::unique_ptr calib{nullptr}; - if (int8) { - if (calibration_images.size() >= static_cast(max_batch_size)) { - config->setFlag(nvinfer1::BuilderFlag::kINT8); - yolo::ImageStream stream(max_batch_size, input_dims, calibration_images); - calib = std::make_unique(stream, calibration_table); - config->setInt8Calibrator(calib.get()); - } else { - tensorrt_common::LOG_WARN(logger_) - << "Fail to find enough images for INT8 calibration. Build FP mode..." << std::endl; - } - } - - // Build engine - plan_ = unique_ptr(builder->buildSerializedNetwork(*network, *config)); - if (!plan_) { - tensorrt_common::LOG_ERROR(logger_) << "Fail to create serialized network" << std::endl; - logger_.stop_throttle(log_thread); - return; - } - engine_ = unique_ptr( - runtime_->deserializeCudaEngine(plan_->data(), plan_->size())); - if (!prepare()) { - tensorrt_common::LOG_ERROR(logger_) << "Fail to create engine" << std::endl; - logger_.stop_throttle(log_thread); - return; - } - logger_.stop_throttle(log_thread); -} - -void Net::save(const std::string & path) const -{ - tensorrt_common::LOG_INFO(logger_) << "Writing to " << path << "..." << std::endl; - std::ofstream file(path, std::ios::out | std::ios::binary); - file.write(reinterpret_cast(plan_->data()), plan_->size()); -} - -void Net::infer(std::vector & buffers, const int batch_size) -{ - if (!context_) { - throw std::runtime_error("Fail to create context"); - } - auto input_dims = engine_->getBindingDimensions(0); - context_->setBindingDimensions( - 0, nvinfer1::Dims4(batch_size, input_dims.d[1], input_dims.d[2], input_dims.d[3])); - context_->enqueueV2(buffers.data(), stream_, nullptr); - cudaStreamSynchronize(stream_); -} - -bool Net::detect(const cv::Mat & in_img, float * out_scores, float * out_boxes, float * out_classes) -{ - const auto input_dims = getInputDims(); - const auto input = preprocess(in_img, input_dims.at(0), input_dims.at(2), input_dims.at(1)); - CHECK_CUDA_ERROR( - cudaMemcpy(input_d_.get(), input.data(), input.size() * sizeof(float), cudaMemcpyHostToDevice)); - std::vector buffers = { - input_d_.get(), out_scores_d_.get(), out_boxes_d_.get(), out_classes_d_.get()}; - try { - infer(buffers, 1); - } catch (const std::runtime_error & e) { - return false; - } - CHECK_CUDA_ERROR(cudaMemcpyAsync( - out_scores, out_scores_d_.get(), sizeof(float) * getMaxDetections(), cudaMemcpyDeviceToHost, - stream_)); - CHECK_CUDA_ERROR(cudaMemcpyAsync( - out_boxes, out_boxes_d_.get(), sizeof(float) * 4 * getMaxDetections(), cudaMemcpyDeviceToHost, - stream_)); - CHECK_CUDA_ERROR(cudaMemcpyAsync( - out_classes, out_classes_d_.get(), sizeof(float) * getMaxDetections(), cudaMemcpyDeviceToHost, - stream_)); - cudaStreamSynchronize(stream_); - return true; -} - -std::vector Net::getInputDims() const -{ - auto dims = engine_->getBindingDimensions(0); - return {dims.d[1], dims.d[2], dims.d[3]}; -} - -int Net::getMaxBatchSize() const -{ - return engine_->getProfileDimensions(0, 0, nvinfer1::OptProfileSelector::kMAX).d[0]; -} - -int Net::getInputSize() const -{ - const auto input_dims = getInputDims(); - const auto input_size = - std::accumulate(std::begin(input_dims), std::end(input_dims), 1, std::multiplies()); - return input_size; -} - -int Net::getMaxDetections() const -{ - return engine_->getBindingDimensions(1).d[1]; -} - -} // namespace yolo diff --git a/perception/tensorrt_yolo/package.xml b/perception/tensorrt_yolo/package.xml deleted file mode 100755 index 12d07647093b5..0000000000000 --- a/perception/tensorrt_yolo/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - tensorrt_yolo - 0.1.0 - The tensorrt_yolo package - Daisuke Nishimatsu - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - autoware_auto_perception_msgs - cv_bridge - image_transport - rclcpp - rclcpp_components - sensor_msgs - tensorrt_common - tier4_perception_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/perception/tensorrt_yolo/schema/tensortt_yolo.json b/perception/tensorrt_yolo/schema/tensortt_yolo.json deleted file mode 100644 index 0b4724078c994..0000000000000 --- a/perception/tensorrt_yolo/schema/tensortt_yolo.json +++ /dev/null @@ -1,108 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "Parameters for tensorrt_yolo", - "type": "object", - "definitions": { - "tensorrt_yolo": { - "type": "object", - "properties": { - "num_anchors": { - "type": "number", - "default": [ - 10.0, 13.0, 16.0, 30.0, 33.0, 23.0, 30.0, 61.0, 62.0, 45.0, 59.0, 119.0, 116.0, 90.0, - 156.0, 198.0, 373.0, 326.0 - ], - "description": "The anchors to create bounding box candidates." - }, - "scale_x_y": { - "type": "number", - "default": [1.0, 1.0, 1.0], - "description": "scale parameter to eliminate grid sensitivity." - }, - "score_thresh": { - "type": "number", - "default": 0.1, - "description": "If the objectness score is less than this value, the object is ignored in yolo layer." - }, - "iou_thresh": { - "type": "number", - "default": 0.45, - "description": "The iou threshold for NMS method." - }, - "detections_per_im": { - "type": "number", - "default": 100, - "description": " The maximum detection number for one frame." - }, - "use_darknet_layer": { - "type": "boolean", - "default": true, - "description": "The flag to use yolo layer in darknet." - }, - "ignore_thresh": { - "type": "number", - "default": 0.5, - "description": "If the output score is less than this value, this object is ignored." - }, - "onnx_file": { - "type": "string", - "description": "The onnx file name for yolo model." - }, - "engine_file": { - "type": "string", - "description": "The tensorrt engine file name for yolo model." - }, - "label_file": { - "type": "string", - "description": "The label file with label names for detected objects written on it." - }, - "calib_image_directory": { - "type": "string", - "description": "The directory name including calibration images for int8 inference." - }, - "calib_cache_file": { - "type": "string", - "description": "The calibration cache file for int8 inference." - }, - "mode": { - "type": "string", - "default": "FP32", - "description": "The inference mode: FP32, FP16, INT8." - }, - "gpu_id": { - "type": "number", - "default": 0, - "description": "GPU device ID that runs the model." - } - }, - "required": [ - "num_anchors", - "scale_x_y", - "score_thresh", - "iou_thresh", - "detections_per_im", - "use_darknet_layer", - "ignore_thresh", - "onnx_file", - "engine_file", - "label_file", - "calib_image_directory", - "calib_cache_file", - "mode", - "gpu_id" - ] - } - }, - "properties": { - "/**": { - "type": "object", - "properties": { - "ros__parameters": { - "$ref": "#/definitions/tensorrt_yolo" - } - }, - "required": ["ros__parameters"] - } - }, - "required": ["/**"] -} diff --git a/perception/tensorrt_yolo/src/nodelet.cpp b/perception/tensorrt_yolo/src/nodelet.cpp deleted file mode 100644 index fdcd8bf12db72..0000000000000 --- a/perception/tensorrt_yolo/src/nodelet.cpp +++ /dev/null @@ -1,219 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "tensorrt_yolo/nodelet.hpp" - -#include - -#include - -#include -#include -#include -#include -#include - -namespace -{ -std::vector getFilePath(const std::string & input_dir) -{ - glob_t glob_buffer; - std::vector files; - glob((input_dir + "*").c_str(), 0, NULL, &glob_buffer); - for (size_t i = 0; i < glob_buffer.gl_pathc; i++) { - files.push_back(glob_buffer.gl_pathv[i]); - } - globfree(&glob_buffer); - return files; -} -} // namespace -namespace object_recognition -{ -TensorrtYoloNodelet::TensorrtYoloNodelet(const rclcpp::NodeOptions & options) -: Node("tensorrt_yolo", options) -{ - using std::placeholders::_1; - - std::string onnx_file = declare_parameter("onnx_file", ""); - std::string engine_file = declare_parameter("engine_file", ""); - std::string label_file = declare_parameter("label_file", ""); - std::string calib_image_directory = declare_parameter("calib_image_directory", ""); - std::string calib_cache_file = declare_parameter("calib_cache_file", ""); - std::string mode = declare_parameter("mode"); - gpu_device_id_ = declare_parameter("gpu_id"); - yolo_config_.num_anchors = declare_parameter("num_anchors"); - auto anchors = declare_parameter( - "anchors", std::vector{ - 10, 13, 16, 30, 33, 23, 30, 61, 62, 45, 59, 119, 116, 90, 156, 198, 373, 326}); - std::vector anchors_float(anchors.begin(), anchors.end()); - yolo_config_.anchors = anchors_float; - auto scale_x_y = declare_parameter("scale_x_y", std::vector{1.0, 1.0, 1.0}); - std::vector scale_x_y_float(scale_x_y.begin(), scale_x_y.end()); - yolo_config_.scale_x_y = scale_x_y_float; - yolo_config_.score_thresh = declare_parameter("score_thresh"); - yolo_config_.iou_thresh = declare_parameter("iou_thresh"); - yolo_config_.detections_per_im = declare_parameter("detections_per_im"); - yolo_config_.use_darknet_layer = declare_parameter("use_darknet_layer"); - yolo_config_.ignore_thresh = declare_parameter("ignore_thresh"); - - if (!yolo::set_cuda_device(gpu_device_id_)) { - RCLCPP_ERROR(this->get_logger(), "Given GPU not exist or suitable"); - } - - if (!readLabelFile(label_file, &labels_)) { - RCLCPP_ERROR(this->get_logger(), "Could not find label file"); - } - std::ifstream fs(engine_file); - const auto calibration_images = getFilePath(calib_image_directory); - if (fs.is_open()) { - RCLCPP_INFO(this->get_logger(), "Found %s", engine_file.c_str()); - net_ptr_.reset(new yolo::Net(engine_file, false)); - if (net_ptr_->getMaxBatchSize() != 1) { - RCLCPP_INFO( - this->get_logger(), "Max batch size %d should be 1. Rebuild engine from file", - net_ptr_->getMaxBatchSize()); - net_ptr_.reset( - new yolo::Net(onnx_file, mode, 1, yolo_config_, calibration_images, calib_cache_file)); - net_ptr_->save(engine_file); - } - } else { - RCLCPP_INFO( - this->get_logger(), "Could not find %s, try making TensorRT engine from onnx", - engine_file.c_str()); - net_ptr_.reset( - new yolo::Net(onnx_file, mode, 1, yolo_config_, calibration_images, calib_cache_file)); - net_ptr_->save(engine_file); - } - RCLCPP_INFO(this->get_logger(), "Inference engine prepared."); - - using std::chrono_literals::operator""ms; - timer_ = rclcpp::create_timer( - this, get_clock(), 100ms, std::bind(&TensorrtYoloNodelet::connectCb, this)); - - std::lock_guard lock(connect_mutex_); - - objects_pub_ = this->create_publisher( - "out/objects", 1); - image_pub_ = image_transport::create_publisher(this, "out/image"); - - out_scores_ = - std::make_unique(net_ptr_->getMaxBatchSize() * net_ptr_->getMaxDetections()); - out_boxes_ = - std::make_unique(net_ptr_->getMaxBatchSize() * net_ptr_->getMaxDetections() * 4); - out_classes_ = - std::make_unique(net_ptr_->getMaxBatchSize() * net_ptr_->getMaxDetections()); -} - -void TensorrtYoloNodelet::connectCb() -{ - using std::placeholders::_1; - std::lock_guard lock(connect_mutex_); - if (objects_pub_->get_subscription_count() == 0 && image_pub_.getNumSubscribers() == 0) { - image_sub_.shutdown(); - } else if (!image_sub_) { - image_sub_ = image_transport::create_subscription( - this, "in/image", std::bind(&TensorrtYoloNodelet::callback, this, _1), "raw", - rmw_qos_profile_sensor_data); - } -} - -void TensorrtYoloNodelet::callback(const sensor_msgs::msg::Image::ConstSharedPtr in_image_msg) -{ - using Label = autoware_auto_perception_msgs::msg::ObjectClassification; - - tier4_perception_msgs::msg::DetectedObjectsWithFeature out_objects; - - if (!yolo::set_cuda_device(gpu_device_id_)) { - RCLCPP_ERROR(this->get_logger(), "Given GPU not exist or suitable"); - return; - } - - cv_bridge::CvImagePtr in_image_ptr; - try { - in_image_ptr = cv_bridge::toCvCopy(in_image_msg, sensor_msgs::image_encodings::BGR8); - } catch (cv_bridge::Exception & e) { - RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); - return; - } - if (!net_ptr_->detect( - in_image_ptr->image, out_scores_.get(), out_boxes_.get(), out_classes_.get())) { - RCLCPP_WARN(this->get_logger(), "Fail to inference"); - return; - } - const auto width = in_image_ptr->image.cols; - const auto height = in_image_ptr->image.rows; - for (int i = 0; i < yolo_config_.detections_per_im; ++i) { - if (out_scores_[i] < yolo_config_.ignore_thresh) { - break; - } - tier4_perception_msgs::msg::DetectedObjectWithFeature object; - object.feature.roi.x_offset = out_boxes_[4 * i] * width; - object.feature.roi.y_offset = out_boxes_[4 * i + 1] * height; - object.feature.roi.width = out_boxes_[4 * i + 2] * width; - object.feature.roi.height = out_boxes_[4 * i + 3] * height; - object.object.classification.emplace_back(autoware_auto_perception_msgs::build