diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index f0e0d3f73499f..3b8bae300e2fa 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -141,7 +141,6 @@ sensing/vehicle_velocity_converter/** ryu.yamamoto@tier4.jp @autowarefoundation/ simulator/dummy_perception_publisher/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners simulator/fault_injection/** keisuke.shima@tier4.jp @autowarefoundation/autoware-global-codeowners simulator/simple_planning_simulator/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners -system/ad_service_state_monitor/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners system/bluetooth_monitor/** fumihito.ito@tier4.jp @autowarefoundation/autoware-global-codeowners system/component_state_monitor/** isamu.takagi@tier4.jp @autowarefoundation/autoware-global-codeowners system/default_ad_api/** isamu.takagi@tier4.jp @autowarefoundation/autoware-global-codeowners diff --git a/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml b/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml index be18812b59eba..45014428d0bd1 100644 --- a/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml +++ b/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - transition_timeout: 3.0 + transition_timeout: 10.0 frequency_hz: 10.0 engage_acceptable_limits: allow_autonomous_in_stopped: true # no check if the velocity is zero, always allowed. diff --git a/control/operation_mode_transition_manager/src/node.cpp b/control/operation_mode_transition_manager/src/node.cpp index 64a40b31289de..ddd37b0206fad 100644 --- a/control/operation_mode_transition_manager/src/node.cpp +++ b/control/operation_mode_transition_manager/src/node.cpp @@ -124,8 +124,8 @@ void OperationModeTransitionManager::changeOperationMode(std::optionalprevious; if (previous) { + compatibility_transition_ = now(); current_mode_ = previous.value(); } else { + compatibility_transition_ = std::nullopt; changeControlMode(ControlModeCommand::Request::MANUAL); } transition_.reset(); - compatibility_transition_ = std::nullopt; } void OperationModeTransitionManager::processTransition() @@ -211,7 +212,7 @@ void OperationModeTransitionManager::onTimer() // Check sync timeout to the compatible interface. if (compatibility_transition_) { - if (transition_timeout_ < (now() - compatibility_transition_.value()).seconds()) { + if (compatibility_timeout_ < (now() - compatibility_transition_.value()).seconds()) { compatibility_transition_ = std::nullopt; } } @@ -224,8 +225,10 @@ void OperationModeTransitionManager::onTimer() } // Reset sync timeout when it is completed. - if (current_mode_ == compatibility_.get_mode()) { - compatibility_transition_ = std::nullopt; + if (compatibility_transition_) { + if (current_mode_ == compatibility_.get_mode()) { + compatibility_transition_ = std::nullopt; + } } if (transition_) { diff --git a/control/operation_mode_transition_manager/src/node.hpp b/control/operation_mode_transition_manager/src/node.hpp index 71073902231a2..fafc68a50b9e4 100644 --- a/control/operation_mode_transition_manager/src/node.hpp +++ b/control/operation_mode_transition_manager/src/node.hpp @@ -71,6 +71,7 @@ class OperationModeTransitionManager : public rclcpp::Node std::optional prev_state_; + static constexpr double compatibility_timeout_ = 1.0; Compatibility compatibility_; std::optional compatibility_transition_; }; diff --git a/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml b/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml index be18812b59eba..45014428d0bd1 100644 --- a/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml +++ b/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - transition_timeout: 3.0 + transition_timeout: 10.0 frequency_hz: 10.0 engage_acceptable_limits: allow_autonomous_in_stopped: true # no check if the velocity is zero, always allowed. diff --git a/launch/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.param.yaml b/launch/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.param.yaml deleted file mode 100644 index 7a16c35390490..0000000000000 --- a/launch/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.param.yaml +++ /dev/null @@ -1,155 +0,0 @@ -# AD Service State Monitor Parameters -/**: - ros__parameters: - # modules_names: string array - module_names: [ - "map", - "sensing", - "perception", - "localization", - "planning", - "control", - "vehicle", - "system" - ] - - # Topic Config - ## names: string array - ## configs: top level - ### names used to declare parameter group - #### module: string - #### timeout[s]: double (0: none) - #### warn_rate[hz]: double (0: none) - topic_configs: - names: [ - "/map/vector_map", - "/map/pointcloud_map", - "/perception/obstacle_segmentation/pointcloud", - "/perception/object_recognition/objects", - "/initialpose3d", - "/localization/pose_twist_fusion_filter/pose", - "/planning/mission_planning/route", - "/planning/scenario_planning/trajectory", - "/control/trajectory_follower/control_cmd", - "/control/command/control_cmd", - "/vehicle/status/velocity_status", - "/vehicle/status/steering_status", - "/system/emergency/control_cmd" - ] - - configs: - /map/vector_map: - module: "map" - timeout: 0.0 - warn_rate: 0.0 - type: "autoware_auto_mapping_msgs/msg/HADMapBin" - transient_local: True - - /map/pointcloud_map: - module: "map" - timeout: 0.0 - warn_rate: 0.0 - type: "sensor_msgs/msg/PointCloud2" - transient_local: True - - /perception/obstacle_segmentation/pointcloud: - module: "sensing" - timeout: 1.0 - warn_rate: 5.0 - type: "sensor_msgs/msg/PointCloud2" - best_effort: True - - /initialpose3d: - module: "localization" - timeout: 0.0 - warn_rate: 0.0 - type: "geometry_msgs/msg/PoseWithCovarianceStamped" - - /localization/pose_twist_fusion_filter/pose: - module: "localization" - timeout: 1.0 - warn_rate: 5.0 - type: "geometry_msgs/msg/PoseStamped" - - # Can be both with feature array or without - # In prediction.launch.xml this is set to without - /perception/object_recognition/objects: - module: "perception" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_perception_msgs/msg/PredictedObjects" - # This topic could have two different types depending on the launch flags used. - # The implementation of subscriptions in ROS2 does not allow for multiple types - # to be defined for a topic. By default this is set to not use have features. - # type: ["tier4_perception_msgs/msg/DynamicObjectArray", "tier4_perception_msgs/msg/DynamicObjectWithFeatureArray"] - - /planning/mission_planning/route: - module: "planning" - timeout: 0.0 - warn_rate: 0.0 - type: "autoware_auto_planning_msgs/msg/HADMapRoute" - transient_local: True - - /planning/scenario_planning/trajectory: - module: "planning" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_planning_msgs/msg/Trajectory" - - /control/trajectory_follower/control_cmd: - module: "control" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_control_msgs/msg/AckermannControlCommand" - - /control/command/control_cmd: - module: "control" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_control_msgs/msg/AckermannControlCommand" - - /vehicle/status/velocity_status: - module: "vehicle" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_vehicle_msgs/msg/VelocityReport" - - /vehicle/status/steering_status: - module: "vehicle" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_vehicle_msgs/msg/SteeringReport" - - /system/emergency/control_cmd: - module: "system" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_control_msgs/msg/AckermannControlCommand" - - # Param Config - ## names: string array - ## configs: top level - ### names used to declare parameter group - #### module: string - # param_configs: - # names: ["/vehicle_info"] - # configs: - # /vehicle_info: - # module: "vehicle" - - # TF Config - ## names: string array - ## configs: top level - ### names used to declare parameter group - #### module: string - #### from: string - #### to: string - #### timeout[s]: double (0: none) - tf_configs: - names: ["map_to_base_link"] - configs: - map_to_base_link: - module: "localization" - from: "map" - to: "base_link" - timeout: 1.0 diff --git a/launch/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.planning_simulation.param.yaml b/launch/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.planning_simulation.param.yaml deleted file mode 100644 index 51fbe99dd9bc3..0000000000000 --- a/launch/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.planning_simulation.param.yaml +++ /dev/null @@ -1,126 +0,0 @@ -# AD Service State Monitor: Planning Simulator Parameters -/**: - ros__parameters: - # modules_names: string array - module_names: [ - "map", - "sensing", - "perception", - "localization", - "planning", - "control", - "vehicle", - "system" - ] - -# Topic Config - ## names: string array - ## configs: top level - ### names used to declare parameter group - #### module: string - #### timeout[s]: double (0: none) - #### warn_rate[hz]: double (0: none) - topic_configs: - names: [ - "/map/vector_map", - "/perception/object_recognition/objects", - "/initialpose3d", - "/planning/mission_planning/route", - "/planning/scenario_planning/trajectory", - "/control/trajectory_follower/control_cmd", - "/control/command/control_cmd", - "/vehicle/status/velocity_status", - "/vehicle/status/steering_status", - "/system/emergency/control_cmd" - ] - - configs: - /map/vector_map: - module: "map" - timeout: 0.0 - warn_rate: 0.0 - type: "autoware_auto_mapping_msgs/msg/HADMapBin" - transient_local: True - - /perception/object_recognition/objects: - module: "perception" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_perception_msgs/msg/PredictedObjects" - - /initialpose3d: - module: "localization" - timeout: 0.0 - warn_rate: 0.0 - type: "geometry_msgs/msg/PoseWithCovarianceStamped" - - /planning/mission_planning/route: - module: "planning" - timeout: 0.0 - warn_rate: 0.0 - type: "autoware_auto_planning_msgs/msg/HADMapRoute" - transient_local: True - - /planning/scenario_planning/trajectory: - module: "planning" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_planning_msgs/msg/Trajectory" - - /control/trajectory_follower/control_cmd: - module: "control" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_control_msgs/msg/AckermannControlCommand" - - /control/command/control_cmd: - module: "control" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_control_msgs/msg/AckermannControlCommand" - - /vehicle/status/velocity_status: - module: "vehicle" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_vehicle_msgs/msg/VelocityReport" - - /vehicle/status/steering_status: - module: "vehicle" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_vehicle_msgs/msg/SteeringReport" - - /system/emergency/control_cmd: - module: "system" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_control_msgs/msg/AckermannControlCommand" - - # Param Config - ## names: string array - ## configs: top level - ### names used to declare parameter group - #### module: string - # param_configs: - # names: ["/vehicle_info"] - # configs: - # /vehicle_info: - # module: "vehicle" - - # TF Config - ## names: string array - ## configs: top level - ### names used to declare parameter group - #### module: string - #### from: string - #### to: string - #### timeout[s]: double (0: none) - tf_configs: - names: ["map_to_base_link"] - configs: - map_to_base_link: - module: "localization" - from: "map" - to: "base_link" - timeout: 1.0 diff --git a/launch/tier4_system_launch/config/component_state_monitor/topics.yaml b/launch/tier4_system_launch/config/component_state_monitor/topics.yaml new file mode 100644 index 0000000000000..b55dfa5aecd9f --- /dev/null +++ b/launch/tier4_system_launch/config/component_state_monitor/topics.yaml @@ -0,0 +1,182 @@ +- module: map + mode: [online, planning_simulation] + type: launch + args: + node_name_suffix: vector_map + topic: /map/vector_map + topic_type: autoware_auto_mapping_msgs/msg/HADMapBin + best_effort: false + transient_local: true + warn_rate: 0.0 + error_rate: 0.0 + timeout: 0.0 + +- module: map + mode: [online] + type: launch + args: + node_name_suffix: pointcloud_map + topic: /map/pointcloud_map + topic_type: sensor_msgs/msg/PointCloud2 + best_effort: false + transient_local: true + warn_rate: 0.0 + error_rate: 0.0 + timeout: 0.0 + +- module: localization + mode: [online, planning_simulation] + type: autonomous + args: + node_name_suffix: initialpose3d + topic: /initialpose3d + topic_type: geometry_msgs/msg/PoseWithCovarianceStamped + best_effort: false + transient_local: false + warn_rate: 0.0 + error_rate: 0.0 + timeout: 0.0 + +- module: localization + mode: [online] + type: autonomous + args: + node_name_suffix: pose_twist_fusion_filter_pose + topic: /localization/pose_twist_fusion_filter/pose + topic_type: geometry_msgs/msg/PoseStamped + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- module: perception + mode: [online] + type: launch + args: + node_name_suffix: obstacle_segmentation_pointcloud + topic: /perception/obstacle_segmentation/pointcloud + topic_type: sensor_msgs/msg/PointCloud2 + best_effort: true + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- module: perception + mode: [online, planning_simulation] + type: autonomous + args: + node_name_suffix: object_recognition_objects + topic: /perception/object_recognition/objects + topic_type: autoware_auto_perception_msgs/msg/PredictedObjects + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- module: planning + mode: [online, planning_simulation] + type: autonomous + args: + node_name_suffix: mission_planning_route + topic: /planning/mission_planning/route + topic_type: autoware_auto_planning_msgs/msg/HADMapRoute + best_effort: false + transient_local: true + warn_rate: 0.0 + error_rate: 0.0 + timeout: 0.0 + +- module: planning + mode: [online, planning_simulation] + type: autonomous + args: + node_name_suffix: scenario_planning_trajectory + topic: /planning/scenario_planning/trajectory + topic_type: autoware_auto_planning_msgs/msg/Trajectory + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- module: control + mode: [online, planning_simulation] + type: autonomous + args: + node_name_suffix: trajectory_follower_control_cmd + topic: /control/trajectory_follower/control_cmd + topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- module: control + mode: [online, planning_simulation] + type: autonomous + args: + node_name_suffix: control_command_control_cmd + topic: /control/command/control_cmd + topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- module: vehicle + mode: [online, planning_simulation] + type: autonomous + args: + node_name_suffix: vehicle_status_velocity_status + topic: /vehicle/status/velocity_status + topic_type: autoware_auto_vehicle_msgs/msg/VelocityReport + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- module: vehicle + mode: [online, planning_simulation] + type: autonomous + args: + node_name_suffix: vehicle_status_steering_status + topic: /vehicle/status/steering_status + topic_type: autoware_auto_vehicle_msgs/msg/SteeringReport + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- module: system + mode: [online, planning_simulation] + type: launch + args: + node_name_suffix: system_emergency_control_cmd + topic: /system/emergency/control_cmd + topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- module: localization + mode: [online, planning_simulation] + type: autonomous + args: + node_name_suffix: transform_map_to_base_link + topic: /tf + frame_id: map + child_frame_id: base_link + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 diff --git a/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.param.yaml b/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.param.yaml index b9a5a4fa79f52..b9de5cc4f2e13 100644 --- a/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.param.yaml +++ b/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.param.yaml @@ -32,7 +32,7 @@ /autoware/planning/node_alive_monitoring: default /autoware/planning/performance_monitoring/trajectory_validation: default - /autoware/sensing/node_alive_monitoring: default + # /autoware/sensing/node_alive_monitoring: default /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default diff --git a/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.planning_simulation.param.yaml b/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.planning_simulation.param.yaml index ce081b75b2836..bf40c334f6498 100644 --- a/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.planning_simulation.param.yaml +++ b/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.planning_simulation.param.yaml @@ -32,7 +32,7 @@ /autoware/planning/node_alive_monitoring: default /autoware/planning/performance_monitoring/trajectory_validation: default - /autoware/sensing/node_alive_monitoring: default + # /autoware/sensing/node_alive_monitoring: default /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 397f9c1d00dc3..50e86b897c612 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -24,24 +24,11 @@ - - - - - - - - - - + diff --git a/launch/tier4_system_launch/package.xml b/launch/tier4_system_launch/package.xml index f496ac854ace3..beb2a4a782e22 100644 --- a/launch/tier4_system_launch/package.xml +++ b/launch/tier4_system_launch/package.xml @@ -12,7 +12,7 @@ autoware_cmake - ad_service_state_monitor + component_state_monitor emergency_handler system_error_monitor system_monitor diff --git a/launch/tier4_system_launch/system_launch.drawio.svg b/launch/tier4_system_launch/system_launch.drawio.svg index 9b32dc4cc60d2..334012a85b351 100644 --- a/launch/tier4_system_launch/system_launch.drawio.svg +++ b/launch/tier4_system_launch/system_launch.drawio.svg @@ -3,43 +3,54 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="1209px" - height="1051px" - viewBox="-0.5 -0.5 1209 1051" - content="<mxfile><diagram id="hw8opUG3N7cuFXbovIOC" name="ページ1">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</diagram></mxfile>" + width="1131px" + height="882px" + viewBox="-0.5 -0.5 1131 882" + content="<mxfile><diagram id="hw8opUG3N7cuFXbovIOC" name="ページ1">7VpLk6M2EP41rtocxoV5++h5JHvYVKVqDklOLhnEYxckIoRt9tdHAvEQAntmjedRMzMHo5Zoqfv7Wt2SvTDu0uMfBGTRn9iHyULX/OPCuF/oum6sTfbBJWUtWTtOLQhJ7NeiVSd4jH9CIdSEtIh9mEsDKcYJjTNZ6GGEoEclGSAEH+RhAU7kWTMQQkXw6IFElf4d+zSqpa7udPKvMA6jZuaVva57UtAMFpbkEfDxoScyHhbGHcGY1k/p8Q4m3HmNX+r3fp/obRdGIKJPecGxxDpo2RgHfWaraCKM2MdtRNOEtVbssR7Ox0zOKEQ5LognRukCJEBCKEYZrbWMJhCnkJKSDSEwATTey9qBwCtsx3UmsQdh1YSF9kkLMaERDjECyUMnvYrBtnahxdWrG0JA2RuQ4RjRvKf5Ly5gA5pgcwXTRKgphDgz3nC1gbfrFXS+b015EhyufhkcAUZUbAgra1Z43PcIj63PDI/xRuFxVu8RHteeFx7hmD1ICmFxXuYUpssEFMiLlkcGhG4nzGe3O8KeQlphMCXx4/1QlGcANbIMeD+qTLjh+FQTbeuJmtFsvf0XemJJ9YBRBBfIh9wkjXUfopjCRzYX7z2wgmHAqDhJ7nCCSfWu4XnQCgK+UErwD9jrMWxjbfiCgz25tuH/TL6HhMYsiW+SOESsL419v+IvEAKPcRKSdsUjJOUq4LEnUgkpegdEWAkiaIeuXmhlUb9WaCqDS3KeMUGTbYpRTDFp6JKVz2ZLD0f7v4KXKbfMI/RG+JAzJYEB7XpPkUks523Spvqbhwy2dp4Npnk1NpxOuaLGk73bbfTfMM6ET79DSkuxu4OCYtnjzD2k/Kff+JcrW1pN8/4olNetUrQm/TvIGpfu/cpmvWq9K2AxXUvWUeck8dqJbf+8pjqVKZp+IQUYanA3e7KGQApfLqLPpoymIS/sXeSMGYN/kAnMJyYCd47INxWu8DVvFO9ywGUXyq4Se0Tfr0Kk5M7JLDuGmYxqr3g0L3X+tLfdKznb0hRnI+wP2f8Zlm2QwZVvQWcsLNe2YwD7umH5mnFprRWqYBqx8PnkygRXGFNc3xzjiqvvDPvKXDFfkSu2uq14OM3Y7ovoNqeAwrdS1dsg5SCiXZ5NUGti4R+1MhgeCwz9JY8FjnvyWHD2qqet99uS/jn1/nOvhGyxWunKbj3HueC5d0ItIgI1y5z3jscZSQ4oiVm1NQbXN7CDiQzM00syAvP4J9hV+jg0wnSm3LpdWPeLqfs8heLtVy1C2aL9gqMPqzNxxSo0aUtTX68l34qT64UHvZvmQkRovVm5sgocBDm89GDWbPave3U6FifOq3zzMIwTR5v5qrrZb7s4yRKAUIzCbR6nBbcNo3cfNO4EeA23taXmyFlklpjRZaWN1lkjpuFqD8GFbn7ZA14IkwKx+sCHv6kJP8LprsjPJ/sZUrRhDzZ79yVLPzUT8Gu3AzNoCwnBZFj5/cr1/zwXuuPrmqrg5jy6qGZdYza2m+Z1P4vZIA63rJDkp6JKldbMwQO/GtKUi50aIwiqwlHVPI4oYy1IlyXoEGVm1fo/aHVsjoTeFatj9dj1GXsfJfbUMuIzHpV4tEduQa4Yj2q1B1PIalzkldsIIFabvYFQVJb04VjhaldkBWt2P42rC87uB4bGw/8=</diagram></mxfile>" > - + + + + + + + + + - -
-
-
+ +
+
+
system.launch.xml

- package: tier4_system_launch + package: system_launch
- system.launch.xml... + system.launch.xml... - + - +
-
-
+
+
system_monitor.launch.py

@@ -48,70 +59,16 @@
- system_monitor.launch.py... + system_monitor.launch.py... - - - - - - + - -
-
-
- online -
-
-
- - online - - - - - - - -
-
-
- planning_simulation -
-
-
- - planning_simulation - - - - - - -
-
-
- $(var run_mode) -
-
-
- - $(var run_mode) - - - - - - -
-
-
+ +
+
+
launch name

@@ -122,32 +79,34 @@
- launch name... + launch name... - + - -
-
-
ex:
+ +
+
+
+ ex: +
- ex: + ex: - + - +
-
-
+
+
node name

@@ -158,19 +117,19 @@
- node name... + node name... - + - +
-
-
+
+
other name

@@ -181,189 +140,153 @@
- other name... - - - - - - -
-
-
- ad_service_state_monitor.launch.xml -
-
-
package: ad_service_state_monitor
-
-
-
-
- args: config_file = - ad_service_state_monitor.param.yaml -
-
-
-
- - ad_service_state_monitor.launch.xml... + other name... - + - +
-
-
- ad_service_state_monitor.launch.xml +
+
+ component_state_monitor.launch.py

-
package: ad_service_state_monitor
-
-
-
- args: config_file = - ad_service_state_monitor.planning_simulation.param.yaml + package: + component_state_monitor
- ad_service_state_monitor.launch.xml... + component_state_monitor.launch.py... - - - - + + - -
-
-
+ +
+
+
online
- online + online - - + + - -
-
-
+ +
+
+
planning_simulation
- planning_simulation + planning_simulation - + - +
-
-
+
+
$(var run_mode)
- $(var run_mode) + $(var run_mode) - + - +
-
-
- system_error_monitor.launch.xml +
+
+ autoware_error_monitor.launch.xml

-
package: system_error_monitor
+
package: autoware_error_monitor

args: config_file = - system_error_monitor.param.yaml + autoware_error_monitor.param.yaml
- system_error_monitor.launch.xml... + autoware_error_monitor.launch.xml... - + - +
-
-
- system_error_monitor.launch.xml +
+
+ autoware_error_monitor.launch.xml

-
package: system_error_monitor
+
package: autoware_error_monitor

args: config_file = - system_error_monitor.planning_simulation.param.yaml + autoware_error_monitor.planning_simulation.param.yaml
- system_error_monitor.launch.xml... + autoware_error_monitor.launch.xml... - + - +
-
-
+
+
emergency_handler.launch.xml

@@ -372,11 +295,9 @@
- emergency_handler.launch.xml... + emergency_handler.launch.xml... - - diff --git a/perception/image_projection_based_fusion/config/roi_sync.param.yaml b/perception/image_projection_based_fusion/config/roi_sync.param.yaml new file mode 100644 index 0000000000000..21ba13787f1c0 --- /dev/null +++ b/perception/image_projection_based_fusion/config/roi_sync.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + input_offset_ms: [61.67, 111.67, 45.0, 28.33, 78.33, 95.0] + timeout_ms: 70.0 + match_threshold_ms: 50.0 diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp index 8501e7822d804..c8275839ae39a 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp @@ -17,6 +17,8 @@ #include #include +#include +#include #include #include @@ -32,13 +34,17 @@ #include #include +#include +#include #include +#include #include namespace image_projection_based_fusion { using autoware_auto_perception_msgs::msg::DetectedObject; using autoware_auto_perception_msgs::msg::DetectedObjects; +using sensor_msgs::msg::PointCloud2; using tier4_perception_msgs::msg::DetectedObjectsWithFeature; using tier4_perception_msgs::msg::DetectedObjectWithFeature; @@ -46,26 +52,28 @@ template class FusionNode : public rclcpp::Node { public: + /** \brief constructor. */ explicit FusionNode(const std::string & node_name, const rclcpp::NodeOptions & options); + /** \brief constructor. + * \param queue_size the maximum queue size + */ + explicit FusionNode( + const std::string & node_name, const rclcpp::NodeOptions & options, int queue_size); protected: void cameraInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, const std::size_t camera_id); - void fusionCallback( - typename Msg::ConstSharedPtr input_msg, - DetectedObjectsWithFeature::ConstSharedPtr input_roi0_msg, - DetectedObjectsWithFeature::ConstSharedPtr input_roi1_msg, - DetectedObjectsWithFeature::ConstSharedPtr input_roi2_msg, - DetectedObjectsWithFeature::ConstSharedPtr input_roi3_msg, - DetectedObjectsWithFeature::ConstSharedPtr input_roi4_msg, - DetectedObjectsWithFeature::ConstSharedPtr input_roi5_msg, - DetectedObjectsWithFeature::ConstSharedPtr input_roi6_msg, - DetectedObjectsWithFeature::ConstSharedPtr input_roi7_msg); - virtual void preprocess(Msg & output_msg); + // callback for Msg subscription + virtual void subCallback(const typename Msg::ConstSharedPtr input_msg); + + // callback for roi subscription + virtual void roiCallback( + const DetectedObjectsWithFeature::ConstSharedPtr input_roi_msg, const std::size_t roi_i); + virtual void fuseOnSingleImage( const Msg & input_msg, const std::size_t image_id, const DetectedObjectsWithFeature & input_roi_msg, @@ -76,6 +84,9 @@ class FusionNode : public rclcpp::Node void publish(const Msg & output_msg); + void timer_callback(); + void setPeriod(const int64_t new_period); + std::size_t rois_number_{1}; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; @@ -84,22 +95,25 @@ class FusionNode : public rclcpp::Node std::map camera_info_map_; std::vector::SharedPtr> camera_info_subs_; - // fusion - typename message_filters::Subscriber sub_; - message_filters::PassThrough passthrough_; - std::vector>> rois_subs_; - inline void dummyCallback(DetectedObjectsWithFeature::ConstSharedPtr input) - { - passthrough_.add(input); - } - using SyncPolicy = message_filters::sync_policies::ApproximateTime< - Msg, DetectedObjectsWithFeature, DetectedObjectsWithFeature, DetectedObjectsWithFeature, - DetectedObjectsWithFeature, DetectedObjectsWithFeature, DetectedObjectsWithFeature, - DetectedObjectsWithFeature, DetectedObjectsWithFeature>; - using Sync = message_filters::Synchronizer; - typename std::shared_ptr sync_ptr_; - - // output + rclcpp::TimerBase::SharedPtr timer_; + double timeout_ms_{}; + double match_threshold_ms_{}; + + /** \brief A vector of subscriber. */ + typename rclcpp::Subscription::SharedPtr sub_; + std::vector::SharedPtr> rois_subs_; + + /** \brief Input point cloud topics. */ + std::vector input_topics_; + std::vector input_offset_ms_; + + // cache for fusion + std::vector is_fused_; + std::pair sub_stdpair_; + std::vector> roi_stdmap_; + std::mutex mutex_; + + // output publisher typename rclcpp::Publisher::SharedPtr pub_ptr_; // debugger @@ -111,6 +125,10 @@ class FusionNode : public rclcpp::Node float filter_scope_maxy_; float filter_scope_minz_; float filter_scope_maxz_; + + /** \brief processing time publisher. **/ + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace image_projection_based_fusion 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 02b7ddd3fb3ca..feed6437f6a68 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 @@ -38,7 +38,7 @@ class PointPaintingFusionNode : public FusionNode::SharedPtr obj_pub_ptr_; + std::vector tan_h_; // horizontal field of view + float score_threshold_{0.0}; std::vector class_names_; std::vector pointcloud_range; @@ -61,7 +63,5 @@ class PointPaintingFusionNode : public FusionNode + @@ -75,6 +76,7 @@ + diff --git a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml index 5719e6d5659f2..1ba46fafe3729 100644 --- a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml @@ -18,6 +18,7 @@ + @@ -73,6 +74,7 @@ + diff --git a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml index be981478e78a2..5b2c30819a4e9 100644 --- a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml @@ -18,6 +18,7 @@ + @@ -66,6 +67,7 @@ + diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index cabcdb944d092..3bcedaac65589 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -23,6 +23,8 @@ #include +#include + #ifdef ROS_DISTRO_GALACTIC #include #include @@ -41,6 +43,7 @@ FusionNode::FusionNode( const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { + // set rois_number rois_number_ = static_cast(declare_parameter("rois_number", 1)); if (rois_number_ < 1) { RCLCPP_WARN( @@ -53,9 +56,16 @@ FusionNode::FusionNode( rois_number_ = 8; } - // subscribers - sub_.subscribe(this, "input", rclcpp::QoS(1).get_rmw_qos_profile()); + // Set parameters + match_threshold_ms_ = declare_parameter("match_threshold_ms"); + timeout_ms_ = declare_parameter("timeout_ms"); + + input_offset_ms_ = declare_parameter("input_offset_ms", std::vector{}); + if (!input_offset_ms_.empty() && rois_number_ != input_offset_ms_.size()) { + throw std::runtime_error("The number of offsets does not match the number of topics."); + } + // sub camera info camera_info_subs_.resize(rois_number_); for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { std::function fnc = @@ -64,69 +74,31 @@ FusionNode::FusionNode( "input/camera_info" + std::to_string(roi_i), rclcpp::QoS{1}.best_effort(), fnc); } + // sub rois rois_subs_.resize(rois_number_); + roi_stdmap_.resize(rois_number_); + is_fused_.resize(rois_number_, false); for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { - rois_subs_.at(roi_i) = - std::make_shared>( - this, "input/rois" + std::to_string(roi_i), rclcpp::QoS{1}.get_rmw_qos_profile()); - } - - // add dummy callback to enable passthrough filter - rois_subs_.at(0)->registerCallback( - std::bind(&FusionNode::dummyCallback, this, std::placeholders::_1)); - switch (rois_number_) { - case 1: - sync_ptr_ = std::make_shared( - SyncPolicy(10), sub_, *rois_subs_.at(0), passthrough_, passthrough_, passthrough_, - passthrough_, passthrough_, passthrough_, passthrough_); - break; - case 2: - sync_ptr_ = std::make_shared( - SyncPolicy(10), sub_, *rois_subs_.at(0), *rois_subs_.at(1), passthrough_, passthrough_, - passthrough_, passthrough_, passthrough_, passthrough_); - break; - case 3: - sync_ptr_ = std::make_shared( - SyncPolicy(10), sub_, *rois_subs_.at(0), *rois_subs_.at(1), *rois_subs_.at(2), passthrough_, - passthrough_, passthrough_, passthrough_, passthrough_); - break; - case 4: - sync_ptr_ = std::make_shared( - SyncPolicy(10), sub_, *rois_subs_.at(0), *rois_subs_.at(1), *rois_subs_.at(2), - *rois_subs_.at(3), passthrough_, passthrough_, passthrough_, passthrough_); - break; - case 5: - sync_ptr_ = std::make_shared( - SyncPolicy(10), sub_, *rois_subs_.at(0), *rois_subs_.at(1), *rois_subs_.at(2), - *rois_subs_.at(3), *rois_subs_.at(4), passthrough_, passthrough_, passthrough_); - break; - case 6: - sync_ptr_ = std::make_shared( - SyncPolicy(10), sub_, *rois_subs_.at(0), *rois_subs_.at(1), *rois_subs_.at(2), - *rois_subs_.at(3), *rois_subs_.at(4), *rois_subs_.at(5), passthrough_, passthrough_); - break; - case 7: - sync_ptr_ = std::make_shared( - SyncPolicy(10), sub_, *rois_subs_.at(0), *rois_subs_.at(1), *rois_subs_.at(2), - *rois_subs_.at(3), *rois_subs_.at(4), *rois_subs_.at(5), *rois_subs_.at(6), passthrough_); - break; - case 8: - sync_ptr_ = std::make_shared( - SyncPolicy(10), sub_, *rois_subs_.at(0), *rois_subs_.at(1), *rois_subs_.at(2), - *rois_subs_.at(3), *rois_subs_.at(4), *rois_subs_.at(5), *rois_subs_.at(6), - *rois_subs_.at(7)); - default: - return; + std::function roi_callback = + std::bind(&FusionNode::roiCallback, this, std::placeholders::_1, roi_i); + rois_subs_.at(roi_i) = this->create_subscription( + "input/rois" + std::to_string(roi_i), rclcpp::QoS{1}.best_effort(), roi_callback); } - sync_ptr_->registerCallback(std::bind( - &FusionNode::fusionCallback, this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, - std::placeholders::_7, std::placeholders::_8, std::placeholders::_9)); + // subscribers + std::function sub_callback = + std::bind(&FusionNode::subCallback, this, std::placeholders::_1); + sub_ = this->create_subscription("input", rclcpp::QoS(1).best_effort(), sub_callback); // publisher pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); + // Set timer + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(timeout_ms_)); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&FusionNode::timer_callback, this)); + // debugger if (declare_parameter("debug_mode", false)) { std::size_t image_buffer_size = @@ -134,6 +106,16 @@ FusionNode::FusionNode( debugger_ = std::make_shared(this, rois_number_, image_buffer_size); } + // initialize debug tool + { + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = std::make_unique(this, "fusion_node"); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + } + filter_scope_minx_ = declare_parameter("filter_scope_minx", -100); filter_scope_maxx_ = declare_parameter("filter_scope_maxx", 100); filter_scope_miny_ = declare_parameter("filter_scope_miny", -100); @@ -157,67 +139,176 @@ void FusionNode::preprocess(Msg & ouput_msg __attribute__((unused))) } template -void FusionNode::fusionCallback( - typename Msg::ConstSharedPtr input_msg, DetectedObjectsWithFeature::ConstSharedPtr input_roi0_msg, - DetectedObjectsWithFeature::ConstSharedPtr input_roi1_msg, - DetectedObjectsWithFeature::ConstSharedPtr input_roi2_msg, - DetectedObjectsWithFeature::ConstSharedPtr input_roi3_msg, - DetectedObjectsWithFeature::ConstSharedPtr input_roi4_msg, - DetectedObjectsWithFeature::ConstSharedPtr input_roi5_msg, - DetectedObjectsWithFeature::ConstSharedPtr input_roi6_msg, - DetectedObjectsWithFeature::ConstSharedPtr input_roi7_msg) +void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_msg) { - Msg output_msg = *input_msg; - - preprocess(output_msg); - - for (std::size_t image_id = 0; image_id < rois_subs_.size(); ++image_id) { - DetectedObjectsWithFeature::ConstSharedPtr input_roi_msg; - switch (image_id) { - case 0: - input_roi_msg = input_roi0_msg; - break; - case 1: - input_roi_msg = input_roi1_msg; - break; - case 2: - input_roi_msg = input_roi2_msg; - break; - case 3: - input_roi_msg = input_roi3_msg; - break; - case 4: - input_roi_msg = input_roi4_msg; - break; - case 5: - input_roi_msg = input_roi5_msg; - break; - case 6: - input_roi_msg = input_roi6_msg; - break; - case 7: - input_roi_msg = input_roi7_msg; - break; - default: - RCLCPP_ERROR(this->get_logger(), "invalid image id. id is %zu", image_id); - return; - } + std::lock_guard lock(mutex_); + auto period = std::chrono::duration_cast( + std::chrono::duration(timeout_ms_)); + try { + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + + stop_watch_ptr_->toc("processing_time", true); + + typename Msg::SharedPtr output_msg = std::make_shared(*input_msg); + + preprocess(*output_msg); - if (camera_info_map_.find(image_id) == camera_info_map_.end()) { - RCLCPP_WARN(this->get_logger(), "no camera info. id is %zu", image_id); + int64_t timestamp_nsec = + (*output_msg).header.stamp.sec * (int64_t)1e9 + (*output_msg).header.stamp.nanosec; + + // if matching rois exist, fuseonsingle + for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { + if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { + RCLCPP_WARN(this->get_logger(), "no camera info. id is %zu", roi_i); continue; } - if (debugger_) { - debugger_->clear(); + + if ((roi_stdmap_.at(roi_i)).size() > 0) { + int64_t min_interval = 1e9; + int64_t matched_stamp = -1; + std::list outdate_stamps; + + for (const auto & [k, v] : roi_stdmap_.at(roi_i)) { + int64_t newstamp = timestamp_nsec + input_offset_ms_.at(roi_i) * (int64_t)1e6; + int64_t interval = abs(int64_t(k) - newstamp); + + if (interval <= min_interval && interval <= match_threshold_ms_ * (int64_t)1e6) { + min_interval = interval; + matched_stamp = k; + } else if (int64_t(k) < newstamp && interval > match_threshold_ms_ * (int64_t)1e6) { + outdate_stamps.push_back(int64_t(k)); + } + } + + // remove outdated stamps + for (auto stamp : outdate_stamps) { + (roi_stdmap_.at(roi_i)).erase(stamp); + } + + // fuseonSingle + if (matched_stamp != -1) { + if (debugger_) { + debugger_->clear(); + } + + fuseOnSingleImage( + *input_msg, roi_i, *((roi_stdmap_.at(roi_i))[matched_stamp]), camera_info_map_.at(roi_i), + *output_msg); + (roi_stdmap_.at(roi_i)).erase(matched_stamp); + is_fused_.at(roi_i) = true; + + // add timestamp interval for debug + if (debug_publisher_) { + double timestamp_interval_ms = (matched_stamp - timestamp_nsec) / 1e6; + debug_publisher_->publish( + "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); + debug_publisher_->publish( + "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", + timestamp_interval_ms - input_offset_ms_.at(roi_i)); + } + } + } + } + + // if all camera fused, postprocess; else, publish the old Msg(if exists) and cache the current + // Msg + if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(rois_number_)) { + timer_->cancel(); + postprocess(*output_msg); + publish(*output_msg); + std::fill(is_fused_.begin(), is_fused_.end(), false); + + // add processing time for debug + if (debug_publisher_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + } + } else { + if (sub_stdpair_.second != nullptr) { + timer_->cancel(); + postprocess(*(sub_stdpair_.second)); + publish(*(sub_stdpair_.second)); + std::fill(is_fused_.begin(), is_fused_.end(), false); + + // add processing time for debug + if (debug_publisher_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + } } - fuseOnSingleImage( - *input_msg, image_id, *input_roi_msg, camera_info_map_.at(image_id), output_msg); + sub_stdpair_.first = int64_t(timestamp_nsec); + sub_stdpair_.second = output_msg; } +} + +template +void FusionNode::roiCallback( + const DetectedObjectsWithFeature::ConstSharedPtr input_roi_msg, const std::size_t roi_i) +{ + int64_t timestamp_nsec = + (*input_roi_msg).header.stamp.sec * (int64_t)1e9 + (*input_roi_msg).header.stamp.nanosec; + + // if cached Msg exist, try to match + if (sub_stdpair_.second != nullptr) { + int64_t newstamp = sub_stdpair_.first + input_offset_ms_.at(roi_i) * (int64_t)1e6; + int64_t interval = abs(timestamp_nsec - newstamp); + + if (interval < match_threshold_ms_ * (int64_t)1e6 && is_fused_.at(roi_i) == false) { + if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { + RCLCPP_WARN(this->get_logger(), "no camera info. id is %zu", roi_i); + (roi_stdmap_.at(roi_i))[timestamp_nsec] = input_roi_msg; + return; + } + if (debugger_) { + debugger_->clear(); + } + + fuseOnSingleImage( + *(sub_stdpair_.second), roi_i, *input_roi_msg, camera_info_map_.at(roi_i), + *(sub_stdpair_.second)); + is_fused_.at(roi_i) = true; - postprocess(output_msg); + if (debug_publisher_) { + double timestamp_interval_ms = (timestamp_nsec - sub_stdpair_.first) / 1e6; + debug_publisher_->publish( + "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); + debug_publisher_->publish( + "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", + timestamp_interval_ms - input_offset_ms_.at(roi_i)); + } - publish(output_msg); + if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(rois_number_)) { + timer_->cancel(); + postprocess(*(sub_stdpair_.second)); + publish(*(sub_stdpair_.second)); + std::fill(is_fused_.begin(), is_fused_.end(), false); + sub_stdpair_.second = nullptr; + + // add processing time for debug + if (debug_publisher_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + } + } + return; + } + } + // store roi msg if not matched + (roi_stdmap_.at(roi_i))[timestamp_nsec] = input_roi_msg; } template @@ -226,6 +317,56 @@ void FusionNode::postprocess(Msg & output_msg __attribute__((unused))) // do nothing by default } +template +void FusionNode::timer_callback() +{ + using std::chrono_literals::operator""ms; + timer_->cancel(); + if (mutex_.try_lock()) { + // timeout, postprocess cached msg + if (sub_stdpair_.second != nullptr) { + postprocess(*(sub_stdpair_.second)); + publish(*(sub_stdpair_.second)); + } + std::fill(is_fused_.begin(), is_fused_.end(), false); + sub_stdpair_.second = nullptr; + + // add processing time for debug + if (debug_publisher_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + } + + mutex_.unlock(); + } else { + try { + std::chrono::nanoseconds period = 10ms; + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } +} + +template +void FusionNode::setPeriod(const int64_t new_period) +{ + if (!timer_) { + return; + } + int64_t old_period = 0; + rcl_ret_t ret = rcl_timer_get_period(timer_->get_timer_handle().get(), &old_period); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't get old period"); + } + ret = rcl_timer_exchange_period(timer_->get_timer_handle().get(), new_period, &old_period); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't exchange_period"); + } +} + template void FusionNode::publish(const Msg & output_msg) { diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 40a99ea33b9f2..f3486d65741f6 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -63,6 +63,18 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); const auto max_area_matrix = this->declare_parameter>("max_area_matrix"); + std::function sub_callback = + std::bind(&PointPaintingFusionNode::subCallback, this, std::placeholders::_1); + sub_ = this->create_subscription( + "~/input/pointcloud", rclcpp::SensorDataQoS().keep_last(3), sub_callback); + + tan_h_.resize(rois_number_); + for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { + auto fx = camera_info_map_[roi_i].k.at(0); + auto x0 = camera_info_map_[roi_i].k.at(2); + tan_h_[roi_i] = x0 / fx; + } + detection_class_remapper_.setParameters( allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); @@ -79,8 +91,6 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt detector_ptr_ = std::make_unique( encoder_param, head_param, densification_param, config); - // sub and pub - sub_.subscribe(this, "~/input/pointcloud", rmw_qos_profile_sensor_data); obj_pub_ptr_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); } @@ -133,7 +143,8 @@ void PointPaintingFusionNode::preprocess(sensor_msgs::msg::PointCloud2 & painted void PointPaintingFusionNode::fuseOnSingleImage( __attribute__((unused)) const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, - const std::size_t image_id, const DetectedObjectsWithFeature & input_roi_msg, + __attribute__((unused)) const std::size_t image_id, + const DetectedObjectsWithFeature & input_roi_msg, const sensor_msgs::msg::CameraInfo & camera_info, sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) { @@ -164,16 +175,17 @@ void PointPaintingFusionNode::fuseOnSingleImage( tf2::doTransform(painted_pointcloud_msg, transformed_pointcloud, transform_stamped); // iterate points - sensor_msgs::PointCloud2Iterator iter_painted_intensity( - painted_pointcloud_msg, "intensity"); sensor_msgs::PointCloud2Iterator iter_car(painted_pointcloud_msg, "CAR"); sensor_msgs::PointCloud2Iterator iter_ped(painted_pointcloud_msg, "PEDESTRIAN"); sensor_msgs::PointCloud2Iterator iter_bic(painted_pointcloud_msg, "BICYCLE"); + for (sensor_msgs::PointCloud2ConstIterator iter_x(transformed_pointcloud, "x"), iter_y(transformed_pointcloud, "y"), iter_z(transformed_pointcloud, "z"); - iter_x != iter_x.end(); - ++iter_x, ++iter_y, ++iter_z, ++iter_painted_intensity, ++iter_car, ++iter_ped, ++iter_bic) { - if (*iter_z <= 0.0) { + iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++iter_car, ++iter_ped, ++iter_bic) { + // filter the points outside of the horizontal field of view + if ( + *iter_z <= 0.0 || (*iter_x / *iter_z) > tan_h_.at(image_id) || + (*iter_x / *iter_z) < -tan_h_.at(image_id)) { continue; } // project @@ -216,6 +228,8 @@ void PointPaintingFusionNode::fuseOnSingleImage( *iter_bic = 1.0; break; } + } + if (debugger_) { debug_image_points.push_back(normalized_projected_point); } } @@ -250,8 +264,6 @@ void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painte output_obj_msg.objects.emplace_back(obj); } - detection_class_remapper_.mapClasses(output_obj_msg); - obj_pub_ptr_->publish(output_obj_msg); } @@ -259,7 +271,6 @@ bool PointPaintingFusionNode::out_of_scope(__attribute__((unused)) const Detecte { return false; } - } // namespace image_projection_based_fusion #include diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp index da15655937609..31f3ce7fa32db 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp" + #include "behavior_path_planner/path_utilities.hpp" #include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp" #include "behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp" diff --git a/system/ad_service_state_monitor/CMakeLists.txt b/system/ad_service_state_monitor/CMakeLists.txt deleted file mode 100644 index 92cf601fb482e..0000000000000 --- a/system/ad_service_state_monitor/CMakeLists.txt +++ /dev/null @@ -1,17 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(ad_service_state_monitor) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_executable(ad_service_state_monitor - src/ad_service_state_monitor_node/main.cpp - src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp - src/ad_service_state_monitor_node/state_machine.cpp - src/ad_service_state_monitor_node/diagnostics.cpp -) - -ament_auto_package(INSTALL_TO_SHARE - launch - config -) diff --git a/system/ad_service_state_monitor/README.md b/system/ad_service_state_monitor/README.md deleted file mode 100644 index 53ff7efc4502f..0000000000000 --- a/system/ad_service_state_monitor/README.md +++ /dev/null @@ -1,46 +0,0 @@ -# ad_service_state_monitor - -## Purpose - -This node manages AutowareState transitions. - -## Inner-workings / Algorithms - -## Inputs / Outputs - -### Input - -| Name | Type | Description | -| ---------------------------------- | ---------------------------------------------------- | ------------------------------------------------- | -| `/planning/mission_planning/route` | `autoware_auto_planning_msgs::msg::HADMapRoute` | Subscribe route | -| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Used to decide whether vehicle is stopped or not | -| `/vehicle/state_report` | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual. | - -### Output - -| Name | Type | Description | -| ------------------ | ----------------------------------------------- | -------------------------------------------------- | -| `/autoware/engage` | `autoware_auto_vehicle_msgs::msg::Engage` | publish disengage flag on AutowareState transition | -| `/autoware/state` | `autoware_auto_system_msgs::msg::AutowareState` | publish AutowareState | - -## Parameters - -### Node Parameters - -| Name | Type | Default Value | Explanation | -| ------------- | ---- | ------------- | ---------------------- | -| `update_rate` | int | `10` | Timer callback period. | - -### Core Parameters - -| Name | Type | Default Value | Explanation | -| ------------------------- | ------ | ------------- | -------------------------------------------------------------------------- | -| `th_arrived_distance_m` | double | 1.0 | threshold distance to check if vehicle has arrived at the route's endpoint | -| `th_stopped_time_sec` | double | 1.0 | threshold time to check if vehicle is stopped | -| `th_stopped_velocity_mps` | double | 0.01 | threshold velocity to check if vehicle is stopped | -| `disengage_on_route` | bool | true | send disengage flag or not when the route is subscribed | -| `disengage_on_goal` | bool | true | send disengage flag or not when the vehicle is arrived goal | - -## Assumptions / Known limits - -TBD. diff --git a/system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml b/system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml deleted file mode 100644 index 9113e8a1f1206..0000000000000 --- a/system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml +++ /dev/null @@ -1,155 +0,0 @@ -# AD Service State Monitor Parameters -/**: - ros__parameters: - # modules_names: string array - module_names: [ - "map", - "sensing", - "perception", - "localization", - "planning", - "control", - "vehicle", - "system" - ] - - # Topic Config - ## names: string array - ## configs: top level - ### names used to declare parameter group - #### module: string - #### timeout[s]: double (0: none) - #### warn_rate[hz]: double (0: none) - topic_configs: - names: [ - "/map/vector_map", - "/map/pointcloud_map", - "/perception/obstacle_segmentation/pointcloud", - "/perception/object_recognition/objects", - "/initialpose3d", - "/localization/pose_twist_fusion_filter/pose", - "/planning/mission_planning/route", - "/planning/scenario_planning/trajectory", - "/control/trajectory_follower/control_cmd", - "/control/command/control_cmd", - "/vehicle/status/velocity_status", - "/vehicle/status/steering_status", - "/system/emergency/control_cmd" - ] - - configs: - /map/vector_map: - module: "map" - timeout: 0.0 - warn_rate: 0.0 - type: "autoware_auto_mapping_msgs/msg/HADMapBin" - transient_local: True - - /map/pointcloud_map: - module: "map" - timeout: 0.0 - warn_rate: 0.0 - type: "sensor_msgs/msg/PointCloud2" - transient_local: True - - /perception/obstacle_segmentation/pointcloud: - module: "sensing" - timeout: 1.0 - warn_rate: 5.0 - type: "sensor_msgs/msg/PointCloud2" - best_effort: True - - # Can be both with feature array or without - # In prediction.launch.xml this is set to without - /perception/object_recognition/objects: - module: "perception" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_perception_msgs/msg/PredictedObjects" - # This topic could have two different types depending on the launch flags used. - # The implementation of subscriptions in ROS2 does not allow for multiple types - # to be defined for a topic. By default this is set to not use have features. - # type: ["tier4_perception_msgs/msg/DynamicObjectArray", "tier4_perception_msgs/msg/DynamicObjectWithFeatureArray"] - - /initialpose3d: - module: "localization" - timeout: 0.0 - warn_rate: 0.0 - type: "geometry_msgs/msg/PoseWithCovarianceStamped" - - /localization/pose_twist_fusion_filter/pose: - module: "localization" - timeout: 1.0 - warn_rate: 5.0 - type: "geometry_msgs/msg/PoseStamped" - - /planning/mission_planning/route: - module: "planning" - timeout: 0.0 - warn_rate: 0.0 - type: "autoware_auto_planning_msgs/msg/HADMapRoute" - transient_local: True - - /planning/scenario_planning/trajectory: - module: "planning" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_planning_msgs/msg/Trajectory" - - /control/trajectory_follower/control_cmd: - module: "control" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_control_msgs/msg/AckermannControlCommand" - - /control/command/control_cmd: - module: "control" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_control_msgs/msg/AckermannControlCommand" - - /vehicle/status/velocity_status: - module: "vehicle" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_vehicle_msgs/msg/VelocityReport" - - /vehicle/status/steering_status: - module: "vehicle" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_vehicle_msgs/msg/SteeringReport" - - /system/emergency/control_cmd: - module: "system" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_control_msgs/msg/AckermannControlCommand" - - # Param Config - ## names: string array - ## configs: top level - ### names used to declare parameter group - #### module: string - # param_configs: - # names: ["/vehicle_info"] - # configs: - # /vehicle_info: - # module: "vehicle" - - # TF Config - ## names: string array - ## configs: top level - ### names used to declare parameter group - #### module: string - #### from: string - #### to: string - #### timeout[s]: double (0: none) - tf_configs: - names: ["map_to_base_link"] - configs: - map_to_base_link: - module: "localization" - from: "map" - to: "base_link" - timeout: 1.0 diff --git a/system/ad_service_state_monitor/config/ad_service_state_monitor.planning_simulation.param.yaml b/system/ad_service_state_monitor/config/ad_service_state_monitor.planning_simulation.param.yaml deleted file mode 100644 index 51fbe99dd9bc3..0000000000000 --- a/system/ad_service_state_monitor/config/ad_service_state_monitor.planning_simulation.param.yaml +++ /dev/null @@ -1,126 +0,0 @@ -# AD Service State Monitor: Planning Simulator Parameters -/**: - ros__parameters: - # modules_names: string array - module_names: [ - "map", - "sensing", - "perception", - "localization", - "planning", - "control", - "vehicle", - "system" - ] - -# Topic Config - ## names: string array - ## configs: top level - ### names used to declare parameter group - #### module: string - #### timeout[s]: double (0: none) - #### warn_rate[hz]: double (0: none) - topic_configs: - names: [ - "/map/vector_map", - "/perception/object_recognition/objects", - "/initialpose3d", - "/planning/mission_planning/route", - "/planning/scenario_planning/trajectory", - "/control/trajectory_follower/control_cmd", - "/control/command/control_cmd", - "/vehicle/status/velocity_status", - "/vehicle/status/steering_status", - "/system/emergency/control_cmd" - ] - - configs: - /map/vector_map: - module: "map" - timeout: 0.0 - warn_rate: 0.0 - type: "autoware_auto_mapping_msgs/msg/HADMapBin" - transient_local: True - - /perception/object_recognition/objects: - module: "perception" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_perception_msgs/msg/PredictedObjects" - - /initialpose3d: - module: "localization" - timeout: 0.0 - warn_rate: 0.0 - type: "geometry_msgs/msg/PoseWithCovarianceStamped" - - /planning/mission_planning/route: - module: "planning" - timeout: 0.0 - warn_rate: 0.0 - type: "autoware_auto_planning_msgs/msg/HADMapRoute" - transient_local: True - - /planning/scenario_planning/trajectory: - module: "planning" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_planning_msgs/msg/Trajectory" - - /control/trajectory_follower/control_cmd: - module: "control" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_control_msgs/msg/AckermannControlCommand" - - /control/command/control_cmd: - module: "control" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_control_msgs/msg/AckermannControlCommand" - - /vehicle/status/velocity_status: - module: "vehicle" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_vehicle_msgs/msg/VelocityReport" - - /vehicle/status/steering_status: - module: "vehicle" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_vehicle_msgs/msg/SteeringReport" - - /system/emergency/control_cmd: - module: "system" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_control_msgs/msg/AckermannControlCommand" - - # Param Config - ## names: string array - ## configs: top level - ### names used to declare parameter group - #### module: string - # param_configs: - # names: ["/vehicle_info"] - # configs: - # /vehicle_info: - # module: "vehicle" - - # TF Config - ## names: string array - ## configs: top level - ### names used to declare parameter group - #### module: string - #### from: string - #### to: string - #### timeout[s]: double (0: none) - tf_configs: - names: ["map_to_base_link"] - configs: - map_to_base_link: - module: "localization" - from: "map" - to: "base_link" - timeout: 1.0 diff --git a/system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state.hpp b/system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state.hpp deleted file mode 100644 index 000ab94812bcd..0000000000000 --- a/system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state.hpp +++ /dev/null @@ -1,90 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AD_SERVICE_STATE_MONITOR__AD_SERVICE_STATE_HPP_ -#define AD_SERVICE_STATE_MONITOR__AD_SERVICE_STATE_HPP_ - -#include - -#include - -enum class AutowareState : int8_t { - InitializingVehicle, - WaitingForRoute, - Planning, - WaitingForEngage, - Driving, - ArrivedGoal, - Finalizing, -}; - -inline AutowareState fromMsg(const int state) -{ - using StateMessage = autoware_auto_system_msgs::msg::AutowareState; - - if (state == StateMessage::INITIALIZING) { - return AutowareState::InitializingVehicle; - } - if (state == StateMessage::WAITING_FOR_ROUTE) { - return AutowareState::WaitingForRoute; - } - if (state == StateMessage::PLANNING) { - return AutowareState::Planning; - } - if (state == StateMessage::WAITING_FOR_ENGAGE) { - return AutowareState::WaitingForEngage; - } - if (state == StateMessage::DRIVING) { - return AutowareState::Driving; - } - if (state == StateMessage::ARRIVED_GOAL) { - return AutowareState::ArrivedGoal; - } - if (state == StateMessage::FINALIZING) { - return AutowareState::Finalizing; - } - - throw std::runtime_error("invalid state"); -} - -inline int toMsg(const AutowareState & state) -{ - using StateMessage = autoware_auto_system_msgs::msg::AutowareState; - - if (state == AutowareState::InitializingVehicle) { - return StateMessage::INITIALIZING; - } - if (state == AutowareState::WaitingForRoute) { - return StateMessage::WAITING_FOR_ROUTE; - } - if (state == AutowareState::Planning) { - return StateMessage::PLANNING; - } - if (state == AutowareState::WaitingForEngage) { - return StateMessage::WAITING_FOR_ENGAGE; - } - if (state == AutowareState::Driving) { - return StateMessage::DRIVING; - } - if (state == AutowareState::ArrivedGoal) { - return StateMessage::ARRIVED_GOAL; - } - if (state == AutowareState::Finalizing) { - return StateMessage::FINALIZING; - } - - throw std::runtime_error("invalid state"); -} - -#endif // AD_SERVICE_STATE_MONITOR__AD_SERVICE_STATE_HPP_ diff --git a/system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state_monitor_node.hpp b/system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state_monitor_node.hpp deleted file mode 100644 index bd358132d556a..0000000000000 --- a/system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state_monitor_node.hpp +++ /dev/null @@ -1,148 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AD_SERVICE_STATE_MONITOR__AD_SERVICE_STATE_MONITOR_NODE_HPP_ -#define AD_SERVICE_STATE_MONITOR__AD_SERVICE_STATE_MONITOR_NODE_HPP_ - -#include "ad_service_state_monitor/ad_service_state.hpp" -#include "ad_service_state_monitor/config.hpp" -#include "ad_service_state_monitor/state_machine.hpp" - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include - -class AutowareStateMonitorNode : public rclcpp::Node -{ -public: - AutowareStateMonitorNode(); - -private: - // Parameter - double update_rate_; - bool disengage_on_route_; - bool disengage_on_goal_; - - std::vector topic_configs_; - std::vector param_configs_; - std::vector tf_configs_; - - // TF - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; - geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; - - // Lanelet - std::shared_ptr lanelet_map_ptr_; - bool is_map_msg_ready_{false}; - - // CallbackGroups - rclcpp::CallbackGroup::SharedPtr callback_group_subscribers_; - rclcpp::CallbackGroup::SharedPtr callback_group_services_; - - // Subscriber - rclcpp::Subscription::SharedPtr sub_autoware_engage_; - rclcpp::Subscription::SharedPtr - sub_control_mode_; - rclcpp::Subscription::SharedPtr sub_route_; - rclcpp::Subscription::SharedPtr sub_modified_goal_; - rclcpp::Subscription::SharedPtr sub_map_; - rclcpp::Subscription::SharedPtr sub_odom_; - - void onAutowareEngage(const autoware_auto_vehicle_msgs::msg::Engage::ConstSharedPtr msg); - void onVehicleControlMode( - const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg); - void onMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); - void onRoute(const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr msg); - void onModifiedGoal(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg); - void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); - - // Topic Buffer - void onTopic( - const std::shared_ptr msg, const std::string & topic_name); - void registerTopicCallback( - const std::string & topic_name, const std::string & topic_type, const bool transient_local, - const bool best_effort); - - std::map sub_topic_map_; - std::map> topic_received_time_buffer_; - - // Service - rclcpp::Service::SharedPtr srv_shutdown_; - rclcpp::Service::SharedPtr srv_reset_route_; - - bool onShutdownService( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response); - bool onResetRouteService( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response); - - // Publisher - rclcpp::Publisher::SharedPtr pub_autoware_state_; - rclcpp::Publisher::SharedPtr pub_autoware_engage_; - - bool isEngaged(); - void setDisengage(); - - // Timer - void onTimer(); - rclcpp::TimerBase::SharedPtr timer_; - - // Stats - TopicStats getTopicStats() const; - ParamStats getParamStats() const; - TfStats getTfStats() const; - - // State Machine - std::shared_ptr state_machine_; - StateInput state_input_; - StateParam state_param_; - - // Lock Variables - std::mutex lock_state_machine_; - std::mutex lock_state_input_; - - // Diagnostic Updater - diagnostic_updater::Updater updater_; - - void setupDiagnosticUpdater(); - void checkTopicStatus( - diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & module_name); - void checkTFStatus( - diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & module_name); -}; - -#endif // AD_SERVICE_STATE_MONITOR__AD_SERVICE_STATE_MONITOR_NODE_HPP_ diff --git a/system/ad_service_state_monitor/include/ad_service_state_monitor/config.hpp b/system/ad_service_state_monitor/include/ad_service_state_monitor/config.hpp deleted file mode 100644 index 656d8233a89b2..0000000000000 --- a/system/ad_service_state_monitor/include/ad_service_state_monitor/config.hpp +++ /dev/null @@ -1,122 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AD_SERVICE_STATE_MONITOR__CONFIG_HPP_ -#define AD_SERVICE_STATE_MONITOR__CONFIG_HPP_ - -#include -#include - -#include -#include -#include - -struct TopicConfig -{ - explicit TopicConfig( - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr interface, - const std::string & namespace_prefix, const std::string & name) - : module( - interface->declare_parameter(namespace_prefix + ".module", - rclcpp::PARAMETER_STRING).get()), - name(name), - type( - interface->declare_parameter(namespace_prefix + ".type", - rclcpp::PARAMETER_STRING).get()), - transient_local( - interface->declare_parameter(namespace_prefix + ".transient_local", - static_cast(false)).get()), - best_effort( - interface->declare_parameter(namespace_prefix + ".best_effort", - static_cast(false)).get()), - timeout( - interface->declare_parameter(namespace_prefix + ".timeout", - rclcpp::PARAMETER_DOUBLE).get()), - warn_rate( - interface->declare_parameter(namespace_prefix + ".warn_rate", - rclcpp::PARAMETER_DOUBLE).get()) - { - } - - std::string module; - std::string name; - std::string type; - bool transient_local; - bool best_effort; - double timeout; - double warn_rate; -}; - -struct ParamConfig -{ - explicit ParamConfig( - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr interface, - const std::string & namespace_prefix, const std::string & name) - : module(interface->declare_parameter(namespace_prefix + ".module", rclcpp::PARAMETER_STRING) - .get()), - name(name) - { - } - - std::string module; - std::string name; -}; - -struct TfConfig -{ - explicit TfConfig( - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr interface, - const std::string & namespace_prefix, [[maybe_unused]] const std::string & name) - : module(interface->declare_parameter(namespace_prefix + ".module", rclcpp::PARAMETER_STRING) - .get()), - from(interface->declare_parameter(namespace_prefix + ".from", rclcpp::PARAMETER_STRING) - .get()), - to(interface->declare_parameter(namespace_prefix + ".to", rclcpp::PARAMETER_STRING) - .get()), - timeout(interface->declare_parameter(namespace_prefix + ".timeout", rclcpp::PARAMETER_DOUBLE) - .get()) - { - } - - std::string module; - std::string from; - std::string to; - double timeout; -}; - -struct TopicStats -{ - rclcpp::Time checked_time; - std::vector ok_list; - std::vector non_received_list; - std::vector> timeout_list; // pair - std::vector> slow_rate_list; // pair -}; - -struct ParamStats -{ - rclcpp::Time checked_time; - std::vector ok_list; - std::vector non_set_list; -}; - -struct TfStats -{ - rclcpp::Time checked_time; - std::vector ok_list; - std::vector non_received_list; - std::vector> timeout_list; // pair -}; - -#endif // AD_SERVICE_STATE_MONITOR__CONFIG_HPP_ diff --git a/system/ad_service_state_monitor/include/ad_service_state_monitor/module_name.hpp b/system/ad_service_state_monitor/include/ad_service_state_monitor/module_name.hpp deleted file mode 100644 index 4e2a7500f3ff6..0000000000000 --- a/system/ad_service_state_monitor/include/ad_service_state_monitor/module_name.hpp +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AD_SERVICE_STATE_MONITOR__MODULE_NAME_HPP_ -#define AD_SERVICE_STATE_MONITOR__MODULE_NAME_HPP_ - -struct ModuleName -{ - static constexpr const char * map = "map"; - static constexpr const char * sensing = "sensing"; - static constexpr const char * localization = "localization"; - static constexpr const char * perception = "perception"; - static constexpr const char * planning = "planning"; - static constexpr const char * control = "control"; - static constexpr const char * vehicle = "vehicle"; - static constexpr const char * system = "system"; -}; - -#endif // AD_SERVICE_STATE_MONITOR__MODULE_NAME_HPP_ diff --git a/system/ad_service_state_monitor/include/ad_service_state_monitor/state_machine.hpp b/system/ad_service_state_monitor/include/ad_service_state_monitor/state_machine.hpp deleted file mode 100644 index 5d503d420c092..0000000000000 --- a/system/ad_service_state_monitor/include/ad_service_state_monitor/state_machine.hpp +++ /dev/null @@ -1,113 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AD_SERVICE_STATE_MONITOR__STATE_MACHINE_HPP_ -#define AD_SERVICE_STATE_MONITOR__STATE_MACHINE_HPP_ - -#include "ad_service_state_monitor/ad_service_state.hpp" -#include "ad_service_state_monitor/config.hpp" -#include "ad_service_state_monitor/module_name.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" - -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -struct StateInput -{ - TopicStats topic_stats; - ParamStats param_stats; - TfStats tf_stats; - - rclcpp::Time current_time; - - geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose; - geometry_msgs::msg::Pose::ConstSharedPtr goal_pose; - geometry_msgs::msg::PoseStamped::ConstSharedPtr modified_goal_pose; - - autoware_auto_vehicle_msgs::msg::Engage::ConstSharedPtr autoware_engage; - autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; - bool is_finalizing = false; - bool is_route_reset_required = false; - autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr route; - nav_msgs::msg::Odometry::ConstSharedPtr odometry; - std::deque odometry_buffer; -}; - -struct StateParam -{ - double th_arrived_distance_m; - double th_arrived_angle; - double th_stopped_time_sec; - double th_stopped_velocity_mps; -}; - -struct Times -{ - rclcpp::Time arrived_goal; - rclcpp::Time initializing_completed; - rclcpp::Time planning_completed; -}; - -struct Flags -{ - bool waiting_after_initializing = false; - bool waiting_after_planning = false; -}; - -class StateMachine -{ -public: - explicit StateMachine(const StateParam & state_param) : state_param_(state_param) {} - - AutowareState getCurrentState() const { return autoware_state_; } - AutowareState updateState(const StateInput & state_input); - std::vector getMessages() const { return msgs_; } - -private: - AutowareState autoware_state_ = AutowareState::InitializingVehicle; - StateInput state_input_; - const StateParam state_param_; - - mutable std::vector msgs_; - mutable Times times_; - mutable Flags flags_; - mutable autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr executing_route_ = nullptr; - - AutowareState judgeAutowareState() const; - - bool isModuleInitialized(const char * module_name) const; - bool isVehicleInitialized() const; - bool hasRoute() const; - bool isRouteReceived() const; - bool isPlanningCompleted() const; - bool isEngaged() const; - bool isOverridden() const; - bool hasArrivedGoal() const; - bool isFinalizing() const; - bool isRouteResetRequired() const; -}; - -#endif // AD_SERVICE_STATE_MONITOR__STATE_MACHINE_HPP_ diff --git a/system/ad_service_state_monitor/launch/ad_service_state_monitor.launch.xml b/system/ad_service_state_monitor/launch/ad_service_state_monitor.launch.xml deleted file mode 100644 index a7cbf13c4a14c..0000000000000 --- a/system/ad_service_state_monitor/launch/ad_service_state_monitor.launch.xml +++ /dev/null @@ -1,54 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/system/ad_service_state_monitor/package.xml b/system/ad_service_state_monitor/package.xml deleted file mode 100644 index ed99be17cbc22..0000000000000 --- a/system/ad_service_state_monitor/package.xml +++ /dev/null @@ -1,40 +0,0 @@ - - - - ad_service_state_monitor - 0.1.1 - The ad_service_state_monitor package ROS2 - Kenji Miyake - Apache License 2.0 - - ament_cmake_auto - - autoware_cmake - - autoware_auto_control_msgs - autoware_auto_planning_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs - diagnostic_updater - fmt - geometry_msgs - lanelet2_extension - nav_msgs - rclcpp - std_msgs - std_srvs - tf2 - tf2_geometry_msgs - tf2_ros - tier4_autoware_utils - - sensor_msgs - tier4_perception_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp deleted file mode 100644 index 662df2efbfe0d..0000000000000 --- a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp +++ /dev/null @@ -1,553 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ad_service_state_monitor/ad_service_state_monitor_node.hpp" - -#include "lanelet2_extension/utility/message_conversion.hpp" -#include "lanelet2_extension/utility/route_checker.hpp" - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -#include -#include -#include -#include -#include - -namespace -{ -template -std::vector getConfigs( - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr interface, - const std::string & config_namespace) -{ - std::string names_key = config_namespace + ".names"; - interface->declare_parameter(names_key, rclcpp::PARAMETER_STRING_ARRAY); - std::vector config_names = interface->get_parameter(names_key).as_string_array(); - - std::vector configs; - configs.reserve(config_names.size()); - - for (auto config_name : config_names) { - configs.emplace_back(interface, config_namespace + ".configs." + config_name, config_name); - } - - return configs; -} - -double calcTopicRate(const std::deque & topic_received_time_buffer) -{ - assert(topic_received_time_buffer.size() >= 2); - - const auto & buf = topic_received_time_buffer; - const auto time_diff = buf.back() - buf.front(); - - return static_cast(buf.size() - 1) / time_diff.seconds(); -} - -geometry_msgs::msg::PoseStamped::SharedPtr getCurrentPose(const tf2_ros::Buffer & tf_buffer) -{ - geometry_msgs::msg::TransformStamped tf_current_pose; - - try { - tf_current_pose = tf_buffer.lookupTransform("map", "base_link", tf2::TimePointZero); - } catch (tf2::TransformException & ex) { - return nullptr; - } - - auto p = std::make_shared(); - p->header = tf_current_pose.header; - p->pose.orientation = tf_current_pose.transform.rotation; - p->pose.position.x = tf_current_pose.transform.translation.x; - p->pose.position.y = tf_current_pose.transform.translation.y; - p->pose.position.z = tf_current_pose.transform.translation.z; - - return p; -} - -} // namespace - -void AutowareStateMonitorNode::onAutowareEngage( - const autoware_auto_vehicle_msgs::msg::Engage::ConstSharedPtr msg) -{ - std::lock_guard lock(lock_state_input_); - state_input_.autoware_engage = msg; -} - -void AutowareStateMonitorNode::onVehicleControlMode( - const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) -{ - std::lock_guard lock(lock_state_input_); - state_input_.control_mode_ = msg; -} - -void AutowareStateMonitorNode::onModifiedGoal( - const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg) -{ - std::lock_guard lock(lock_state_input_); - state_input_.modified_goal_pose = msg; -} - -void AutowareStateMonitorNode::onMap( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) -{ - lanelet_map_ptr_ = std::make_shared(); - lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr_); - is_map_msg_ready_ = true; -} - -void AutowareStateMonitorNode::onRoute( - const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr msg) -{ - if (!is_map_msg_ready_) { - RCLCPP_WARN(this->get_logger(), "Map msg is not ready yet. Skip route msg."); - return; - } - bool is_route_valid = lanelet::utils::route::isRouteValid(*msg, lanelet_map_ptr_); - if (!is_route_valid) { - return; - } - - { - std::lock_guard lock(lock_state_input_); - state_input_.route = msg; - - // Get goal pose - geometry_msgs::msg::Pose::SharedPtr p = std::make_shared(); - *p = msg->goal_pose; - state_input_.goal_pose = geometry_msgs::msg::Pose::ConstSharedPtr(p); - } - - if (disengage_on_route_ && isEngaged()) { - RCLCPP_INFO(this->get_logger(), "new route received and disengage Autoware"); - setDisengage(); - } -} - -void AutowareStateMonitorNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) -{ - std::lock_guard lock(lock_state_input_); - - state_input_.odometry = msg; - - state_input_.odometry_buffer.push_back(msg); - - // Delete old data in buffer - while (true) { - const auto time_diff = rclcpp::Time(msg->header.stamp) - - rclcpp::Time(state_input_.odometry_buffer.front()->header.stamp); - - if (time_diff.seconds() < state_param_.th_stopped_time_sec) { - break; - } - - state_input_.odometry_buffer.pop_front(); - } - - constexpr size_t odometry_buffer_size = 200; // 40Hz * 5sec - if (state_input_.odometry_buffer.size() > odometry_buffer_size) { - state_input_.odometry_buffer.pop_front(); - } -} - -bool AutowareStateMonitorNode::onShutdownService( - const std::shared_ptr request_header, - [[maybe_unused]] const std::shared_ptr request, - const std::shared_ptr response) -{ - (void)request_header; - - { - std::lock_guard lock(lock_state_input_); - state_input_.is_finalizing = true; - } - - const auto t_start = this->get_clock()->now(); - constexpr double timeout = 3.0; - while (rclcpp::ok()) { - // rclcpp::spin_some(this->get_node_base_interface()); - - { - std::unique_lock lock(lock_state_machine_); - if (state_machine_->getCurrentState() == AutowareState::Finalizing) { - lock.unlock(); - - response->success = true; - response->message = "Shutdown Autoware."; - return true; - } - } - - if ((this->get_clock()->now() - t_start).seconds() > timeout) { - response->success = false; - response->message = "Shutdown timeout."; - return true; - } - - rclcpp::Rate(10.0).sleep(); - } - - response->success = false; - response->message = "Shutdown failure."; - return true; -} - -bool AutowareStateMonitorNode::onResetRouteService( - [[maybe_unused]] const std::shared_ptr request_header, - [[maybe_unused]] const std::shared_ptr request, - const std::shared_ptr response) -{ - { - std::unique_lock lock(lock_state_machine_); - if (state_machine_->getCurrentState() != AutowareState::WaitingForEngage) { - lock.unlock(); - - response->success = false; - response->message = "Reset route can be accepted only under WaitingForEngage."; - return true; - } - } - - { - std::lock_guard lock(lock_state_input_); - state_input_.is_route_reset_required = true; - } - - const auto t_start = this->now(); - constexpr double timeout = 3.0; - while (rclcpp::ok()) { - { - // To avoid dead lock, 2-phase lock is required here. - // If you change the order of the locks below, it may be dead-lock. - std::unique_lock lock_state_input(lock_state_input_); - std::unique_lock lock_state_machine(lock_state_machine_); - if (state_machine_->getCurrentState() == AutowareState::WaitingForRoute) { - state_input_.is_route_reset_required = false; - - lock_state_machine.unlock(); - lock_state_input.unlock(); - - response->success = true; - response->message = "Reset route."; - return true; - } - } - - if ((this->now() - t_start).seconds() > timeout) { - response->success = false; - response->message = "Reset route timeout."; - return true; - } - - rclcpp::Rate(10.0).sleep(); - } - - response->success = false; - response->message = "Reset route failure."; - return true; -} - -void AutowareStateMonitorNode::onTimer() -{ - AutowareState prev_autoware_state; - AutowareState autoware_state; - - { - std::unique_lock lock_state_input(lock_state_input_); - - // Prepare state input - state_input_.current_pose = getCurrentPose(tf_buffer_); - if (state_input_.current_pose == nullptr) { - RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 5000 /* ms */, - "Fail lookupTransform base_link to map"); - } - - state_input_.topic_stats = getTopicStats(); - state_input_.param_stats = getParamStats(); - state_input_.tf_stats = getTfStats(); - state_input_.current_time = this->now(); - - // Update state - // To avoid dead lock, 2-phase lock is required here. - std::lock_guard lock_state_machine(lock_state_machine_); - - prev_autoware_state = state_machine_->getCurrentState(); - autoware_state = state_machine_->updateState(state_input_); - } - - if (autoware_state != prev_autoware_state) { - RCLCPP_INFO( - this->get_logger(), "state changed: %i -> %i", toMsg(prev_autoware_state), - toMsg(autoware_state)); - } - - // Disengage on event - if (disengage_on_goal_ && isEngaged() && autoware_state == AutowareState::ArrivedGoal) { - RCLCPP_INFO(this->get_logger(), "arrived goal and disengage Autoware"); - setDisengage(); - } - - // Publish state message - { - autoware_auto_system_msgs::msg::AutowareState autoware_state_msg; - autoware_state_msg.state = toMsg(autoware_state); - - pub_autoware_state_->publish(autoware_state_msg); - } - - // Publish diag message - updater_.force_update(); -} - -// TODO(jilaada): Use generic subscription base -void AutowareStateMonitorNode::onTopic( - [[maybe_unused]] const std::shared_ptr msg, - const std::string & topic_name) -{ - const auto now = this->now(); - - auto & buf = topic_received_time_buffer_.at(topic_name); - buf.push_back(now); - - constexpr size_t topic_received_time_buffer_size = 10; - if (buf.size() > topic_received_time_buffer_size) { - buf.pop_front(); - } -} - -void AutowareStateMonitorNode::registerTopicCallback( - const std::string & topic_name, const std::string & topic_type, const bool transient_local, - const bool best_effort) -{ - // Initialize buffer - topic_received_time_buffer_[topic_name] = {}; - - // Register callback - using Callback = std::function)>; - const auto callback = static_cast( - std::bind(&AutowareStateMonitorNode::onTopic, this, std::placeholders::_1, topic_name)); - auto qos = rclcpp::QoS{1}; - if (transient_local) { - qos.transient_local(); - } - if (best_effort) { - qos.best_effort(); - } - - auto subscriber_option = rclcpp::SubscriptionOptions(); - subscriber_option.callback_group = callback_group_subscribers_; - - sub_topic_map_[topic_name] = - this->create_generic_subscription(topic_name, topic_type, qos, callback, subscriber_option); -} - -TopicStats AutowareStateMonitorNode::getTopicStats() const -{ - TopicStats topic_stats; - topic_stats.checked_time = this->now(); - - for (const auto & topic_config : topic_configs_) { - // Alias - const auto & buf = topic_received_time_buffer_.at(topic_config.name); - - // Check at least once received - if (buf.empty()) { - topic_stats.non_received_list.push_back(topic_config); - continue; - } - - // Check timeout - const auto last_received_time = buf.back(); - const auto time_diff = (topic_stats.checked_time - last_received_time).seconds(); - const auto is_timeout = (topic_config.timeout != 0) && (time_diff > topic_config.timeout); - if (is_timeout) { - topic_stats.timeout_list.emplace_back(topic_config, last_received_time); - continue; - } - - // Check topic rate - if (!is_timeout && buf.size() >= 2) { - const auto topic_rate = calcTopicRate(buf); - if (topic_config.warn_rate != 0 && topic_rate < topic_config.warn_rate) { - topic_stats.slow_rate_list.emplace_back(topic_config, topic_rate); - continue; - } - } - - // No error - topic_stats.ok_list.push_back(topic_config); - } - - return topic_stats; -} - -ParamStats AutowareStateMonitorNode::getParamStats() const -{ - ParamStats param_stats; - param_stats.checked_time = this->now(); - - for (const auto & param_config : param_configs_) { - const bool result = this->has_parameter("param_configs.configs." + param_config.name); - if (!result) { - param_stats.non_set_list.push_back(param_config); - continue; - } - - // No error - param_stats.ok_list.push_back(param_config); - } - - return param_stats; -} - -TfStats AutowareStateMonitorNode::getTfStats() const -{ - TfStats tf_stats; - tf_stats.checked_time = this->now(); - - for (const auto & tf_config : tf_configs_) { - try { - const auto transform = - tf_buffer_.lookupTransform(tf_config.from, tf_config.to, tf2::TimePointZero); - - const auto last_received_time = transform.header.stamp; - const auto time_diff = (tf_stats.checked_time - last_received_time).seconds(); - if (time_diff > tf_config.timeout) { - tf_stats.timeout_list.emplace_back(tf_config, last_received_time); - continue; - } - } catch (tf2::TransformException & ex) { - tf_stats.non_received_list.push_back(tf_config); - continue; - } - - // No error - tf_stats.ok_list.push_back(tf_config); - } - - return tf_stats; -} - -bool AutowareStateMonitorNode::isEngaged() -{ - std::lock_guard lock(lock_state_input_); - if (!state_input_.autoware_engage) { - return false; - } - - return state_input_.autoware_engage->engage; -} - -void AutowareStateMonitorNode::setDisengage() -{ - autoware_auto_vehicle_msgs::msg::Engage msg; - msg.stamp = this->now(); - msg.engage = false; - pub_autoware_engage_->publish(msg); -} - -AutowareStateMonitorNode::AutowareStateMonitorNode() -: Node("ad_service_state_monitor"), - tf_buffer_(this->get_clock()), - tf_listener_(tf_buffer_), - updater_(this) -{ - using std::placeholders::_1; - using std::placeholders::_2; - using std::placeholders::_3; - // Parameter - update_rate_ = this->declare_parameter("update_rate", 10.0); - disengage_on_route_ = this->declare_parameter("disengage_on_route", true); - disengage_on_goal_ = this->declare_parameter("disengage_on_goal", true); - - // Parameter for StateMachine - state_param_.th_arrived_distance_m = this->declare_parameter("th_arrived_distance_m", 1.0); - const auto th_arrived_angle_deg = this->declare_parameter("th_arrived_angle_deg", 45.0); - state_param_.th_arrived_angle = tier4_autoware_utils::deg2rad(th_arrived_angle_deg); - state_param_.th_stopped_time_sec = this->declare_parameter("th_stopped_time_sec", 1.0); - state_param_.th_stopped_velocity_mps = this->declare_parameter("th_stopped_velocity_mps", 0.01); - - // State Machine - state_machine_ = std::make_shared(state_param_); - - // Config - topic_configs_ = getConfigs(this->get_node_parameters_interface(), "topic_configs"); - tf_configs_ = getConfigs(this->get_node_parameters_interface(), "tf_configs"); - - // Callback Groups - callback_group_subscribers_ = - this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - callback_group_services_ = - this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - auto subscriber_option = rclcpp::SubscriptionOptions(); - subscriber_option.callback_group = callback_group_subscribers_; - - // Topic Callback - for (const auto & topic_config : topic_configs_) { - registerTopicCallback( - topic_config.name, topic_config.type, topic_config.transient_local, topic_config.best_effort); - } - - // Subscriber - sub_autoware_engage_ = this->create_subscription( - "input/autoware_engage", 1, std::bind(&AutowareStateMonitorNode::onAutowareEngage, this, _1), - subscriber_option); - sub_control_mode_ = this->create_subscription( - "input/control_mode", 1, std::bind(&AutowareStateMonitorNode::onVehicleControlMode, this, _1), - subscriber_option); - sub_map_ = create_subscription( - "input/vector_map", rclcpp::QoS{1}.transient_local(), - std::bind(&AutowareStateMonitorNode::onMap, this, _1), subscriber_option); - sub_route_ = this->create_subscription( - "input/route", rclcpp::QoS{1}.transient_local(), - std::bind(&AutowareStateMonitorNode::onRoute, this, _1), subscriber_option); - sub_modified_goal_ = this->create_subscription( - "input/modified_goal", 1, std::bind(&AutowareStateMonitorNode::onModifiedGoal, this, _1), - subscriber_option); - sub_odom_ = this->create_subscription( - "input/odometry", 100, std::bind(&AutowareStateMonitorNode::onOdometry, this, _1), - subscriber_option); - - // Service - srv_shutdown_ = this->create_service( - "service/shutdown", std::bind(&AutowareStateMonitorNode::onShutdownService, this, _1, _2, _3), - rmw_qos_profile_services_default, callback_group_services_); - srv_reset_route_ = this->create_service( - "service/reset_route", - std::bind(&AutowareStateMonitorNode::onResetRouteService, this, _1, _2, _3), - rmw_qos_profile_services_default, callback_group_services_); - - // Publisher - pub_autoware_state_ = this->create_publisher( - "output/autoware_state", 1); - pub_autoware_engage_ = - this->create_publisher("output/autoware_engage", 1); - - // Diagnostic Updater - setupDiagnosticUpdater(); - - // Wait for first topics - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - - // Timer - const auto period_ns = rclcpp::Rate(update_rate_).period(); - timer_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&AutowareStateMonitorNode::onTimer, this), - callback_group_subscribers_); -} diff --git a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/diagnostics.cpp b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/diagnostics.cpp deleted file mode 100644 index 503547370029b..0000000000000 --- a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/diagnostics.cpp +++ /dev/null @@ -1,181 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ad_service_state_monitor/ad_service_state_monitor_node.hpp" - -#include -#include - -#define FMT_HEADER_ONLY -#include - -void AutowareStateMonitorNode::setupDiagnosticUpdater() -{ - updater_.setHardwareID("ad_service_state_monitor"); - - const auto module_names = this->declare_parameter>("module_names"); - - // Topic - for (const auto & module_name : module_names) { - const auto diag_name = fmt::format("{}_topic_status", module_name); - - updater_.add( - diag_name, - std::bind( - &AutowareStateMonitorNode::checkTopicStatus, this, std::placeholders::_1, module_name)); - } - - // TF - updater_.add( - "localization_tf_status", - std::bind( - &AutowareStateMonitorNode::checkTFStatus, this, std::placeholders::_1, "localization")); -} - -void AutowareStateMonitorNode::checkTopicStatus( - diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & module_name) -{ - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - - std::lock_guard lock(lock_state_input_); - const auto & topic_stats = state_input_.topic_stats; - - // OK - for (const auto & topic_config : topic_stats.ok_list) { - if (topic_config.module != module_name) { - continue; - } - - stat.add(fmt::format("{} status", topic_config.name), "OK"); - } - - // Check topic received - for (const auto & topic_config : topic_stats.non_received_list) { - if (topic_config.module != module_name) { - continue; - } - - stat.add(fmt::format("{} status", topic_config.name), "Not Received"); - - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - } - - // Check topic rate - for (const auto & topic_config_pair : topic_stats.slow_rate_list) { - const auto & topic_config = topic_config_pair.first; - const auto & topic_rate = topic_config_pair.second; - - if (topic_config.module != module_name) { - continue; - } - - const auto & name = topic_config.name; - stat.add(fmt::format("{} status", name), "Slow Rate"); - stat.addf(fmt::format("{} warn_rate", name), "%.2f [Hz]", topic_config.warn_rate); - stat.addf(fmt::format("{} measured_rate", name), "%.2f [Hz]", topic_rate); - - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - } - - // Check topic timeout - for (const auto & topic_config_pair : topic_stats.timeout_list) { - const auto & topic_config = topic_config_pair.first; - const auto & last_received_time = topic_config_pair.second; - - if (topic_config.module != module_name) { - continue; - } - - const auto & name = topic_config.name; - stat.add(fmt::format("{} status", name), "Timeout"); - stat.addf(fmt::format("{} timeout", name), "%.2f [s]", topic_config.timeout); - stat.addf(fmt::format("{} checked_time", name), "%.2f [s]", topic_stats.checked_time.seconds()); - stat.addf(fmt::format("{} last_received_time", name), "%.2f [s]", last_received_time.seconds()); - - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - } - - // Create message - std::string msg; - if (level == diagnostic_msgs::msg::DiagnosticStatus::OK) { - msg = "OK"; - } else if (level == diagnostic_msgs::msg::DiagnosticStatus::WARN) { - msg = "Warn"; - } else if (level == diagnostic_msgs::msg::DiagnosticStatus::ERROR) { - msg = "Error"; - } - - stat.summary(level, msg); -} - -void AutowareStateMonitorNode::checkTFStatus( - diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & module_name) -{ - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - - std::lock_guard lock(lock_state_input_); - const auto & tf_stats = state_input_.tf_stats; - - // OK - for (const auto & tf_config : tf_stats.ok_list) { - if (tf_config.module != module_name) { - continue; - } - - const auto name = fmt::format("{}2{}", tf_config.from, tf_config.to); - stat.add(fmt::format("{} status", name), "OK"); - } - - // Check tf received - for (const auto & tf_config : tf_stats.non_received_list) { - if (tf_config.module != module_name) { - continue; - } - - const auto name = fmt::format("{}2{}", tf_config.from, tf_config.to); - stat.add(fmt::format("{} status", name), "Not Received"); - - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - } - - // Check tf timeout - for (const auto & tf_config_pair : tf_stats.timeout_list) { - const auto & tf_config = tf_config_pair.first; - const auto & last_received_time = tf_config_pair.second; - - if (tf_config.module != module_name) { - continue; - } - - const auto name = fmt::format("{}2{}", tf_config.from, tf_config.to); - stat.add(fmt::format("{} status", name), "Timeout"); - stat.addf(fmt::format("{} timeout", name), "%.2f [s]", tf_config.timeout); - stat.addf(fmt::format("{} checked_time", name), "%.2f [s]", tf_stats.checked_time.seconds()); - stat.addf(fmt::format("{} last_received_time", name), "%.2f [s]", last_received_time.seconds()); - - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - } - - // Create message - std::string msg; - if (level == diagnostic_msgs::msg::DiagnosticStatus::OK) { - msg = "OK"; - } else if (level == diagnostic_msgs::msg::DiagnosticStatus::WARN) { - msg = "Warn"; - } else if (level == diagnostic_msgs::msg::DiagnosticStatus::ERROR) { - msg = "Error"; - } - - stat.summary(level, msg); -} diff --git a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/main.cpp b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/main.cpp deleted file mode 100644 index 84e28f401e7e6..0000000000000 --- a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/main.cpp +++ /dev/null @@ -1,31 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ad_service_state_monitor/ad_service_state_monitor_node.hpp" - -#include - -#include - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - auto executor = std::make_shared(); - executor->add_node(node); - executor->spin(); - rclcpp::shutdown(); - - return 0; -} diff --git a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/state_machine.cpp b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/state_machine.cpp deleted file mode 100644 index 8eada23c7c7eb..0000000000000 --- a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/state_machine.cpp +++ /dev/null @@ -1,350 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ad_service_state_monitor/state_machine.hpp" - -#include -#include - -#define FMT_HEADER_ONLY // NOLINT -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -namespace -{ -double calcDistance2d(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) -{ - return std::hypot(p1.x - p2.x, p1.y - p2.y); -} - -double calcDistance2d(const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2) -{ - return calcDistance2d(p1.position, p2.position); -} - -bool isValidAngle( - const geometry_msgs::msg::Pose & current_pose, const geometry_msgs::msg::Pose & ref_pose, - const double th_angle_rad) -{ - const double yaw_curr = tf2::getYaw(current_pose.orientation); - const double yaw_ref = tf2::getYaw(ref_pose.orientation); - const double yaw_diff = tier4_autoware_utils::normalizeRadian(yaw_curr - yaw_ref); - return std::fabs(yaw_diff) < th_angle_rad; -} - -bool isNearGoal( - const geometry_msgs::msg::Pose & current_pose, const geometry_msgs::msg::Pose & goal_pose, - const double th_dist) -{ - return calcDistance2d(current_pose, goal_pose) < th_dist; -} - -bool isStopped( - const std::deque & odometry_buffer, - const double th_stopped_velocity_mps) -{ - for (const auto & odometry : odometry_buffer) { - if (std::abs(odometry->twist.twist.linear.x) > th_stopped_velocity_mps) { - return false; - } - } - return true; -} - -template -std::vector filterConfigByModuleName(const std::vector & configs, const char * module_name) -{ - std::vector filtered; - - for (const auto & config : configs) { - if (config.module == module_name) { - filtered.push_back(config); - } - } - - return filtered; -} - -} // namespace - -bool StateMachine::isModuleInitialized(const char * module_name) const -{ - const auto non_received_topics = - filterConfigByModuleName(state_input_.topic_stats.non_received_list, module_name); - const auto non_set_params = - filterConfigByModuleName(state_input_.param_stats.non_set_list, module_name); - const auto non_received_tfs = - filterConfigByModuleName(state_input_.tf_stats.non_received_list, module_name); - - if (non_received_topics.empty() && non_set_params.empty() && non_received_tfs.empty()) { - return true; - } - - for (const auto & topic_config : non_received_topics) { - const auto msg = fmt::format("topic `{}` is not received yet", topic_config.name); - msgs_.push_back(msg); - } - - for (const auto & param_config : non_set_params) { - const auto msg = fmt::format("param `{}` is not set", param_config.name); - msgs_.push_back(msg); - } - - for (const auto & tf_config : non_received_tfs) { - const auto msg = - fmt::format("tf from `{}` to `{}` is not received yet", tf_config.from, tf_config.to); - msgs_.push_back(msg); - } - - { - const auto msg = fmt::format("module `{}` is not initialized", module_name); - msgs_.push_back(msg); - } - - return false; -} - -bool StateMachine::isVehicleInitialized() const -{ - if (!isModuleInitialized(ModuleName::map)) { - return false; - } - - if (!isModuleInitialized(ModuleName::vehicle)) { - return false; - } - - if (!isModuleInitialized(ModuleName::sensing)) { - return false; - } - - if (!isModuleInitialized(ModuleName::localization)) { - return false; - } - - if (!isModuleInitialized(ModuleName::perception)) { - return false; - } - - // TODO(Kenji Miyake): Check if the vehicle is on a lane? - - return true; -} - -bool StateMachine::hasRoute() const { return state_input_.route != nullptr; } - -bool StateMachine::isRouteReceived() const { return state_input_.route != executing_route_; } - -bool StateMachine::isPlanningCompleted() const -{ - if (!isModuleInitialized(ModuleName::planning)) { - return false; - } - - if (!isModuleInitialized(ModuleName::control)) { - return false; - } - - return true; -} - -bool StateMachine::isEngaged() const -{ - if (!state_input_.autoware_engage) { - return false; - } - - if (state_input_.autoware_engage->engage != 1) { - return false; - } - - if (!state_input_.control_mode_) { - return false; - } - - if ( - state_input_.control_mode_->mode == - autoware_auto_vehicle_msgs::msg::ControlModeReport::MANUAL) { - return false; - } - - return true; -} - -bool StateMachine::isOverridden() const { return !isEngaged(); } - -bool StateMachine::hasArrivedGoal() const -{ - geometry_msgs::msg::Pose goal_pose = *state_input_.goal_pose; - if ( - state_input_.modified_goal_pose != nullptr && - rclcpp::Time(state_input_.route->header.stamp).seconds() == - rclcpp::Time(state_input_.modified_goal_pose->header.stamp).seconds()) { - goal_pose = state_input_.modified_goal_pose->pose; - } - - const auto is_valid_goal_angle = - isValidAngle(state_input_.current_pose->pose, goal_pose, state_param_.th_arrived_angle); - const auto is_near_goal = - isNearGoal(state_input_.current_pose->pose, goal_pose, state_param_.th_arrived_distance_m); - const auto is_stopped = - isStopped(state_input_.odometry_buffer, state_param_.th_stopped_velocity_mps); - - if (is_valid_goal_angle && is_near_goal && is_stopped) { - return true; - } - - return false; -} - -bool StateMachine::isFinalizing() const { return state_input_.is_finalizing; } - -bool StateMachine::isRouteResetRequired() const { return state_input_.is_route_reset_required; } - -AutowareState StateMachine::updateState(const StateInput & state_input) -{ - msgs_ = {}; - state_input_ = state_input; - autoware_state_ = judgeAutowareState(); - return autoware_state_; -} - -AutowareState StateMachine::judgeAutowareState() const -{ - if (isFinalizing()) { - return AutowareState::Finalizing; - } - - switch (autoware_state_) { - case (AutowareState::InitializingVehicle): { - if (isVehicleInitialized()) { - if (!flags_.waiting_after_initializing) { - flags_.waiting_after_initializing = true; - times_.initializing_completed = state_input_.current_time; - break; - } - - // Wait after initialize completed to avoid sync error - constexpr double wait_time_after_initializing = 1.0; - const auto time_from_initializing = - state_input_.current_time - times_.initializing_completed; - if (time_from_initializing.seconds() > wait_time_after_initializing) { - flags_.waiting_after_initializing = false; - return AutowareState::WaitingForRoute; - } - } - - break; - } - - case (AutowareState::WaitingForRoute): { - if (isRouteReceived()) { - return AutowareState::Planning; - } - - if (hasRoute() && isEngaged() && !hasArrivedGoal()) { - return AutowareState::Driving; - } - - break; - } - - case (AutowareState::Planning): { - executing_route_ = state_input_.route; - - if (isPlanningCompleted()) { - if (!flags_.waiting_after_planning) { - flags_.waiting_after_planning = true; - times_.planning_completed = state_input_.current_time; - break; - } - - // Wait after planning completed to avoid sync error - constexpr double wait_time_after_planning = 3.0; - const auto time_from_planning = state_input_.current_time - times_.planning_completed; - if (time_from_planning.seconds() > wait_time_after_planning) { - flags_.waiting_after_planning = false; - return AutowareState::WaitingForEngage; - } - } - - break; - } - - case (AutowareState::WaitingForEngage): { - if (isRouteResetRequired()) { - return AutowareState::WaitingForRoute; - } - - if (isRouteReceived()) { - return AutowareState::Planning; - } - - if (isEngaged()) { - return AutowareState::Driving; - } - - if (hasArrivedGoal()) { - times_.arrived_goal = state_input_.current_time; - return AutowareState::ArrivedGoal; - } - - break; - } - - case (AutowareState::Driving): { - if (isRouteReceived()) { - return AutowareState::Planning; - } - - if (isOverridden()) { - return AutowareState::WaitingForEngage; - } - - if (hasArrivedGoal()) { - times_.arrived_goal = state_input_.current_time; - return AutowareState::ArrivedGoal; - } - - break; - } - - case (AutowareState::ArrivedGoal): { - constexpr double wait_time_after_arrived_goal = 2.0; - const auto time_from_arrived_goal = state_input_.current_time - times_.arrived_goal; - if (time_from_arrived_goal.seconds() > wait_time_after_arrived_goal) { - return AutowareState::WaitingForRoute; - } - - break; - } - - case (AutowareState::Finalizing): { - break; - } - - default: { - throw std::runtime_error("invalid state"); - } - } - - // continue previous state when break - return autoware_state_; -} diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index 2c4a28c250069..12b6eef559fe4 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -12,9 +12,11 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/operation_mode.cpp src/routing.cpp src/utils/route_conversion.cpp + src/compatibility/autoware_state.cpp ) rclcpp_components_register_nodes(${PROJECT_NAME} + "default_ad_api::AutowareStateNode" "default_ad_api::FailSafeNode" "default_ad_api::InterfaceNode" "default_ad_api::LocalizationNode" diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index c98ebf55944ec..7856530bbf981 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -30,6 +30,7 @@ def create_api_node(node_name, class_name, **kwargs): def generate_launch_description(): components = [ + create_api_node("autoware_state", "AutowareStateNode"), create_api_node("fail_safe", "FailSafeNode"), create_api_node("interface", "InterfaceNode"), create_api_node("localization", "LocalizationNode"), diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index bc0d1edecb8dd..b03b630996bf6 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -15,12 +15,14 @@ autoware_adapi_v1_msgs autoware_adapi_version_msgs autoware_auto_planning_msgs + autoware_auto_system_msgs autoware_planning_msgs component_interface_specs component_interface_utils motion_utils rclcpp rclcpp_components + std_srvs tier4_control_msgs tier4_system_msgs diff --git a/system/default_ad_api/src/compatibility/autoware_state.cpp b/system/default_ad_api/src/compatibility/autoware_state.cpp new file mode 100644 index 0000000000000..5778edf66559f --- /dev/null +++ b/system/default_ad_api/src/compatibility/autoware_state.cpp @@ -0,0 +1,147 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware_state.hpp" + +#include +#include + +namespace default_ad_api +{ + +AutowareStateNode::AutowareStateNode(const rclcpp::NodeOptions & options) +: Node("autoware_state", options) +{ + const std::vector module_names = { + "sensing", "perception", "map", "localization", "planning", "control", "vehicle", "system", + }; + + for (size_t i = 0; i < module_names.size(); ++i) { + const auto name = "/system/component_state_monitor/component/launch/" + module_names[i]; + const auto qos = rclcpp::QoS(1).transient_local(); + const auto callback = [this, i](const ModeChangeAvailable::ConstSharedPtr msg) { + component_states_[i] = msg->available; + }; + sub_component_states_.push_back(create_subscription(name, qos, callback)); + } + + pub_autoware_state_ = create_publisher("/autoware/state", 1); + srv_autoware_shutdown_ = create_service( + "/autoware/shutdown", + std::bind(&AutowareStateNode::on_shutdown, this, std::placeholders::_1, std::placeholders::_2)); + + const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_sub(sub_localization_, this, &AutowareStateNode::on_localization); + adaptor.init_sub(sub_routing_, this, &AutowareStateNode::on_routing); + adaptor.init_sub(sub_operation_mode_, this, &AutowareStateNode::on_operation_mode); + + // TODO(Takagi, Isamu): remove default value + const auto rate = rclcpp::Rate(declare_parameter("update_rate", 10.0)); + timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); }); + + component_states_.resize(module_names.size()); + launch_state_ = LaunchState::Initializing; + localization_state_.state = LocalizationState::UNKNOWN; + routing_state_.state = RoutingState::UNKNOWN; + operation_mode_state_.mode = OperationModeState::UNKNOWN; +} + +void AutowareStateNode::on_localization(const LocalizationState::ConstSharedPtr msg) +{ + localization_state_ = *msg; +} +void AutowareStateNode::on_routing(const RoutingState::ConstSharedPtr msg) +{ + routing_state_ = *msg; +} +void AutowareStateNode::on_operation_mode(const OperationModeState::ConstSharedPtr msg) +{ + operation_mode_state_ = *msg; +} + +void AutowareStateNode::on_shutdown( + const Trigger::Request::SharedPtr, const Trigger::Response::SharedPtr res) +{ + launch_state_ = LaunchState::Finalizing; + res->success = true; + res->message = "Shutdown Autoware."; +} + +void AutowareStateNode::on_timer() +{ + const auto convert_state = [this]() { + if (launch_state_ == LaunchState::Finalizing) { + return AutowareState::FINALIZING; + } + if (launch_state_ == LaunchState::Initializing) { + return AutowareState::INITIALIZING; + } + if (localization_state_.state == LocalizationState::UNKNOWN) { + return AutowareState::INITIALIZING; + } + if (routing_state_.state == RoutingState::UNKNOWN) { + return AutowareState::INITIALIZING; + } + if (operation_mode_state_.mode == OperationModeState::UNKNOWN) { + return AutowareState::INITIALIZING; + } + if (localization_state_.state != LocalizationState::INITIALIZED) { + return AutowareState::INITIALIZING; + } + if (routing_state_.state == RoutingState::UNSET) { + return AutowareState::WAITING_FOR_ROUTE; + } + if (routing_state_.state == RoutingState::ARRIVED) { + return AutowareState::ARRIVED_GOAL; + } + if (operation_mode_state_.mode != OperationModeState::STOP) { + if (operation_mode_state_.is_autoware_control_enabled) { + return AutowareState::DRIVING; + } + } + if (operation_mode_state_.is_autonomous_mode_available) { + return AutowareState::WAITING_FOR_ENGAGE; + } + return AutowareState::PLANNING; + }; + + // Update launch state. + if (launch_state_ == LaunchState::Initializing) { + bool is_initialized = true; + for (const auto & state : component_states_) { + is_initialized &= state; + } + if (is_initialized) { + launch_state_ = LaunchState::Running; + } + } + + // Update routing state to reproduce old logic. + if (routing_state_.state == RoutingState::ARRIVED) { + const auto duration = now() - rclcpp::Time(routing_state_.stamp); + if (2.0 < duration.seconds()) { + routing_state_.state = RoutingState::UNSET; + } + } + + AutowareState msg; + msg.stamp = now(); + msg.state = convert_state(); + pub_autoware_state_->publish(msg); +} + +} // namespace default_ad_api + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::AutowareStateNode) diff --git a/system/default_ad_api/src/compatibility/autoware_state.hpp b/system/default_ad_api/src/compatibility/autoware_state.hpp new file mode 100644 index 0000000000000..4cd6233f5df0a --- /dev/null +++ b/system/default_ad_api/src/compatibility/autoware_state.hpp @@ -0,0 +1,72 @@ +// 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 COMPATIBILITY__AUTOWARE_STATE_HPP_ +#define COMPATIBILITY__AUTOWARE_STATE_HPP_ + +#include +#include +#include + +#include +#include +#include + +#include + +// This file should be included after messages. +#include "../utils/types.hpp" + +namespace default_ad_api +{ + +class AutowareStateNode : public rclcpp::Node +{ +public: + explicit AutowareStateNode(const rclcpp::NodeOptions & options); + +private: + using ModeChangeAvailable = tier4_system_msgs::msg::ModeChangeAvailable; + rclcpp::TimerBase::SharedPtr timer_; + // emergency + Sub sub_localization_; + Sub sub_routing_; + Sub sub_operation_mode_; + + using AutowareState = autoware_auto_system_msgs::msg::AutowareState; + using LocalizationState = autoware_ad_api::localization::InitializationState::Message; + using RoutingState = autoware_ad_api::routing::RouteState::Message; + using OperationModeState = autoware_ad_api::operation_mode::OperationModeState::Message; + using Trigger = std_srvs::srv::Trigger; + std::vector component_states_; + std::vector::SharedPtr> sub_component_states_; + rclcpp::Publisher::SharedPtr pub_autoware_state_; + rclcpp::Service::SharedPtr srv_autoware_shutdown_; + + enum class LaunchState { Initializing, Running, Finalizing }; + LaunchState launch_state_; + LocalizationState localization_state_; + RoutingState routing_state_; + OperationModeState operation_mode_state_; + + void on_timer(); + void on_localization(const LocalizationState::ConstSharedPtr msg); + void on_routing(const RoutingState::ConstSharedPtr msg); + void on_operation_mode(const OperationModeState::ConstSharedPtr msg); + void on_shutdown(const Trigger::Request::SharedPtr, const Trigger::Response::SharedPtr); +}; + +} // namespace default_ad_api + +#endif // COMPATIBILITY__AUTOWARE_STATE_HPP_ diff --git a/system/default_ad_api/src/interface.cpp b/system/default_ad_api/src/interface.cpp index 36dcfe3d0043f..65d16010951a4 100644 --- a/system/default_ad_api/src/interface.cpp +++ b/system/default_ad_api/src/interface.cpp @@ -20,8 +20,8 @@ namespace default_ad_api InterfaceNode::InterfaceNode(const rclcpp::NodeOptions & options) : Node("interface", options) { const auto on_interface_version = [](auto, auto res) { - res->major = 0; - res->minor = 1; + res->major = 1; + res->minor = 0; res->patch = 0; }; diff --git a/system/default_ad_api/src/routing.cpp b/system/default_ad_api/src/routing.cpp index 914f882eb006d..e4a6dae6e8e89 100644 --- a/system/default_ad_api/src/routing.cpp +++ b/system/default_ad_api/src/routing.cpp @@ -29,16 +29,40 @@ RoutingNode::RoutingNode(const rclcpp::NodeOptions & options) : Node("routing", adaptor.init_pub(pub_route_); adaptor.init_sub(sub_state_, this, &RoutingNode::on_state); adaptor.init_sub(sub_route_, this, &RoutingNode::on_route); + adaptor.init_cli(cli_clear_route_, group_cli_); + adaptor.init_srv(srv_clear_route_, this, &RoutingNode::on_clear_route); adaptor.init_cli(cli_set_route_, group_cli_); adaptor.init_srv(srv_set_route_, this, &RoutingNode::on_set_route); adaptor.relay_service(cli_set_route_points_, srv_set_route_points_, group_cli_); - adaptor.relay_service(cli_clear_route_, srv_clear_route_, group_cli_); + + adaptor.init_cli(cli_operation_mode_, group_cli_); + adaptor.init_sub(sub_operation_mode_, this, &RoutingNode::on_operation_mode); +} + +void RoutingNode::change_stop_mode() +{ + using OperationModeRequest = system_interface::ChangeOperationMode::Service::Request; + if (is_auto_mode) { + const auto req = std::make_shared(); + req->mode = OperationModeRequest::STOP; + cli_operation_mode_->async_send_request(req); + } +} + +void RoutingNode::on_operation_mode(const OperationModeState::Message::ConstSharedPtr msg) +{ + is_auto_mode = msg->mode == OperationModeState::Message::AUTONOMOUS; } void RoutingNode::on_state(const State::Message::ConstSharedPtr msg) { pub_state_->publish(*msg); + // Change operation mode to stop when the vehicle arrives. + if (msg->state == State::Message::ARRIVED) { + change_stop_mode(); + } + // TODO(Takagi, Isamu): Remove when the mission planner supports an empty route. if (msg->state == State::Message::UNSET) { pub_route_->publish(conversion::create_empty_route(msg->stamp)); @@ -50,6 +74,14 @@ void RoutingNode::on_route(const Route::Message::ConstSharedPtr msg) pub_route_->publish(conversion::convert_route(*msg)); } +void RoutingNode::on_clear_route( + const autoware_ad_api::routing::ClearRoute::Service::Request::SharedPtr req, + const autoware_ad_api::routing::ClearRoute::Service::Response::SharedPtr res) +{ + change_stop_mode(); + *res = *cli_clear_route_->call(req); +} + void RoutingNode::on_set_route( const autoware_ad_api::routing::SetRoute::Service::Request::SharedPtr req, const autoware_ad_api::routing::SetRoute::Service::Response::SharedPtr res) diff --git a/system/default_ad_api/src/routing.hpp b/system/default_ad_api/src/routing.hpp index 8ba3831ee96fb..47d23378f2c12 100644 --- a/system/default_ad_api/src/routing.hpp +++ b/system/default_ad_api/src/routing.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -43,11 +44,20 @@ class RoutingNode : public rclcpp::Node Cli cli_set_route_points_; Cli cli_set_route_; Cli cli_clear_route_; + Cli cli_operation_mode_; + Sub sub_operation_mode_; + bool is_auto_mode = false; + using OperationModeState = system_interface::OperationModeState; using State = planning_interface::RouteState; using Route = planning_interface::Route; + void change_stop_mode(); + void on_operation_mode(const OperationModeState::Message::ConstSharedPtr msg); void on_state(const State::Message::ConstSharedPtr msg); void on_route(const Route::Message::ConstSharedPtr msg); + void on_clear_route( + const autoware_ad_api::routing::ClearRoute::Service::Request::SharedPtr req, + const autoware_ad_api::routing::ClearRoute::Service::Response::SharedPtr res); void on_set_route( const autoware_ad_api::routing::SetRoute::Service::Request::SharedPtr req, const autoware_ad_api::routing::SetRoute::Service::Response::SharedPtr res); diff --git a/system/default_ad_api/test/node/interface_version.py b/system/default_ad_api/test/node/interface_version.py index 12c3918ef902a..19be3dc714c22 100644 --- a/system/default_ad_api/test/node/interface_version.py +++ b/system/default_ad_api/test/node/interface_version.py @@ -28,9 +28,9 @@ rclpy.spin_until_future_complete(node, future) response = future.result() -if response.major != 0: +if response.major != 1: exit(1) -if response.minor != 1: +if response.minor != 0: exit(1) if response.patch != 0: exit(1) diff --git a/system/system_error_monitor/config/diagnostic_aggregator/sensing.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/sensing.param.yaml index 887e671ff072c..27dc4518d4f30 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/sensing.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/sensing.param.yaml @@ -1,15 +1,2 @@ /**: - ros__parameters: - sensing: - type: diagnostic_aggregator/AnalyzerGroup - path: sensing - analyzers: - node_alive_monitoring: - type: diagnostic_aggregator/AnalyzerGroup - path: node_alive_monitoring - analyzers: - topic_status: - type: diagnostic_aggregator/GenericAnalyzer - path: topic_status - contains: [": sensing_topic_status"] - timeout: 1.0 + ros__parameters: {} diff --git a/system/system_error_monitor/config/system_error_monitor.param.yaml b/system/system_error_monitor/config/system_error_monitor.param.yaml index b9a5a4fa79f52..b9de5cc4f2e13 100644 --- a/system/system_error_monitor/config/system_error_monitor.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.param.yaml @@ -32,7 +32,7 @@ /autoware/planning/node_alive_monitoring: default /autoware/planning/performance_monitoring/trajectory_validation: default - /autoware/sensing/node_alive_monitoring: default + # /autoware/sensing/node_alive_monitoring: default /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default diff --git a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml index ce081b75b2836..bf40c334f6498 100644 --- a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml @@ -32,7 +32,7 @@ /autoware/planning/node_alive_monitoring: default /autoware/planning/performance_monitoring/trajectory_validation: default - /autoware/sensing/node_alive_monitoring: default + # /autoware/sensing/node_alive_monitoring: default /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default