Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat/use operation mode availability for emergency handler #895

Closed
wants to merge 5 commits into from
Closed

feat/use operation mode availability for emergency handler #895

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
abd0d46
use operation mode availability
asana17 Sep 27, 2023
133aac0
fix emergency_holding behavior
asana17 Sep 29, 2023
31e9865
style(pre-commit): autofix
pre-commit-ci[bot] Sep 29, 2023
e36e40f
feat: change state machine and add isArrivedAtGoal
TetsuKawa Dec 14, 2023
940ff02
style(pre-commit): autofix
pre-commit-ci[bot] Dec 14, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 4 additions & 1 deletion system/emergency_handler/config/emergency_handler.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,10 +3,13 @@
/**:
ros__parameters:
update_rate: 10
timeout_hazard_status: 0.5
timeout_operation_mode_availability: 0.5
use_emergency_holding: false
timeout_emergency_recovery: 5.0
timeout_takeover_request: 10.0
use_takeover_request: false
use_parking_after_stopped: false
use_pull_over: false
use_comfortable_stop: false

# setting whether to turn hazard lamp on for each situation
Expand Down
38 changes: 29 additions & 9 deletions system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,16 +17,19 @@

// Core
#include <memory>
#include <optional>
#include <string>
#include <variant>

// Autoware
#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
#include <autoware_auto_system_msgs/msg/hazard_status_stamped.hpp>
#include <autoware_auto_vehicle_msgs/msg/control_mode_report.hpp>
#include <autoware_auto_vehicle_msgs/msg/gear_command.hpp>
#include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
#include <tier4_system_msgs/msg/mrm_behavior_status.hpp>
#include <tier4_system_msgs/msg/operation_mode_availability.hpp>
#include <tier4_system_msgs/srv/operate_mrm.hpp>

// ROS 2 core
Expand All @@ -44,10 +47,13 @@ struct HazardLampPolicy
struct Param
{
int update_rate;
double timeout_hazard_status;
double timeout_operation_mode_availability;
bool use_emergency_holding;
double timeout_emergency_recovery;
double timeout_takeover_request;
bool use_takeover_request;
bool use_parking_after_stopped;
bool use_pull_over;
bool use_comfortable_stop;
HazardLampPolicy turning_hazard_on{};
};
Expand All @@ -59,35 +65,44 @@ class EmergencyHandler : public rclcpp::Node

private:
// Subscribers
rclcpp::Subscription<autoware_auto_system_msgs::msg::HazardStatusStamped>::SharedPtr
sub_hazard_status_stamped_;
rclcpp::Subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
sub_prev_control_command_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>::SharedPtr
sub_control_mode_;
rclcpp::Subscription<tier4_system_msgs::msg::OperationModeAvailability>::SharedPtr
sub_operation_mode_availability_;
rclcpp::Subscription<tier4_system_msgs::msg::MrmBehaviorStatus>::SharedPtr
sub_mrm_pull_over_status_;
rclcpp::Subscription<tier4_system_msgs::msg::MrmBehaviorStatus>::SharedPtr
sub_mrm_comfortable_stop_status_;
rclcpp::Subscription<tier4_system_msgs::msg::MrmBehaviorStatus>::SharedPtr
sub_mrm_emergency_stop_status_;
rclcpp::Subscription<autoware_adapi_v1_msgs::msg::OperationModeState>::SharedPtr
sub_operation_mode_state_;

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::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_;
tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_emergency_stop_status_;
autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr operation_mode_state_;

void onHazardStatusStamped(
const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg);
void onPrevControlCommand(
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 onOperationModeAvailability(
const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr msg);
void onMrmPullOverStatus(const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg);
void onMrmComfortableStopStatus(
const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg);
void onMrmEmergencyStopStatus(
const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg);
void onOperationModeState(
const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg);

// Publisher
rclcpp::Publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
Expand All @@ -109,6 +124,8 @@ class EmergencyHandler : public rclcpp::Node
void publishMrmState();

// Clients
rclcpp::CallbackGroup::SharedPtr client_mrm_pull_over_group_;
rclcpp::Client<tier4_system_msgs::srv::OperateMrm>::SharedPtr client_mrm_pull_over_;
rclcpp::CallbackGroup::SharedPtr client_mrm_comfortable_stop_group_;
rclcpp::Client<tier4_system_msgs::srv::OperateMrm>::SharedPtr client_mrm_comfortable_stop_;
rclcpp::CallbackGroup::SharedPtr client_mrm_emergency_stop_group_;
Expand All @@ -132,17 +149,20 @@ class EmergencyHandler : public rclcpp::Node
void onTimer();

// Heartbeat
rclcpp::Time stamp_hazard_status_;
rclcpp::Time stamp_operation_mode_availability_;
std::optional<rclcpp::Time> stamp_autonomous_become_unavailable_ = std::nullopt;

// Algorithm
rclcpp::Time takeover_requested_time_;
bool is_takeover_request_ = false;
bool is_emergency_holding_ = false;
void transitionTo(const int new_state);
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);
bool isEmergency() const;
bool isArrivedAtGoal();
};

#endif // EMERGENCY_HANDLER__EMERGENCY_HANDLER_CORE_HPP_
10 changes: 8 additions & 2 deletions system/emergency_handler/launch/emergency_handler.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,32 +1,38 @@
<launch>
<arg name="input_hazard_status" default="/system/emergency/hazard_status"/>
<arg name="input_operation_mode_availability" default="/system/operation_mode/availability"/>
<!-- To be replaced by ControlCommand -->
<arg name="input_prev_control_command" default="/control/command/control_cmd"/>
<arg name="input_odometry" default="/localization/kinematic_state"/>
<arg name="input_control_mode" default="/vehicle/status/control_mode"/>
<arg name="input_mrm_pull_over_state" default="/system/mrm/pull_over/status"/>
<arg name="input_mrm_comfortable_stop_state" default="/system/mrm/comfortable_stop/status"/>
<arg name="input_mrm_emergency_stop_state" default="/system/mrm/emergency_stop/status"/>
<arg name="input_api_operation_mode_state" default="/api/operation_mode/state"/>

<arg name="output_gear" default="/system/emergency/gear_cmd"/>
<arg name="output_hazard" default="/system/emergency/hazard_lights_cmd"/>
<arg name="output_mrm_state" default="/system/fail_safe/mrm_state"/>
<arg name="output_mrm_pull_over_operate" default="/system/mrm/pull_over/operate"/>
<arg name="output_mrm_comfortable_stop_operate" default="/system/mrm/comfortable_stop/operate"/>
<arg name="output_mrm_emergency_stop_operate" default="/system/mrm/emergency_stop/operate"/>

<arg name="config_file" default="$(find-pkg-share emergency_handler)/config/emergency_handler.param.yaml"/>

<!-- emergency_handler -->
<node pkg="emergency_handler" exec="emergency_handler" name="emergency_handler" output="screen">
<remap from="~/input/hazard_status" to="$(var input_hazard_status)"/>
<remap from="~/input/operation_mode_availability" to="$(var input_operation_mode_availability)"/>
<remap from="~/input/prev_control_command" to="$(var input_prev_control_command)"/>
<remap from="~/input/odometry" to="$(var input_odometry)"/>
<remap from="~/input/control_mode" to="$(var input_control_mode)"/>
<remap from="~/input/mrm/pull_over/status" to="$(var input_mrm_pull_over_state)"/>
<remap from="~/input/mrm/comfortable_stop/status" to="$(var input_mrm_comfortable_stop_state)"/>
<remap from="~/input/mrm/emergency_stop/status" to="$(var input_mrm_emergency_stop_state)"/>
<remap from="~/input/api/operation_mode/state" to="$(var input_api_operation_mode_state)"/>

<remap from="~/output/gear" to="$(var output_gear)"/>
<remap from="~/output/hazard" to="$(var output_hazard)"/>
<remap from="~/output/mrm/state" to="$(var output_mrm_state)"/>
<remap from="~/output/mrm/pull_over/operate" to="$(var output_mrm_pull_over_operate)"/>
<remap from="~/output/mrm/comfortable_stop/operate" to="$(var output_mrm_comfortable_stop_operate)"/>
<remap from="~/output/mrm/emergency_stop/operate" to="$(var output_mrm_emergency_stop_operate)"/>

Expand Down
Loading
Loading