Skip to content

Commit

Permalink
feat(mrm_handler): add mrm_handler (#6400)
Browse files Browse the repository at this point in the history
* feat: add mrm_handler

Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>

* style(pre-commit): autofix

Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>

* modify: update README

Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>

* feat: refactor isArrivedAtGoal()

Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>

* modify: fix error massages gramatically.

Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>

* feat: update the person in charge of the unimplemented parts

Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>

* modify: fix typo in schema.json

Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>

* modify: fix copyright

Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>

* modify: fix mistakes in README

Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>

* modify: correct a type mistake in README

Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>

* style(pre-commit): autofix

Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>

---------

Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Makoto Kurihara <mkuri8m@gmail.com>
Signed-off-by: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
  • Loading branch information
3 people authored and HansRobo committed Mar 12, 2024
1 parent 3ea07c2 commit 1da908c
Showing 10 changed files with 1,382 additions and 0 deletions.
15 changes: 15 additions & 0 deletions system/mrm_handler/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,15 @@
cmake_minimum_required(VERSION 3.14)
project(mrm_handler)

find_package(autoware_cmake REQUIRED)
autoware_package()

ament_auto_add_executable(mrm_handler
src/mrm_handler/mrm_handler_node.cpp
src/mrm_handler/mrm_handler_core.cpp
)

ament_auto_package(INSTALL_TO_SHARE
launch
config
)
44 changes: 44 additions & 0 deletions system/mrm_handler/README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,44 @@
# mrm_handler

## Purpose

MRM Handler is a node to select a proper MRM from a system failure state contained in OperationModeAvailability.

## Inner-workings / Algorithms

### State Transitions

![mrm-state](image/mrm-state.svg)

## Inputs / Outputs

### 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. |

### 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 |

## Parameters

{{ json_to_markdown("system/mrm_handler/schema/mrm_handler.schema.json") }}

## Assumptions / Known limits

TBD.
17 changes: 17 additions & 0 deletions system/mrm_handler/config/mrm_handler.param.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
# Default configuration for mrm handler
---
/**:
ros__parameters:
update_rate: 10
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
turning_hazard_on:
emergency: true
362 changes: 362 additions & 0 deletions system/mrm_handler/image/mrm-state.svg
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
160 changes: 160 additions & 0 deletions system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,160 @@
// Copyright 2024 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_HANDLER__MRM_HANDLER_CORE_HPP_
#define MRM_HANDLER__MRM_HANDLER_CORE_HPP_

// 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_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
#include <rclcpp/create_timer.hpp>
#include <rclcpp/rclcpp.hpp>

#include <diagnostic_msgs/msg/diagnostic_array.hpp>
#include <nav_msgs/msg/odometry.hpp>

struct HazardLampPolicy
{
bool emergency;
};

struct Param
{
int update_rate;
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{};
};

class MrmHandler : public rclcpp::Node
{
public:
MrmHandler();

private:
// Subscribers
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_;

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 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<tier4_vehicle_msgs::msg::ShiftStamped>::SharedPtr pub_shift_;
// rclcpp::Publisher<tier4_vehicle_msgs::msg::TurnSignal>::SharedPtr pub_turn_signal_;
rclcpp::Publisher<autoware_auto_vehicle_msgs::msg::HazardLightsCommand>::SharedPtr
pub_hazard_cmd_;
rclcpp::Publisher<autoware_auto_vehicle_msgs::msg::GearCommand>::SharedPtr pub_gear_cmd_;

autoware_auto_vehicle_msgs::msg::HazardLightsCommand createHazardCmdMsg();
autoware_auto_vehicle_msgs::msg::GearCommand createGearCmdMsg();
void publishControlCommands();

rclcpp::Publisher<autoware_adapi_v1_msgs::msg::MrmState>::SharedPtr pub_mrm_state_;

autoware_adapi_v1_msgs::msg::MrmState mrm_state_;
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_;
rclcpp::Client<tier4_system_msgs::srv::OperateMrm>::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_;

// Parameters
Param param_;

bool isDataReady();
void onTimer();

// Heartbeat
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;
bool isArrivedAtGoal();
};

#endif // MRM_HANDLER__MRM_HANDLER_CORE_HPP_
39 changes: 39 additions & 0 deletions system/mrm_handler/launch/mrm_handler.launch.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
<launch>
<arg name="input_operation_mode_availability" default="/system/operation_mode/availability"/>
<!-- To be replaced by ControlCommand -->
<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_manager/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_manager/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 mrm_handler)/config/mrm_handler.param.yaml"/>

<!-- mrm_handler -->
<node pkg="mrm_handler" exec="mrm_handler" name="mrm_handler" output="screen">
<remap from="~/input/operation_mode_availability" to="$(var input_operation_mode_availability)"/>
<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)"/>

<param from="$(var config_file)"/>
</node>
</launch>
31 changes: 31 additions & 0 deletions system/mrm_handler/package.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,31 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>mrm_handler</name>
<version>0.1.0</version>
<description>The mrm_handler ROS 2 package</description>
<maintainer email="makoto.kurihara@tier4.jp">Makoto Kurihara</maintainer>
<maintainer email="ryuta.kambe@tier4.jp">Ryuta Kambe</maintainer>
<maintainer email="tetsuhiro.kawaguchi@tier4.jp">Tetsuhiro Kawaguchi</maintainer>
<license>Apache License 2.0</license>

<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>autoware_adapi_v1_msgs</depend>
<depend>autoware_auto_control_msgs</depend>
<depend>autoware_auto_system_msgs</depend>
<depend>autoware_auto_vehicle_msgs</depend>
<depend>nav_msgs</depend>
<depend>rclcpp</depend>
<depend>std_msgs</depend>
<depend>std_srvs</depend>
<depend>tier4_system_msgs</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>autoware_lint_common</test_depend>

<export>
<build_type>ament_cmake</build_type>
</export>
</package>
Loading

0 comments on commit 1da908c

Please sign in to comment.