diff --git a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml b/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml index 308d8dad27664..47aedbae68430 100644 --- a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml +++ b/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml @@ -26,6 +26,7 @@ + diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index 81cedb34f622a..c944d3c6d536d 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -70,8 +70,6 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) this->create_publisher("output/operation_mode", durable_qos); // Subscriber - emergency_state_sub_ = this->create_subscription( - "input/emergency_state", 1, std::bind(&VehicleCmdGate::onEmergencyState, this, _1)); external_emergency_stop_heartbeat_sub_ = this->create_subscription( "input/external_emergency_stop_heartbeat", 1, std::bind(&VehicleCmdGate::onExternalEmergencyStopHeartbeat, this, _1)); @@ -84,6 +82,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) operation_mode_sub_ = this->create_subscription( "input/operation_mode", rclcpp::QoS(1).transient_local(), [this](const OperationModeState::SharedPtr msg) { current_operation_mode_ = *msg; }); + mrm_state_sub_ = this->create_subscription( + "input/mrm_state", 1, std::bind(&VehicleCmdGate::onMrmState, this, _1)); // Subscriber for auto auto_control_cmd_sub_ = this->create_subscription( @@ -546,14 +546,6 @@ AckermannControlCommand VehicleCmdGate::createEmergencyStopControlCmd() const return cmd; } -void VehicleCmdGate::onEmergencyState(EmergencyState::ConstSharedPtr msg) -{ - is_system_emergency_ = (msg->state == EmergencyState::MRM_OPERATING) || - (msg->state == EmergencyState::MRM_SUCCEEDED) || - (msg->state == EmergencyState::MRM_FAILED); - emergency_state_heartbeat_received_time_ = std::make_shared(this->now()); -} - void VehicleCmdGate::onExternalEmergencyStopHeartbeat( [[maybe_unused]] Heartbeat::ConstSharedPtr msg) { @@ -587,6 +579,15 @@ void VehicleCmdGate::onSteering(SteeringReport::ConstSharedPtr msg) current_steer_ = msg->steering_tire_angle; } +void VehicleCmdGate::onMrmState(MrmState::ConstSharedPtr msg) +{ + is_system_emergency_ = + (msg->state == MrmState::MRM_OPERATING || msg->state == MrmState::MRM_SUCCEEDED || + msg->state == MrmState::MRM_FAILED) && + (msg->behavior == MrmState::EMERGENCY_STOP); + emergency_state_heartbeat_received_time_ = std::make_shared(this->now()); +} + double VehicleCmdGate::getDt() { if (!prev_time_) { diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 0cd80245fd972..533e49b3aef3a 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -23,6 +23,7 @@ #include #include +#include #include #include #include @@ -38,6 +39,7 @@ #include #include #include +#include #include #include @@ -45,9 +47,9 @@ namespace vehicle_cmd_gate { +using autoware_adapi_v1_msgs::msg::MrmState; using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_system_msgs::msg::EmergencyState; using autoware_auto_vehicle_msgs::msg::GearCommand; using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; using autoware_auto_vehicle_msgs::msg::SteeringReport; @@ -56,6 +58,7 @@ using tier4_control_msgs::msg::GateMode; using tier4_external_api_msgs::msg::Emergency; using tier4_external_api_msgs::msg::Heartbeat; using tier4_external_api_msgs::srv::SetEmergency; +using tier4_system_msgs::msg::MrmBehaviorStatus; using tier4_vehicle_msgs::msg::VehicleEmergencyStamped; using diagnostic_msgs::msg::DiagnosticStatus; @@ -93,22 +96,23 @@ class VehicleCmdGate : public rclcpp::Node rclcpp::Publisher::SharedPtr operation_mode_pub_; // Subscription - rclcpp::Subscription::SharedPtr emergency_state_sub_; rclcpp::Subscription::SharedPtr external_emergency_stop_heartbeat_sub_; rclcpp::Subscription::SharedPtr gate_mode_sub_; rclcpp::Subscription::SharedPtr steer_sub_; rclcpp::Subscription::SharedPtr operation_mode_sub_; + rclcpp::Subscription::SharedPtr mrm_state_sub_; void onGateMode(GateMode::ConstSharedPtr msg); - void onEmergencyState(EmergencyState::ConstSharedPtr msg); void onExternalEmergencyStopHeartbeat(Heartbeat::ConstSharedPtr msg); void onSteering(SteeringReport::ConstSharedPtr msg); + void onMrmState(MrmState::ConstSharedPtr msg); bool is_engaged_; bool is_system_emergency_ = false; bool is_external_emergency_stop_ = false; double current_steer_ = 0; GateMode current_gate_mode_; + MrmState current_mrm_state_; // Heartbeat std::shared_ptr emergency_state_heartbeat_received_time_; diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 86be1019df728..e5754fce856e9 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -187,6 +187,7 @@ def launch_setup(context, *args, **kwargs): ("input/emergency/control_cmd", "/system/emergency/control_cmd"), ("input/emergency/hazard_lights_cmd", "/system/emergency/hazard_lights_cmd"), ("input/emergency/gear_cmd", "/system/emergency/gear_cmd"), + ("input/mrm_state", "/system/fail_safe/mrm_state"), ("output/vehicle_cmd_emergency", "/control/command/emergency_cmd"), ("output/control_cmd", "/control/command/control_cmd"), ("output/gear_cmd", "/control/command/gear_cmd"), diff --git a/launch/tier4_system_launch/config/emergency_handler/emergency_handler.param.yaml b/launch/tier4_system_launch/config/emergency_handler/emergency_handler.param.yaml index 912e48460cdae..652a984ce539a 100644 --- a/launch/tier4_system_launch/config/emergency_handler/emergency_handler.param.yaml +++ b/launch/tier4_system_launch/config/emergency_handler/emergency_handler.param.yaml @@ -7,6 +7,7 @@ timeout_takeover_request: 10.0 use_takeover_request: false use_parking_after_stopped: false + use_comfortable_stop: false # setting whether to turn hazard lamp on for each situation turning_hazard_on: diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 5ee6d27c05fef..258a9820a9565 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -66,5 +66,13 @@ + + + + + + + + diff --git a/system/emergency_handler/README.md b/system/emergency_handler/README.md index 530a27a2204c3..92aa248733db1 100644 --- a/system/emergency_handler/README.md +++ b/system/emergency_handler/README.md @@ -14,21 +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. | +| 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 | ### Output -| Name | Type | Description | -| ----------------------------------- | ---------------------------------------------------------- | ----------------------------------------------------- | -| `/system/emergency/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | Required to execute proper MRM | -| `/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) | -| `/system/emergency/emergency_state` | `autoware_auto_system_msgs::msg::EmergencyStateStamped` | Used to inform the emergency situation of the vehicle | +| 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 | ## Parameters @@ -45,6 +48,7 @@ Emergency Handler is a node to select proper MRM from from system failure state | timeout_hazard_status | double | `0.5` | If the input `hazard_status` topic cannot be received for more than `timeout_hazard_status`, vehicle will make an emergency stop. | | use_parking_after_stopped | bool | `false` | If this parameter is true, it will publish PARKING shift command. | | turning_hazard_on.emergency | bool | `true` | If this parameter is true, hazard lamps will be turned on during emergency state. | +| use_comfortable_stop | bool | `false` | If this parameter is true, operate comfortable stop when latent faults occur. | ## Assumptions / Known limits diff --git a/system/emergency_handler/config/emergency_handler.param.yaml b/system/emergency_handler/config/emergency_handler.param.yaml index 912e48460cdae..652a984ce539a 100644 --- a/system/emergency_handler/config/emergency_handler.param.yaml +++ b/system/emergency_handler/config/emergency_handler.param.yaml @@ -7,6 +7,7 @@ timeout_takeover_request: 10.0 use_takeover_request: false use_parking_after_stopped: false + use_comfortable_stop: false # setting whether to turn hazard lamp on for each situation turning_hazard_on: 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 3a20956ddd841..987e44ad340d5 100644 --- a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp +++ b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp @@ -20,12 +20,14 @@ #include // Autoware +#include #include -#include #include #include #include #include +#include +#include // ROS2 core #include @@ -46,6 +48,7 @@ struct Param double timeout_takeover_request; bool use_takeover_request; bool use_parking_after_stopped; + bool use_comfortable_stop; HazardLampPolicy turning_hazard_on{}; }; @@ -63,11 +66,17 @@ class EmergencyHandler : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_odom_; 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_; nav_msgs::msg::Odometry::ConstSharedPtr odom_; autoware_auto_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); @@ -75,6 +84,10 @@ class EmergencyHandler : public rclcpp::Node const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg); void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); void onControlMode(const autoware_auto_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 @@ -85,12 +98,30 @@ class EmergencyHandler : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_hazard_cmd_; rclcpp::Publisher::SharedPtr pub_gear_cmd_; - rclcpp::Publisher::SharedPtr pub_emergency_state_; autoware_auto_vehicle_msgs::msg::HazardLightsCommand createHazardCmdMsg(); autoware_auto_vehicle_msgs::msg::GearCommand createGearCmdMsg(); void publishControlCommands(); + rclcpp::Publisher::SharedPtr pub_mrm_state_; + + autoware_adapi_v1_msgs::msg::MrmState mrm_state_; + void publishMrmState(); + + // Clients + rclcpp::CallbackGroup::SharedPtr client_mrm_comfortable_stop_group_; + rclcpp::Client::SharedPtr client_mrm_comfortable_stop_; + rclcpp::CallbackGroup::SharedPtr client_mrm_emergency_stop_group_; + rclcpp::Client::SharedPtr client_mrm_emergency_stop_; + + void callMrmBehavior( + const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const; + void cancelMrmBehavior( + const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const; + void logMrmCallingResult( + const tier4_system_msgs::srv::OperateMrm::Response & result, const std::string & behavior, + bool is_call) const; + // Timer rclcpp::TimerBase::SharedPtr timer_; @@ -104,15 +135,14 @@ class EmergencyHandler : public rclcpp::Node rclcpp::Time stamp_hazard_status_; // Algorithm - autoware_auto_system_msgs::msg::EmergencyState::_state_type emergency_state_{ - autoware_auto_system_msgs::msg::EmergencyState::NORMAL}; rclcpp::Time takeover_requested_time_; - + bool is_takeover_request_ = false; void transitionTo(const int new_state); - void updateEmergencyState(); + void updateMrmState(); + void operateMrm(); + autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior(); bool isStopped(); bool isEmergency(const autoware_auto_system_msgs::msg::HazardStatus & hazard_status); - autoware_auto_control_msgs::msg::AckermannControlCommand selectAlternativeControlCommand(); }; #endif // EMERGENCY_HANDLER__EMERGENCY_HANDLER_CORE_HPP_ diff --git a/system/emergency_handler/launch/emergency_handler.launch.xml b/system/emergency_handler/launch/emergency_handler.launch.xml index 7b77815e060d5..308eaf90dbb69 100644 --- a/system/emergency_handler/launch/emergency_handler.launch.xml +++ b/system/emergency_handler/launch/emergency_handler.launch.xml @@ -4,11 +4,14 @@ + + - - + + + @@ -18,11 +21,14 @@ + + - - + + + diff --git a/system/emergency_handler/package.xml b/system/emergency_handler/package.xml index e19b8c8054c9a..babcd28537459 100644 --- a/system/emergency_handler/package.xml +++ b/system/emergency_handler/package.xml @@ -5,12 +5,14 @@ 0.1.0 The emergency_handler ROS2 package Kenji Miyake + Makoto Kurihara Apache License 2.0 ament_cmake_auto autoware_cmake + autoware_adapi_v1_msgs autoware_auto_control_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs @@ -18,6 +20,7 @@ rclcpp std_msgs std_srvs + tier4_system_msgs ament_lint_auto autoware_lint_common 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 8a695b8cb227d..de8ca1cca88a2 100644 --- a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp +++ b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp @@ -26,6 +26,7 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler") param_.timeout_takeover_request = declare_parameter("timeout_takeover_request", 10.0); param_.use_takeover_request = declare_parameter("use_takeover_request", false); param_.use_parking_after_stopped = declare_parameter("use_parking_after_stopped", false); + param_.use_comfortable_stop = declare_parameter("use_comfortable_stop", false); param_.turning_hazard_on.emergency = declare_parameter("turning_hazard_on.emergency", true); using std::placeholders::_1; @@ -44,6 +45,12 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler") // subscribe control mode 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}, + std::bind(&EmergencyHandler::onMrmComfortableStopStatus, this, _1)); + sub_mrm_emergency_stop_status_ = create_subscription( + "~/input/mrm/emergency_stop/status", rclcpp::QoS{1}, + std::bind(&EmergencyHandler::onMrmEmergencyStopStatus, this, _1)); // Publisher pub_control_command_ = create_publisher( @@ -52,14 +59,32 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler") "~/output/hazard", rclcpp::QoS{1}); pub_gear_cmd_ = create_publisher("~/output/gear", rclcpp::QoS{1}); - pub_emergency_state_ = create_publisher( - "~/output/emergency_state", rclcpp::QoS{1}); + pub_mrm_state_ = + create_publisher("~/output/mrm/state", rclcpp::QoS{1}); + + // Clients + client_mrm_comfortable_stop_group_ = + create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + client_mrm_comfortable_stop_ = create_client( + "~/output/mrm/comfortable_stop/operate", rmw_qos_profile_services_default, + client_mrm_comfortable_stop_group_); + client_mrm_emergency_stop_group_ = + create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + client_mrm_emergency_stop_ = create_client( + "~/output/mrm/emergency_stop/operate", rmw_qos_profile_services_default, + client_mrm_emergency_stop_group_); // 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); + mrm_comfortable_stop_status_ = + std::make_shared(); + mrm_emergency_stop_status_ = std::make_shared(); + mrm_state_.stamp = this->now(); + mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::NORMAL; + mrm_state_.behavior = autoware_adapi_v1_msgs::msg::MrmState::NONE; // Timer const auto update_period_ns = rclcpp::Rate(param_.update_rate).period(); @@ -94,6 +119,18 @@ void EmergencyHandler::onControlMode( control_mode_ = msg; } +void EmergencyHandler::onMrmComfortableStopStatus( + const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg) +{ + mrm_comfortable_stop_status_ = msg; +} + +void EmergencyHandler::onMrmEmergencyStopStatus( + const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg) +{ + mrm_emergency_stop_status_ = msg; +} + autoware_auto_vehicle_msgs::msg::HazardLightsCommand EmergencyHandler::createHazardCmdMsg() { using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; @@ -123,16 +160,6 @@ void EmergencyHandler::publishControlCommands() // Create timestamp const auto stamp = this->now(); - // Publish ControlCommand - { - autoware_auto_control_msgs::msg::AckermannControlCommand msg; - msg = selectAlternativeControlCommand(); - msg.stamp = stamp; - msg.lateral.stamp = stamp; - msg.longitudinal.stamp = stamp; - pub_control_command_->publish(msg); - } - // Publish hazard command pub_hazard_cmd_->publish(createHazardCmdMsg()); @@ -147,14 +174,104 @@ void EmergencyHandler::publishControlCommands() } pub_gear_cmd_->publish(msg); } +} - // Publish Emergency State - { - autoware_auto_system_msgs::msg::EmergencyState emergency_state; - emergency_state.stamp = stamp; - emergency_state.state = emergency_state_; - pub_emergency_state_->publish(emergency_state); +void EmergencyHandler::publishMrmState() +{ + mrm_state_.stamp = this->now(); + pub_mrm_state_->publish(mrm_state_); +} + +void EmergencyHandler::operateMrm() +{ + using autoware_adapi_v1_msgs::msg::MrmState; + + if (mrm_state_.state == MrmState::NORMAL) { + // Cancel MRM behavior when returning to NORMAL state + const auto current_mrm_behavior = MrmState::NONE; + if (current_mrm_behavior != mrm_state_.behavior) { + cancelMrmBehavior(mrm_state_.behavior); + mrm_state_.behavior = current_mrm_behavior; + } + return; + } + if (mrm_state_.state == MrmState::MRM_OPERATING) { + const auto current_mrm_behavior = getCurrentMrmBehavior(); + if (current_mrm_behavior != mrm_state_.behavior) { + cancelMrmBehavior(mrm_state_.behavior); + callMrmBehavior(current_mrm_behavior); + mrm_state_.behavior = current_mrm_behavior; + } + return; } + if (mrm_state_.state == MrmState::MRM_SUCCEEDED) { + // TODO(mkuri): operate MRC behavior + // Do nothing + return; + } + if (mrm_state_.state == MrmState::MRM_FAILED) { + // Do nothing + return; + } + RCLCPP_WARN(this->get_logger(), "invalid MRM state: %d", mrm_state_.state); +} + +void EmergencyHandler::callMrmBehavior( + const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const +{ + using autoware_adapi_v1_msgs::msg::MrmState; + + auto request = std::make_shared(); + request->operate = true; + + if (mrm_behavior == MrmState::COMFORTABLE_STOP) { + auto result = client_mrm_comfortable_stop_->async_send_request(request).get(); + if (result->response.success == true) { + RCLCPP_WARN(this->get_logger(), "Comfortable stop is operated"); + } else { + RCLCPP_ERROR(this->get_logger(), "Comfortable stop is failed to operate"); + } + return; + } + if (mrm_behavior == MrmState::EMERGENCY_STOP) { + auto result = client_mrm_emergency_stop_->async_send_request(request).get(); + if (result->response.success == true) { + RCLCPP_WARN(this->get_logger(), "Emergency stop is operated"); + } else { + RCLCPP_ERROR(this->get_logger(), "Emergency stop is failed to operate"); + } + return; + } + RCLCPP_WARN(this->get_logger(), "invalid MRM behavior: %d", mrm_behavior); +} + +void EmergencyHandler::cancelMrmBehavior( + const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const +{ + using autoware_adapi_v1_msgs::msg::MrmState; + + auto request = std::make_shared(); + request->operate = false; + + if (mrm_behavior == MrmState::COMFORTABLE_STOP) { + auto result = client_mrm_comfortable_stop_->async_send_request(request).get(); + if (result->response.success == true) { + RCLCPP_WARN(this->get_logger(), "Comfortable stop is canceled"); + } else { + RCLCPP_ERROR(this->get_logger(), "Comfortable stop is failed to cancel"); + } + return; + } + if (mrm_behavior == MrmState::EMERGENCY_STOP) { + auto result = client_mrm_emergency_stop_->async_send_request(request).get(); + if (result->response.success == true) { + RCLCPP_WARN(this->get_logger(), "Emergency stop is canceled"); + } else { + RCLCPP_ERROR(this->get_logger(), "Emergency stop is failed to cancel"); + } + return; + } + RCLCPP_WARN(this->get_logger(), "invalid MRM behavior: %d", mrm_behavior); } bool EmergencyHandler::isDataReady() @@ -166,6 +283,23 @@ bool EmergencyHandler::isDataReady() return false; } + if ( + param_.use_comfortable_stop && mrm_comfortable_stop_status_->state == + tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), + "waiting for mrm comfortable stop to become available..."); + return false; + } + + if ( + mrm_emergency_stop_status_->state == tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), + "waiting for mrm emergency stop to become available..."); + return false; + } + return true; } @@ -180,36 +314,35 @@ void EmergencyHandler::onTimer() RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), "heartbeat_hazard_status is timeout"); - emergency_state_ = autoware_auto_system_msgs::msg::EmergencyState::MRM_OPERATING; + mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING; publishControlCommands(); return; } // Update Emergency State - updateEmergencyState(); + updateMrmState(); // Publish control commands publishControlCommands(); + operateMrm(); + publishMrmState(); } void EmergencyHandler::transitionTo(const int new_state) { - using autoware_auto_system_msgs::msg::EmergencyState; + using autoware_adapi_v1_msgs::msg::MrmState; const auto state2string = [](const int state) { - if (state == EmergencyState::NORMAL) { + if (state == MrmState::NORMAL) { return "NORMAL"; } - if (state == EmergencyState::OVERRIDE_REQUESTING) { - return "OVERRIDE_REQUESTING"; - } - if (state == EmergencyState::MRM_OPERATING) { + if (state == MrmState::MRM_OPERATING) { return "MRM_OPERATING"; } - if (state == EmergencyState::MRM_SUCCEEDED) { + if (state == MrmState::MRM_SUCCEEDED) { return "MRM_SUCCEEDED"; } - if (state == EmergencyState::MRM_FAILED) { + if (state == MrmState::MRM_FAILED) { return "MRM_FAILED"; } @@ -218,15 +351,15 @@ void EmergencyHandler::transitionTo(const int new_state) }; RCLCPP_INFO( - this->get_logger(), "EmergencyState changed: %s -> %s", state2string(emergency_state_), + this->get_logger(), "MRM State changed: %s -> %s", state2string(mrm_state_.state), state2string(new_state)); - emergency_state_ = new_state; + mrm_state_.state = new_state; } -void EmergencyHandler::updateEmergencyState() +void EmergencyHandler::updateMrmState() { - using autoware_auto_system_msgs::msg::EmergencyState; + using autoware_adapi_v1_msgs::msg::MrmState; using autoware_auto_vehicle_msgs::msg::ControlModeReport; // Check emergency @@ -237,15 +370,15 @@ void EmergencyHandler::updateEmergencyState() const bool is_takeover_done = control_mode_->mode == ControlModeReport::MANUAL; // State Machine - if (emergency_state_ == EmergencyState::NORMAL) { + if (mrm_state_.state == MrmState::NORMAL) { // NORMAL if (is_auto_mode && is_emergency) { if (param_.use_takeover_request) { takeover_requested_time_ = this->get_clock()->now(); - transitionTo(EmergencyState::OVERRIDE_REQUESTING); + is_takeover_request_ = true; return; } else { - transitionTo(EmergencyState::MRM_OPERATING); + transitionTo(MrmState::MRM_OPERATING); return; } } @@ -253,38 +386,67 @@ void EmergencyHandler::updateEmergencyState() // Emergency // Send recovery events if "not emergency" or "takeover done" if (!is_emergency) { - transitionTo(EmergencyState::NORMAL); + transitionTo(MrmState::NORMAL); return; } // TODO(Kenji Miyake): Check if human can safely override, for example using DSM if (is_takeover_done) { - transitionTo(EmergencyState::NORMAL); + transitionTo(MrmState::NORMAL); return; } - if (emergency_state_ == EmergencyState::OVERRIDE_REQUESTING) { + if (is_takeover_request_) { const auto time_from_takeover_request = this->get_clock()->now() - takeover_requested_time_; if (time_from_takeover_request.seconds() > param_.timeout_takeover_request) { - transitionTo(EmergencyState::MRM_OPERATING); + transitionTo(MrmState::MRM_OPERATING); return; } - } else if (emergency_state_ == EmergencyState::MRM_OPERATING) { + } else if (mrm_state_.state == MrmState::MRM_OPERATING) { // TODO(Kenji Miyake): Check MRC is accomplished if (isStopped()) { - transitionTo(EmergencyState::MRM_SUCCEEDED); + transitionTo(MrmState::MRM_SUCCEEDED); return; } - } else if (emergency_state_ == EmergencyState::MRM_SUCCEEDED) { + } else if (mrm_state_.state == MrmState::MRM_SUCCEEDED) { // Do nothing(only checking common recovery events) - } else if (emergency_state_ == EmergencyState::MRM_FAILED) { + } else if (mrm_state_.state == MrmState::MRM_FAILED) { // Do nothing(only checking common recovery events) } else { - const auto msg = "invalid state: " + std::to_string(emergency_state_); + const auto msg = "invalid state: " + std::to_string(mrm_state_.state); throw std::runtime_error(msg); } } } +autoware_adapi_v1_msgs::msg::MrmState::_behavior_type EmergencyHandler::getCurrentMrmBehavior() +{ + using autoware_adapi_v1_msgs::msg::MrmState; + using autoware_auto_system_msgs::msg::HazardStatus; + + // Get hazard level + const auto level = hazard_status_stamped_->status.level; + + // State machine + if (mrm_state_.behavior == MrmState::NONE) { + if (level == HazardStatus::LATENT_FAULT) { + if (param_.use_comfortable_stop) { + return MrmState::COMFORTABLE_STOP; + } + return MrmState::EMERGENCY_STOP; + } + if (level == HazardStatus::SINGLE_POINT_FAULT) { + return MrmState::EMERGENCY_STOP; + } + } + if (mrm_state_.behavior == MrmState::COMFORTABLE_STOP) { + if (level == HazardStatus::SINGLE_POINT_FAULT) { + return MrmState::EMERGENCY_STOP; + } + } + + return mrm_state_.behavior; +} + bool EmergencyHandler::isEmergency( const autoware_auto_system_msgs::msg::HazardStatus & hazard_status) { @@ -300,19 +462,3 @@ bool EmergencyHandler::isStopped() return false; } - -autoware_auto_control_msgs::msg::AckermannControlCommand -EmergencyHandler::selectAlternativeControlCommand() -{ - // TODO(jilaada): Add safe_stop planner - - // Emergency Stop - { - autoware_auto_control_msgs::msg::AckermannControlCommand emergency_stop_cmd; - emergency_stop_cmd.lateral = prev_control_command_->lateral; - emergency_stop_cmd.longitudinal.speed = 0.0; - emergency_stop_cmd.longitudinal.acceleration = -2.5; - - return emergency_stop_cmd; - } -} diff --git a/system/emergency_handler/src/emergency_handler/emergency_handler_node.cpp b/system/emergency_handler/src/emergency_handler/emergency_handler_node.cpp index a923083cafc15..5b60117cc3ff4 100644 --- a/system/emergency_handler/src/emergency_handler/emergency_handler_node.cpp +++ b/system/emergency_handler/src/emergency_handler/emergency_handler_node.cpp @@ -21,8 +21,11 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); + rclcpp::executors::MultiThreadedExecutor executor; auto node = std::make_shared(); - rclcpp::spin(node); + executor.add_node(node); + executor.spin(); + executor.remove_node(node); rclcpp::shutdown(); return 0; diff --git a/system/mrm_comfortable_stop_operator/CMakeLists.txt b/system/mrm_comfortable_stop_operator/CMakeLists.txt new file mode 100644 index 0000000000000..19bc4f6d66d5f --- /dev/null +++ b/system/mrm_comfortable_stop_operator/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.14) +project(mrm_comfortable_stop_operator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(mrm_comfortable_stop_operator_component SHARED + src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp +) + +rclcpp_components_register_node(mrm_comfortable_stop_operator_component + PLUGIN "mrm_comfortable_stop_operator::MrmComfortableStopOperator" + EXECUTABLE mrm_comfortable_stop_operator) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/system/mrm_comfortable_stop_operator/README.md b/system/mrm_comfortable_stop_operator/README.md new file mode 100644 index 0000000000000..dffa28f1dfa49 --- /dev/null +++ b/system/mrm_comfortable_stop_operator/README.md @@ -0,0 +1,43 @@ +# mrm_comfortable_stop_operator + +## Purpose + +MRM comfortable stop operator is a node that generates comfortable stop commands according to the comfortable stop MRM order. + +## Inner-workings / Algorithms + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| -------------------------------------- | ------------------------------------ | ------------------- | +| `~/input/mrm/comfortable_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | MRM execution order | + +### Output + +| Name | Type | Description | +| -------------------------------------- | ----------------------------------------------------- | ---------------------------- | +| `~/output/mrm/comfortable_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | MRM execution status | +| `~/output/velocity_limit` | `tier4_planning_msgs::msg::VelocityLimit` | Velocity limit command | +| `~/output/velocity_limit/clear` | `tier4_planning_msgs::msg::VelocityLimitClearCommand` | Velocity limit clear command | + +## Parameters + +### Node Parameters + +| Name | Type | Default value | Explanation | +| ----------- | ---- | ------------- | ----------------------------- | +| update_rate | int | `10` | Timer callback frequency [Hz] | + +### Core Parameters + +| Name | Type | Default value | Explanation | +| ---------------- | ------ | ------------- | ------------------------------------------------- | +| min_acceleration | double | `-1.0` | Minimum acceleration for comfortable stop [m/s^2] | +| max_jerk | double | `0.3` | Maximum jerk for comfortable stop [m/s^3] | +| min_jerk | double | `-0.3` | Minimum jerk for comfortable stop [m/s^3] | + +## Assumptions / Known limits + +TBD. diff --git a/system/mrm_comfortable_stop_operator/config/mrm_comfortable_stop_operator.config.yaml b/system/mrm_comfortable_stop_operator/config/mrm_comfortable_stop_operator.config.yaml new file mode 100644 index 0000000000000..81bc9b9c0b5d0 --- /dev/null +++ b/system/mrm_comfortable_stop_operator/config/mrm_comfortable_stop_operator.config.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + update_rate: 10 + min_acceleration: -1.0 + max_jerk: 0.3 + min_jerk: -0.3 diff --git a/system/mrm_comfortable_stop_operator/include/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp b/system/mrm_comfortable_stop_operator/include/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp new file mode 100644 index 0000000000000..c6f9a4be8902a --- /dev/null +++ b/system/mrm_comfortable_stop_operator/include/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp @@ -0,0 +1,79 @@ +// 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 MRM_COMFORTABLE_STOP_OPERATOR__MRM_COMFORTABLE_STOP_OPERATOR_CORE_HPP_ +#define MRM_COMFORTABLE_STOP_OPERATOR__MRM_COMFORTABLE_STOP_OPERATOR_CORE_HPP_ + +// Core +#include + +// Autoware +#include +#include +#include +#include +#include + +// ROS2 core +#include + +namespace mrm_comfortable_stop_operator +{ + +struct Parameters +{ + int update_rate; // [Hz] + double min_acceleration; // [m/s^2] + double max_jerk; // [m/s^3] + double min_jerk; // [m/s^3] +}; + +class MrmComfortableStopOperator : public rclcpp::Node +{ +public: + explicit MrmComfortableStopOperator(const rclcpp::NodeOptions & node_options); + +private: + // Parameters + Parameters params_; + + // Server + rclcpp::Service::SharedPtr service_operation_; + + void operateComfortableStop( + const tier4_system_msgs::srv::OperateMrm::Request::SharedPtr request, + const tier4_system_msgs::srv::OperateMrm::Response::SharedPtr response); + + // Publisher + rclcpp::Publisher::SharedPtr pub_status_; + rclcpp::Publisher::SharedPtr pub_velocity_limit_; + rclcpp::Publisher::SharedPtr + pub_velocity_limit_clear_command_; + + void publishStatus() const; + void publishVelocityLimit() const; + void publishVelocityLimitClearCommand() const; + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + + void onTimer() const; + + // States + tier4_system_msgs::msg::MrmBehaviorStatus status_; +}; + +} // namespace mrm_comfortable_stop_operator + +#endif // MRM_COMFORTABLE_STOP_OPERATOR__MRM_COMFORTABLE_STOP_OPERATOR_CORE_HPP_ diff --git a/system/mrm_comfortable_stop_operator/launch/mrm_comfortable_stop_operator.launch.py b/system/mrm_comfortable_stop_operator/launch/mrm_comfortable_stop_operator.launch.py new file mode 100644 index 0000000000000..02e181c21ea72 --- /dev/null +++ b/system/mrm_comfortable_stop_operator/launch/mrm_comfortable_stop_operator.launch.py @@ -0,0 +1,63 @@ +# Copyright 2022 The Autoware Contributors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +import yaml + + +def generate_launch_description(): + param_path = os.path.join( + get_package_share_directory("mrm_comfortable_stop_operator"), + "config/mrm_comfortable_stop_operator.config.yaml", + ) + + with open(param_path, "r") as f: + param = yaml.safe_load(f)["/**"]["ros__parameters"] + + component = ComposableNode( + package="mrm_comfortable_stop_operator", + plugin="mrm_comfortable_stop_operator::MrmComfortableStopOperator", + name="mrm_comfortable_stop_operator", + parameters=[ + param, + ], + remappings=[ + ("~/input/mrm/comfortable_stop/operate", "/system/mrm/comfortable_stop/operate"), + ("~/output/mrm/comfortable_stop/status", "/system/mrm/comfortable_stop/status"), + ("~/output/velocity_limit", "/planning/scenario_planning/max_velocity_candidates"), + ("~/output/velocity_limit/clear", "/planning/scenario_planning/clear_velocity_limit"), + ], + ) + + container = ComposableNodeContainer( + name="mrm_comfortable_stop_operator_container", + namespace="mrm_comfortable_stop_operator", + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[ + component, + ], + output="screen", + ) + + return launch.LaunchDescription( + [ + container, + ] + ) diff --git a/system/mrm_comfortable_stop_operator/package.xml b/system/mrm_comfortable_stop_operator/package.xml new file mode 100644 index 0000000000000..a108f52f4bd66 --- /dev/null +++ b/system/mrm_comfortable_stop_operator/package.xml @@ -0,0 +1,28 @@ + + + + mrm_comfortable_stop_operator + 0.1.0 + The MRM comfortable stop operator package + Makoto Kurihara + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + autoware_adapi_v1_msgs + rclcpp + rclcpp_components + std_msgs + std_srvs + tier4_planning_msgs + tier4_system_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp b/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp new file mode 100644 index 0000000000000..a2e39f36e7076 --- /dev/null +++ b/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp @@ -0,0 +1,104 @@ +// 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 "mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp" + +namespace mrm_comfortable_stop_operator +{ + +MrmComfortableStopOperator::MrmComfortableStopOperator(const rclcpp::NodeOptions & node_options) +: Node("mrm_comfortable_stop_operator", node_options) +{ + // Parameter + params_.update_rate = static_cast(declare_parameter("update_rate", 1)); + params_.min_acceleration = declare_parameter("min_acceleration", -1.0); + params_.max_jerk = declare_parameter("max_jerk", 0.3); + params_.min_jerk = declare_parameter("min_jerk", 0.3); + + // Server + service_operation_ = create_service( + "~/input/mrm/comfortable_stop/operate", std::bind( + &MrmComfortableStopOperator::operateComfortableStop, + this, std::placeholders::_1, std::placeholders::_2)); + + // Publisher + pub_status_ = create_publisher( + "~/output/mrm/comfortable_stop/status", 1); + pub_velocity_limit_ = create_publisher( + "~/output/velocity_limit", rclcpp::QoS{1}.transient_local()); + pub_velocity_limit_clear_command_ = + create_publisher( + "~/output/velocity_limit/clear", rclcpp::QoS{1}.transient_local()); + + // Timer + const auto update_period_ns = rclcpp::Rate(params_.update_rate).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), update_period_ns, std::bind(&MrmComfortableStopOperator::onTimer, this)); + + // Initialize + status_.state = tier4_system_msgs::msg::MrmBehaviorStatus::AVAILABLE; +} + +void MrmComfortableStopOperator::operateComfortableStop( + const tier4_system_msgs::srv::OperateMrm::Request::SharedPtr request, + const tier4_system_msgs::srv::OperateMrm::Response::SharedPtr response) +{ + if (request->operate == true) { + publishVelocityLimit(); + status_.state = tier4_system_msgs::msg::MrmBehaviorStatus::OPERATING; + response->response.success = true; + } else { + publishVelocityLimitClearCommand(); + status_.state = tier4_system_msgs::msg::MrmBehaviorStatus::AVAILABLE; + response->response.success = true; + } +} + +void MrmComfortableStopOperator::publishStatus() const +{ + auto status = status_; + status.stamp = this->now(); + pub_status_->publish(status); +} + +void MrmComfortableStopOperator::publishVelocityLimit() const +{ + auto velocity_limit = tier4_planning_msgs::msg::VelocityLimit(); + velocity_limit.stamp = this->now(); + velocity_limit.max_velocity = 0; + velocity_limit.use_constraints = true; + velocity_limit.constraints.min_acceleration = static_cast(params_.min_acceleration); + velocity_limit.constraints.max_jerk = static_cast(params_.max_jerk); + velocity_limit.constraints.min_jerk = static_cast(params_.min_jerk); + velocity_limit.sender = "mrm_comfortable_stop_operator"; + + pub_velocity_limit_->publish(velocity_limit); +} + +void MrmComfortableStopOperator::publishVelocityLimitClearCommand() const +{ + auto velocity_limit_clear_command = tier4_planning_msgs::msg::VelocityLimitClearCommand(); + velocity_limit_clear_command.stamp = this->now(); + velocity_limit_clear_command.command = true; + velocity_limit_clear_command.sender = "mrm_comfortable_stop_operator"; + + pub_velocity_limit_clear_command_->publish(velocity_limit_clear_command); +} + +void MrmComfortableStopOperator::onTimer() const { publishStatus(); } + +} // namespace mrm_comfortable_stop_operator + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(mrm_comfortable_stop_operator::MrmComfortableStopOperator) diff --git a/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_node.cpp b/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_node.cpp new file mode 100644 index 0000000000000..03bad4198776d --- /dev/null +++ b/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_node.cpp @@ -0,0 +1,27 @@ +// 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 "mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp" + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; +} diff --git a/system/mrm_emergency_stop_operator/CMakeLists.txt b/system/mrm_emergency_stop_operator/CMakeLists.txt new file mode 100644 index 0000000000000..77d300f5c370b --- /dev/null +++ b/system/mrm_emergency_stop_operator/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.14) +project(mrm_emergency_stop_operator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(mrm_emergency_stop_operator_component SHARED + src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp +) + +rclcpp_components_register_node(mrm_emergency_stop_operator_component + PLUGIN "mrm_emergency_stop_operator::MrmEmergencyStopOperator" + EXECUTABLE mrm_emergency_stop_operator) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/system/mrm_emergency_stop_operator/READEME.md b/system/mrm_emergency_stop_operator/READEME.md new file mode 100644 index 0000000000000..0866202f4352a --- /dev/null +++ b/system/mrm_emergency_stop_operator/READEME.md @@ -0,0 +1,43 @@ +# mrm_emergency_stop_operator + +## Purpose + +MRM emergency stop operator is a node that generates emergency stop commands according to the emergency stop MRM order. + +## Inner-workings / Algorithms + +## Inputs / Outputs + +### 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. | +| | | | + +### 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 | + +## Parameters + +### Node Parameters + +| Name | Type | Default value | Explanation | +| ----------- | ---- | ------------- | ----------------------------- | +| update_rate | int | `30` | Timer callback frequency [Hz] | + +### Core Parameters + +| Name | Type | Default value | Explanation | +| ------------------- | ------ | ------------- | ---------------------------------------------- | +| target_acceleration | double | `-2.5` | Target acceleration for emergency stop [m/s^2] | +| target_jerk | double | `-1.5` | Target jerk for emergency stop [m/s^3] | + +## Assumptions / Known limits + +TBD. diff --git a/system/mrm_emergency_stop_operator/config/mrm_emergency_stop_operator.config.yaml b/system/mrm_emergency_stop_operator/config/mrm_emergency_stop_operator.config.yaml new file mode 100644 index 0000000000000..1ee2699a23a82 --- /dev/null +++ b/system/mrm_emergency_stop_operator/config/mrm_emergency_stop_operator.config.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + update_rate: 30 + target_acceleration: -2.5 + target_jerk: -1.5 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 new file mode 100644 index 0000000000000..2ac6321caab55 --- /dev/null +++ b/system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp @@ -0,0 +1,87 @@ +// 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 MRM_EMERGENCY_STOP_OPERATOR__MRM_EMERGENCY_STOP_OPERATOR_CORE_HPP_ +#define MRM_EMERGENCY_STOP_OPERATOR__MRM_EMERGENCY_STOP_OPERATOR_CORE_HPP_ + +// Core +#include +#include + +// Autoware +#include +#include +#include + +// ROS2 core +#include + +namespace mrm_emergency_stop_operator +{ +using autoware_auto_control_msgs::msg::AckermannControlCommand; +using tier4_system_msgs::msg::MrmBehaviorStatus; +using tier4_system_msgs::srv::OperateMrm; + +struct Parameters +{ + int update_rate; // [Hz] + double target_acceleration; // [m/s^2] + double target_jerk; // [m/s^3] +}; + +class MrmEmergencyStopOperator : public rclcpp::Node +{ +public: + explicit MrmEmergencyStopOperator(const rclcpp::NodeOptions & node_options); + +private: + // Parameters + Parameters params_; + + // Subscriber + rclcpp::Subscription::SharedPtr sub_control_cmd_; + + void onControlCommand(AckermannControlCommand::ConstSharedPtr msg); + + // Server + rclcpp::Service::SharedPtr service_operation_; + + void operateEmergencyStop( + const OperateMrm::Request::SharedPtr request, const OperateMrm::Response::SharedPtr response); + + // Publisher + rclcpp::Publisher::SharedPtr pub_status_; + rclcpp::Publisher::SharedPtr pub_control_cmd_; + + void publishStatus() const; + void publishControlCommand(const AckermannControlCommand & command) const; + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + + void onTimer(); + + // States + MrmBehaviorStatus status_; + AckermannControlCommand prev_control_cmd_; + bool is_prev_control_cmd_subscribed_; + + // Algorithm + AckermannControlCommand calcTargetAcceleration( + const AckermannControlCommand & prev_control_cmd) const; +}; + +} // namespace mrm_emergency_stop_operator + +#endif // MRM_EMERGENCY_STOP_OPERATOR__MRM_EMERGENCY_STOP_OPERATOR_CORE_HPP_ diff --git a/system/mrm_emergency_stop_operator/launch/mrm_emergency_stop_operator.launch.py b/system/mrm_emergency_stop_operator/launch/mrm_emergency_stop_operator.launch.py new file mode 100644 index 0000000000000..6b1e4550389c2 --- /dev/null +++ b/system/mrm_emergency_stop_operator/launch/mrm_emergency_stop_operator.launch.py @@ -0,0 +1,63 @@ +# Copyright 2022 The Autoware Contributors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +import yaml + + +def generate_launch_description(): + param_path = os.path.join( + get_package_share_directory("mrm_emergency_stop_operator"), + "config/mrm_emergency_stop_operator.config.yaml", + ) + + with open(param_path, "r") as f: + param = yaml.safe_load(f)["/**"]["ros__parameters"] + + component = ComposableNode( + package="mrm_emergency_stop_operator", + plugin="mrm_emergency_stop_operator::MrmEmergencyStopOperator", + name="mrm_emergency_stop_operator", + parameters=[ + param, + ], + remappings=[ + ("~/input/mrm/emergency_stop/operate", "/system/mrm/emergency_stop/operate"), + ("~/input/control/control_cmd", "/control/command/control_cmd"), + ("~/output/mrm/emergency_stop/status", "/system/mrm/emergency_stop/status"), + ("~/output/mrm/emergency_stop/control_cmd", "/system/emergency/control_cmd"), + ], + ) + + container = ComposableNodeContainer( + name="mrm_emergency_stop_operator_container", + namespace="mrm_emergency_stop_operator", + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[ + component, + ], + output="screen", + ) + + return launch.LaunchDescription( + [ + container, + ] + ) diff --git a/system/mrm_emergency_stop_operator/package.xml b/system/mrm_emergency_stop_operator/package.xml new file mode 100644 index 0000000000000..0761a1cbb9980 --- /dev/null +++ b/system/mrm_emergency_stop_operator/package.xml @@ -0,0 +1,28 @@ + + + + mrm_emergency_stop_operator + 0.1.0 + The MRM emergency stop operator package + Makoto Kurihara + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + autoware_adapi_v1_msgs + autoware_auto_control_msgs + rclcpp + rclcpp_components + std_msgs + std_srvs + tier4_system_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + 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 new file mode 100644 index 0000000000000..08814dbbd710d --- /dev/null +++ b/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp @@ -0,0 +1,137 @@ +// 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 "mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp" + +namespace mrm_emergency_stop_operator +{ + +MrmEmergencyStopOperator::MrmEmergencyStopOperator(const rclcpp::NodeOptions & node_options) +: Node("mrm_emergency_stop_operator", node_options) +{ + // Parameter + params_.update_rate = static_cast(declare_parameter("update_rate", 30)); + params_.target_acceleration = declare_parameter("target_acceleration", -2.5); + params_.target_jerk = declare_parameter("target_jerk", -1.5); + + // Subscriber + sub_control_cmd_ = create_subscription( + "~/input/control/control_cmd", 1, + std::bind(&MrmEmergencyStopOperator::onControlCommand, this, std::placeholders::_1)); + + // Server + service_operation_ = create_service( + "~/input/mrm/emergency_stop/operate", std::bind( + &MrmEmergencyStopOperator::operateEmergencyStop, this, + std::placeholders::_1, std::placeholders::_2)); + + // Publisher + pub_status_ = create_publisher("~/output/mrm/emergency_stop/status", 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(); + timer_ = rclcpp::create_timer( + this, get_clock(), update_period_ns, std::bind(&MrmEmergencyStopOperator::onTimer, this)); + + // Initialize + status_.state = MrmBehaviorStatus::AVAILABLE; + is_prev_control_cmd_subscribed_ = false; +} + +void MrmEmergencyStopOperator::onControlCommand(AckermannControlCommand::ConstSharedPtr msg) +{ + if (status_.state != MrmBehaviorStatus::OPERATING) { + prev_control_cmd_ = *msg; + is_prev_control_cmd_subscribed_ = true; + } +} + +void MrmEmergencyStopOperator::operateEmergencyStop( + const OperateMrm::Request::SharedPtr request, const OperateMrm::Response::SharedPtr response) +{ + if (request->operate == true) { + status_.state = MrmBehaviorStatus::OPERATING; + response->response.success = true; + } else { + status_.state = MrmBehaviorStatus::AVAILABLE; + response->response.success = true; + } +} + +void MrmEmergencyStopOperator::publishStatus() const +{ + auto status = status_; + status.stamp = this->now(); + pub_status_->publish(status); +} + +void MrmEmergencyStopOperator::publishControlCommand(const AckermannControlCommand & command) const +{ + pub_control_cmd_->publish(command); +} + +void MrmEmergencyStopOperator::onTimer() +{ + if (status_.state == MrmBehaviorStatus::OPERATING) { + auto control_cmd = calcTargetAcceleration(prev_control_cmd_); + publishControlCommand(control_cmd); + prev_control_cmd_ = control_cmd; + } else { + publishControlCommand(prev_control_cmd_); + } + publishStatus(); +} + +AckermannControlCommand MrmEmergencyStopOperator::calcTargetAcceleration( + const AckermannControlCommand & prev_control_cmd) const +{ + auto control_cmd = AckermannControlCommand(); + + 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.acceleration = static_cast(params_.target_acceleration); + control_cmd.longitudinal.jerk = 0.0; + control_cmd.lateral.stamp = this->now(); + control_cmd.lateral.steering_tire_angle = 0.0; + control_cmd.lateral.steering_tire_rotation_rate = 0.0; + return control_cmd; + } + + control_cmd = prev_control_cmd; + const auto dt = (this->now() - prev_control_cmd.stamp).seconds(); + + 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.acceleration = static_cast(std::max( + prev_control_cmd.longitudinal.acceleration + params_.target_jerk * dt, + params_.target_acceleration)); + if (prev_control_cmd.longitudinal.acceleration == params_.target_acceleration) { + control_cmd.longitudinal.jerk = 0.0; + } else { + control_cmd.longitudinal.jerk = static_cast(params_.target_jerk); + } + + return control_cmd; +} + +} // namespace mrm_emergency_stop_operator + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(mrm_emergency_stop_operator::MrmEmergencyStopOperator)