diff --git a/system/component_state_monitor/config/topics.yaml b/system/component_state_monitor/config/topics.yaml index 098aa0e56b1ae..3845a96108d2b 100644 --- a/system/component_state_monitor/config/topics.yaml +++ b/system/component_state_monitor/config/topics.yaml @@ -4,7 +4,7 @@ args: node_name_suffix: vector_map topic: /map/vector_map - topic_type: autoware_auto_mapping_msgs/msg/HADMapBin + topic_type: autoware_map_msgs/msg/LaneletMapBin best_effort: false transient_local: true warn_rate: 0.0 @@ -69,7 +69,7 @@ args: node_name_suffix: object_recognition_objects topic: /perception/object_recognition/objects - topic_type: autoware_auto_perception_msgs/msg/PredictedObjects + topic_type: autoware_perception_msgs/msg/PredictedObjects best_effort: false transient_local: false warn_rate: 5.0 @@ -82,7 +82,7 @@ args: node_name_suffix: traffic_light_recognition_traffic_signals topic: /perception/traffic_light_recognition/traffic_signals - topic_type: autoware_perception_msgs/msg/TrafficSignalArray + topic_type: autoware_perception_msgs/msg/TrafficLightGroupArray best_effort: false transient_local: false warn_rate: 5.0 @@ -108,7 +108,7 @@ args: node_name_suffix: scenario_planning_trajectory topic: /planning/scenario_planning/trajectory - topic_type: autoware_auto_planning_msgs/msg/Trajectory + topic_type: autoware_planning_msgs/msg/Trajectory best_effort: false transient_local: false warn_rate: 5.0 @@ -121,7 +121,7 @@ args: node_name_suffix: trajectory_follower_control_cmd topic: /control/trajectory_follower/control_cmd - topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand + topic_type: autoware_control_msgs/msg/Control best_effort: false transient_local: false warn_rate: 5.0 @@ -134,7 +134,7 @@ args: node_name_suffix: control_command_control_cmd topic: /control/command/control_cmd - topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand + topic_type: autoware_control_msgs/msg/Control best_effort: false transient_local: false warn_rate: 5.0 @@ -147,7 +147,7 @@ args: node_name_suffix: vehicle_status_velocity_status topic: /vehicle/status/velocity_status - topic_type: autoware_auto_vehicle_msgs/msg/VelocityReport + topic_type: autoware_vehicle_msgs/msg/VelocityReport best_effort: false transient_local: false warn_rate: 5.0 @@ -160,7 +160,7 @@ args: node_name_suffix: vehicle_status_steering_status topic: /vehicle/status/steering_status - topic_type: autoware_auto_vehicle_msgs/msg/SteeringReport + topic_type: autoware_vehicle_msgs/msg/SteeringReport best_effort: false transient_local: false warn_rate: 5.0 @@ -173,7 +173,7 @@ args: node_name_suffix: system_emergency_control_cmd topic: /system/emergency/control_cmd - topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand + topic_type: autoware_control_msgs/msg/Control best_effort: false transient_local: false warn_rate: 5.0 diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index f9b5a167bdab1..77276c7acb0e3 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -15,10 +15,9 @@ autoware_ad_api_specs autoware_adapi_v1_msgs autoware_adapi_version_msgs - autoware_auto_planning_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs autoware_planning_msgs + autoware_system_msgs + autoware_vehicle_msgs component_interface_specs component_interface_utils diagnostic_graph_utils diff --git a/system/default_ad_api/src/compatibility/autoware_state.hpp b/system/default_ad_api/src/compatibility/autoware_state.hpp index 4cd6233f5df0a..eff74728efcb0 100644 --- a/system/default_ad_api/src/compatibility/autoware_state.hpp +++ b/system/default_ad_api/src/compatibility/autoware_state.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -44,7 +44,7 @@ class AutowareStateNode : public rclcpp::Node Sub sub_routing_; Sub sub_operation_mode_; - using AutowareState = autoware_auto_system_msgs::msg::AutowareState; + using AutowareState = autoware_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; diff --git a/system/default_ad_api/src/perception.cpp b/system/default_ad_api/src/perception.cpp index 61d09444c70a4..794bbf141ca8b 100644 --- a/system/default_ad_api/src/perception.cpp +++ b/system/default_ad_api/src/perception.cpp @@ -24,7 +24,7 @@ using ObjectClassification = autoware_adapi_v1_msgs::msg::ObjectClassification; using DynamicObject = autoware_adapi_v1_msgs::msg::DynamicObject; using DynamicObjectPath = autoware_adapi_v1_msgs::msg::DynamicObjectPath; using API_Shape = shape_msgs::msg::SolidPrimitive; -using Shape = autoware_auto_perception_msgs::msg::Shape; +using Shape = autoware_perception_msgs::msg::Shape; std::unordered_map shape_type_ = { {Shape::BOUNDING_BOX, API_Shape::BOX}, diff --git a/system/default_ad_api/src/perception.hpp b/system/default_ad_api/src/perception.hpp index d1c71fdb08025..ff3e4801b07ac 100644 --- a/system/default_ad_api/src/perception.hpp +++ b/system/default_ad_api/src/perception.hpp @@ -22,7 +22,7 @@ #include #include #include -#include +#include #include #include diff --git a/system/emergency_handler/README.md b/system/emergency_handler/README.md index 3d658dbb4586f..f34ebcf8d9b9a 100644 --- a/system/emergency_handler/README.md +++ b/system/emergency_handler/README.md @@ -14,24 +14,24 @@ Emergency Handler is a node to select proper MRM from from system failure state ### Input -| Name | Type | Description | -| ----------------------------------------- | ---------------------------------------------------------- | ----------------------------------------------------------------------------- | -| `/system/emergency/hazard_status` | `autoware_auto_system_msgs::msg::HazardStatusStamped` | Used to select proper MRM from system failure state contained in HazardStatus | -| `/control/vehicle_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | Used as reference when generate Emergency Control Command | -| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Used to decide whether vehicle is stopped or not | -| `/vehicle/status/control_mode` | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual | -| `/system/api/mrm/comfortable_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM comfortable stop operation is available | -| `/system/api/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM emergency stop operation is available | +| Name | Type | Description | +| ----------------------------------------- | ------------------------------------------------ | ----------------------------------------------------------------------------- | +| `/system/emergency/hazard_status` | `autoware_system_msgs::msg::HazardStatusStamped` | Used to select proper MRM from system failure state contained in HazardStatus | +| `/control/vehicle_cmd` | `autoware_control_msgs::msg::Control` | Used as reference when generate Emergency Control Command | +| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Used to decide whether vehicle is stopped or not | +| `/vehicle/status/control_mode` | `autoware_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual | +| `/system/api/mrm/comfortable_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM comfortable stop operation is available | +| `/system/api/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM emergency stop operation is available | ### Output -| Name | Type | Description | -| ------------------------------------------ | ------------------------------------------------------ | ----------------------------------------------------- | -| `/system/emergency/shift_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | Required to execute proper MRM (send gear cmd) | -| `/system/emergency/hazard_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | Required to execute proper MRM (send turn signal cmd) | -| `/api/fail_safe/mrm_state` | `autoware_adapi_v1_msgs::msg::MrmState` | Inform MRM execution state and selected MRM behavior | -| `/system/api/mrm/comfortable_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM comfortable stop | -| `/system/api/mrm/emergency_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM emergency stop | +| Name | Type | Description | +| ------------------------------------------ | ------------------------------------------------- | ----------------------------------------------------- | +| `/system/emergency/shift_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | Required to execute proper MRM (send gear cmd) | +| `/system/emergency/hazard_cmd` | `autoware_vehicle_msgs::msg::HazardLightsCommand` | Required to execute proper MRM (send turn signal cmd) | +| `/api/fail_safe/mrm_state` | `autoware_adapi_v1_msgs::msg::MrmState` | Inform MRM execution state and selected MRM behavior | +| `/system/api/mrm/comfortable_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM comfortable stop | +| `/system/api/mrm/emergency_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM emergency stop | ## Parameters diff --git a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp index a78b35be26daf..5ff269705456d 100644 --- a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp +++ b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp @@ -21,11 +21,11 @@ // Autoware #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include @@ -57,48 +57,43 @@ class EmergencyHandler : public rclcpp::Node private: // Subscribers - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_hazard_status_stamped_; - rclcpp::Subscription::SharedPtr - sub_prev_control_command_; + rclcpp::Subscription::SharedPtr sub_prev_control_command_; rclcpp::Subscription::SharedPtr sub_odom_; - rclcpp::Subscription::SharedPtr - sub_control_mode_; + rclcpp::Subscription::SharedPtr sub_control_mode_; rclcpp::Subscription::SharedPtr sub_mrm_comfortable_stop_status_; rclcpp::Subscription::SharedPtr sub_mrm_emergency_stop_status_; - autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr hazard_status_stamped_; - autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr prev_control_command_; + autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr hazard_status_stamped_; + autoware_control_msgs::msg::Control::ConstSharedPtr prev_control_command_; nav_msgs::msg::Odometry::ConstSharedPtr odom_; - autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; + autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_comfortable_stop_status_; tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_emergency_stop_status_; void onHazardStatusStamped( - const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg); - void onPrevControlCommand( - const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg); + const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg); + void onPrevControlCommand(const autoware_control_msgs::msg::Control::ConstSharedPtr msg); void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); - void onControlMode(const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg); + void onControlMode(const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg); void onMrmComfortableStopStatus( const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); void onMrmEmergencyStopStatus( const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); // Publisher - rclcpp::Publisher::SharedPtr - pub_control_command_; + rclcpp::Publisher::SharedPtr pub_control_command_; // rclcpp::Publisher::SharedPtr pub_shift_; // rclcpp::Publisher::SharedPtr pub_turn_signal_; - rclcpp::Publisher::SharedPtr - pub_hazard_cmd_; - rclcpp::Publisher::SharedPtr pub_gear_cmd_; + rclcpp::Publisher::SharedPtr pub_hazard_cmd_; + rclcpp::Publisher::SharedPtr pub_gear_cmd_; - autoware_auto_vehicle_msgs::msg::HazardLightsCommand createHazardCmdMsg(); - autoware_auto_vehicle_msgs::msg::GearCommand createGearCmdMsg(); + autoware_vehicle_msgs::msg::HazardLightsCommand createHazardCmdMsg(); + autoware_vehicle_msgs::msg::GearCommand createGearCmdMsg(); void publishControlCommands(); rclcpp::Publisher::SharedPtr pub_mrm_state_; diff --git a/system/emergency_handler/package.xml b/system/emergency_handler/package.xml index 97344f9b8c594..aa2090c86edb8 100644 --- a/system/emergency_handler/package.xml +++ b/system/emergency_handler/package.xml @@ -13,9 +13,9 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_control_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_system_msgs + autoware_vehicle_msgs nav_msgs rclcpp rclcpp_components diff --git a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp index 2770a14691d95..b96cb0d0549f9 100644 --- a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp +++ b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp @@ -31,18 +31,16 @@ EmergencyHandler::EmergencyHandler(const rclcpp::NodeOptions & options) using std::placeholders::_1; // Subscriber - sub_hazard_status_stamped_ = - create_subscription( - "~/input/hazard_status", rclcpp::QoS{1}, - std::bind(&EmergencyHandler::onHazardStatusStamped, this, _1)); - sub_prev_control_command_ = - create_subscription( - "~/input/prev_control_command", rclcpp::QoS{1}, - std::bind(&EmergencyHandler::onPrevControlCommand, this, _1)); + sub_hazard_status_stamped_ = create_subscription( + "~/input/hazard_status", rclcpp::QoS{1}, + std::bind(&EmergencyHandler::onHazardStatusStamped, this, _1)); + sub_prev_control_command_ = create_subscription( + "~/input/prev_control_command", rclcpp::QoS{1}, + std::bind(&EmergencyHandler::onPrevControlCommand, this, _1)); sub_odom_ = create_subscription( "~/input/odometry", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onOdometry, this, _1)); // subscribe control mode - sub_control_mode_ = create_subscription( + sub_control_mode_ = create_subscription( "~/input/control_mode", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onControlMode, this, _1)); sub_mrm_comfortable_stop_status_ = create_subscription( "~/input/mrm/comfortable_stop/status", rclcpp::QoS{1}, @@ -52,12 +50,12 @@ EmergencyHandler::EmergencyHandler(const rclcpp::NodeOptions & options) std::bind(&EmergencyHandler::onMrmEmergencyStopStatus, this, _1)); // Publisher - pub_control_command_ = create_publisher( + pub_control_command_ = create_publisher( "~/output/control_command", rclcpp::QoS{1}); - pub_hazard_cmd_ = create_publisher( + pub_hazard_cmd_ = create_publisher( "~/output/hazard", rclcpp::QoS{1}); pub_gear_cmd_ = - create_publisher("~/output/gear", rclcpp::QoS{1}); + create_publisher("~/output/gear", rclcpp::QoS{1}); pub_mrm_state_ = create_publisher("~/output/mrm/state", rclcpp::QoS{1}); @@ -75,9 +73,9 @@ EmergencyHandler::EmergencyHandler(const rclcpp::NodeOptions & options) // Initialize odom_ = std::make_shared(); - control_mode_ = std::make_shared(); - prev_control_command_ = autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr( - new autoware_auto_control_msgs::msg::AckermannControlCommand); + control_mode_ = std::make_shared(); + prev_control_command_ = + autoware_control_msgs::msg::Control::ConstSharedPtr(new autoware_control_msgs::msg::Control); mrm_comfortable_stop_status_ = std::make_shared(); mrm_emergency_stop_status_ = std::make_shared(); @@ -93,19 +91,18 @@ EmergencyHandler::EmergencyHandler(const rclcpp::NodeOptions & options) } void EmergencyHandler::onHazardStatusStamped( - const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg) + const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg) { hazard_status_stamped_ = msg; stamp_hazard_status_ = this->now(); } void EmergencyHandler::onPrevControlCommand( - const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg) + const autoware_control_msgs::msg::Control::ConstSharedPtr msg) { - auto control_command = new autoware_auto_control_msgs::msg::AckermannControlCommand(*msg); + auto control_command = new autoware_control_msgs::msg::Control(*msg); control_command->stamp = msg->stamp; - prev_control_command_ = - autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr(control_command); + prev_control_command_ = autoware_control_msgs::msg::Control::ConstSharedPtr(control_command); } void EmergencyHandler::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) @@ -114,7 +111,7 @@ void EmergencyHandler::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr } void EmergencyHandler::onControlMode( - const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) + const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) { control_mode_ = msg; } @@ -131,9 +128,9 @@ void EmergencyHandler::onMrmEmergencyStopStatus( mrm_emergency_stop_status_ = msg; } -autoware_auto_vehicle_msgs::msg::HazardLightsCommand EmergencyHandler::createHazardCmdMsg() +autoware_vehicle_msgs::msg::HazardLightsCommand EmergencyHandler::createHazardCmdMsg() { - using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; + using autoware_vehicle_msgs::msg::HazardLightsCommand; HazardLightsCommand msg; // Check emergency @@ -155,7 +152,7 @@ autoware_auto_vehicle_msgs::msg::HazardLightsCommand EmergencyHandler::createHaz void EmergencyHandler::publishControlCommands() { - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_vehicle_msgs::msg::GearCommand; // Create timestamp const auto stamp = this->now(); @@ -373,7 +370,7 @@ void EmergencyHandler::transitionTo(const int new_state) void EmergencyHandler::updateMrmState() { using autoware_adapi_v1_msgs::msg::MrmState; - using autoware_auto_vehicle_msgs::msg::ControlModeReport; + using autoware_vehicle_msgs::msg::ControlModeReport; // Check emergency const bool is_emergency = isEmergency(); @@ -416,7 +413,7 @@ void EmergencyHandler::updateMrmState() autoware_adapi_v1_msgs::msg::MrmState::_behavior_type EmergencyHandler::getCurrentMrmBehavior() { using autoware_adapi_v1_msgs::msg::MrmState; - using autoware_auto_system_msgs::msg::HazardStatus; + using autoware_system_msgs::msg::HazardStatus; // Get hazard level auto level = hazard_status_stamped_->status.level; diff --git a/system/hazard_status_converter/package.xml b/system/hazard_status_converter/package.xml index 35ff28998ee80..addf51edbb8e9 100644 --- a/system/hazard_status_converter/package.xml +++ b/system/hazard_status_converter/package.xml @@ -11,7 +11,7 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_system_msgs + autoware_system_msgs diagnostic_graph_utils diagnostic_msgs rclcpp diff --git a/system/hazard_status_converter/src/converter.cpp b/system/hazard_status_converter/src/converter.cpp index 9aa354c124bba..e8213b441bb33 100644 --- a/system/hazard_status_converter/src/converter.cpp +++ b/system/hazard_status_converter/src/converter.cpp @@ -61,7 +61,7 @@ void Converter::on_update(DiagGraph::ConstSharedPtr graph) { using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; using DiagnosticLevel = DiagnosticStatus::_level_type; - using HazardStatus = autoware_auto_system_msgs::msg::HazardStatus; + using HazardStatus = autoware_system_msgs::msg::HazardStatus; using HazardLevel = HazardStatus::_level_type; const auto get_hazard_level = [](DiagnosticLevel unit_level, DiagnosticLevel root_level) { diff --git a/system/hazard_status_converter/src/converter.hpp b/system/hazard_status_converter/src/converter.hpp index ad14ddde2bf5c..442eedf588429 100644 --- a/system/hazard_status_converter/src/converter.hpp +++ b/system/hazard_status_converter/src/converter.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include @@ -31,7 +31,7 @@ class Converter : public rclcpp::Node explicit Converter(const rclcpp::NodeOptions & options); private: - using HazardStatusStamped = autoware_auto_system_msgs::msg::HazardStatusStamped; + using HazardStatusStamped = autoware_system_msgs::msg::HazardStatusStamped; using DiagGraph = diagnostic_graph_utils::DiagGraph; using DiagUnit = diagnostic_graph_utils::DiagUnit; void on_create(DiagGraph::ConstSharedPtr graph); diff --git a/system/mrm_emergency_stop_operator/README.md b/system/mrm_emergency_stop_operator/README.md index 0866202f4352a..203303c0ea748 100644 --- a/system/mrm_emergency_stop_operator/README.md +++ b/system/mrm_emergency_stop_operator/README.md @@ -10,18 +10,18 @@ MRM emergency stop operator is a node that generates emergency stop commands acc ### Input -| Name | Type | Description | -| ------------------------------------ | ---------------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------- | -| `~/input/mrm/emergency_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | MRM execution order | -| `~/input/control/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | Control command output from the last node of the control component. Used for the initial value of the emergency stop command. | -| | | | +| Name | Type | Description | +| ------------------------------------ | ------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------- | +| `~/input/mrm/emergency_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | MRM execution order | +| `~/input/control/control_cmd` | `autoware_control_msgs::msg::Control` | Control command output from the last node of the control component. Used for the initial value of the emergency stop command. | +| | | | ### Output -| Name | Type | Description | -| ----------------------------------------- | ---------------------------------------------------------- | ---------------------- | -| `~/output/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | MRM execution status | -| `~/output/mrm/emergency_stop/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | Emergency stop command | +| Name | Type | Description | +| ----------------------------------------- | ------------------------------------------- | ---------------------- | +| `~/output/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | MRM execution status | +| `~/output/mrm/emergency_stop/control_cmd` | `autoware_control_msgs::msg::Control` | Emergency stop command | ## Parameters diff --git a/system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp b/system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp index 5b91f56973ae9..d7995a51ac8fb 100644 --- a/system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp +++ b/system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp @@ -20,7 +20,7 @@ #include // Autoware -#include +#include #include #include @@ -30,7 +30,7 @@ #include namespace mrm_emergency_stop_operator { -using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_control_msgs::msg::Control; using tier4_system_msgs::msg::MrmBehaviorStatus; using tier4_system_msgs::srv::OperateMrm; @@ -55,9 +55,9 @@ class MrmEmergencyStopOperator : public rclcpp::Node const std::vector & parameters); // Subscriber - rclcpp::Subscription::SharedPtr sub_control_cmd_; + rclcpp::Subscription::SharedPtr sub_control_cmd_; - void onControlCommand(AckermannControlCommand::ConstSharedPtr msg); + void onControlCommand(Control::ConstSharedPtr msg); // Server rclcpp::Service::SharedPtr service_operation_; @@ -67,10 +67,10 @@ class MrmEmergencyStopOperator : public rclcpp::Node // Publisher rclcpp::Publisher::SharedPtr pub_status_; - rclcpp::Publisher::SharedPtr pub_control_cmd_; + rclcpp::Publisher::SharedPtr pub_control_cmd_; void publishStatus() const; - void publishControlCommand(const AckermannControlCommand & command) const; + void publishControlCommand(const Control & command) const; // Timer rclcpp::TimerBase::SharedPtr timer_; @@ -79,12 +79,11 @@ class MrmEmergencyStopOperator : public rclcpp::Node // States MrmBehaviorStatus status_; - AckermannControlCommand prev_control_cmd_; + Control prev_control_cmd_; bool is_prev_control_cmd_subscribed_; // Algorithm - AckermannControlCommand calcTargetAcceleration( - const AckermannControlCommand & prev_control_cmd) const; + Control calcTargetAcceleration(const Control & prev_control_cmd) const; }; } // namespace mrm_emergency_stop_operator diff --git a/system/mrm_emergency_stop_operator/package.xml b/system/mrm_emergency_stop_operator/package.xml index 6ca4e3a815f72..74ee8c7183741 100644 --- a/system/mrm_emergency_stop_operator/package.xml +++ b/system/mrm_emergency_stop_operator/package.xml @@ -12,7 +12,7 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_control_msgs + autoware_control_msgs rclcpp rclcpp_components std_msgs diff --git a/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp b/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp index 9ee9d7a685df3..589a2fd14d990 100644 --- a/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp +++ b/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp @@ -28,7 +28,7 @@ MrmEmergencyStopOperator::MrmEmergencyStopOperator(const rclcpp::NodeOptions & n params_.target_jerk = declare_parameter("target_jerk", -1.5); // Subscriber - sub_control_cmd_ = create_subscription( + sub_control_cmd_ = create_subscription( "~/input/control/control_cmd", 1, std::bind(&MrmEmergencyStopOperator::onControlCommand, this, std::placeholders::_1)); @@ -40,8 +40,7 @@ MrmEmergencyStopOperator::MrmEmergencyStopOperator(const rclcpp::NodeOptions & n // Publisher pub_status_ = create_publisher("~/output/mrm/emergency_stop/status", 1); - pub_control_cmd_ = - create_publisher("~/output/mrm/emergency_stop/control_cmd", 1); + pub_control_cmd_ = create_publisher("~/output/mrm/emergency_stop/control_cmd", 1); // Timer const auto update_period_ns = rclcpp::Rate(params_.update_rate).period(); @@ -70,7 +69,7 @@ rcl_interfaces::msg::SetParametersResult MrmEmergencyStopOperator::onParameter( return result; } -void MrmEmergencyStopOperator::onControlCommand(AckermannControlCommand::ConstSharedPtr msg) +void MrmEmergencyStopOperator::onControlCommand(Control::ConstSharedPtr msg) { if (status_.state != MrmBehaviorStatus::OPERATING) { prev_control_cmd_ = *msg; @@ -97,7 +96,7 @@ void MrmEmergencyStopOperator::publishStatus() const pub_status_->publish(status); } -void MrmEmergencyStopOperator::publishControlCommand(const AckermannControlCommand & command) const +void MrmEmergencyStopOperator::publishControlCommand(const Control & command) const { pub_control_cmd_->publish(command); } @@ -114,15 +113,14 @@ void MrmEmergencyStopOperator::onTimer() publishStatus(); } -AckermannControlCommand MrmEmergencyStopOperator::calcTargetAcceleration( - const AckermannControlCommand & prev_control_cmd) const +Control MrmEmergencyStopOperator::calcTargetAcceleration(const Control & prev_control_cmd) const { - auto control_cmd = AckermannControlCommand(); + auto control_cmd = Control(); if (!is_prev_control_cmd_subscribed_) { control_cmd.stamp = this->now(); control_cmd.longitudinal.stamp = this->now(); - control_cmd.longitudinal.speed = 0.0; + control_cmd.longitudinal.velocity = 0.0; control_cmd.longitudinal.acceleration = static_cast(params_.target_acceleration); control_cmd.longitudinal.jerk = 0.0; control_cmd.lateral.stamp = this->now(); @@ -136,8 +134,8 @@ AckermannControlCommand MrmEmergencyStopOperator::calcTargetAcceleration( control_cmd.stamp = this->now(); control_cmd.longitudinal.stamp = this->now(); - control_cmd.longitudinal.speed = static_cast(std::max( - prev_control_cmd.longitudinal.speed + prev_control_cmd.longitudinal.acceleration * dt, 0.0)); + control_cmd.longitudinal.velocity = static_cast(std::max( + prev_control_cmd.longitudinal.velocity + prev_control_cmd.longitudinal.acceleration * dt, 0.0)); control_cmd.longitudinal.acceleration = static_cast(std::max( prev_control_cmd.longitudinal.acceleration + params_.target_jerk * dt, params_.target_acceleration)); diff --git a/system/mrm_handler/README.md b/system/mrm_handler/README.md index 4944d8a833643..8ccb95e6ca8d3 100644 --- a/system/mrm_handler/README.md +++ b/system/mrm_handler/README.md @@ -14,26 +14,26 @@ MRM Handler is a node to select a proper MRM from a system failure state contain ### Input -| Name | Type | Description | -| -------------------------------------- | ---------------------------------------------------- | --------------------------------------------------------------------------------------------------- | -| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Used to decide whether vehicle is stopped or not | -| `/system/operation_mode/availability` | `tier4_system_msgs::msg::OperationModeAvailability` | Used to select proper MRM from system available mrm behavior contained in operationModeAvailability | -| `/vehicle/status/control_mode` | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual | -| `/system/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM emergency stop operation is available | -| `/system/mrm/comfortable_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM comfortable stop operation is available | -| `/system/mrm/pull_over_manager/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM pull over operation is available | -| `/api/operation_mode/state` | `autoware_adapi_v1_msgs::msg::OperationModeState` | Used to check whether the current operation mode is AUTO or STOP. | +| Name | Type | Description | +| -------------------------------------- | --------------------------------------------------- | --------------------------------------------------------------------------------------------------- | +| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Used to decide whether vehicle is stopped or not | +| `/system/operation_mode/availability` | `tier4_system_msgs::msg::OperationModeAvailability` | Used to select proper MRM from system available mrm behavior contained in operationModeAvailability | +| `/vehicle/status/control_mode` | `autoware_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual | +| `/system/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM emergency stop operation is available | +| `/system/mrm/comfortable_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM comfortable stop operation is available | +| `/system/mrm/pull_over_manager/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM pull over operation is available | +| `/api/operation_mode/state` | `autoware_adapi_v1_msgs::msg::OperationModeState` | Used to check whether the current operation mode is AUTO or STOP. | ### Output -| Name | Type | Description | -| --------------------------------------- | ------------------------------------------------------ | ----------------------------------------------------- | -| `/system/emergency/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | Required to execute proper MRM (send gear cmd) | -| `/system/emergency/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | Required to execute proper MRM (send turn signal cmd) | -| `/system/fail_safe/mrm_state` | `autoware_adapi_v1_msgs::msg::MrmState` | Inform MRM execution state and selected MRM behavior | -| `/system/mrm/emergency_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM emergency stop | -| `/system/mrm/comfortable_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM comfortable stop | -| `/system/mrm/pull_over_manager/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM pull over | +| Name | Type | Description | +| --------------------------------------- | ------------------------------------------------- | ----------------------------------------------------- | +| `/system/emergency/gear_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | Required to execute proper MRM (send gear cmd) | +| `/system/emergency/hazard_lights_cmd` | `autoware_vehicle_msgs::msg::HazardLightsCommand` | Required to execute proper MRM (send turn signal cmd) | +| `/system/fail_safe/mrm_state` | `autoware_adapi_v1_msgs::msg::MrmState` | Inform MRM execution state and selected MRM behavior | +| `/system/mrm/emergency_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM emergency stop | +| `/system/mrm/comfortable_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM comfortable stop | +| `/system/mrm/pull_over_manager/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM pull over | ## Parameters diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp index c7aaca96aae49..8c58d961953ce 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -24,9 +24,9 @@ // Autoware #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -68,8 +68,7 @@ class MrmHandler : public rclcpp::Node // Subscribers rclcpp::Subscription::SharedPtr sub_odom_; - rclcpp::Subscription::SharedPtr - sub_control_mode_; + rclcpp::Subscription::SharedPtr sub_control_mode_; rclcpp::Subscription::SharedPtr sub_operation_mode_availability_; rclcpp::Subscription::SharedPtr @@ -82,7 +81,7 @@ class MrmHandler : public rclcpp::Node sub_operation_mode_state_; nav_msgs::msg::Odometry::ConstSharedPtr odom_; - autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; + autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr operation_mode_availability_; tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_pull_over_status_; tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_comfortable_stop_status_; @@ -90,7 +89,7 @@ class MrmHandler : public rclcpp::Node autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr operation_mode_state_; void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); - void onControlMode(const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg); + void onControlMode(const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg); void onOperationModeAvailability( const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr msg); void onMrmPullOverStatus(const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); @@ -105,9 +104,8 @@ class MrmHandler : public rclcpp::Node // rclcpp::Publisher::SharedPtr pub_shift_; // rclcpp::Publisher::SharedPtr pub_turn_signal_; - rclcpp::Publisher::SharedPtr - pub_hazard_cmd_; - rclcpp::Publisher::SharedPtr pub_gear_cmd_; + rclcpp::Publisher::SharedPtr pub_hazard_cmd_; + rclcpp::Publisher::SharedPtr pub_gear_cmd_; void publishHazardCmd(); void publishGearCmd(); diff --git a/system/mrm_handler/package.xml b/system/mrm_handler/package.xml index c15bc98a6c190..2db22cecaa82d 100644 --- a/system/mrm_handler/package.xml +++ b/system/mrm_handler/package.xml @@ -13,9 +13,9 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_control_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_system_msgs + autoware_vehicle_msgs nav_msgs rclcpp rclcpp_components diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index 5e709946c2f46..afcc7ebd208ab 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -40,7 +40,7 @@ MrmHandler::MrmHandler(const rclcpp::NodeOptions & options) : Node("mrm_handler" sub_odom_ = create_subscription( "~/input/odometry", rclcpp::QoS{1}, std::bind(&MrmHandler::onOdometry, this, _1)); // subscribe control mode - sub_control_mode_ = create_subscription( + sub_control_mode_ = create_subscription( "~/input/control_mode", rclcpp::QoS{1}, std::bind(&MrmHandler::onControlMode, this, _1)); sub_operation_mode_availability_ = create_subscription( @@ -60,10 +60,10 @@ MrmHandler::MrmHandler(const rclcpp::NodeOptions & options) : Node("mrm_handler" std::bind(&MrmHandler::onOperationModeState, this, _1)); // Publisher - pub_hazard_cmd_ = create_publisher( + pub_hazard_cmd_ = create_publisher( "~/output/hazard", rclcpp::QoS{1}); pub_gear_cmd_ = - create_publisher("~/output/gear", rclcpp::QoS{1}); + create_publisher("~/output/gear", rclcpp::QoS{1}); pub_mrm_state_ = create_publisher("~/output/mrm/state", rclcpp::QoS{1}); @@ -85,7 +85,7 @@ MrmHandler::MrmHandler(const rclcpp::NodeOptions & options) : Node("mrm_handler" // Initialize odom_ = std::make_shared(); - control_mode_ = std::make_shared(); + control_mode_ = std::make_shared(); mrm_pull_over_status_ = std::make_shared(); mrm_comfortable_stop_status_ = std::make_shared(); @@ -108,7 +108,7 @@ void MrmHandler::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) } void MrmHandler::onControlMode( - const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) + const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) { control_mode_ = msg; } @@ -167,7 +167,7 @@ void MrmHandler::onOperationModeState( void MrmHandler::publishHazardCmd() { - using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; + using autoware_vehicle_msgs::msg::HazardLightsCommand; HazardLightsCommand msg; msg.stamp = this->now(); @@ -187,7 +187,7 @@ void MrmHandler::publishHazardCmd() void MrmHandler::publishGearCmd() { - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_vehicle_msgs::msg::GearCommand; GearCommand msg; msg.stamp = this->now(); @@ -444,7 +444,7 @@ void MrmHandler::transitionTo(const int new_state) void MrmHandler::updateMrmState() { using autoware_adapi_v1_msgs::msg::MrmState; - using autoware_auto_vehicle_msgs::msg::ControlModeReport; + using autoware_vehicle_msgs::msg::ControlModeReport; // Check emergency const bool is_emergency = isEmergency(); diff --git a/system/system_error_monitor/README.md b/system/system_error_monitor/README.md index a62d4b0f00576..e765239f30f4d 100644 --- a/system/system_error_monitor/README.md +++ b/system/system_error_monitor/README.md @@ -90,19 +90,19 @@ endif ### Input -| Name | Type | Description | -| ---------------------------- | ---------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------ | -| `/diagnostics_agg` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostic information aggregated based [diagnostic_aggregator setting](./config/diagnostic_aggregator) is used to | -| `/autoware/state` | `autoware_auto_system_msgs::msg::AutowareState` | Required to ignore error during Route, Planning and Finalizing. | -| `/control/current_gate_mode` | `tier4_control_msgs::msg::GateMode` | Required to select the appropriate module from `autonomous_driving` or `external_control` | -| `/vehicle/control_mode` | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | Required to not hold emergency during manual driving | +| Name | Type | Description | +| ---------------------------- | ----------------------------------------------- | ------------------------------------------------------------------------------------------------------------------ | +| `/diagnostics_agg` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostic information aggregated based [diagnostic_aggregator setting](./config/diagnostic_aggregator) is used to | +| `/autoware/state` | `autoware_system_msgs::msg::AutowareState` | Required to ignore error during Route, Planning and Finalizing. | +| `/control/current_gate_mode` | `tier4_control_msgs::msg::GateMode` | Required to select the appropriate module from `autonomous_driving` or `external_control` | +| `/vehicle/control_mode` | `autoware_vehicle_msgs::msg::ControlModeReport` | Required to not hold emergency during manual driving | ### Output -| Name | Type | Description | -| --------------------------------- | ----------------------------------------------------- | ------------------------------------------------------------------------------------ | -| `/system/emergency/hazard_status` | `autoware_auto_system_msgs::msg::HazardStatusStamped` | HazardStatus contains system hazard level, emergency hold status and failure details | -| `/diagnostics_err` | `diagnostic_msgs::msg::DiagnosticArray` | This has the same contents as HazardStatus. This is used for visualization | +| Name | Type | Description | +| --------------------------------- | ------------------------------------------------ | ------------------------------------------------------------------------------------ | +| `/system/emergency/hazard_status` | `autoware_system_msgs::msg::HazardStatusStamped` | HazardStatus contains system hazard level, emergency hold status and failure details | +| `/diagnostics_err` | `diagnostic_msgs::msg::DiagnosticArray` | This has the same contents as HazardStatus. This is used for visualization | ## Parameters diff --git a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp index 8829bcc1edde2..22f1f4e9012c7 100644 --- a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp +++ b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp @@ -20,9 +20,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -86,7 +86,7 @@ class AutowareErrorMonitor : public rclcpp::Node rclcpp::Time emergency_state_switch_time_; rclcpp::Time initialized_time_; - autoware_auto_system_msgs::msg::HazardStatus hazard_status_{}; + autoware_system_msgs::msg::HazardStatus hazard_status_{}; std::unordered_map required_modules_map_; std::string current_mode_; @@ -101,28 +101,25 @@ class AutowareErrorMonitor : public rclcpp::Node // Subscriber rclcpp::Subscription::SharedPtr sub_diag_array_; - rclcpp::Subscription::SharedPtr - sub_autoware_state_; + rclcpp::Subscription::SharedPtr sub_autoware_state_; rclcpp::Subscription::SharedPtr sub_current_gate_mode_; - rclcpp::Subscription::SharedPtr - sub_control_mode_; - void onAutowareState(const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr msg); + rclcpp::Subscription::SharedPtr sub_control_mode_; + void onAutowareState(const autoware_system_msgs::msg::AutowareState::ConstSharedPtr msg); void onCurrentGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg); - void onControlMode(const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg); + void onControlMode(const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg); void onDiagArray(const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg); const size_t diag_buffer_size_ = 100; std::unordered_map diag_buffer_map_; diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr diag_array_; - autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr autoware_state_; + autoware_system_msgs::msg::AutowareState::ConstSharedPtr autoware_state_; tier4_control_msgs::msg::GateMode::ConstSharedPtr current_gate_mode_; - autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; + autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; // Publisher - rclcpp::Publisher::SharedPtr - pub_hazard_status_; + rclcpp::Publisher::SharedPtr pub_hazard_status_; rclcpp::Publisher::SharedPtr pub_diagnostics_err_; - void publishHazardStatus(const autoware_auto_system_msgs::msg::HazardStatus & hazard_status); + void publishHazardStatus(const autoware_system_msgs::msg::HazardStatus & hazard_status); // Service rclcpp::Service::SharedPtr srv_clear_emergency_; @@ -141,14 +138,14 @@ class AutowareErrorMonitor : public rclcpp::Node uint8_t getHazardLevel(const DiagConfig & required_module, const int diag_level) const; void appendHazardDiag( const DiagConfig & required_module, const diagnostic_msgs::msg::DiagnosticStatus & diag, - autoware_auto_system_msgs::msg::HazardStatus * hazard_status) const; - autoware_auto_system_msgs::msg::HazardStatus judgeHazardStatus() const; + autoware_system_msgs::msg::HazardStatus * hazard_status) const; + autoware_system_msgs::msg::HazardStatus judgeHazardStatus() const; void updateHazardStatus(); void updateTimeoutHazardStatus(); bool canAutoRecovery() const; bool isEmergencyHoldingRequired() const; - void loggingErrors(const autoware_auto_system_msgs::msg::HazardStatus & diag_array); + void loggingErrors(const autoware_system_msgs::msg::HazardStatus & diag_array); std::unique_ptr logger_configure_; }; diff --git a/system/system_error_monitor/package.xml b/system/system_error_monitor/package.xml index ed50eda95611a..0c8e2fc0241aa 100644 --- a/system/system_error_monitor/package.xml +++ b/system/system_error_monitor/package.xml @@ -10,8 +10,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_system_msgs + autoware_vehicle_msgs diagnostic_msgs fmt rclcpp diff --git a/system/system_error_monitor/src/system_error_monitor_core.cpp b/system/system_error_monitor/src/system_error_monitor_core.cpp index e4ca33204f53e..ad67521298643 100644 --- a/system/system_error_monitor/src/system_error_monitor_core.cpp +++ b/system/system_error_monitor/src/system_error_monitor_core.cpp @@ -96,9 +96,9 @@ bool isOverLevel(const int & diag_level, const std::string & failure_level_str) } std::vector & getTargetDiagnosticsRef( - const int hazard_level, autoware_auto_system_msgs::msg::HazardStatus * hazard_status) + const int hazard_level, autoware_system_msgs::msg::HazardStatus * hazard_status) { - using autoware_auto_system_msgs::msg::HazardStatus; + using autoware_system_msgs::msg::HazardStatus; if (hazard_level == HazardStatus::NO_FAULT) { return hazard_status->diag_no_fault; @@ -117,8 +117,7 @@ std::vector & getTargetDiagnosticsRef( } diagnostic_msgs::msg::DiagnosticArray convertHazardStatusToDiagnosticArray( - rclcpp::Clock::SharedPtr clock, - const autoware_auto_system_msgs::msg::HazardStatus & hazard_status) + rclcpp::Clock::SharedPtr clock, const autoware_system_msgs::msg::HazardStatus & hazard_status) { using diagnostic_msgs::msg::DiagnosticStatus; @@ -150,11 +149,10 @@ diagnostic_msgs::msg::DiagnosticArray convertHazardStatusToDiagnosticArray( } std::set getErrorModules( - const autoware_auto_system_msgs::msg::HazardStatus & hazard_status, - const int emergency_hazard_level) + const autoware_system_msgs::msg::HazardStatus & hazard_status, const int emergency_hazard_level) { std::set error_modules; - using autoware_auto_system_msgs::msg::HazardStatus; + using autoware_system_msgs::msg::HazardStatus; if (emergency_hazard_level <= HazardStatus::SINGLE_POINT_FAULT) { for (const auto & diag_spf : hazard_status.diag_single_point_fault) { error_modules.insert(diag_spf.name); @@ -175,10 +173,10 @@ std::set getErrorModules( } int isInNoFaultCondition( - const autoware_auto_system_msgs::msg::AutowareState & autoware_state, + const autoware_system_msgs::msg::AutowareState & autoware_state, const tier4_control_msgs::msg::GateMode & current_gate_mode) { - using autoware_auto_system_msgs::msg::AutowareState; + using autoware_system_msgs::msg::AutowareState; using tier4_control_msgs::msg::GateMode; const auto is_in_autonomous_ignore_state = @@ -221,7 +219,7 @@ AutowareErrorMonitor::AutowareErrorMonitor(const rclcpp::NodeOptions & options) get_parameter_or("hazard_recovery_timeout", params_.hazard_recovery_timeout, 5.0); get_parameter_or( "emergency_hazard_level", params_.emergency_hazard_level, - autoware_auto_system_msgs::msg::HazardStatus::LATENT_FAULT); + autoware_system_msgs::msg::HazardStatus::LATENT_FAULT); get_parameter_or("use_emergency_hold", params_.use_emergency_hold, false); get_parameter_or( "use_emergency_hold_in_manual_driving", params_.use_emergency_hold_in_manual_driving, false); @@ -237,15 +235,15 @@ AutowareErrorMonitor::AutowareErrorMonitor(const rclcpp::NodeOptions & options) sub_current_gate_mode_ = create_subscription( "~/input/current_gate_mode", rclcpp::QoS{1}, std::bind(&AutowareErrorMonitor::onCurrentGateMode, this, _1)); - sub_autoware_state_ = create_subscription( + sub_autoware_state_ = create_subscription( "~/input/autoware_state", rclcpp::QoS{1}, std::bind(&AutowareErrorMonitor::onAutowareState, this, _1)); - sub_control_mode_ = create_subscription( + sub_control_mode_ = create_subscription( "~/input/control_mode", rclcpp::QoS{1}, std::bind(&AutowareErrorMonitor::onControlMode, this, _1)); // Publisher - pub_hazard_status_ = create_publisher( + pub_hazard_status_ = create_publisher( "~/output/hazard_status", rclcpp::QoS{1}); pub_diagnostics_err_ = create_publisher( "~/output/diagnostics_err", rclcpp::QoS{1}); @@ -256,10 +254,10 @@ AutowareErrorMonitor::AutowareErrorMonitor(const rclcpp::NodeOptions & options) std::bind(&AutowareErrorMonitor::onClearEmergencyService, this, _1, _2)); // Initialize - autoware_auto_vehicle_msgs::msg::ControlModeReport vehicle_state_report; - vehicle_state_report.mode = autoware_auto_vehicle_msgs::msg::ControlModeReport::MANUAL; - control_mode_ = std::make_shared( - vehicle_state_report); + autoware_vehicle_msgs::msg::ControlModeReport vehicle_state_report; + vehicle_state_report.mode = autoware_vehicle_msgs::msg::ControlModeReport::MANUAL; + control_mode_ = + std::make_shared(vehicle_state_report); // Timer initialized_time_ = this->now(); @@ -363,7 +361,7 @@ void AutowareErrorMonitor::onCurrentGateMode( } void AutowareErrorMonitor::onAutowareState( - const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr msg) + const autoware_system_msgs::msg::AutowareState::ConstSharedPtr msg) { autoware_state_ = msg; @@ -372,7 +370,7 @@ void AutowareErrorMonitor::onAutowareState( } void AutowareErrorMonitor::onControlMode( - const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) + const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) { control_mode_ = msg; @@ -486,7 +484,7 @@ boost::optional AutowareErrorMonitor::getLatestDiag( uint8_t AutowareErrorMonitor::getHazardLevel( const DiagConfig & required_module, const int diag_level) const { - using autoware_auto_system_msgs::msg::HazardStatus; + using autoware_system_msgs::msg::HazardStatus; if (isOverLevel(diag_level, required_module.spf_at)) { return HazardStatus::SINGLE_POINT_FAULT; @@ -503,7 +501,7 @@ uint8_t AutowareErrorMonitor::getHazardLevel( void AutowareErrorMonitor::appendHazardDiag( const DiagConfig & required_module, const diagnostic_msgs::msg::DiagnosticStatus & hazard_diag, - autoware_auto_system_msgs::msg::HazardStatus * hazard_status) const + autoware_system_msgs::msg::HazardStatus * hazard_status) const { const auto hazard_level = getHazardLevel(required_module, hazard_diag.level); @@ -520,12 +518,12 @@ void AutowareErrorMonitor::appendHazardDiag( hazard_status->level = std::max(hazard_status->level, hazard_level); } -autoware_auto_system_msgs::msg::HazardStatus AutowareErrorMonitor::judgeHazardStatus() const +autoware_system_msgs::msg::HazardStatus AutowareErrorMonitor::judgeHazardStatus() const { - using autoware_auto_system_msgs::msg::HazardStatus; + using autoware_system_msgs::msg::HazardStatus; using diagnostic_msgs::msg::DiagnosticStatus; - autoware_auto_system_msgs::msg::HazardStatus hazard_status; + autoware_system_msgs::msg::HazardStatus hazard_status; for (const auto & required_module : required_modules_map_.at(current_mode_)) { const auto & diag_name = required_module.name; const auto latest_diag = getLatestDiag(diag_name); @@ -566,7 +564,7 @@ autoware_auto_system_msgs::msg::HazardStatus AutowareErrorMonitor::judgeHazardSt // Ignore error when vehicle is not ready to start if (isInNoFaultCondition(*autoware_state_, *current_gate_mode_)) { - hazard_status.level = autoware_auto_system_msgs::msg::HazardStatus::NO_FAULT; + hazard_status.level = autoware_system_msgs::msg::HazardStatus::NO_FAULT; } return hazard_status; @@ -604,7 +602,7 @@ void AutowareErrorMonitor::updateTimeoutHazardStatus() { const bool prev_emergency_status = hazard_status_.emergency; - hazard_status_.level = autoware_auto_system_msgs::msg::HazardStatus::SINGLE_POINT_FAULT; + hazard_status_.level = autoware_system_msgs::msg::HazardStatus::SINGLE_POINT_FAULT; hazard_status_.emergency = true; if (hazard_status_.emergency != prev_emergency_status) { @@ -663,7 +661,7 @@ bool AutowareErrorMonitor::isEmergencyHoldingRequired() const // Don't hold status during manual driving const bool is_manual_driving = - (control_mode_->mode == autoware_auto_vehicle_msgs::msg::ControlModeReport::MANUAL); + (control_mode_->mode == autoware_vehicle_msgs::msg::ControlModeReport::MANUAL); const auto no_hold_condition = (!params_.use_emergency_hold_in_manual_driving && is_manual_driving); if (no_hold_condition) { @@ -674,9 +672,9 @@ bool AutowareErrorMonitor::isEmergencyHoldingRequired() const } void AutowareErrorMonitor::publishHazardStatus( - const autoware_auto_system_msgs::msg::HazardStatus & hazard_status) + const autoware_system_msgs::msg::HazardStatus & hazard_status) { - autoware_auto_system_msgs::msg::HazardStatusStamped hazard_status_stamped; + autoware_system_msgs::msg::HazardStatusStamped hazard_status_stamped; hazard_status_stamped.stamp = this->now(); hazard_status_stamped.status = hazard_status; pub_hazard_status_->publish(hazard_status_stamped); @@ -700,7 +698,7 @@ bool AutowareErrorMonitor::onClearEmergencyService( } void AutowareErrorMonitor::loggingErrors( - const autoware_auto_system_msgs::msg::HazardStatus & hazard_status) + const autoware_system_msgs::msg::HazardStatus & hazard_status) { if ( autoware_state_ && current_gate_mode_ &&