diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS
index 7a54fa35a31fb..90f0aa3ef40fe 100644
--- a/.github/CODEOWNERS
+++ b/.github/CODEOWNERS
@@ -1,160 +1,160 @@
### Copied from .github/CODEOWNERS-manual ###
### Automatically generated from package.xml ###
-control/external_cmd_selector/** kenji.miyake@tier4.jp
-control/obstacle_collision_checker/** kenji.miyake@tier4.jp
-control/vehicle_cmd_gate/** takamasa.horibe@tier4.jp
-control/trajectory_follower/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
-control/trajectory_follower_nodes/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
-control/joy_controller/** kenji.miyake@tier4.jp
-control/lane_departure_checker/** kyoichi.sugahara@tier4.jp
-control/shift_decider/** takamasa.horibe@tier4.jp
-control/pure_pursuit/** takamasa.horibe@tier4.jp
-control/operation_mode_transition_manager/** takamasa.horibe@tier4.jp
-control/control_performance_analysis/** ali.boyali@tier4.jp berkay@leodrive.ai
-common/autoware_ad_api_msgs/** isamu.takagi@tier4.jp
-common/motion_common/** christopherj.ho@gmail.com
-common/component_interface_specs/** isamu.takagi@tier4.jp
-common/motion_testing/** christopherj.ho@gmail.com
+common/autoware_ad_api_specs/** isamu.takagi@tier4.jp
+common/autoware_auto_common/** opensource@apex.ai
+common/autoware_auto_geometry/** opensource@apex.ai
common/autoware_auto_perception_rviz_plugin/** opensource@apex.ai taichi.higashide@tier4.jp
common/autoware_auto_tf2/** jit.ray.c@gmail.com
-common/signal_processing/** takayuki.murooka@tier4.jp
-common/tier4_perception_rviz_plugin/** yukihiro.saito@tier4.jp
-common/path_distance_calculator/** isamu.takagi@tier4.jp
-common/kalman_filter/** yukihiro.saito@tier4.jp
-common/tier4_traffic_light_rviz_plugin/** satoshi.ota@tier4.jp
-common/vehicle_constants_manager/** mfc@leodrive.ai
+common/autoware_point_types/** taichi.higashide@tier4.jp
+common/autoware_testing/** adam.dabrowski@robotec.ai
+common/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp
+common/component_interface_specs/** isamu.takagi@tier4.jp
common/component_interface_utils/** isamu.takagi@tier4.jp
-common/tier4_planning_rviz_plugin/** yukihiro.saito@tier4.jp
-common/perception_utils/** takayuki.murooka@tier4.jp
-common/web_controller/** yukihiro.saito@tier4.jp
+common/fake_test_node/** opensource@apex.ai
+common/global_parameter_loader/** kenji.miyake@tier4.jp
common/goal_distance_calculator/** taiki.tanaka@tier4.jp
-common/motion_utils/** yutaka.shimizu@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp
+common/grid_map_utils/** maxime.clement@tier4.jp
+common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp
+common/kalman_filter/** yukihiro.saito@tier4.jp
+common/motion_common/** christopherj.ho@gmail.com
+common/motion_testing/** christopherj.ho@gmail.com
+common/motion_utils/** yutaka.shimizu@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp taiki.tanaka@tier4.jp
+common/neural_networks_provider/** ambroise.vincent@arm.com
+common/osqp_interface/** maxime.clement@tier4.jp
+common/path_distance_calculator/** isamu.takagi@tier4.jp
+common/perception_utils/** takayuki.murooka@tier4.jp
+common/polar_grid/** yukihiro.saito@tier4.jp
+common/signal_processing/** takayuki.murooka@tier4.jp
+common/tier4_api_utils/** isamu.takagi@tier4.jp
+common/tier4_autoware_utils/** takamasa.horibe@tier4.jp kenji.miyake@tier4.jp
+common/tier4_calibration_rviz_plugin/** tomoya.kimura@tier4.jp
+common/tier4_control_rviz_plugin/** taiki.tanaka@tier4.jp
+common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp
common/tier4_debug_tools/** kenji.miyake@tier4.jp
common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp
-common/autoware_point_types/** taichi.higashide@tier4.jp
+common/tier4_perception_rviz_plugin/** yukihiro.saito@tier4.jp
+common/tier4_planning_rviz_plugin/** yukihiro.saito@tier4.jp
common/tier4_screen_capture_rviz_plugin/** taiki.tanaka@tier4.jp
-common/tier4_control_rviz_plugin/** taiki.tanaka@tier4.jp
-common/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp
-common/osqp_interface/** maxime.clement@tier4.jp
common/tier4_simulated_clock_rviz_plugin/** maxime.clement@tier4.jp
-common/neural_networks_provider/** ambroise.vincent@arm.com
-common/global_parameter_loader/** kenji.miyake@tier4.jp
-common/autoware_auto_common/** opensource@apex.ai
-common/tvm_utility/** ambroise.vincent@arm.com
-common/autoware_auto_geometry/** opensource@apex.ai
-common/autoware_ad_api_specs/** isamu.takagi@tier4.jp
common/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp
-common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp
-common/grid_map_utils/** maxime.clement@tier4.jp
-common/tier4_autoware_utils/** takamasa.horibe@tier4.jp kenji.miyake@tier4.jp
-common/fake_test_node/** opensource@apex.ai
-common/autoware_testing/** adam.dabrowski@robotec.ai
-common/tier4_api_utils/** isamu.takagi@tier4.jp
-common/tier4_calibration_rviz_plugin/** tomoya.kimura@tier4.jp
-common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp
-common/time_utils/** christopherj.ho@gmail.com
-common/polar_grid/** yukihiro.saito@tier4.jp
+common/tier4_traffic_light_rviz_plugin/** satoshi.ota@tier4.jp
common/tier4_vehicle_rviz_plugin/** yukihiro.saito@tier4.jp
-launch/tier4_vehicle_launch/** yukihiro.saito@tier4.jp
-launch/tier4_system_launch/** kenji.miyake@tier4.jp
-launch/tier4_map_launch/** ryohsuke.mitsudome@tier4.jp
-launch/tier4_control_launch/** takamasa.horibe@tier4.jp
-launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp
+common/time_utils/** christopherj.ho@gmail.com
+common/trtexec_vendor/** daisuke.nishimatsu@tier4.jp
+common/tvm_utility/** ambroise.vincent@arm.com
+common/vehicle_constants_manager/** mfc@leodrive.ai
+common/web_controller/** yukihiro.saito@tier4.jp
+control/control_performance_analysis/** ali.boyali@tier4.jp berkay@leodrive.ai
+control/external_cmd_selector/** kenji.miyake@tier4.jp
+control/joy_controller/** kenji.miyake@tier4.jp
+control/lane_departure_checker/** kyoichi.sugahara@tier4.jp
+control/obstacle_collision_checker/** kenji.miyake@tier4.jp
+control/operation_mode_transition_manager/** takamasa.horibe@tier4.jp
+control/pure_pursuit/** takamasa.horibe@tier4.jp
+control/shift_decider/** takamasa.horibe@tier4.jp
+control/trajectory_follower/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
+control/trajectory_follower_nodes/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
+control/vehicle_cmd_gate/** takamasa.horibe@tier4.jp
+evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai
+evaluator/localization_evaluator/** dominik.jargot@robotec.ai
launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp
+launch/tier4_control_launch/** takamasa.horibe@tier4.jp
+launch/tier4_localization_launch/** yamato.ando@tier4.jp
+launch/tier4_map_launch/** ryohsuke.mitsudome@tier4.jp
launch/tier4_perception_launch/** yukihiro.saito@tier4.jp
launch/tier4_planning_launch/** takamasa.horibe@tier4.jp
-launch/tier4_localization_launch/** yamato.ando@tier4.jp
+launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp
launch/tier4_simulator_launch/** keisuke.shima@tier4.jp
-evaluator/localization_evaluator/** dominik.jargot@robotec.ai
-evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai
+launch/tier4_system_launch/** kenji.miyake@tier4.jp fumihito.ito@tier4.jp
+launch/tier4_vehicle_launch/** yukihiro.saito@tier4.jp
+localization/ekf_localizer/** takamasa.horibe@tier4.jp koji.minoda@tier4.jp yamato.ando@tier4.jp
+localization/gyro_odometer/** yamato.ando@tier4.jp
+localization/initial_pose_button_panel/** yamato.ando@tier4.jp
+localization/localization_error_monitor/** yamato.ando@tier4.jp
+localization/ndt/** yamato.ando@tier4.jp
+localization/ndt_pcl_modified/** yamato.ando@tier4.jp
+localization/ndt_scan_matcher/** yamato.ando@tier4.jp
+localization/pose2twist/** yamato.ando@tier4.jp
+localization/pose_initializer/** yamato.ando@tier4.jp isamu.takagi@tier4.jp
+localization/stop_filter/** koji.minoda@tier4.jp yamato.ando@tier4.jp
+localization/twist2accel/** koji.minoda@tier4.jp yamato.ando@tier4.jp
map/map_loader/** ryohsuke.mitsudome@tier4.jp kenji.miyake@tier4.jp
map/map_tf_generator/** azumi.suzuki@tier4.jp
map/util/lanelet2_map_preprocessor/** ryohsuke.mitsudome@tier4.jp
-simulator/simple_planning_simulator/** takamasa.horibe@tier4.jp
-simulator/dummy_perception_publisher/** yukihiro.saito@tier4.jp
-simulator/fault_injection/** keisuke.shima@tier4.jp
-tools/simulator_test/simulator_compatibility_test/** shpark@morai.ai
-sensing/livox/livox_tag_filter/** kenji.miyake@tier4.jp
-sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp
-sensing/probabilistic_occupancy_grid_map/** yukihiro.saito@tier4.jp
-sensing/imu_corrector/** yamato.ando@tier4.jp
-sensing/geo_pos_conv/** yamato.ando@tier4.jp
-sensing/image_transport_decompressor/** yukihiro.saito@tier4.jp
-sensing/vehicle_velocity_converter/** ryu.yamamoto@tier4.jp
-sensing/image_diagnostics/** dai.nguyen@tier4.jp
-sensing/tier4_pcl_extensions/** ryu.yamamoto@tier4.jp
-sensing/gnss_poser/** yamato.ando@tier4.jp
-planning/freespace_planner/** takamasa.horibe@tier4.jp
-planning/obstacle_cruise_planner/** takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp
-planning/planning_error_monitor/** yutaka.shimizu@tier4.jp
-planning/surround_obstacle_checker/** satoshi.ota@tier4.jp
-planning/costmap_generator/** kenji.miyake@tier4.jp
-planning/behavior_path_planner/** zulfaqar.azmi@tier4.jp kosuke.takeuchi@tier4.jp kosuke.takeuchi@tier4.jp fumiya.watanabe@tier4.jp takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp takayuki.murooka@tier4.jp
-planning/obstacle_stop_planner/** satoshi.ota@tier4.jp
-planning/behavior_velocity_planner/** mamoru.sobue@tier4.jp satoshi.ota@tier4.jp kyoichi.sugahara@tier4.jp taiki.tanaka@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp mamoru.sobue@tier4.jp yutaka.shimizu@tier4.jp kosuke.takeuchi@tier4.jp tomohito.ando@tier4.jp
-planning/rtc_auto_approver/** fumiya.watanabe@tier4.jp
-planning/scenario_selector/** kenji.miyake@tier4.jp
-planning/planning_evaluator/** maxime.clement@tier4.jp
-planning/rtc_interface/** fumiya.watanabe@tier4.jp
-planning/planning_debug_tools/** takamasa.horibe@tier4.jp taiki.tanaka@tier4.jp
-planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp
-planning/freespace_planning_algorithms/** takamasa.horibe@tier4.jp
-planning/obstacle_avoidance_planner/** takayuki.murooka@tier4.jp
-planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp
-planning/route_handler/** fumiya.watanabe@tier4.jp
-planning/mission_planner/** ryohsuke.mitsudome@tier4.jp
-perception/shape_estimation/** yukihiro.saito@tier4.jp
-perception/front_vehicle_velocity_estimator/** satoshi.tanaka@tier4.jp
-perception/traffic_light_visualization/** yukihiro.saito@tier4.jp
-perception/detection_by_tracker/** yukihiro.saito@tier4.jp
-perception/object_range_splitter/** yukihiro.saito@tier4.jp
-perception/heatmap_visualizer/** kotaro.uetake@tier4.jp
-perception/detected_object_feature_remover/** tomoya.kimura@tier4.jp
+perception/compare_map_segmentation/** abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp
perception/crosswalk_traffic_light_estimator/** satoshi.ota@tier4.jp
+perception/detected_object_feature_remover/** tomoya.kimura@tier4.jp
+perception/detected_object_validation/** yukihiro.saito@tier4.jp
+perception/detection_by_tracker/** 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
+perception/ground_segmentation/** abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp
+perception/heatmap_visualizer/** kotaro.uetake@tier4.jp
perception/image_projection_based_fusion/** yukihiro.saito@tier4.jp yusuke.muramatsu@tier4.jp
-perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp
+perception/lidar_apollo_instance_segmentation/** yukihiro.saito@tier4.jp
perception/lidar_centerpoint/** yusuke.muramatsu@tier4.jp
perception/map_based_prediction/** yutaka.shimizu@tier4.jp
perception/multi_object_tracker/** yukihiro.saito@tier4.jp jilada.eccleston@tier4.jp
perception/object_merger/** yukihiro.saito@tier4.jp
-perception/elevation_map_loader/** kosuke.takeuchi@tier4.jp taichi.higashide@tier4.jp
-perception/traffic_light_map_based_detector/** yukihiro.saito@tier4.jp
-perception/tensorrt_yolo/** daisuke.nishimatsu@tier4.jp
-perception/lidar_apollo_instance_segmentation/** yukihiro.saito@tier4.jp
+perception/object_range_splitter/** yukihiro.saito@tier4.jp
+perception/occupancy_grid_map_outlier_filter/** abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp
perception/radar_fusion_to_detected_object/** satoshi.tanaka@tier4.jp
perception/radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp
+perception/shape_estimation/** yukihiro.saito@tier4.jp
+perception/tensorrt_yolo/** daisuke.nishimatsu@tier4.jp
perception/traffic_light_classifier/** yukihiro.saito@tier4.jp
-perception/detected_object_validation/** yukihiro.saito@tier4.jp
-perception/ground_segmentation/** abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp
-perception/occupancy_grid_map_outlier_filter/** abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp
-perception/compare_map_segmentation/** abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp
-vehicle/steer_offset_estimator/** taiki.tanaka@tier4.jp
-vehicle/raw_vehicle_cmd_converter/** takamasa.horibe@tier4.jp
-vehicle/vehicle_info_util/** yamato.ando@tier4.jp
-vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/** tomoya.kimura@tier4.jp
-vehicle/external_cmd_converter/** takamasa.horibe@tier4.jp
-localization/pose_initializer/** yamato.ando@tier4.jp
-localization/ndt_pcl_modified/** yamato.ando@tier4.jp
-localization/pose2twist/** yamato.ando@tier4.jp
-localization/ndt_scan_matcher/** yamato.ando@tier4.jp
-localization/gyro_odometer/** yamato.ando@tier4.jp
-localization/localization_error_monitor/** yamato.ando@tier4.jp
-localization/initial_pose_button_panel/** yamato.ando@tier4.jp
-localization/twist2accel/** koji.minoda@tier4.jp yamato.ando@tier4.jp
-localization/ndt/** yamato.ando@tier4.jp
-localization/ekf_localizer/** takamasa.horibe@tier4.jp koji.minoda@tier4.jp yamato.ando@tier4.jp
-localization/stop_filter/** koji.minoda@tier4.jp yamato.ando@tier4.jp
+perception/traffic_light_map_based_detector/** yukihiro.saito@tier4.jp
+perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp
+perception/traffic_light_visualization/** yukihiro.saito@tier4.jp
+planning/behavior_path_planner/** zulfaqar.azmi@tier4.jp kosuke.takeuchi@tier4.jp kosuke.takeuchi@tier4.jp fumiya.watanabe@tier4.jp takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp takayuki.murooka@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp shumpei.wakabayashi@tier4.jp
+planning/behavior_velocity_planner/** mamoru.sobue@tier4.jp satoshi.ota@tier4.jp kyoichi.sugahara@tier4.jp taiki.tanaka@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp mamoru.sobue@tier4.jp yutaka.shimizu@tier4.jp kosuke.takeuchi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp shumpei.wakabayashi@tier4.jp
+planning/costmap_generator/** kenji.miyake@tier4.jp
+planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp
+planning/freespace_planner/** takamasa.horibe@tier4.jp
+planning/freespace_planning_algorithms/** takamasa.horibe@tier4.jp
+planning/mission_planner/** ryohsuke.mitsudome@tier4.jp
+planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp
+planning/obstacle_avoidance_planner/** takayuki.murooka@tier4.jp
+planning/obstacle_cruise_planner/** takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp
+planning/obstacle_stop_planner/** satoshi.ota@tier4.jp
+planning/planning_debug_tools/** takamasa.horibe@tier4.jp taiki.tanaka@tier4.jp
+planning/planning_error_monitor/** yutaka.shimizu@tier4.jp
+planning/planning_evaluator/** maxime.clement@tier4.jp
+planning/route_handler/** fumiya.watanabe@tier4.jp
+planning/rtc_auto_mode_manager/** fumiya.watanabe@tier4.jp
+planning/rtc_interface/** fumiya.watanabe@tier4.jp
+planning/scenario_selector/** kenji.miyake@tier4.jp
+planning/surround_obstacle_checker/** satoshi.ota@tier4.jp
+sensing/geo_pos_conv/** yamato.ando@tier4.jp
+sensing/gnss_poser/** yamato.ando@tier4.jp
+sensing/image_diagnostics/** dai.nguyen@tier4.jp
+sensing/image_transport_decompressor/** yukihiro.saito@tier4.jp
+sensing/imu_corrector/** yamato.ando@tier4.jp
+sensing/livox/livox_tag_filter/** kenji.miyake@tier4.jp
+sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp
+sensing/probabilistic_occupancy_grid_map/** yukihiro.saito@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
+simulator/fault_injection/** keisuke.shima@tier4.jp
+simulator/simple_planning_simulator/** takamasa.horibe@tier4.jp
+system/ad_service_state_monitor/** kenji.miyake@tier4.jp
+system/bluetooth_monitor/** fumihito.ito@tier4.jp
+system/default_ad_api/** isamu.takagi@tier4.jp
system/default_ad_api_helpers/ad_api_adaptors/** isamu.takagi@tier4.jp
system/default_ad_api_helpers/automatic_pose_initializer/** isamu.takagi@tier4.jp
-system/velodyne_monitor/** fumihito.ito@tier4.jp
+system/dummy_diag_publisher/** kenji.miyake@tier4.jp
system/dummy_infrastructure/** kenji.miyake@tier4.jp
-system/topic_state_monitor/** kenji.miyake@tier4.jp
-system/bluetooth_monitor/** fumihito.ito@tier4.jp
-system/ad_service_state_monitor/** kenji.miyake@tier4.jp
-system/system_monitor/** fumihito.ito@tier4.jp
system/emergency_handler/** kenji.miyake@tier4.jp
-system/dummy_diag_publisher/** kenji.miyake@tier4.jp
-system/system_error_monitor/** kenji.miyake@tier4.jp
-system/default_ad_api/** isamu.takagi@tier4.jp
+system/system_error_monitor/** kenji.miyake@tier4.jp fumihito.ito@tier4.jp
+system/system_monitor/** fumihito.ito@tier4.jp
+system/topic_state_monitor/** kenji.miyake@tier4.jp
+system/velodyne_monitor/** fumihito.ito@tier4.jp
+tools/simulator_test/simulator_compatibility_test/** shpark@morai.ai
+vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/** tomoya.kimura@tier4.jp
+vehicle/external_cmd_converter/** takamasa.horibe@tier4.jp
+vehicle/raw_vehicle_cmd_converter/** takamasa.horibe@tier4.jp
+vehicle/steer_offset_estimator/** taiki.tanaka@tier4.jp
+vehicle/vehicle_info_util/** yamato.ando@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp shumpei.wakabayashi@tier4.jp
diff --git a/.github/PULL_REQUEST_TEMPLATE/standard-change.md b/.github/PULL_REQUEST_TEMPLATE/standard-change.md
index 2df18b2bd013e..cfdf7101b5afd 100644
--- a/.github/PULL_REQUEST_TEMPLATE/standard-change.md
+++ b/.github/PULL_REQUEST_TEMPLATE/standard-change.md
@@ -4,7 +4,7 @@
## Related links
-
+
## Tests performed
diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml
index 248dbb09b61a4..f8db906263843 100644
--- a/.github/workflows/build-and-test-differential.yaml
+++ b/.github/workflows/build-and-test-differential.yaml
@@ -80,7 +80,7 @@ jobs:
- name: Get modified files
id: get-modified-files
- uses: tj-actions/changed-files@v28
+ uses: tj-actions/changed-files@v29
with:
files: |
**/*.cpp
diff --git a/build_depends.humble.repos b/build_depends.humble.repos
index 7c8f1df6c523c..5e8ce5953172a 100644
--- a/build_depends.humble.repos
+++ b/build_depends.humble.repos
@@ -12,6 +12,10 @@ repositories:
type: git
url: https://github.com/autowarefoundation/autoware_msgs.git
version: main
+ core/autoware_adapi_msgs:
+ type: git
+ url: https://github.com/autowarefoundation/autoware_adapi_msgs.git
+ version: main
core/external/autoware_auto_msgs:
type: git
url: https://github.com/tier4/autoware_auto_msgs.git
diff --git a/build_depends.repos b/build_depends.repos
index b510096aef881..2b47af3ef8e81 100644
--- a/build_depends.repos
+++ b/build_depends.repos
@@ -12,6 +12,10 @@ repositories:
type: git
url: https://github.com/autowarefoundation/autoware_msgs.git
version: main
+ core/autoware_adapi_msgs:
+ type: git
+ url: https://github.com/autowarefoundation/autoware_adapi_msgs.git
+ version: main
core/external/autoware_auto_msgs:
type: git
url: https://github.com/tier4/autoware_auto_msgs.git
diff --git a/common/autoware_ad_api_msgs/CMakeLists.txt b/common/autoware_ad_api_msgs/CMakeLists.txt
deleted file mode 100644
index 2cb112502f676..0000000000000
--- a/common/autoware_ad_api_msgs/CMakeLists.txt
+++ /dev/null
@@ -1,26 +0,0 @@
-cmake_minimum_required(VERSION 3.14)
-project(autoware_ad_api_msgs)
-
-find_package(autoware_cmake REQUIRED)
-autoware_package()
-
-rosidl_generate_interfaces(${PROJECT_NAME}
- common/msg/ResponseStatus.msg
- interface/srv/InterfaceVersion.srv
- localization/msg/LocalizationInitializationState.msg
- localization/srv/InitializeLocalization.srv
- routing/msg/RouteState.msg
- routing/msg/Route.msg
- routing/msg/RouteData.msg
- routing/msg/RoutePrimitive.msg
- routing/msg/RouteSegment.msg
- routing/srv/ClearRoute.srv
- routing/srv/SetRoute.srv
- routing/srv/SetRoutePoints.srv
- DEPENDENCIES
- builtin_interfaces
- std_msgs
- geometry_msgs
-)
-
-ament_auto_package()
diff --git a/common/autoware_ad_api_msgs/README.md b/common/autoware_ad_api_msgs/README.md
deleted file mode 100644
index a596e8c5ba8b4..0000000000000
--- a/common/autoware_ad_api_msgs/README.md
+++ /dev/null
@@ -1,39 +0,0 @@
-# autoware_ad_api_msgs
-
-## ResponseStatus
-
-This message is a response status commonly used in the service type API. Each API can define its own status codes.
-The status codes are primarily used to indicate the error cause, such as invalid parameter and timeout.
-If the API succeeds, set success to true, code to zero, and message to the empty string.
-Alternatively, codes and messages can be used for warnings or additional information.
-If the API fails, set success to false, code to the related status code, and message to the information.
-The status code zero is reserved for success. The status code 50000 or over are also reserved for typical cases.
-
-| Name | Code | Description |
-| ---------- | ----: | ------------------------------------ |
-| SUCCESS | 0 | This API has completed successfully. |
-| DEPRECATED | 50000 | This API is deprecated. |
-
-## InterfaceVersion
-
-Considering the product life cycle, there may be multiple vehicles using different versions of the AD API due to changes in requirements or functional improvements. For example, one vehicle uses `v1` for stability and another vehicle uses `v2` to enable more advanced functionality.
-
-In that situation, the AD API users such as developers of a web service have to switch the application behavior based on the version that each vehicle uses.
-The version of AD API follows [Semantic Versioning][semver] in order to provide an intuitive understanding of the changes between versions.
-
-## Routing
-
-The routing service support two formats. One uses pose and the other uses map dependent data directly.
-The body part of the route message is optional, since the route does not exist when it is cleared by the service.
-[See also routing API][api-routing].
-
-## Localization
-
-The initialization state does not reflect localization errors. Use diagnostics for that purpose.
-[See also localization API][api-localization].
-
-
-
-[semver]: https://semver.org/
-[api-localization]: https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/localization/
-[api-routing]: https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/routing/
diff --git a/common/autoware_ad_api_msgs/common/msg/ResponseStatus.msg b/common/autoware_ad_api_msgs/common/msg/ResponseStatus.msg
deleted file mode 100644
index 4fe0e5d4208e3..0000000000000
--- a/common/autoware_ad_api_msgs/common/msg/ResponseStatus.msg
+++ /dev/null
@@ -1,10 +0,0 @@
-# constants for code
-uint16 DEPRECATED = 50000
-uint16 SERVICE_UNREADY = 50001
-uint16 SERVICE_TIMEOUT = 50002
-uint16 TRANSFORM_ERROR = 50003
-
-# variables
-bool success
-uint16 code
-string message
diff --git a/common/autoware_ad_api_msgs/interface/srv/InterfaceVersion.srv b/common/autoware_ad_api_msgs/interface/srv/InterfaceVersion.srv
deleted file mode 100644
index 93ba2680f1859..0000000000000
--- a/common/autoware_ad_api_msgs/interface/srv/InterfaceVersion.srv
+++ /dev/null
@@ -1,4 +0,0 @@
----
-uint16 major
-uint16 minor
-uint16 patch
diff --git a/common/autoware_ad_api_msgs/localization/msg/LocalizationInitializationState.msg b/common/autoware_ad_api_msgs/localization/msg/LocalizationInitializationState.msg
deleted file mode 100644
index 8a4d790dbc321..0000000000000
--- a/common/autoware_ad_api_msgs/localization/msg/LocalizationInitializationState.msg
+++ /dev/null
@@ -1,7 +0,0 @@
-uint16 UNKNOWN = 0
-uint16 UNINITIALIZED = 1
-uint16 INITIALIZING = 2
-uint16 INITIALIZED = 3
-
-builtin_interfaces/Time stamp
-uint16 state
diff --git a/common/autoware_ad_api_msgs/localization/srv/InitializeLocalization.srv b/common/autoware_ad_api_msgs/localization/srv/InitializeLocalization.srv
deleted file mode 100644
index d3ffaece7789a..0000000000000
--- a/common/autoware_ad_api_msgs/localization/srv/InitializeLocalization.srv
+++ /dev/null
@@ -1,7 +0,0 @@
-geometry_msgs/PoseWithCovarianceStamped[<=1] pose
----
-uint16 ERROR_UNSAFE = 1
-uint16 ERROR_GNSS_SUPPORT = 2
-uint16 ERROR_GNSS = 3
-uint16 ERROR_ESTIMATION = 4
-autoware_ad_api_msgs/ResponseStatus status
diff --git a/common/autoware_ad_api_msgs/package.xml b/common/autoware_ad_api_msgs/package.xml
deleted file mode 100644
index db10bd2cdb877..0000000000000
--- a/common/autoware_ad_api_msgs/package.xml
+++ /dev/null
@@ -1,30 +0,0 @@
-
-
-
- autoware_ad_api_msgs
- 0.0.0
- The autoware_ad_api_msgs package
- Takagi, Isamu
- Apache License 2.0
-
- ament_cmake_auto
-
- autoware_cmake
- builtin_interfaces
- rosidl_default_generators
-
- geometry_msgs
- std_msgs
-
- builtin_interfaces
- rosidl_default_runtime
-
- ament_lint_auto
- autoware_lint_common
-
- rosidl_interface_packages
-
-
- ament_cmake
-
-
diff --git a/common/autoware_ad_api_msgs/routing/msg/Route.msg b/common/autoware_ad_api_msgs/routing/msg/Route.msg
deleted file mode 100644
index 3e6ff3a06e18b..0000000000000
--- a/common/autoware_ad_api_msgs/routing/msg/Route.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-std_msgs/Header header
-autoware_ad_api_msgs/RouteData[<=1] data
diff --git a/common/autoware_ad_api_msgs/routing/msg/RouteData.msg b/common/autoware_ad_api_msgs/routing/msg/RouteData.msg
deleted file mode 100644
index e5d03efb14008..0000000000000
--- a/common/autoware_ad_api_msgs/routing/msg/RouteData.msg
+++ /dev/null
@@ -1,3 +0,0 @@
-geometry_msgs/Pose start
-geometry_msgs/Pose goal
-autoware_ad_api_msgs/RouteSegment[] segments
diff --git a/common/autoware_ad_api_msgs/routing/msg/RoutePrimitive.msg b/common/autoware_ad_api_msgs/routing/msg/RoutePrimitive.msg
deleted file mode 100644
index f7d9312611e51..0000000000000
--- a/common/autoware_ad_api_msgs/routing/msg/RoutePrimitive.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-int64 id
-string type # The same id may be used for each type.
diff --git a/common/autoware_ad_api_msgs/routing/msg/RouteSegment.msg b/common/autoware_ad_api_msgs/routing/msg/RouteSegment.msg
deleted file mode 100644
index a3f3d12d3872d..0000000000000
--- a/common/autoware_ad_api_msgs/routing/msg/RouteSegment.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-autoware_ad_api_msgs/RoutePrimitive preferred
-autoware_ad_api_msgs/RoutePrimitive[] alternatives # Does not include the preferred primitive.
diff --git a/common/autoware_ad_api_msgs/routing/msg/RouteState.msg b/common/autoware_ad_api_msgs/routing/msg/RouteState.msg
deleted file mode 100644
index 9dfe77ddee175..0000000000000
--- a/common/autoware_ad_api_msgs/routing/msg/RouteState.msg
+++ /dev/null
@@ -1,8 +0,0 @@
-uint16 UNKNOWN = 0
-uint16 UNSET = 1
-uint16 SET = 2
-uint16 ARRIVED = 3
-uint16 CHANGING = 4
-
-builtin_interfaces/Time stamp
-uint16 state
diff --git a/common/autoware_ad_api_msgs/routing/srv/ClearRoute.srv b/common/autoware_ad_api_msgs/routing/srv/ClearRoute.srv
deleted file mode 100644
index 38a3199e9d091..0000000000000
--- a/common/autoware_ad_api_msgs/routing/srv/ClearRoute.srv
+++ /dev/null
@@ -1,2 +0,0 @@
----
-autoware_ad_api_msgs/ResponseStatus status
diff --git a/common/autoware_ad_api_msgs/routing/srv/SetRoute.srv b/common/autoware_ad_api_msgs/routing/srv/SetRoute.srv
deleted file mode 100644
index 7299b12fe8141..0000000000000
--- a/common/autoware_ad_api_msgs/routing/srv/SetRoute.srv
+++ /dev/null
@@ -1,6 +0,0 @@
-std_msgs/Header header
-geometry_msgs/Pose goal
-autoware_ad_api_msgs/RouteSegment[] segments
----
-uint16 ERROR_ROUTE_EXISTS = 1
-autoware_ad_api_msgs/ResponseStatus status
diff --git a/common/autoware_ad_api_msgs/routing/srv/SetRoutePoints.srv b/common/autoware_ad_api_msgs/routing/srv/SetRoutePoints.srv
deleted file mode 100644
index c05a211fae4fb..0000000000000
--- a/common/autoware_ad_api_msgs/routing/srv/SetRoutePoints.srv
+++ /dev/null
@@ -1,8 +0,0 @@
-std_msgs/Header header
-geometry_msgs/Pose goal
-geometry_msgs/Pose[] waypoints
----
-uint16 ERROR_ROUTE_EXISTS = 1
-uint16 ERROR_PLANNER_UNREADY = 2
-uint16 ERROR_PLANNER_FAILED = 3
-autoware_ad_api_msgs/ResponseStatus status
diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/interface.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/interface.hpp
index a304bc32da20c..95dca9671e768 100644
--- a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/interface.hpp
+++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/interface.hpp
@@ -15,14 +15,14 @@
#ifndef AUTOWARE_AD_API_SPECS__INTERFACE_HPP_
#define AUTOWARE_AD_API_SPECS__INTERFACE_HPP_
-#include
+#include
namespace autoware_ad_api::interface
{
struct Version
{
- using Service = autoware_ad_api_msgs::srv::InterfaceVersion;
+ using Service = autoware_adapi_version_msgs::srv::InterfaceVersion;
static constexpr char name[] = "/api/interface/version";
};
diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/localization.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/localization.hpp
index 112a8882d2873..a2db70fb8cb7a 100644
--- a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/localization.hpp
+++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/localization.hpp
@@ -17,21 +17,21 @@
#include
-#include
-#include
+#include
+#include
namespace autoware_ad_api::localization
{
struct Initialize
{
- using Service = autoware_ad_api_msgs::srv::InitializeLocalization;
+ using Service = autoware_adapi_v1_msgs::srv::InitializeLocalization;
static constexpr char name[] = "/api/localization/initialize";
};
struct InitializationState
{
- using Message = autoware_ad_api_msgs::msg::LocalizationInitializationState;
+ using Message = autoware_adapi_v1_msgs::msg::LocalizationInitializationState;
static constexpr char name[] = "/api/localization/initialization_state";
static constexpr size_t depth = 3;
static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/motion.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/motion.hpp
new file mode 100644
index 0000000000000..dcec2a4670f04
--- /dev/null
+++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/motion.hpp
@@ -0,0 +1,43 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE_AD_API_SPECS__MOTION_HPP_
+#define AUTOWARE_AD_API_SPECS__MOTION_HPP_
+
+#include
+
+#include
+#include
+
+namespace autoware_ad_api::motion
+{
+
+struct AcceptStart
+{
+ using Service = autoware_ad_api_msgs::srv::AcceptStart;
+ static constexpr char name[] = "/api/motion/accept_start";
+};
+
+struct State
+{
+ using Message = autoware_ad_api_msgs::msg::MotionState;
+ static constexpr char name[] = "/api/motion/state";
+ static constexpr size_t depth = 1;
+ static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+ static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
+} // namespace autoware_ad_api::motion
+
+#endif // AUTOWARE_AD_API_SPECS__MOTION_HPP_
diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/routing.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/routing.hpp
index 336ea670c0c8b..76fac74f667f9 100644
--- a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/routing.hpp
+++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/routing.hpp
@@ -17,36 +17,36 @@
#include
-#include
-#include
-#include
-#include
-#include
+#include
+#include
+#include
+#include
+#include
namespace autoware_ad_api::routing
{
struct SetRoutePoints
{
- using Service = autoware_ad_api_msgs::srv::SetRoutePoints;
+ using Service = autoware_adapi_v1_msgs::srv::SetRoutePoints;
static constexpr char name[] = "/api/routing/set_route_points";
};
struct SetRoute
{
- using Service = autoware_ad_api_msgs::srv::SetRoute;
+ using Service = autoware_adapi_v1_msgs::srv::SetRoute;
static constexpr char name[] = "/api/routing/set_route";
};
struct ClearRoute
{
- using Service = autoware_ad_api_msgs::srv::ClearRoute;
+ using Service = autoware_adapi_v1_msgs::srv::ClearRoute;
static constexpr char name[] = "/api/routing/clear_route";
};
struct RouteState
{
- using Message = autoware_ad_api_msgs::msg::RouteState;
+ using Message = autoware_adapi_v1_msgs::msg::RouteState;
static constexpr char name[] = "/api/routing/state";
static constexpr size_t depth = 3;
static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
@@ -55,7 +55,7 @@ struct RouteState
struct Route
{
- using Message = autoware_ad_api_msgs::msg::Route;
+ using Message = autoware_adapi_v1_msgs::msg::Route;
static constexpr char name[] = "/api/routing/route";
static constexpr size_t depth = 1;
static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
diff --git a/common/component_interface_specs/include/component_interface_specs/control.hpp b/common/component_interface_specs/include/component_interface_specs/control.hpp
new file mode 100644
index 0000000000000..ac99135eb6ef6
--- /dev/null
+++ b/common/component_interface_specs/include/component_interface_specs/control.hpp
@@ -0,0 +1,53 @@
+// 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 COMPONENT_INTERFACE_SPECS__CONTROL_HPP_
+#define COMPONENT_INTERFACE_SPECS__CONTROL_HPP_
+
+#include
+
+#include
+#include
+#include
+
+namespace control_interface
+{
+
+struct SetPause
+{
+ using Service = tier4_control_msgs::srv::SetPause;
+ static constexpr char name[] = "/control/vehicle_cmd_gate/set_pause";
+};
+
+struct IsPaused
+{
+ using Message = tier4_control_msgs::msg::IsPaused;
+ static constexpr char name[] = "/control/vehicle_cmd_gate/is_paused";
+ static constexpr size_t depth = 1;
+ static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+ static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
+struct IsStartRequested
+{
+ using Message = tier4_control_msgs::msg::IsStartRequested;
+ static constexpr char name[] = "/control/vehicle_cmd_gate/is_start_requested";
+ static constexpr size_t depth = 1;
+ static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+ static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
+} // namespace control_interface
+
+#endif // COMPONENT_INTERFACE_SPECS__CONTROL_HPP_
diff --git a/common/component_interface_specs/include/component_interface_specs/localization.hpp b/common/component_interface_specs/include/component_interface_specs/localization.hpp
index 050400bc2d625..a1a39f5cd57af 100644
--- a/common/component_interface_specs/include/component_interface_specs/localization.hpp
+++ b/common/component_interface_specs/include/component_interface_specs/localization.hpp
@@ -17,21 +17,21 @@
#include
-#include
-#include
+#include
+#include
namespace localization_interface
{
struct Initialize
{
- using Service = autoware_ad_api_msgs::srv::InitializeLocalization;
+ using Service = autoware_adapi_v1_msgs::srv::InitializeLocalization;
static constexpr char name[] = "/localization/initialize";
};
struct InitializationState
{
- using Message = autoware_ad_api_msgs::msg::LocalizationInitializationState;
+ using Message = autoware_adapi_v1_msgs::msg::LocalizationInitializationState;
static constexpr char name[] = "/localization/initialization_state";
static constexpr size_t depth = 3;
static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
diff --git a/common/component_interface_specs/include/component_interface_specs/planning.hpp b/common/component_interface_specs/include/component_interface_specs/planning.hpp
index 08ba456976804..641f36ffa0504 100644
--- a/common/component_interface_specs/include/component_interface_specs/planning.hpp
+++ b/common/component_interface_specs/include/component_interface_specs/planning.hpp
@@ -17,36 +17,36 @@
#include
-#include
-#include
-#include
-#include
-#include
+#include
+#include
+#include
+#include
+#include
namespace planning_interface
{
struct SetRoutePoints
{
- using Service = autoware_ad_api_msgs::srv::SetRoutePoints;
+ using Service = autoware_adapi_v1_msgs::srv::SetRoutePoints;
static constexpr char name[] = "/planning/mission_planning/set_route_points";
};
struct SetRoute
{
- using Service = autoware_ad_api_msgs::srv::SetRoute;
+ using Service = autoware_adapi_v1_msgs::srv::SetRoute;
static constexpr char name[] = "/planning/mission_planning/set_route";
};
struct ClearRoute
{
- using Service = autoware_ad_api_msgs::srv::ClearRoute;
+ using Service = autoware_adapi_v1_msgs::srv::ClearRoute;
static constexpr char name[] = "/planning/mission_planning/clear_route";
};
struct RouteState
{
- using Message = autoware_ad_api_msgs::msg::RouteState;
+ using Message = autoware_adapi_v1_msgs::msg::RouteState;
static constexpr char name[] = "/planning/mission_planning/route_state";
static constexpr size_t depth = 3;
static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
@@ -55,7 +55,7 @@ struct RouteState
struct Route
{
- using Message = autoware_ad_api_msgs::msg::Route;
+ using Message = autoware_adapi_v1_msgs::msg::Route;
static constexpr char name[] = "/planning/mission_planning/route";
static constexpr size_t depth = 1;
static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp.hpp
index 11f343cd2a900..283b99b6f34cc 100644
--- a/common/component_interface_utils/include/component_interface_utils/rclcpp.hpp
+++ b/common/component_interface_utils/include/component_interface_utils/rclcpp.hpp
@@ -32,6 +32,10 @@ class NodeAdaptor
private:
using CallbackGroup = rclcpp::CallbackGroup::SharedPtr;
+ template
+ using MessageCallback =
+ void (InstanceT::*)(const typename SharedPtrT::element_type::SpecType::Message::ConstSharedPtr);
+
template
using ServiceCallback = void (InstanceT::*)(
const typename SharedPtrT::element_type::SpecType::Service::Request::SharedPtr,
@@ -92,14 +96,25 @@ class NodeAdaptor
srv, [cli, timeout](auto req, auto res) { *res = *cli->call(req, timeout); }, group);
}
+ /// Create a subscription wrapper.
+ template
+ void init_sub(
+ SharedPtrT & sub, InstanceT * instance,
+ MessageCallback && callback) const
+ {
+ using std::placeholders::_1;
+ init_sub(sub, std::bind(callback, instance, _1));
+ }
+
/// Create a service wrapper for logging.
template
void init_srv(
- SharedPtrT & srv, InstanceT * instance, ServiceCallback callback,
+ SharedPtrT & srv, InstanceT * instance, ServiceCallback && callback,
CallbackGroup group = nullptr) const
{
- init_srv(
- srv, [instance, callback](auto req, auto res) { (instance->*callback)(req, res); }, group);
+ using std::placeholders::_1;
+ using std::placeholders::_2;
+ init_srv(srv, std::bind(callback, instance, _1, _2), group);
}
private:
diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp/exceptions.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp/exceptions.hpp
index a243ba60c1954..87cb925c05ea4 100644
--- a/common/component_interface_utils/include/component_interface_utils/rclcpp/exceptions.hpp
+++ b/common/component_interface_utils/include/component_interface_utils/rclcpp/exceptions.hpp
@@ -15,7 +15,7 @@
#ifndef COMPONENT_INTERFACE_UTILS__RCLCPP__EXCEPTIONS_HPP_
#define COMPONENT_INTERFACE_UTILS__RCLCPP__EXCEPTIONS_HPP_
-#include
+#include
#include
#include
@@ -26,7 +26,7 @@ namespace component_interface_utils
class ServiceException : public std::runtime_error
{
public:
- using ResponseStatus = autoware_ad_api_msgs::msg::ResponseStatus;
+ using ResponseStatus = autoware_adapi_v1_msgs::msg::ResponseStatus;
using ResponseStatusCode = ResponseStatus::_code_type;
ServiceException(ResponseStatusCode code, const std::string & message)
@@ -34,6 +34,15 @@ class ServiceException : public std::runtime_error
{
code_ = code;
}
+
+ template
+ void set(T & status) const
+ {
+ status.success = false;
+ status.code = code_;
+ status.message = what();
+ }
+
ResponseStatus status() const
{
ResponseStatus status;
diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp/service_server.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp/service_server.hpp
index 30d4d2c5a14dd..f8ba8ab980fcb 100644
--- a/common/component_interface_utils/include/component_interface_utils/rclcpp/service_server.hpp
+++ b/common/component_interface_utils/include/component_interface_utils/rclcpp/service_server.hpp
@@ -70,7 +70,7 @@ class Service
try {
callback(request, response);
} catch (const ServiceException & error) {
- response->status = error.status();
+ error.set(response->status);
}
}
RCLCPP_INFO_STREAM(logger, "service exit: " << SpecT::name << "\n" << to_yaml(*response));
diff --git a/common/component_interface_utils/include/component_interface_utils/status.hpp b/common/component_interface_utils/include/component_interface_utils/status.hpp
new file mode 100644
index 0000000000000..bf4d67ae5c833
--- /dev/null
+++ b/common/component_interface_utils/include/component_interface_utils/status.hpp
@@ -0,0 +1,31 @@
+// 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 COMPONENT_INTERFACE_UTILS__STATUS_HPP_
+#define COMPONENT_INTERFACE_UTILS__STATUS_HPP_
+
+namespace component_interface_utils::status
+{
+
+template
+void copy(const T1 & src, T2 & dst) // NOLINT(build/include_what_you_use): cpplint false positive
+{
+ dst->status.success = src->status.success;
+ dst->status.code = src->status.code;
+ dst->status.message = src->status.message;
+}
+
+} // namespace component_interface_utils::status
+
+#endif // COMPONENT_INTERFACE_UTILS__STATUS_HPP_
diff --git a/common/component_interface_utils/package.xml b/common/component_interface_utils/package.xml
index 6a67111bc2c99..94e925b7b4b7a 100644
--- a/common/component_interface_utils/package.xml
+++ b/common/component_interface_utils/package.xml
@@ -11,7 +11,7 @@
autoware_cmake
- autoware_ad_api_msgs
+ autoware_adapi_v1_msgs
rclcpp
rclcpp_components
diff --git a/common/motion_utils/package.xml b/common/motion_utils/package.xml
index 8d7aa9167c2ab..d8f6340a473e4 100644
--- a/common/motion_utils/package.xml
+++ b/common/motion_utils/package.xml
@@ -7,6 +7,8 @@
Yutaka Shimizu
Satoshi Ota
Takayuki Murooka
+
+ Taiki Tanaka
Apache License 2.0
Yutaka Shimizu
diff --git a/common/perception_utils/include/perception_utils/object_classification.hpp b/common/perception_utils/include/perception_utils/object_classification.hpp
new file mode 100644
index 0000000000000..841466c6f8db5
--- /dev/null
+++ b/common/perception_utils/include/perception_utils/object_classification.hpp
@@ -0,0 +1,50 @@
+// 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 PERCEPTION_UTILS__OBJECT_CLASSIFICATION_HPP_
+#define PERCEPTION_UTILS__OBJECT_CLASSIFICATION_HPP_
+
+#include "autoware_auto_perception_msgs/msg/object_classification.hpp"
+
+namespace perception_utils
+{
+using autoware_auto_perception_msgs::msg::ObjectClassification;
+
+bool isVehicle(const uint8_t object_classification)
+{
+ return object_classification == ObjectClassification::BICYCLE ||
+ object_classification == ObjectClassification::BUS ||
+ object_classification == ObjectClassification::CAR ||
+ object_classification == ObjectClassification::MOTORCYCLE ||
+ object_classification == ObjectClassification::TRAILER ||
+ object_classification == ObjectClassification::TRUCK;
+}
+
+bool isCarLikeVehicle(const uint8_t object_classification)
+{
+ return object_classification == ObjectClassification::BUS ||
+ object_classification == ObjectClassification::CAR ||
+ object_classification == ObjectClassification::TRAILER ||
+ object_classification == ObjectClassification::TRUCK;
+}
+
+bool isLargeVehicle(const uint8_t object_classification)
+{
+ return object_classification == ObjectClassification::BUS ||
+ object_classification == ObjectClassification::TRAILER ||
+ object_classification == ObjectClassification::TRUCK;
+}
+} // namespace perception_utils
+
+#endif // PERCEPTION_UTILS__OBJECT_CLASSIFICATION_HPP_
diff --git a/common/perception_utils/package.xml b/common/perception_utils/package.xml
index c338601d9ec46..79d08bc0ed15a 100644
--- a/common/perception_utils/package.xml
+++ b/common/perception_utils/package.xml
@@ -5,6 +5,7 @@
0.1.0
The perception_utils package
Takayuki Murooka
+ Satoshi Tanaka
Apache License 2.0
ament_cmake_auto
diff --git a/common/trtexec_vendor/CMakeLists.txt b/common/trtexec_vendor/CMakeLists.txt
new file mode 100644
index 0000000000000..220e7e6d06f81
--- /dev/null
+++ b/common/trtexec_vendor/CMakeLists.txt
@@ -0,0 +1,88 @@
+cmake_minimum_required(VERSION 3.14)
+project(trtexec_vendor)
+
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 17)
+ set(CMAKE_CXX_STANDARD_REQUIRED ON)
+ set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(CUDA)
+find_package(cudnn_cmake_module REQUIRED)
+find_package(CUDNN)
+find_package(tensorrt_cmake_module REQUIRED)
+find_package(TENSORRT)
+
+if(NOT (${CUDA_FOUND} AND ${CUDNN_FOUND} AND ${TENSORRT_FOUND}))
+ message(WARNING "cuda, cudnn, tensorrt libraries are not found")
+ return()
+endif()
+
+if(${TENSORRT_VERSION} VERSION_LESS 8.2.1)
+ message(WARNING "The tensorrt version less than 8.2.1 isn't supported.")
+ return()
+endif()
+
+set(TRTEXEC_DEFAULT_BIN /usr/src/tensorrt/bin/trtexec)
+if(NOT EXISTS TRTEXEC_DEFAULT_BIN)
+ include(FetchContent)
+ if(${TENSORRT_VERSION} VERSION_EQUAL 8.4.2)
+ set(TENSORRT_VERSION 8.4.1)
+ elseif(${TENSORRT_VERSION} VERSION_LESS_EQUAL 8.2.5 AND ${TENSORRT_VERSION} VERSION_GREATER 8.2.1)
+ set(TENSORRT_VERSION 8.2.1)
+ endif()
+ fetchcontent_declare(tensorrt
+ GIT_REPOSITORY https://github.com/NVIDIA/TensorRT
+ GIT_TAG ${TENSORRT_VERSION}
+ GIT_SUBMODULES ""
+ )
+ fetchcontent_getproperties(tensorrt)
+ if(NOT tensorrt_POPULATED)
+ fetchcontent_populate(tensorrt)
+ endif()
+ set(TRTEXEC_SOURCES
+ ${tensorrt_SOURCE_DIR}/samples/trtexec/trtexec.cpp
+ ${tensorrt_SOURCE_DIR}/samples/common/sampleEngines.cpp
+ ${tensorrt_SOURCE_DIR}/samples/common/sampleInference.cpp
+ ${tensorrt_SOURCE_DIR}/samples/common/sampleOptions.cpp
+ ${tensorrt_SOURCE_DIR}/samples/common/sampleReporting.cpp
+ ${tensorrt_SOURCE_DIR}/samples/common/logger.cpp
+ )
+ if(${TENSORRT_VERSION} VERSION_GREATER_EQUAL 8.4)
+ list(APPEND TRTEXEC_SOURCES
+ ${tensorrt_SOURCE_DIR}/samples/common/sampleUtils.cpp
+ )
+ endif()
+ cuda_add_executable(${PROJECT_NAME}
+ ${TRTEXEC_SOURCES}
+ )
+ target_link_libraries(${PROJECT_NAME}
+ ${TENSORRT_LIBRARIES}
+ )
+ target_include_directories(${PROJECT_NAME}
+ PRIVATE ${tensorrt_SOURCE_DIR}/samples/common
+ )
+
+ set_target_properties(${PROJECT_NAME}
+ PROPERTIES OUTPUT_NAME trtexec
+ )
+
+ install(TARGETS ${PROJECT_NAME}
+ ARCHIVE DESTINATION lib
+ LIBRARY DESTINATION lib
+ RUNTIME DESTINATION bin
+ )
+endif()
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/${PROJECT_NAME}.sh.in")
+
+ament_package()
diff --git a/common/trtexec_vendor/env-hooks/trtexec_vendor.sh.in b/common/trtexec_vendor/env-hooks/trtexec_vendor.sh.in
new file mode 100644
index 0000000000000..7c2ef456ecfec
--- /dev/null
+++ b/common/trtexec_vendor/env-hooks/trtexec_vendor.sh.in
@@ -0,0 +1,5 @@
+TRTEXEC_DEFAULT_BIN_DIR=/usr/src/tensorrt/bin
+TRTEXEC_DEFAULT_BIN=$TRTEXEC_DEFAULT_BIN_DIR/trtexec
+if [ -f $TRTEXEC_DEFAULT_BIN ]; then
+ ament_prepend_unique_value PATH $TRTEXEC_DEFAULT_BIN_DIR
+fi
diff --git a/common/trtexec_vendor/package.xml b/common/trtexec_vendor/package.xml
new file mode 100644
index 0000000000000..f197858060f3a
--- /dev/null
+++ b/common/trtexec_vendor/package.xml
@@ -0,0 +1,20 @@
+
+
+
+ trtexec_vendor
+ 0.1.0
+ The vendor package of trtexec
+ Daisuke Nishimatsu
+ Apache 2.0
+
+ ament_cmake
+ cudnn_cmake_module
+ tensorrt_cmake_module
+
+ ament_lint_auto
+ autoware_lint_common
+
+
+ ament_cmake
+
+
diff --git a/control/trajectory_follower/include/trajectory_follower/input_data.hpp b/control/trajectory_follower/include/trajectory_follower/input_data.hpp
index 505e5e0de2545..b356095e2f78e 100644
--- a/control/trajectory_follower/include/trajectory_follower/input_data.hpp
+++ b/control/trajectory_follower/include/trajectory_follower/input_data.hpp
@@ -17,6 +17,7 @@
#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
+#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
#include "nav_msgs/msg/odometry.hpp"
namespace autoware
@@ -32,6 +33,7 @@ struct InputData
autoware_auto_planning_msgs::msg::Trajectory::SharedPtr current_trajectory_ptr;
nav_msgs::msg::Odometry::SharedPtr current_odometry_ptr;
autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr current_steering_ptr;
+ geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr current_accel_ptr;
};
} // namespace trajectory_follower
} // namespace control
diff --git a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp
index 60c7cca13ad02..c7b5e73d5982c 100644
--- a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp
+++ b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp
@@ -97,7 +97,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal
// pointers for ros topic
nav_msgs::msg::Odometry::ConstSharedPtr m_current_kinematic_state_ptr{nullptr};
- nav_msgs::msg::Odometry::ConstSharedPtr m_prev_kienmatic_state_ptr{nullptr};
+ geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr m_current_accel_ptr{nullptr};
autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr m_trajectory_ptr{nullptr};
// vehicle info
@@ -185,9 +185,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal
double m_ego_nearest_dist_threshold;
double m_ego_nearest_yaw_threshold;
- // 1st order lowpass filter for acceleration
- std::shared_ptr m_lpf_acc{nullptr};
-
// buffer of send command
std::vector m_ctrl_cmd_vec;
@@ -213,6 +210,13 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal
*/
void setKinematicState(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
+ /**
+ * @brief set current acceleration with received message
+ * @param [in] msg trajectory message
+ */
+ void setCurrentAcceleration(
+ const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg);
+
/**
* @brief set reference trajectory with received message
* @param [in] msg trajectory message
diff --git a/control/trajectory_follower/src/pid_longitudinal_controller.cpp b/control/trajectory_follower/src/pid_longitudinal_controller.cpp
index f8919d06638a4..53c3f71fc11bd 100644
--- a/control/trajectory_follower/src/pid_longitudinal_controller.cpp
+++ b/control/trajectory_follower/src/pid_longitudinal_controller.cpp
@@ -199,26 +199,27 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) : node
// set parameter callback
m_set_param_res = node_->add_on_set_parameters_callback(
std::bind(&PidLongitudinalController::paramCallback, this, _1));
-
- // set lowpass filter for acc
- m_lpf_acc = std::make_shared(0.0, 0.2);
}
void PidLongitudinalController::setInputData(InputData const & input_data)
{
setTrajectory(input_data.current_trajectory_ptr);
setKinematicState(input_data.current_odometry_ptr);
+ setCurrentAcceleration(input_data.current_accel_ptr);
}
void PidLongitudinalController::setKinematicState(const nav_msgs::msg::Odometry::ConstSharedPtr msg)
{
if (!msg) return;
-
- if (m_current_kinematic_state_ptr) {
- m_prev_kienmatic_state_ptr = m_current_kinematic_state_ptr;
- }
m_current_kinematic_state_ptr = msg;
}
+void PidLongitudinalController::setCurrentAcceleration(
+ const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg)
+{
+ if (!msg) return;
+ m_current_accel_ptr = msg;
+}
+
void PidLongitudinalController::setTrajectory(
const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg)
{
@@ -365,7 +366,7 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac
boost::optional PidLongitudinalController::run()
{
// wait for initial pointers
- if (!m_current_kinematic_state_ptr || !m_prev_kienmatic_state_ptr || !m_trajectory_ptr) {
+ if (!m_current_kinematic_state_ptr || !m_trajectory_ptr || !m_current_accel_ptr) {
return boost::none;
}
@@ -415,7 +416,8 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData
control_data.dt = getDt();
// current velocity and acceleration
- control_data.current_motion = getCurrentMotion();
+ control_data.current_motion.vel = m_current_kinematic_state_ptr->twist.twist.linear.x;
+ control_data.current_motion.acc = m_current_accel_ptr->accel.accel.linear.x;
// nearest idx
const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
@@ -699,23 +701,6 @@ float64_t PidLongitudinalController::getDt()
return std::max(std::min(dt, max_dt), min_dt);
}
-PidLongitudinalController::Motion PidLongitudinalController::getCurrentMotion() const
-{
- const float64_t dv = m_current_kinematic_state_ptr->twist.twist.linear.x -
- m_prev_kienmatic_state_ptr->twist.twist.linear.x;
- const float64_t dt = std::max(
- (rclcpp::Time(m_current_kinematic_state_ptr->header.stamp) -
- rclcpp::Time(m_prev_kienmatic_state_ptr->header.stamp))
- .seconds(),
- 1e-03);
- const float64_t accel = dv / dt;
-
- const float64_t current_vel = m_current_kinematic_state_ptr->twist.twist.linear.x;
- const float64_t current_acc = m_lpf_acc->filter(accel);
-
- return Motion{current_vel, current_acc};
-}
-
enum PidLongitudinalController::Shift PidLongitudinalController::getCurrentShift(
const size_t nearest_idx) const
{
diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp
index 91b99fc7837c9..2c8d51b4b69e2 100644
--- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp
+++ b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp
@@ -37,6 +37,8 @@
#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
#include "autoware_auto_system_msgs/msg/float32_multi_array_diagnostic.hpp"
#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp"
+#include "geometry_msgs/msg/accel_stamped.hpp"
+#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "tf2_msgs/msg/tf_message.hpp"
@@ -82,6 +84,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
rclcpp::Subscription::SharedPtr sub_ref_path_;
rclcpp::Subscription::SharedPtr sub_odometry_;
rclcpp::Subscription::SharedPtr sub_steering_;
+ rclcpp::Subscription::SharedPtr sub_accel_;
rclcpp::Publisher::SharedPtr
control_cmd_pub_;
@@ -102,6 +105,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr);
void onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg);
void onSteering(const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg);
+ void onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg);
bool isTimeOut();
LateralControllerMode getLateralControllerMode(const std::string & algorithm_name) const;
LongitudinalControllerMode getLongitudinalControllerMode(
diff --git a/control/trajectory_follower_nodes/src/controller_node.cpp b/control/trajectory_follower_nodes/src/controller_node.cpp
index 2eaf51d8a7652..f720dfc9c27fd 100644
--- a/control/trajectory_follower_nodes/src/controller_node.cpp
+++ b/control/trajectory_follower_nodes/src/controller_node.cpp
@@ -76,6 +76,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control
"~/input/current_steering", rclcpp::QoS{1}, std::bind(&Controller::onSteering, this, _1));
sub_odometry_ = create_subscription(
"~/input/current_odometry", rclcpp::QoS{1}, std::bind(&Controller::onOdometry, this, _1));
+ sub_accel_ = create_subscription(
+ "~/input/current_accel", rclcpp::QoS{1}, std::bind(&Controller::onAccel, this, _1));
control_cmd_pub_ = create_publisher(
"~/output/control_cmd", rclcpp::QoS{1}.transient_local());
@@ -120,6 +122,11 @@ void Controller::onSteering(const autoware_auto_vehicle_msgs::msg::SteeringRepor
input_data_.current_steering_ptr = msg;
}
+void Controller::onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg)
+{
+ input_data_.current_accel_ptr = msg;
+}
+
bool Controller::isTimeOut()
{
const auto now = this->now();
diff --git a/control/trajectory_follower_nodes/test/test_controller_node.cpp b/control/trajectory_follower_nodes/test/test_controller_node.cpp
index d0a2b96302124..acb172f66ca28 100644
--- a/control/trajectory_follower_nodes/test/test_controller_node.cpp
+++ b/control/trajectory_follower_nodes/test/test_controller_node.cpp
@@ -24,6 +24,7 @@
#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp"
+#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
@@ -38,6 +39,7 @@ using Trajectory = autoware_auto_planning_msgs::msg::Trajectory;
using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint;
using VehicleOdometry = nav_msgs::msg::Odometry;
using SteeringReport = autoware_auto_vehicle_msgs::msg::SteeringReport;
+using geometry_msgs::msg::AccelWithCovarianceStamped;
using FakeNodeFixture = autoware::tools::testing::FakeTestNode;
@@ -68,815 +70,415 @@ std::shared_ptr makeNode()
return node;
}
-TEST_F(FakeNodeFixture, no_input)
+class ControllerTester
{
- // Data to test
+public:
+ explicit ControllerTester(FakeNodeFixture * _fnf) : fnf(_fnf) {}
+
+ std::shared_ptr node = makeNode();
+
+ FakeNodeFixture * fnf;
AckermannControlCommand::SharedPtr cmd_msg;
bool received_control_command = false;
- // Node
- std::shared_ptr node = makeNode();
- // Publisher/Subscribers
+
+ void publish_default_odom()
+ {
+ VehicleOdometry odom_msg;
+ odom_msg.header.stamp = node->now();
+ odom_pub->publish(odom_msg);
+ };
+
+ void publish_odom_vx(const double vx)
+ {
+ VehicleOdometry odom_msg;
+ odom_msg.header.stamp = node->now();
+ odom_msg.twist.twist.linear.x = vx;
+ odom_pub->publish(odom_msg);
+ };
+
+ void publish_default_steer()
+ {
+ SteeringReport steer_msg;
+ steer_msg.stamp = node->now();
+ steer_pub->publish(steer_msg);
+ };
+
+ void publish_steer_angle(const double steer)
+ {
+ SteeringReport steer_msg;
+ steer_msg.stamp = node->now();
+ steer_msg.steering_tire_angle = steer;
+ steer_pub->publish(steer_msg);
+ };
+
+ void publish_default_acc()
+ {
+ AccelWithCovarianceStamped acc_msg;
+ acc_msg.header.stamp = node->now();
+ accel_pub->publish(acc_msg);
+ };
+
+ void publish_default_traj()
+ {
+ Trajectory traj_msg;
+ traj_msg.header.stamp = node->now();
+ traj_msg.header.frame_id = "map";
+ traj_pub->publish(traj_msg);
+ };
+
+ void send_default_transform()
+ {
+ // Dummy transform: ego is at (0.0, 0.0) in map frame
+ geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform();
+ transform.header.stamp = node->now();
+ br->sendTransform(transform);
+
+ // Spin for transform to be published
+ test_utils::spinWhile(node);
+ };
+
rclcpp::Publisher::SharedPtr traj_pub =
- this->create_publisher("controller/input/reference_trajectory");
+ fnf->create_publisher("controller/input/reference_trajectory");
+
rclcpp::Publisher::SharedPtr odom_pub =
- this->create_publisher("controller/input/current_odometry");
+ fnf->create_publisher("controller/input/current_odometry");
+
rclcpp::Publisher::SharedPtr steer_pub =
- this->create_publisher("controller/input/current_steering");
+ fnf->create_publisher("controller/input/current_steering");
+
+ rclcpp::Publisher::SharedPtr accel_pub =
+ fnf->create_publisher("controller/input/current_accel");
+
rclcpp::Subscription::SharedPtr cmd_sub =
- this->create_subscription(
- "controller/output/control_cmd", *this->get_fake_node(),
- [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) {
+ fnf->create_subscription(
+ "controller/output/control_cmd", *fnf->get_fake_node(),
+ [this](const AckermannControlCommand::SharedPtr msg) {
cmd_msg = msg;
received_control_command = true;
});
+
std::shared_ptr br =
- std::make_shared(this->get_fake_node());
+ std::make_shared(fnf->get_fake_node());
+};
+
+TrajectoryPoint make_traj_point(const double px, const double py, const float vx)
+{
+ TrajectoryPoint p;
+ p.pose.position.x = px;
+ p.pose.position.y = py;
+ p.longitudinal_velocity_mps = vx;
+ return p;
+}
+
+TEST_F(FakeNodeFixture, no_input)
+{
+ ControllerTester tester(this);
// No published data: expect a stopped command
test_utils::waitForMessage(
- node, this, received_control_command, std::chrono::seconds{1LL}, false);
- ASSERT_FALSE(received_control_command);
+ tester.node, this, tester.received_control_command, std::chrono::seconds{1LL}, false);
+ ASSERT_FALSE(tester.received_control_command);
}
TEST_F(FakeNodeFixture, empty_trajectory)
{
- // Data to test
- AckermannControlCommand::SharedPtr cmd_msg;
- bool received_control_command = false;
- // Node
- std::shared_ptr node = makeNode();
- // Publisher/Subscribers
- rclcpp::Publisher::SharedPtr traj_pub =
- this->create_publisher("controller/input/reference_trajectory");
- rclcpp::Publisher::SharedPtr odom_pub =
- this->create_publisher("controller/input/current_odometry");
- rclcpp::Publisher::SharedPtr steer_pub =
- this->create_publisher("controller/input/current_steering");
- rclcpp::Subscription::SharedPtr cmd_sub =
- this->create_subscription(
- "controller/output/control_cmd", *this->get_fake_node(),
- [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) {
- cmd_msg = msg;
- received_control_command = true;
- });
- std::shared_ptr br =
- std::make_shared(this->get_fake_node());
-
- // Dummy transform: ego is at (0.0, 0.0) in map frame
- geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform();
- transform.header.stamp = node->now();
- br->sendTransform(transform);
+ ControllerTester tester(this);
- // Spin for transform to be published
- test_utils::spinWhile(node);
+ tester.send_default_transform();
// Empty trajectory: expect a stopped command
- Trajectory traj_msg;
- traj_msg.header.stamp = node->now();
- traj_msg.header.frame_id = "map";
- VehicleOdometry odom_msg;
- SteeringReport steer_msg;
- traj_msg.header.stamp = node->now();
- odom_msg.header.stamp = node->now();
- odom_msg.twist.twist.linear.x = 0.0;
- steer_msg.stamp = node->now();
- steer_msg.steering_tire_angle = 0.0;
- traj_pub->publish(traj_msg);
- odom_pub->publish(odom_msg);
- steer_pub->publish(steer_msg);
+ tester.publish_default_traj();
+ tester.publish_default_odom();
+ tester.publish_default_acc();
+ tester.publish_default_steer();
test_utils::waitForMessage(
- node, this, received_control_command, std::chrono::seconds{1LL}, false);
- ASSERT_FALSE(received_control_command);
+ tester.node, this, tester.received_control_command, std::chrono::seconds{1LL}, false);
+ ASSERT_FALSE(tester.received_control_command);
}
// lateral
TEST_F(FakeNodeFixture, straight_trajectory)
{
- // Data to test
- AckermannControlCommand::SharedPtr cmd_msg;
- bool received_control_command = false;
- // Node
- std::shared_ptr node = makeNode();
- // Publisher/Subscribers
- rclcpp::Publisher::SharedPtr traj_pub =
- this->create_publisher("controller/input/reference_trajectory");
- rclcpp::Publisher::SharedPtr odom_pub =
- this->create_publisher("controller/input/current_odometry");
- rclcpp::Publisher::SharedPtr steer_pub =
- this->create_publisher("controller/input/current_steering");
- rclcpp::Subscription::SharedPtr cmd_sub =
- this->create_subscription(
- "controller/output/control_cmd", *this->get_fake_node(),
- [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) {
- cmd_msg = msg;
- received_control_command = true;
- });
- std::shared_ptr br =
- std::make_shared(this->get_fake_node());
+ ControllerTester tester(this);
- // Dummy transform: ego is at (0.0, 0.0) in map frame
- geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform();
- transform.header.stamp = node->now();
- br->sendTransform(transform);
+ tester.send_default_transform();
+ tester.publish_odom_vx(1.0);
+ tester.publish_default_steer();
+ tester.publish_default_acc();
- // Spin for transform to be published
- test_utils::spinWhile(node);
-
- // Straight trajectory: expect no steering
- received_control_command = false;
Trajectory traj_msg;
- traj_msg.header.stamp = node->now();
+ traj_msg.header.stamp = tester.node->now();
traj_msg.header.frame_id = "map";
- VehicleOdometry odom_msg;
- SteeringReport steer_msg;
- TrajectoryPoint p;
- traj_msg.header.stamp = node->now();
- p.pose.position.x = -1.0;
- p.pose.position.y = 0.0;
- p.longitudinal_velocity_mps = 1.0f;
- traj_msg.points.push_back(p);
- p.pose.position.x = 0.0;
- p.pose.position.y = 0.0;
- p.longitudinal_velocity_mps = 1.0f;
- traj_msg.points.push_back(p);
- p.pose.position.x = 1.0;
- p.pose.position.y = 0.0;
- p.longitudinal_velocity_mps = 1.0f;
- traj_msg.points.push_back(p);
- p.pose.position.x = 2.0;
- p.pose.position.y = 0.0;
- p.longitudinal_velocity_mps = 1.0f;
- traj_msg.points.push_back(p);
- traj_pub->publish(traj_msg);
- odom_msg.header.stamp = node->now();
- odom_msg.twist.twist.linear.x = 1.0;
- steer_msg.stamp = node->now();
- steer_msg.steering_tire_angle = 0.0;
- odom_pub->publish(odom_msg);
- steer_pub->publish(steer_msg);
-
- test_utils::waitForMessage(node, this, received_control_command);
- ASSERT_TRUE(received_control_command);
- EXPECT_EQ(cmd_msg->lateral.steering_tire_angle, 0.0f);
- EXPECT_EQ(cmd_msg->lateral.steering_tire_rotation_rate, 0.0f);
- EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp));
+ traj_msg.points.push_back(make_traj_point(-1.0, 0.0, 1.0f));
+ traj_msg.points.push_back(make_traj_point(0.0, 0.0, 1.0f));
+ traj_msg.points.push_back(make_traj_point(1.0, 0.0, 1.0f));
+ traj_msg.points.push_back(make_traj_point(2.0, 0.0, 1.0f));
+ tester.traj_pub->publish(traj_msg);
+
+ test_utils::waitForMessage(tester.node, this, tester.received_control_command);
+ ASSERT_TRUE(tester.received_control_command);
+ EXPECT_EQ(tester.cmd_msg->lateral.steering_tire_angle, 0.0f);
+ EXPECT_EQ(tester.cmd_msg->lateral.steering_tire_rotation_rate, 0.0f);
+ EXPECT_GT(rclcpp::Time(tester.cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp));
}
TEST_F(FakeNodeFixture, right_turn)
{
- // Data to test
- AckermannControlCommand::SharedPtr cmd_msg;
- bool received_control_command = false;
- // Node
- std::shared_ptr node = makeNode();
- // Publisher/Subscribers
- rclcpp::Publisher::SharedPtr traj_pub =
- this->create_publisher("controller/input/reference_trajectory");
- rclcpp::Publisher::SharedPtr odom_pub =
- this->create_publisher("controller/input/current_odometry");
- rclcpp::Publisher::SharedPtr steer_pub =
- this->create_publisher("controller/input/current_steering");
- rclcpp::Subscription::SharedPtr cmd_sub =
- this->create_subscription(
- "controller/output/control_cmd", *this->get_fake_node(),
- [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) {
- cmd_msg = msg;
- received_control_command = true;
- });
- std::shared_ptr br =
- std::make_shared(this->get_fake_node());
+ ControllerTester tester(this);
- // Dummy transform: ego is at (0.0, 0.0) in map frame
- geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform();
- transform.header.stamp = node->now();
- br->sendTransform(transform);
-
- // Spin for transform to be published
- test_utils::spinWhile(node);
+ tester.send_default_transform();
+ tester.publish_odom_vx(1.0);
+ tester.publish_default_steer();
+ tester.publish_default_acc();
// Right turning trajectory: expect right steering
- received_control_command = false;
Trajectory traj_msg;
- traj_msg.header.stamp = node->now();
+ traj_msg.header.stamp = tester.node->now();
traj_msg.header.frame_id = "map";
- VehicleOdometry odom_msg;
- SteeringReport steer_msg;
- TrajectoryPoint p;
- traj_msg.points.clear();
- p.pose.position.x = -1.0;
- p.pose.position.y = -1.0;
- p.longitudinal_velocity_mps = 1.0f;
- traj_msg.points.push_back(p);
- p.pose.position.x = 0.0;
- p.pose.position.y = 0.0;
- p.longitudinal_velocity_mps = 1.0f;
- traj_msg.points.push_back(p);
- p.pose.position.x = 1.0;
- p.pose.position.y = -1.0;
- p.longitudinal_velocity_mps = 1.0f;
- traj_msg.points.push_back(p);
- p.pose.position.x = 2.0;
- p.pose.position.y = -2.0;
- p.longitudinal_velocity_mps = 1.0f;
- traj_msg.points.push_back(p);
- traj_pub->publish(traj_msg);
- odom_msg.header.stamp = node->now();
- odom_msg.twist.twist.linear.x = 1.0;
- steer_msg.stamp = node->now();
- steer_msg.steering_tire_angle = 0.0;
- odom_pub->publish(odom_msg);
- steer_pub->publish(steer_msg);
-
- test_utils::waitForMessage(node, this, received_control_command);
- ASSERT_TRUE(received_control_command);
- EXPECT_LT(cmd_msg->lateral.steering_tire_angle, 0.0f);
- EXPECT_LT(cmd_msg->lateral.steering_tire_rotation_rate, 0.0f);
- EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp));
+ traj_msg.points.push_back(make_traj_point(-1.0, -1.0, 1.0f));
+ traj_msg.points.push_back(make_traj_point(0.0, 0.0, 1.0f));
+ traj_msg.points.push_back(make_traj_point(1.0, -1.0, 1.0f));
+ traj_msg.points.push_back(make_traj_point(2.0, -2.0, 1.0f));
+ tester.traj_pub->publish(traj_msg);
+
+ test_utils::waitForMessage(tester.node, this, tester.received_control_command);
+ ASSERT_TRUE(tester.received_control_command);
+ EXPECT_LT(tester.cmd_msg->lateral.steering_tire_angle, 0.0f);
+ EXPECT_LT(tester.cmd_msg->lateral.steering_tire_rotation_rate, 0.0f);
+ EXPECT_GT(rclcpp::Time(tester.cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp));
}
TEST_F(FakeNodeFixture, left_turn)
{
- // Data to test
- AckermannControlCommand::SharedPtr cmd_msg;
- bool received_control_command = false;
- // Node
- std::shared_ptr node = makeNode();
- // Publisher/Subscribers
- rclcpp::Publisher::SharedPtr traj_pub =
- this->create_publisher("controller/input/reference_trajectory");
- rclcpp::Publisher::SharedPtr odom_pub =
- this->create_publisher("controller/input/current_odometry");
- rclcpp::Publisher::SharedPtr steer_pub =
- this->create_publisher("controller/input/current_steering");
- rclcpp::Subscription::SharedPtr cmd_sub =
- this->create_subscription(
- "controller/output/control_cmd", *this->get_fake_node(),
- [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) {
- cmd_msg = msg;
- received_control_command = true;
- });
- std::shared_ptr br =
- std::make_shared(this->get_fake_node());
-
- // Dummy transform: ego is at (0.0, 0.0) in map frame
- geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform();
- transform.header.stamp = node->now();
- br->sendTransform(transform);
+ ControllerTester tester(this);
- // Spin for transform to be published
- test_utils::spinWhile(node);
+ tester.send_default_transform();
+ tester.publish_odom_vx(1.0);
+ tester.publish_default_steer();
+ tester.publish_default_acc();
// Left turning trajectory: expect left steering
- received_control_command = false;
Trajectory traj_msg;
- traj_msg.header.stamp = node->now();
+ traj_msg.header.stamp = tester.node->now();
traj_msg.header.frame_id = "map";
- VehicleOdometry odom_msg;
- SteeringReport steer_msg;
- TrajectoryPoint p;
- traj_msg.points.clear();
- p.pose.position.x = -1.0;
- p.pose.position.y = 1.0;
- p.longitudinal_velocity_mps = 1.0f;
- traj_msg.points.push_back(p);
- p.pose.position.x = 0.0;
- p.pose.position.y = 0.0;
- p.longitudinal_velocity_mps = 1.0f;
- traj_msg.points.push_back(p);
- p.pose.position.x = 1.0;
- p.pose.position.y = 1.0;
- p.longitudinal_velocity_mps = 1.0f;
- traj_msg.points.push_back(p);
- p.pose.position.x = 2.0;
- p.pose.position.y = 2.0;
- p.longitudinal_velocity_mps = 1.0f;
- traj_msg.points.push_back(p);
- traj_pub->publish(traj_msg);
- odom_msg.header.stamp = node->now();
- odom_msg.twist.twist.linear.x = 1.0;
- steer_msg.stamp = node->now();
- steer_msg.steering_tire_angle = 0.0;
- odom_pub->publish(odom_msg);
- steer_pub->publish(steer_msg);
-
- test_utils::waitForMessage(node, this, received_control_command);
- ASSERT_TRUE(received_control_command);
- EXPECT_GT(cmd_msg->lateral.steering_tire_angle, 0.0f);
- EXPECT_GT(cmd_msg->lateral.steering_tire_rotation_rate, 0.0f);
- EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp));
+ traj_msg.points.push_back(make_traj_point(-1.0, 1.0, 1.0f));
+ traj_msg.points.push_back(make_traj_point(0.0, 0.0, 1.0f));
+ traj_msg.points.push_back(make_traj_point(1.0, 1.0, 1.0f));
+ traj_msg.points.push_back(make_traj_point(2.0, 2.0, 1.0f));
+ tester.traj_pub->publish(traj_msg);
+
+ test_utils::waitForMessage(tester.node, this, tester.received_control_command);
+ ASSERT_TRUE(tester.received_control_command);
+ EXPECT_GT(tester.cmd_msg->lateral.steering_tire_angle, 0.0f);
+ EXPECT_GT(tester.cmd_msg->lateral.steering_tire_rotation_rate, 0.0f);
+ EXPECT_GT(rclcpp::Time(tester.cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp));
}
TEST_F(FakeNodeFixture, stopped)
{
- // Data to test
- AckermannControlCommand::SharedPtr cmd_msg;
- bool received_control_command = false;
- // Node
- std::shared_ptr node = makeNode();
- // Publisher/Subscribers
- rclcpp::Publisher::SharedPtr traj_pub =
- this->create_publisher("controller/input/reference_trajectory");
- rclcpp::Publisher::SharedPtr odom_pub =
- this->create_publisher("controller/input/current_odometry");
- rclcpp::Publisher::SharedPtr steer_pub =
- this->create_publisher("controller/input/current_steering");
- rclcpp::Subscription::SharedPtr cmd_sub =
- this->create_subscription(
- "controller/output/control_cmd", *this->get_fake_node(),
- [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) {
- cmd_msg = msg;
- received_control_command = true;
- });
- std::shared_ptr br =
- std::make_shared(this->get_fake_node());
+ ControllerTester tester(this);
- // Dummy transform: ego is at (0.0, 0.0) in map frame
- geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform();
- transform.header.stamp = node->now();
- br->sendTransform(transform);
+ tester.send_default_transform();
+ tester.publish_default_odom();
+ tester.publish_default_acc();
- // Spin for transform to be published
- test_utils::spinWhile(node);
+ const double steering_tire_angle = -0.5;
+ tester.publish_steer_angle(steering_tire_angle);
// Straight trajectory: expect no steering
- received_control_command = false;
Trajectory traj_msg;
- traj_msg.header.stamp = node->now();
+ traj_msg.header.stamp = tester.node->now();
traj_msg.header.frame_id = "map";
- VehicleOdometry odom_msg;
- SteeringReport steer_msg;
- TrajectoryPoint p;
- traj_msg.header.stamp = node->now();
- p.pose.position.x = -1.0;
- p.pose.position.y = 0.0;
- // Set a 0 current velocity and 0 target velocity -> stopped state
- p.longitudinal_velocity_mps = 0.0f;
- traj_msg.points.push_back(p);
- p.pose.position.x = 0.0;
- p.pose.position.y = 0.0;
- p.longitudinal_velocity_mps = 0.0f;
- traj_msg.points.push_back(p);
- p.pose.position.x = 1.0;
- p.pose.position.y = 0.0;
- p.longitudinal_velocity_mps = 0.0f;
- traj_msg.points.push_back(p);
- p.pose.position.x = 2.0;
- p.pose.position.y = 0.0;
- p.longitudinal_velocity_mps = 0.0f;
- traj_msg.points.push_back(p);
- traj_pub->publish(traj_msg);
- odom_msg.header.stamp = node->now();
- odom_msg.twist.twist.linear.x = 0.0;
- steer_msg.stamp = node->now();
- steer_msg.steering_tire_angle = -0.5;
- odom_pub->publish(odom_msg);
- steer_pub->publish(steer_msg);
-
- test_utils::waitForMessage(node, this, received_control_command);
- ASSERT_TRUE(received_control_command);
- EXPECT_EQ(cmd_msg->lateral.steering_tire_angle, steer_msg.steering_tire_angle);
- EXPECT_EQ(cmd_msg->lateral.steering_tire_rotation_rate, 0.0f);
- EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp));
+ traj_msg.points.push_back(make_traj_point(-1.0, 0.0, 0.0f));
+ traj_msg.points.push_back(make_traj_point(0.0, 0.0, 0.0f));
+ traj_msg.points.push_back(make_traj_point(1.0, 0.0, 0.0f));
+ traj_msg.points.push_back(make_traj_point(2.0, 0.0, 0.0f));
+ tester.traj_pub->publish(traj_msg);
+
+ test_utils::waitForMessage(tester.node, this, tester.received_control_command);
+ ASSERT_TRUE(tester.received_control_command);
+ EXPECT_EQ(tester.cmd_msg->lateral.steering_tire_angle, steering_tire_angle);
+ EXPECT_EQ(tester.cmd_msg->lateral.steering_tire_rotation_rate, 0.0f);
+ EXPECT_GT(rclcpp::Time(tester.cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp));
}
// longitudinal
TEST_F(FakeNodeFixture, longitudinal_keep_velocity)
{
- // Data to test
- AckermannControlCommand::SharedPtr cmd_msg;
- bool received_control_command = false;
- // Node
- std::shared_ptr node = makeNode();
- // Publisher/Subscribers
- rclcpp::Publisher::SharedPtr traj_pub =
- this->create_publisher("controller/input/reference_trajectory");
- rclcpp::Publisher::SharedPtr odom_pub =
- this->create_publisher("controller/input/current_odometry");
- rclcpp::Publisher::SharedPtr steer_pub =
- this->create_publisher("controller/input/current_steering");
- rclcpp::Subscription::SharedPtr cmd_sub =
- this->create_subscription(
- "controller/output/control_cmd", *this->get_fake_node(),
- [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) {
- cmd_msg = msg;
- received_control_command = true;
- });
- std::shared_ptr br =
- std::make_shared(this->get_fake_node());
-
- // Dummy transform: ego is at (0.0, 0.0) in map frame
- geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform();
- transform.header.stamp = node->now();
- br->sendTransform(transform);
- /// Already running at target vel + Non stopping trajectory -> no change in velocity
- // Publish velocity
- VehicleOdometry odom_msg;
- odom_msg.header.stamp = node->now();
- odom_msg.twist.twist.linear.x = 1.0;
- odom_pub->publish(odom_msg);
- // the node needs to receive two velocity msg
- rclcpp::spin_some(node);
- rclcpp::spin_some(this->get_fake_node());
- odom_msg.header.stamp = node->now();
- odom_pub->publish(odom_msg);
- // Publish steering
- SteeringReport steer_msg;
- steer_msg.stamp = node->now();
- steer_msg.steering_tire_angle = 0.0;
- steer_pub->publish(steer_msg);
+ ControllerTester tester(this);
+
+ tester.send_default_transform();
+ tester.publish_odom_vx(1.0);
+ tester.publish_default_steer();
+ tester.publish_default_acc();
+
// Publish non stopping trajectory
- Trajectory traj;
- traj.header.stamp = node->now();
- traj.header.frame_id = "map";
- TrajectoryPoint point;
- point.pose.position.x = 0.0;
- point.pose.position.y = 0.0;
- point.longitudinal_velocity_mps = 1.0;
- traj.points.push_back(point);
- point.pose.position.x = 50.0;
- point.pose.position.y = 0.0;
- point.longitudinal_velocity_mps = 1.0;
- traj.points.push_back(point);
- point.pose.position.x = 100.0;
- point.pose.position.y = 0.0;
- point.longitudinal_velocity_mps = 1.0;
- traj.points.push_back(point);
- traj_pub->publish(traj);
- test_utils::waitForMessage(node, this, received_control_command);
-
- ASSERT_TRUE(received_control_command);
- EXPECT_DOUBLE_EQ(cmd_msg->longitudinal.speed, 1.0);
- EXPECT_DOUBLE_EQ(cmd_msg->longitudinal.acceleration, 0.0);
+ Trajectory traj_msg;
+ traj_msg.header.stamp = tester.node->now();
+ traj_msg.header.frame_id = "map";
+ traj_msg.points.push_back(make_traj_point(0.0, 0.0, 1.0f));
+ traj_msg.points.push_back(make_traj_point(50.0, 0.0, 1.0f));
+ traj_msg.points.push_back(make_traj_point(100.0, 0.0, 1.0f));
+ tester.traj_pub->publish(traj_msg);
+
+ test_utils::waitForMessage(tester.node, this, tester.received_control_command);
+
+ ASSERT_TRUE(tester.received_control_command);
+ EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 1.0);
+ EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.acceleration, 0.0);
// Generate another control message
- received_control_command = false;
- traj_pub->publish(traj);
- test_utils::waitForMessage(node, this, received_control_command);
- ASSERT_TRUE(received_control_command);
- EXPECT_DOUBLE_EQ(cmd_msg->longitudinal.speed, 1.0);
- EXPECT_DOUBLE_EQ(cmd_msg->longitudinal.acceleration, 0.0);
+ tester.traj_pub->publish(traj_msg);
+ test_utils::waitForMessage(tester.node, this, tester.received_control_command);
+ ASSERT_TRUE(tester.received_control_command);
+ EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 1.0);
+ EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.acceleration, 0.0);
}
TEST_F(FakeNodeFixture, longitudinal_slow_down)
{
- // Data to test
- AckermannControlCommand::SharedPtr cmd_msg;
- bool received_control_command = false;
- // Node
- std::shared_ptr node = makeNode();
- // Publisher/Subscribers
- rclcpp::Publisher::SharedPtr traj_pub =
- this->create_publisher("controller/input/reference_trajectory");
- rclcpp::Publisher::SharedPtr odom_pub =
- this->create_publisher("controller/input/current_odometry");
- rclcpp::Publisher::SharedPtr steer_pub =
- this->create_publisher("controller/input/current_steering");
- rclcpp::Subscription::SharedPtr cmd_sub =
- this->create_subscription(
- "controller/output/control_cmd", *this->get_fake_node(),
- [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) {
- cmd_msg = msg;
- received_control_command = true;
- });
- std::shared_ptr br =
- std::make_shared(this->get_fake_node());
-
- // Dummy transform: ego is at (0.0, 0.0) in map frame
- geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform();
- transform.header.stamp = node->now();
- br->sendTransform(transform);
- /// Already running at target vel + Non stopping trajectory -> no change in velocity
- // Publish velocity
- VehicleOdometry odom_msg;
- odom_msg.header.stamp = node->now();
- odom_msg.twist.twist.linear.x = 1.0;
- odom_pub->publish(odom_msg);
- // the node needs to receive two velocity msg
- rclcpp::spin_some(node);
- rclcpp::spin_some(this->get_fake_node());
- odom_msg.header.stamp = node->now();
- odom_pub->publish(odom_msg);
- // Publish steering
- SteeringReport steer_msg;
- steer_msg.stamp = node->now();
- steer_msg.steering_tire_angle = 0.0;
- steer_pub->publish(steer_msg);
+ ControllerTester tester(this);
+
+ tester.send_default_transform();
+ tester.publish_default_acc();
+ tester.publish_default_steer();
+
+ const double odom_vx = 1.0;
+ tester.publish_odom_vx(odom_vx);
+
// Publish non stopping trajectory
Trajectory traj;
- traj.header.stamp = node->now();
+ traj.header.stamp = tester.node->now();
traj.header.frame_id = "map";
- TrajectoryPoint point;
- point.pose.position.x = 0.0;
- point.pose.position.y = 0.0;
- point.longitudinal_velocity_mps = 0.5;
- traj.points.push_back(point);
- point.pose.position.x = 50.0;
- point.pose.position.y = 0.0;
- point.longitudinal_velocity_mps = 0.5;
- traj.points.push_back(point);
- point.pose.position.x = 100.0;
- point.pose.position.y = 0.0;
- point.longitudinal_velocity_mps = 0.5;
- traj.points.push_back(point);
- traj_pub->publish(traj);
- test_utils::waitForMessage(node, this, received_control_command);
-
- ASSERT_TRUE(received_control_command);
- EXPECT_LT(cmd_msg->longitudinal.speed, static_cast(odom_msg.twist.twist.linear.x));
- EXPECT_LT(cmd_msg->longitudinal.acceleration, 0.0f);
+ traj.points.push_back(make_traj_point(0.0, 0.0, 0.5f));
+ traj.points.push_back(make_traj_point(50.0, 0.0, 0.5f));
+ traj.points.push_back(make_traj_point(100.0, 0.0, 0.5f));
+ tester.traj_pub->publish(traj);
+
+ test_utils::waitForMessage(tester.node, this, tester.received_control_command);
+
+ ASSERT_TRUE(tester.received_control_command);
+ EXPECT_LT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx));
+ EXPECT_LT(tester.cmd_msg->longitudinal.acceleration, 0.0f);
// Generate another control message
- received_control_command = false;
- traj_pub->publish(traj);
- test_utils::waitForMessage(node, this, received_control_command);
- ASSERT_TRUE(received_control_command);
- EXPECT_LT(cmd_msg->longitudinal.speed, static_cast(odom_msg.twist.twist.linear.x));
- EXPECT_LT(cmd_msg->longitudinal.acceleration, 0.0f);
+ tester.traj_pub->publish(traj);
+ test_utils::waitForMessage(tester.node, this, tester.received_control_command);
+ ASSERT_TRUE(tester.received_control_command);
+ EXPECT_LT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx));
+ EXPECT_LT(tester.cmd_msg->longitudinal.acceleration, 0.0f);
}
TEST_F(FakeNodeFixture, longitudinal_accelerate)
{
- // Data to test
- AckermannControlCommand::SharedPtr cmd_msg;
- bool received_control_command = false;
- // Node
- std::shared_ptr node = makeNode();
- // Publisher/Subscribers
- rclcpp::Publisher::SharedPtr traj_pub =
- this->create_publisher("controller/input/reference_trajectory");
- rclcpp::Publisher::SharedPtr odom_pub =
- this->create_publisher("controller/input/current_odometry");
- rclcpp::Publisher::SharedPtr steer_pub =
- this->create_publisher("controller/input/current_steering");
- rclcpp::Subscription::SharedPtr cmd_sub =
- this->create_subscription(
- "controller/output/control_cmd", *this->get_fake_node(),
- [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) {
- cmd_msg = msg;
- received_control_command = true;
- });
- std::shared_ptr br =
- std::make_shared(this->get_fake_node());
-
- // Dummy transform: ego is at (0.0, 0.0) in map frame
- geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform();
- transform.header.stamp = node->now();
- br->sendTransform(transform);
- /// Below target vel + Non stopping trajectory -> accelerate
- // Publish velocity
- VehicleOdometry odom_msg;
- odom_msg.header.stamp = node->now();
- odom_msg.twist.twist.linear.x = 0.5;
- odom_pub->publish(odom_msg);
- // the node needs to receive two velocity msg
- rclcpp::spin_some(node);
- rclcpp::spin_some(this->get_fake_node());
- odom_msg.header.stamp = node->now();
- odom_pub->publish(odom_msg);
- // Publish steering
- SteeringReport steer_msg;
- steer_msg.stamp = node->now();
- steer_msg.steering_tire_angle = 0.0;
- steer_pub->publish(steer_msg);
+ ControllerTester tester(this);
+
+ tester.send_default_transform();
+ tester.publish_default_steer();
+ tester.publish_default_acc();
+
+ const double odom_vx = 0.5;
+ tester.publish_odom_vx(odom_vx);
+
// Publish non stopping trajectory
Trajectory traj;
- traj.header.stamp = node->now();
+ traj.header.stamp = tester.node->now();
traj.header.frame_id = "map";
- TrajectoryPoint point;
- point.pose.position.x = 0.0;
- point.pose.position.y = 0.0;
- point.longitudinal_velocity_mps = 1.0;
- traj.points.push_back(point);
- point.pose.position.x = 50.0;
- point.pose.position.y = 0.0;
- point.longitudinal_velocity_mps = 1.0;
- traj.points.push_back(point);
- point.pose.position.x = 100.0;
- point.pose.position.y = 0.0;
- point.longitudinal_velocity_mps = 1.0;
- traj.points.push_back(point);
- traj_pub->publish(traj);
- test_utils::waitForMessage(node, this, received_control_command);
-
- ASSERT_TRUE(received_control_command);
- EXPECT_GT(cmd_msg->longitudinal.speed, static_cast(odom_msg.twist.twist.linear.x));
- EXPECT_GT(cmd_msg->longitudinal.acceleration, 0.0f);
+ traj.points.push_back(make_traj_point(0.0, 0.0, 1.0f));
+ traj.points.push_back(make_traj_point(50.0, 0.0, 1.0f));
+ traj.points.push_back(make_traj_point(100.0, 0.0, 1.0f));
+ tester.traj_pub->publish(traj);
+
+ test_utils::waitForMessage(tester.node, this, tester.received_control_command);
+
+ ASSERT_TRUE(tester.received_control_command);
+ EXPECT_GT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx));
+ EXPECT_GT(tester.cmd_msg->longitudinal.acceleration, 0.0f);
// Generate another control message
- received_control_command = false;
- traj_pub->publish(traj);
- test_utils::waitForMessage(node, this, received_control_command);
- ASSERT_TRUE(received_control_command);
- EXPECT_GT(cmd_msg->longitudinal.speed, static_cast(odom_msg.twist.twist.linear.x));
- EXPECT_GT(cmd_msg->longitudinal.acceleration, 0.0f);
+ tester.traj_pub->publish(traj);
+ test_utils::waitForMessage(tester.node, this, tester.received_control_command);
+ ASSERT_TRUE(tester.received_control_command);
+ EXPECT_GT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx));
+ EXPECT_GT(tester.cmd_msg->longitudinal.acceleration, 0.0f);
}
TEST_F(FakeNodeFixture, longitudinal_stopped)
{
- // Data to test
- AckermannControlCommand::SharedPtr cmd_msg;
- bool received_control_command = false;
- // Node
- std::shared_ptr node = makeNode();
- // Publisher/Subscribers
- rclcpp::Publisher::SharedPtr traj_pub =
- this->create_publisher("controller/input/reference_trajectory");
- rclcpp::Publisher::SharedPtr odom_pub =
- this->create_publisher("controller/input/current_odometry");
- rclcpp::Publisher::SharedPtr steer_pub =
- this->create_publisher("controller/input/current_steering");
- rclcpp::Subscription::SharedPtr cmd_sub =
- this->create_subscription(
- "controller/output/control_cmd", *this->get_fake_node(),
- [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) {
- cmd_msg = msg;
- received_control_command = true;
- });
- std::shared_ptr br =
- std::make_shared(this->get_fake_node());
-
- // Dummy transform: ego is at (0.0, 0.0) in map frame
- geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform();
- transform.header.stamp = node->now();
- br->sendTransform(transform);
- /// Below target vel + Non stopping trajectory -> accelerate
- // Publish velocity
- VehicleOdometry odom_msg;
- odom_msg.header.stamp = node->now();
- odom_msg.twist.twist.linear.x = 0.0;
- odom_pub->publish(odom_msg);
- // the node needs to receive two velocity msg
- rclcpp::spin_some(node);
- rclcpp::spin_some(this->get_fake_node());
- odom_msg.header.stamp = node->now();
- odom_pub->publish(odom_msg);
- // Publish steering
- SteeringReport steer_msg;
- steer_msg.stamp = node->now();
- steer_msg.steering_tire_angle = 0.0;
- steer_pub->publish(steer_msg);
+ ControllerTester tester(this);
+
+ tester.send_default_transform();
+ tester.publish_default_odom();
+ tester.publish_default_steer();
+ tester.publish_default_acc();
+
// Publish stopping trajectory
Trajectory traj;
- traj.header.stamp = node->now();
+ traj.header.stamp = tester.node->now();
traj.header.frame_id = "map";
- TrajectoryPoint point;
- point.pose.position.x = 0.0;
- point.pose.position.y = 0.0;
- point.longitudinal_velocity_mps = 0.0;
- traj.points.push_back(point);
- point.pose.position.x = 50.0;
- point.pose.position.y = 0.0;
- point.longitudinal_velocity_mps = 0.0;
- traj.points.push_back(point);
- point.pose.position.x = 100.0;
- point.pose.position.y = 0.0;
- point.longitudinal_velocity_mps = 0.0;
- traj.points.push_back(point);
- traj_pub->publish(traj);
- test_utils::waitForMessage(node, this, received_control_command);
-
- ASSERT_TRUE(received_control_command);
- EXPECT_DOUBLE_EQ(cmd_msg->longitudinal.speed, 0.0f);
+ traj.points.push_back(make_traj_point(0.0, 0.0, 0.0f));
+ traj.points.push_back(make_traj_point(50.0, 0.0, 0.0f));
+ traj.points.push_back(make_traj_point(100.0, 0.0, 0.0f));
+ tester.traj_pub->publish(traj);
+
+ test_utils::waitForMessage(tester.node, this, tester.received_control_command);
+
+ ASSERT_TRUE(tester.received_control_command);
+ EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 0.0f);
EXPECT_LT(
- cmd_msg->longitudinal.acceleration, 0.0f); // when stopped negative acceleration to brake
+ tester.cmd_msg->longitudinal.acceleration,
+ 0.0f); // when stopped negative acceleration to brake
}
TEST_F(FakeNodeFixture, longitudinal_reverse)
{
- // Data to test
- AckermannControlCommand::SharedPtr cmd_msg;
- bool received_control_command = false;
- // Node
- std::shared_ptr node = makeNode();
- // Publisher/Subscribers
- rclcpp::Publisher::SharedPtr traj_pub =
- this->create_publisher("controller/input/reference_trajectory");
- rclcpp::Publisher::SharedPtr odom_pub =
- this->create_publisher("controller/input/current_odometry");
- rclcpp::Publisher::SharedPtr steer_pub =
- this->create_publisher("controller/input/current_steering");
- rclcpp::Subscription::SharedPtr cmd_sub =
- this->create_subscription(
- "controller/output/control_cmd", *this->get_fake_node(),
- [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) {
- cmd_msg = msg;
- received_control_command = true;
- });
- std::shared_ptr br =
- std::make_shared(this->get_fake_node());
-
- // Dummy transform: ego is at (0.0, 0.0) in map frame
- geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform();
- transform.header.stamp = node->now();
- br->sendTransform(transform);
- /// Below target vel + Non stopping trajectory -> accelerate
- // Publish velocity
- VehicleOdometry odom_msg;
- odom_msg.header.stamp = node->now();
- odom_msg.twist.twist.linear.x = 0.0;
- odom_pub->publish(odom_msg);
- // the node needs to receive two velocity msg
- rclcpp::spin_some(node);
- rclcpp::spin_some(this->get_fake_node());
- odom_msg.header.stamp = node->now();
- odom_pub->publish(odom_msg);
- // Publish steering
- SteeringReport steer_msg;
- steer_msg.stamp = node->now();
- steer_msg.steering_tire_angle = 0.0;
- steer_pub->publish(steer_msg);
+ ControllerTester tester(this);
+
+ tester.send_default_transform();
+
+ tester.publish_default_odom();
+ tester.publish_default_steer();
+ tester.publish_default_acc();
+
// Publish reverse
Trajectory traj;
- traj.header.stamp = node->now();
+ traj.header.stamp = tester.node->now();
traj.header.frame_id = "map";
- TrajectoryPoint point;
- point.pose.position.x = 0.0;
- point.pose.position.y = 0.0;
- point.longitudinal_velocity_mps = -1.0;
- traj.points.push_back(point);
- point.pose.position.x = 50.0;
- point.pose.position.y = 0.0;
- point.longitudinal_velocity_mps = -1.0;
- traj.points.push_back(point);
- point.pose.position.x = 100.0;
- point.pose.position.y = 0.0;
- point.longitudinal_velocity_mps = -1.0;
- traj.points.push_back(point);
- traj_pub->publish(traj);
- test_utils::waitForMessage(node, this, received_control_command);
-
- ASSERT_TRUE(received_control_command);
- EXPECT_LT(cmd_msg->longitudinal.speed, 0.0f);
- EXPECT_GT(cmd_msg->longitudinal.acceleration, 0.0f);
+ traj.points.push_back(make_traj_point(0.0, 0.0, -1.0f));
+ traj.points.push_back(make_traj_point(50.0, 0.0, -1.0f));
+ traj.points.push_back(make_traj_point(100.0, 0.0, -1.0f));
+ tester.traj_pub->publish(traj);
+
+ test_utils::waitForMessage(tester.node, this, tester.received_control_command);
+
+ ASSERT_TRUE(tester.received_control_command);
+ EXPECT_LT(tester.cmd_msg->longitudinal.speed, 0.0f);
+ EXPECT_GT(tester.cmd_msg->longitudinal.acceleration, 0.0f);
}
TEST_F(FakeNodeFixture, longitudinal_emergency)
{
- // Data to test
- AckermannControlCommand::SharedPtr cmd_msg;
- bool received_control_command = false;
- // Node
- std::shared_ptr node = makeNode();
- // Publisher/Subscribers
- rclcpp::Publisher::SharedPtr traj_pub =
- this->create_publisher("controller/input/reference_trajectory");
- rclcpp::Publisher::SharedPtr odom_pub =
- this->create_publisher("controller/input/current_odometry");
- rclcpp::Publisher::SharedPtr steer_pub =
- this->create_publisher("controller/input/current_steering");
- rclcpp::Subscription::SharedPtr cmd_sub =
- this->create_subscription(
- "controller/output/control_cmd", *this->get_fake_node(),
- [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) {
- cmd_msg = msg;
- received_control_command = true;
- });
- std::shared_ptr br =
- std::make_shared(this->get_fake_node());
-
- // Dummy transform: ego is at (0.0, 0.0) in map frame
- geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform();
- transform.header.stamp = node->now();
- br->sendTransform(transform);
- /// Below target vel + Non stopping trajectory -> accelerate
- // Publish velocity
- VehicleOdometry odom_msg;
- odom_msg.header.stamp = node->now();
- odom_msg.twist.twist.linear.x = 0.0;
- odom_pub->publish(odom_msg);
- // the node needs to receive two velocity msg
- rclcpp::spin_some(node);
- rclcpp::spin_some(this->get_fake_node());
- odom_msg.header.stamp = node->now();
- odom_pub->publish(odom_msg);
- // Publish steering
- SteeringReport steer_msg;
- steer_msg.stamp = node->now();
- steer_msg.steering_tire_angle = 0.0;
- steer_pub->publish(steer_msg);
+ ControllerTester tester(this);
+
+ tester.send_default_transform();
+ tester.publish_default_odom();
+ tester.publish_default_steer();
+ tester.publish_default_acc();
+
// Publish trajectory starting away from the current ego pose
Trajectory traj;
- traj.header.stamp = node->now();
+ traj.header.stamp = tester.node->now();
traj.header.frame_id = "map";
- TrajectoryPoint point;
- point.pose.position.x = 10.0;
- point.pose.position.y = 0.0;
- point.longitudinal_velocity_mps = 1.0;
- traj.points.push_back(point);
- point.pose.position.x = 50.0;
- point.pose.position.y = 0.0;
- point.longitudinal_velocity_mps = 1.0;
- traj.points.push_back(point);
- point.pose.position.x = 100.0;
- point.pose.position.y = 0.0;
- point.longitudinal_velocity_mps = 1.0;
- traj.points.push_back(point);
- traj_pub->publish(traj);
- test_utils::waitForMessage(node, this, received_control_command);
-
- ASSERT_TRUE(received_control_command);
+ traj.points.push_back(make_traj_point(10.0, 0.0, 1.0f));
+ traj.points.push_back(make_traj_point(50.0, 0.0, 1.0f));
+ traj.points.push_back(make_traj_point(100.0, 0.0, 1.0f));
+ tester.traj_pub->publish(traj);
+
+ test_utils::waitForMessage(tester.node, this, tester.received_control_command);
+
+ ASSERT_TRUE(tester.received_control_command);
// Emergencies (e.g., far from trajectory) produces braking command (0 vel, negative accel)
- EXPECT_DOUBLE_EQ(cmd_msg->longitudinal.speed, 0.0f);
- EXPECT_LT(cmd_msg->longitudinal.acceleration, 0.0f);
+ EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 0.0f);
+ EXPECT_LT(tester.cmd_msg->longitudinal.acceleration, 0.0f);
}
diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp
index 41ed6c9aa62fe..2a000d9902174 100644
--- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp
+++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp
@@ -11,7 +11,8 @@
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "vehicle_cmd_gate/vehicle_cmd_filter.hpp"
+
+#include "vehicle_cmd_filter.hpp"
#include
#include
diff --git a/control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_filter.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp
similarity index 94%
rename from control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_filter.hpp
rename to control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp
index 5562240a35c49..4cb6b6a1cef8f 100644
--- a/control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_filter.hpp
+++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp
@@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#ifndef VEHICLE_CMD_GATE__VEHICLE_CMD_FILTER_HPP_
-#define VEHICLE_CMD_GATE__VEHICLE_CMD_FILTER_HPP_
+#ifndef VEHICLE_CMD_FILTER_HPP_
+#define VEHICLE_CMD_FILTER_HPP_
#include
@@ -74,4 +74,4 @@ class VehicleCmdFilter
double limitDiff(const double curr, const double prev, const double diff_lim) const;
};
-#endif // VEHICLE_CMD_GATE__VEHICLE_CMD_FILTER_HPP_
+#endif // VEHICLE_CMD_FILTER_HPP_
diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
index 4c36a36d476ec..c3fe705ac51a7 100644
--- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
+++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "vehicle_cmd_gate/vehicle_cmd_gate.hpp"
+#include "vehicle_cmd_gate.hpp"
#include
#include
diff --git a/control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp
similarity index 98%
rename from control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_gate.hpp
rename to control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp
index 3af3a5e8814e0..1c3da0ea6e6d0 100644
--- a/control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_gate.hpp
+++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp
@@ -12,10 +12,10 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#ifndef VEHICLE_CMD_GATE__VEHICLE_CMD_GATE_HPP_
-#define VEHICLE_CMD_GATE__VEHICLE_CMD_GATE_HPP_
+#ifndef VEHICLE_CMD_GATE_HPP_
+#define VEHICLE_CMD_GATE_HPP_
-#include "vehicle_cmd_gate/vehicle_cmd_filter.hpp"
+#include "vehicle_cmd_filter.hpp"
#include
#include
@@ -252,4 +252,4 @@ class VehicleCmdGate : public rclcpp::Node
};
} // namespace vehicle_cmd_gate
-#endif // VEHICLE_CMD_GATE__VEHICLE_CMD_GATE_HPP_
+#endif // VEHICLE_CMD_GATE_HPP_
diff --git a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp
index 382380ce82b95..47c9bde90b8b2 100644
--- a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp
+++ b/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "vehicle_cmd_gate/vehicle_cmd_filter.hpp"
+#include "../../src/vehicle_cmd_filter.hpp"
#include
diff --git a/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml b/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml
index a98ac01548732..1feed0327ef4d 100644
--- a/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml
+++ b/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml
@@ -3,13 +3,15 @@
+
-
-
+
+
+
diff --git a/launch/tier4_autoware_api_launch/launch/include/external_api_adaptor.launch.py b/launch/tier4_autoware_api_launch/launch/include/external_api_adaptor.launch.py
index e742ad59cd4e6..fc730ff19bf40 100644
--- a/launch/tier4_autoware_api_launch/launch/include/external_api_adaptor.launch.py
+++ b/launch/tier4_autoware_api_launch/launch/include/external_api_adaptor.launch.py
@@ -54,6 +54,7 @@ def add_launch_arg(name: str, default_value=None, description=None):
_create_api_node("operator", "Operator"),
_create_api_node("metadata_packages", "MetadataPackages"),
_create_api_node("route", "Route"),
+ _create_api_node("rtc_controller", "RTCController"),
_create_api_node("service", "Service"),
_create_api_node("vehicle_status", "VehicleStatus"),
_create_api_node("velocity", "Velocity"),
diff --git a/launch/tier4_autoware_api_launch/package.xml b/launch/tier4_autoware_api_launch/package.xml
index 9b7abb00a8c91..44a78ff274eca 100644
--- a/launch/tier4_autoware_api_launch/package.xml
+++ b/launch/tier4_autoware_api_launch/package.xml
@@ -12,9 +12,11 @@
autoware_cmake
+ ad_api_adaptors
autoware_iv_external_api_adaptor
autoware_iv_internal_api_adaptor
awapi_awiv_adapter
+ default_ad_api
path_distance_calculator
topic_tools
diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py
index 471c1ad4541c0..044c5e3a8136a 100644
--- a/launch/tier4_control_launch/launch/control.launch.py
+++ b/launch/tier4_control_launch/launch/control.launch.py
@@ -99,6 +99,7 @@ def launch_setup(context, *args, **kwargs):
("~/input/reference_trajectory", "/planning/scenario_planning/trajectory"),
("~/input/current_odometry", "/localization/kinematic_state"),
("~/input/current_steering", "/vehicle/status/steering_status"),
+ ("~/input/current_accel", "/localization/acceleration"),
("~/output/predicted_trajectory", "lateral/predicted_trajectory"),
("~/output/lateral_diagnostic", "lateral/diagnostic"),
("~/output/slope_angle", "longitudinal/slope_angle"),
diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml
index e77c988f4ed26..13102ea5f9b70 100644
--- a/launch/tier4_localization_launch/package.xml
+++ b/launch/tier4_localization_launch/package.xml
@@ -12,6 +12,7 @@
autoware_cmake
+ automatic_pose_initializer
ekf_localizer
gyro_odometer
ndt_scan_matcher
diff --git a/launch/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml b/launch/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml
index 195b558c35d07..1790315ae557d 100644
--- a/launch/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml
+++ b/launch/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml
@@ -26,10 +26,10 @@
# NOTE(yukke42): The size of trailer is 20 m length x 3 m width.
#UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN
[100.00, 100.00, 100.00, 100.00, 100.00, 100.00, 100.00, 100.00, #UNKNOWN
- 12.10, 12.10, 36.00, 40.00, 60.00, 10000.00, 10000.00, 10000.00, #CAR
- 36.00, 12.10, 36.00, 40.00, 60.00, 10000.00, 10000.00, 10000.00, #TRUCK
- 40.00, 12.10, 36.00, 40.00, 60.00, 10000.00, 10000.00, 10000.00, #BUS
- 60.00, 12.10, 36.00, 40.00, 60.00, 10000.00, 10000.00, 10000.00, #TRAILER
+ 12.10, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #CAR
+ 36.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #TRUCK
+ 60.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #BUS
+ 60.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #TRAILER
2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #MOTORBIKE
2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #BICYCLE
2.00, 10000.00, 10000.00, 10000.00, 10000.00, 1.50, 1.50, 1.00] #PEDESTRIAN
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
index 775456e4721ec..9778aaf079eca 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
@@ -28,6 +28,9 @@
+
+
+
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
index 728948c10b1bd..5746033e98ae1 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
@@ -11,6 +11,9 @@
+
+
+
@@ -68,6 +71,9 @@
+
+
+
diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml
index 0ea817ffefc81..79eb257de6889 100644
--- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml
+++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml
@@ -14,6 +14,7 @@
detection_area_right_margin: 0.5 # [m]
detection_area_left_margin: 0.5 # [m]
detection_area_length: 200.0 # [m]
+ detection_area_angle_threshold: 0.785 # [rad]
min_predicted_path_confidence: 0.05
collision_start_margin_time: 5.0 # [s]
collision_end_margin_time: 2.0 # [s]
diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml
index 240ea8def4db8..f8f9a85deed5c 100644
--- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml
+++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml
@@ -11,8 +11,6 @@
min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss]
safe_distance_margin : 6.0 # This is also used as a stop margin [m]
- lpf_gain_for_accel: 0.2 # gain of low pass filter for ego acceleration [-]
-
nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index
nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index
min_behavior_stop_margin: 3.0 # [m]
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py
index f86b9a8858bff..99ab74f0856fe 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py
@@ -160,6 +160,7 @@ def launch_setup(context, *args, **kwargs):
"/planning/scenario_planning/clear_velocity_limit",
),
("~/output/trajectory", "/planning/scenario_planning/lane_driving/trajectory"),
+ ("~/input/acceleration", "/localization/acceleration"),
(
"~/input/pointcloud",
"/perception/obstacle_segmentation/pointcloud",
@@ -198,6 +199,7 @@ def launch_setup(context, *args, **kwargs):
remappings=[
("~/input/trajectory", "obstacle_avoidance_planner/trajectory"),
("~/input/odometry", "/localization/kinematic_state"),
+ ("~/input/acceleration", "/localization/acceleration"),
("~/input/objects", "/perception/object_recognition/objects"),
("~/output/trajectory", "/planning/scenario_planning/lane_driving/trajectory"),
("~/output/velocity_limit", "/planning/scenario_planning/max_velocity_candidates"),
diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml
index e7b0bdbdd3bfe..f9cbe4541961e 100644
--- a/launch/tier4_planning_launch/package.xml
+++ b/launch/tier4_planning_launch/package.xml
@@ -11,14 +11,18 @@
autoware_cmake
+ behavior_path_planner
behavior_velocity_planner
costmap_generator
+ external_cmd_selector
external_velocity_limit_selector
freespace_planner
mission_planner
motion_velocity_smoother
obstacle_avoidance_planner
+ obstacle_cruise_planner
obstacle_stop_planner
+ planning_error_monitor
scenario_selector
surround_obstacle_checker
diff --git a/launch/tier4_system_launch/config/system_monitor/hdd_monitor.param.yaml b/launch/tier4_system_launch/config/system_monitor/hdd_monitor.param.yaml
index 77a23eb0f9aa8..04dd3afd09280 100644
--- a/launch/tier4_system_launch/config/system_monitor/hdd_monitor.param.yaml
+++ b/launch/tier4_system_launch/config/system_monitor/hdd_monitor.param.yaml
@@ -5,10 +5,19 @@
disks: # Until multi type lists are allowed, name N the disks as disk0...disk{N-1}
disk0:
name: /dev/sda3
+ temp_attribute_id: 0xC2
temp_warn: 55.0
temp_error: 70.0
+ power_on_hours_attribute_id: 0x09
power_on_hours_warn: 3000000
+ total_data_written_attribute_id: 0xF1
total_data_written_warn: 4915200 # =150TB (1unit=32MB)
total_data_written_safety_factor: 0.05
+ recovered_error_attribute_id: 0xC3
+ recovered_error_warn: 1
free_warn: 5120 # MB(8hour)
free_error: 100 # MB(last 1 minute)
+ read_data_rate_warn: 360.0 # MB/s
+ write_data_rate_warn: 103.5 # MB/s
+ read_iops_warn: 63360.0 # IOPS
+ write_iops_warn: 24120.0 # IOPS
diff --git a/launch/tier4_system_launch/config/system_monitor/net_monitor.param.yaml b/launch/tier4_system_launch/config/system_monitor/net_monitor.param.yaml
index e91aa4ca3fbb3..d72b8d1334c17 100644
--- a/launch/tier4_system_launch/config/system_monitor/net_monitor.param.yaml
+++ b/launch/tier4_system_launch/config/system_monitor/net_monitor.param.yaml
@@ -5,3 +5,5 @@
monitor_program: "greengrass"
crc_error_check_duration: 1
crc_error_count_threshold: 1
+ reassembles_failed_check_duration: 1
+ reassembles_failed_check_count: 1
diff --git a/launch/tier4_system_launch/package.xml b/launch/tier4_system_launch/package.xml
index f5675b60cce3c..f496ac854ace3 100644
--- a/launch/tier4_system_launch/package.xml
+++ b/launch/tier4_system_launch/package.xml
@@ -5,6 +5,7 @@
0.1.0
The tier4_system_launch package
Kenji Miyake
+ Fumihito Ito
Apache License 2.0
ament_cmake_auto
diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt
index dea57e13b2073..f38815d303d05 100644
--- a/localization/ekf_localizer/CMakeLists.txt
+++ b/localization/ekf_localizer/CMakeLists.txt
@@ -15,6 +15,7 @@ ament_auto_find_build_dependencies()
ament_auto_add_library(ekf_localizer_lib SHARED
src/ekf_localizer.cpp
+ src/mahalanobis.cpp
src/measurement.cpp
src/state_transition.cpp
)
@@ -41,7 +42,9 @@ if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
set(TEST_FILES
+ test/test_mahalanobis.cpp
test/test_measurement.cpp
+ test/test_numeric.cpp
test/test_state_transition.cpp)
foreach(filepath ${TEST_FILES})
diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
index fbe4e3719f488..584325b67d1da 100644
--- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
+++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
@@ -266,18 +266,6 @@ class EKFLocalizer : public rclcpp::Node
*/
void measurementUpdateTwist(const geometry_msgs::msg::TwistWithCovarianceStamped & twist);
- /**
- * @brief check whether a measurement value falls within the mahalanobis distance threshold
- * @param dist_max mahalanobis distance threshold
- * @param estimated current estimated state
- * @param measured measured state
- * @param estimated_cov current estimation covariance
- * @return whether it falls within the mahalanobis distance threshold
- */
- bool mahalanobisGate(
- const double & dist_max, const Eigen::MatrixXd & estimated, const Eigen::MatrixXd & measured,
- const Eigen::MatrixXd & estimated_cov) const;
-
/**
* @brief get transform from frame_id
*/
diff --git a/localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp b/localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp
new file mode 100644
index 0000000000000..ff7c9bcb29270
--- /dev/null
+++ b/localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp
@@ -0,0 +1,28 @@
+// Copyright 2022 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef EKF_LOCALIZER__MAHALANOBIS_HPP_
+#define EKF_LOCALIZER__MAHALANOBIS_HPP_
+
+#include
+#include
+
+double squaredMahalanobis(
+ const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C);
+
+bool mahalanobisGate(
+ const double & dist_max, const Eigen::MatrixXd & x, const Eigen::MatrixXd & obj_x,
+ const Eigen::MatrixXd & cov);
+
+#endif // EKF_LOCALIZER__MAHALANOBIS_HPP_
diff --git a/localization/ekf_localizer/include/ekf_localizer/numeric.hpp b/localization/ekf_localizer/include/ekf_localizer/numeric.hpp
new file mode 100644
index 0000000000000..0719c64e65ac1
--- /dev/null
+++ b/localization/ekf_localizer/include/ekf_localizer/numeric.hpp
@@ -0,0 +1,26 @@
+// Copyright 2022 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef EKF_LOCALIZER__NUMERIC_HPP_
+#define EKF_LOCALIZER__NUMERIC_HPP_
+
+#include
+
+#include
+
+inline bool hasInf(const Eigen::MatrixXd & v) { return v.array().isInf().any(); }
+
+inline bool hasNan(const Eigen::MatrixXd & v) { return v.array().isNaN().any(); }
+
+#endif // EKF_LOCALIZER__NUMERIC_HPP_
diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp
index dcece543291f7..be9bba8197be2 100644
--- a/localization/ekf_localizer/src/ekf_localizer.cpp
+++ b/localization/ekf_localizer/src/ekf_localizer.cpp
@@ -14,8 +14,10 @@
#include "ekf_localizer/ekf_localizer.hpp"
+#include "ekf_localizer/mahalanobis.hpp"
#include "ekf_localizer/matrix_types.hpp"
#include "ekf_localizer/measurement.hpp"
+#include "ekf_localizer/numeric.hpp"
#include "ekf_localizer/state_index.hpp"
#include "ekf_localizer/state_transition.hpp"
#include "ekf_localizer/warning.hpp"
@@ -484,7 +486,7 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar
Eigen::MatrixXd y(dim_y, 1);
y << pose.pose.pose.position.x, pose.pose.pose.position.y, yaw;
- if (isnan(y.array()).any() || isinf(y.array()).any()) {
+ if (hasNan(y) || hasInf(y)) {
warning_.warn(
"[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose message.");
return;
@@ -565,7 +567,7 @@ void EKFLocalizer::measurementUpdateTwist(
Eigen::MatrixXd y(dim_y, 1);
y << twist.twist.twist.linear.x, twist.twist.twist.angular.z;
- if (isnan(y.array()).any() || isinf(y.array()).any()) {
+ if (hasNan(y) || hasInf(y)) {
warning_.warn(
"[EKF] twist measurement matrix includes NaN of Inf. ignore update. check twist message.");
return;
@@ -603,24 +605,6 @@ void EKFLocalizer::measurementUpdateTwist(
DEBUG_PRINT_MAT((X_result - X_curr).transpose());
}
-/*
- * mahalanobisGate
- */
-bool EKFLocalizer::mahalanobisGate(
- const double & dist_max, const Eigen::MatrixXd & x, const Eigen::MatrixXd & obj_x,
- const Eigen::MatrixXd & cov) const
-{
- Eigen::MatrixXd mahalanobis_squared = (x - obj_x).transpose() * cov.inverse() * (x - obj_x);
- DEBUG_INFO(
- get_logger(), "measurement update: mahalanobis = %f, gate limit = %f",
- std::sqrt(mahalanobis_squared(0)), dist_max);
- if (mahalanobis_squared(0) > dist_max * dist_max) {
- return false;
- }
-
- return true;
-}
-
/*
* publishEstimateResult
*/
diff --git a/localization/ekf_localizer/src/mahalanobis.cpp b/localization/ekf_localizer/src/mahalanobis.cpp
new file mode 100644
index 0000000000000..0584165855bdb
--- /dev/null
+++ b/localization/ekf_localizer/src/mahalanobis.cpp
@@ -0,0 +1,29 @@
+// Copyright 2022 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "ekf_localizer/mahalanobis.hpp"
+
+double squaredMahalanobis(
+ const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C)
+{
+ const Eigen::VectorXd d = x - y;
+ return d.dot(C.inverse() * d);
+}
+
+bool mahalanobisGate(
+ const double & dist_max, const Eigen::MatrixXd & x, const Eigen::MatrixXd & obj_x,
+ const Eigen::MatrixXd & cov)
+{
+ return squaredMahalanobis(x, obj_x, cov) <= dist_max * dist_max;
+}
diff --git a/localization/ekf_localizer/test/test_mahalanobis.cpp b/localization/ekf_localizer/test/test_mahalanobis.cpp
new file mode 100644
index 0000000000000..a9f6c1a1bc38b
--- /dev/null
+++ b/localization/ekf_localizer/test/test_mahalanobis.cpp
@@ -0,0 +1,52 @@
+// Copyright 2022 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "ekf_localizer/mahalanobis.hpp"
+
+#include
+
+constexpr double tolerance = 1e-8;
+
+TEST(MahalanobisGate, SquaredMahalanobis)
+{
+ {
+ Eigen::Vector2d x(0, 1);
+ Eigen::Vector2d y(3, 2);
+ Eigen::Matrix2d C;
+ C << 10, 0, 0, 10;
+
+ EXPECT_NEAR(squaredMahalanobis(x, y, C), 1.0, tolerance);
+ }
+
+ {
+ Eigen::Vector2d x(4, 1);
+ Eigen::Vector2d y(1, 5);
+ Eigen::Matrix2d C;
+ C << 5, 0, 0, 5;
+
+ EXPECT_NEAR(squaredMahalanobis(x, y, C), 5.0, tolerance);
+ }
+}
+
+TEST(MahalanobisGate, MahalanobisGate)
+{
+ Eigen::Vector2d x(0, 1);
+ Eigen::Vector2d y(3, 2);
+ Eigen::Matrix2d C;
+ C << 10, 0, 0, 10;
+
+ EXPECT_FALSE(mahalanobisGate(0.99, x, y, C));
+ EXPECT_FALSE(mahalanobisGate(1.00, x, y, C));
+ EXPECT_TRUE(mahalanobisGate(1.01, x, y, C));
+}
diff --git a/localization/ekf_localizer/test/test_numeric.cpp b/localization/ekf_localizer/test/test_numeric.cpp
new file mode 100644
index 0000000000000..baf4f46db79a9
--- /dev/null
+++ b/localization/ekf_localizer/test/test_numeric.cpp
@@ -0,0 +1,47 @@
+// Copyright 2022 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "ekf_localizer/numeric.hpp"
+
+#include
+
+#include
+
+TEST(Numeric, HasNan)
+{
+ const Eigen::VectorXd empty(0);
+ const double inf = std::numeric_limits::infinity();
+ const double nan = std::nan("");
+
+ EXPECT_FALSE(hasNan(empty));
+ EXPECT_FALSE(hasNan(Eigen::Vector3d(0., 0., 1.)));
+ EXPECT_FALSE(hasNan(Eigen::Vector3d(1e16, 0., 1.)));
+ EXPECT_FALSE(hasNan(Eigen::Vector3d(0., 1., inf)));
+
+ EXPECT_TRUE(hasNan(Eigen::Vector3d(nan, 1., 0.)));
+}
+
+TEST(Numeric, HasInf)
+{
+ const Eigen::VectorXd empty(0);
+ const double inf = std::numeric_limits::infinity();
+ const double nan = std::nan("");
+
+ EXPECT_FALSE(hasInf(empty));
+ EXPECT_FALSE(hasInf(Eigen::Vector3d(0., 0., 1.)));
+ EXPECT_FALSE(hasInf(Eigen::Vector3d(1e16, 0., 1.)));
+ EXPECT_FALSE(hasInf(Eigen::Vector3d(nan, 1., 0.)));
+
+ EXPECT_TRUE(hasInf(Eigen::Vector3d(0., 1., inf)));
+}
diff --git a/localization/pose_initializer/docs/pose_initializer.md b/localization/pose_initializer/docs/pose_initializer.md
index 08d27ae83b51d..c1b2cc3d8b353 100644
--- a/localization/pose_initializer/docs/pose_initializer.md
+++ b/localization/pose_initializer/docs/pose_initializer.md
@@ -21,9 +21,9 @@ Finally, it publishes the initial pose to `ekf_localizer`.
### Services
-| Name | Type | Description |
-| -------------------------- | ------------------------------------------------- | --------------------- |
-| `/localization/initialize` | autoware_ad_api_msgs::srv::InitializeLocalization | initial pose from api |
+| Name | Type | Description |
+| -------------------------- | --------------------------------------------------- | --------------------- |
+| `/localization/initialize` | autoware_adapi_v1_msgs::srv::InitializeLocalization | initial pose from api |
### Clients
@@ -40,7 +40,7 @@ Finally, it publishes the initial pose to `ekf_localizer`.
### Publications
-| Name | Type | Description |
-| ------------------------------------ | ---------------------------------------------------------- | --------------------------- |
-| `/localization/initialization_state` | autoware_ad_api_msgs::msg::LocalizationInitializationState | pose initialization state |
-| `/initialpose3d` | geometry_msgs::msg::PoseWithCovarianceStamped | calculated initial ego pose |
+| Name | Type | Description |
+| ------------------------------------ | ------------------------------------------------------------ | --------------------------- |
+| `/localization/initialization_state` | autoware_adapi_v1_msgs::msg::LocalizationInitializationState | pose initialization state |
+| `/initialpose3d` | geometry_msgs::msg::PoseWithCovarianceStamped | calculated initial ego pose |
diff --git a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp
index 33f33a05e3393..72408f4af8185 100644
--- a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp
+++ b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp
@@ -22,6 +22,9 @@
#include
#include
+#include
+
+using autoware_auto_mapping_msgs::msg::HADMapBin;
class Lanelet2MapLoaderNode : public rclcpp::Node
{
@@ -29,7 +32,12 @@ class Lanelet2MapLoaderNode : public rclcpp::Node
explicit Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options);
private:
- rclcpp::Publisher::SharedPtr pub_map_bin_;
+ rclcpp::Publisher::SharedPtr pub_map_bin_;
+
+ lanelet::LaneletMapPtr load_map(
+ const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type);
+ HADMapBin create_map_bin_msg(
+ const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename);
};
#endif // MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_
diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp
index 58330e44abf5c..dc5d560b98e8e 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
@@ -51,47 +51,73 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options
{
const auto lanelet2_filename = declare_parameter("lanelet2_map_path", "");
const auto lanelet2_map_projector_type = declare_parameter("lanelet2_map_projector_type", "MGRS");
+ const auto center_line_resolution = declare_parameter("center_line_resolution", 5.0);
+
+ // load map from file
+ const auto map = load_map(lanelet2_filename, lanelet2_map_projector_type);
+ if (!map) {
+ return;
+ }
+
+ // overwrite centerline
+ lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false);
+
+ // create map bin msg
+ const auto map_bin_msg = create_map_bin_msg(map, lanelet2_filename);
+
+ // create publisher and publish
+ pub_map_bin_ =
+ create_publisher("output/lanelet2_map", rclcpp::QoS{1}.transient_local());
+ pub_map_bin_->publish(map_bin_msg);
+}
+
+lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map(
+ const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type)
+{
lanelet::ErrorMessages errors{};
- lanelet::LaneletMapPtr map;
if (lanelet2_map_projector_type == "MGRS") {
lanelet::projection::MGRSProjector projector{};
- map = lanelet::load(lanelet2_filename, projector, &errors);
+ const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors);
+ if (errors.empty()) {
+ return map;
+ }
} else if (lanelet2_map_projector_type == "UTM") {
- double map_origin_lat = this->declare_parameter("latitude", 0.0);
- double map_origin_lon = this->declare_parameter("longitude", 0.0);
+ const double map_origin_lat = declare_parameter("latitude", 0.0);
+ const double map_origin_lon = declare_parameter("longitude", 0.0);
lanelet::GPSPoint position{map_origin_lat, map_origin_lon};
lanelet::Origin origin{position};
lanelet::projection::UtmProjector projector{origin};
- map = lanelet::load(lanelet2_filename, projector, &errors);
+
+ const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors);
+ if (errors.empty()) {
+ return map;
+ }
} else {
- RCLCPP_ERROR(this->get_logger(), "lanelet2_map_projector_type is not supported");
+ RCLCPP_ERROR(get_logger(), "lanelet2_map_projector_type is not supported");
+ return nullptr;
}
for (const auto & error : errors) {
- RCLCPP_ERROR_STREAM(this->get_logger(), error);
+ RCLCPP_ERROR_STREAM(get_logger(), error);
}
- if (!errors.empty()) {
- return;
- }
-
- const auto center_line_resolution = this->declare_parameter("center_line_resolution", 5.0);
- lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false);
+ return nullptr;
+}
+HADMapBin Lanelet2MapLoaderNode::create_map_bin_msg(
+ const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename)
+{
std::string format_version{}, map_version{};
lanelet::io_handlers::AutowareOsmParser::parseVersions(
lanelet2_filename, &format_version, &map_version);
- pub_map_bin_ = this->create_publisher(
- "output/lanelet2_map", rclcpp::QoS{1}.transient_local());
-
- autoware_auto_mapping_msgs::msg::HADMapBin map_bin_msg;
- map_bin_msg.header.stamp = this->now();
+ HADMapBin map_bin_msg;
+ map_bin_msg.header.stamp = now();
map_bin_msg.header.frame_id = "map";
map_bin_msg.format_version = format_version;
map_bin_msg.map_version = map_version;
lanelet::utils::conversion::toBinMsg(map, &map_bin_msg);
- pub_map_bin_->publish(map_bin_msg);
+ return map_bin_msg;
}
#include
diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt
index ad048abd41962..c44a6fb3cc215 100644
--- a/perception/image_projection_based_fusion/CMakeLists.txt
+++ b/perception/image_projection_based_fusion/CMakeLists.txt
@@ -177,6 +177,11 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL)
EXECUTABLE pointpainting_fusion_node
)
+ install(
+ TARGETS pointpainting_cuda_lib
+ DESTINATION lib
+ )
+
else()
message("Skipping build of some nodes due to missing dependencies")
endif()
diff --git a/perception/image_projection_based_fusion/config/pointpainting.param.yaml b/perception/image_projection_based_fusion/config/pointpainting.param.yaml
index 3aeebe2db4148..0be983c455dce 100755
--- a/perception/image_projection_based_fusion/config/pointpainting.param.yaml
+++ b/perception/image_projection_based_fusion/config/pointpainting.param.yaml
@@ -1,7 +1,6 @@
/**:
ros__parameters:
class_names: ["CAR", "PEDESTRIAN", "BICYCLE"]
- rename_car_to_truck_and_bus: true
point_feature_size: 6 # x, y, z and car, pedestrian, bicycle
max_voxel_size: 40000
point_cloud_range: [-76.8, -76.8, -3.0, 76.8, 76.8, 5.0]
diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp
index 117722b72320c..53f21089b768d 100644
--- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp
+++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp
@@ -21,6 +21,7 @@
#include
#include
#include
+#include
#include