|
| 1 | +// Copyright 2024 TIER IV, Inc. |
| 2 | +// |
| 3 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +// you may not use this file except in compliance with the License. |
| 5 | +// You may obtain a copy of the License at |
| 6 | +// |
| 7 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +// |
| 9 | +// Unless required by applicable law or agreed to in writing, software |
| 10 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +// See the License for the specific language governing permissions and |
| 13 | +// limitations under the License. |
| 14 | + |
| 15 | +#ifndef MRM_HANDLER__MRM_HANDLER_CORE_HPP_ |
| 16 | +#define MRM_HANDLER__MRM_HANDLER_CORE_HPP_ |
| 17 | + |
| 18 | +// Core |
| 19 | +#include <memory> |
| 20 | +#include <optional> |
| 21 | +#include <string> |
| 22 | +#include <variant> |
| 23 | + |
| 24 | +// Autoware |
| 25 | +#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp> |
| 26 | +#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp> |
| 27 | +#include <autoware_auto_vehicle_msgs/msg/control_mode_report.hpp> |
| 28 | +#include <autoware_auto_vehicle_msgs/msg/gear_command.hpp> |
| 29 | +#include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp> |
| 30 | +#include <tier4_system_msgs/msg/mrm_behavior_status.hpp> |
| 31 | +#include <tier4_system_msgs/msg/operation_mode_availability.hpp> |
| 32 | +#include <tier4_system_msgs/srv/operate_mrm.hpp> |
| 33 | + |
| 34 | +// ROS 2 core |
| 35 | +#include <rclcpp/create_timer.hpp> |
| 36 | +#include <rclcpp/rclcpp.hpp> |
| 37 | + |
| 38 | +#include <diagnostic_msgs/msg/diagnostic_array.hpp> |
| 39 | +#include <nav_msgs/msg/odometry.hpp> |
| 40 | + |
| 41 | +struct HazardLampPolicy |
| 42 | +{ |
| 43 | + bool emergency; |
| 44 | +}; |
| 45 | + |
| 46 | +struct Param |
| 47 | +{ |
| 48 | + int update_rate; |
| 49 | + double timeout_operation_mode_availability; |
| 50 | + bool use_emergency_holding; |
| 51 | + double timeout_emergency_recovery; |
| 52 | + double timeout_takeover_request; |
| 53 | + bool use_takeover_request; |
| 54 | + bool use_parking_after_stopped; |
| 55 | + bool use_pull_over; |
| 56 | + bool use_comfortable_stop; |
| 57 | + HazardLampPolicy turning_hazard_on{}; |
| 58 | +}; |
| 59 | + |
| 60 | +class MrmHandler : public rclcpp::Node |
| 61 | +{ |
| 62 | +public: |
| 63 | + MrmHandler(); |
| 64 | + |
| 65 | +private: |
| 66 | + // Subscribers |
| 67 | + rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_; |
| 68 | + rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>::SharedPtr |
| 69 | + sub_control_mode_; |
| 70 | + rclcpp::Subscription<tier4_system_msgs::msg::OperationModeAvailability>::SharedPtr |
| 71 | + sub_operation_mode_availability_; |
| 72 | + rclcpp::Subscription<tier4_system_msgs::msg::MrmBehaviorStatus>::SharedPtr |
| 73 | + sub_mrm_pull_over_status_; |
| 74 | + rclcpp::Subscription<tier4_system_msgs::msg::MrmBehaviorStatus>::SharedPtr |
| 75 | + sub_mrm_comfortable_stop_status_; |
| 76 | + rclcpp::Subscription<tier4_system_msgs::msg::MrmBehaviorStatus>::SharedPtr |
| 77 | + sub_mrm_emergency_stop_status_; |
| 78 | + rclcpp::Subscription<autoware_adapi_v1_msgs::msg::OperationModeState>::SharedPtr |
| 79 | + sub_operation_mode_state_; |
| 80 | + |
| 81 | + nav_msgs::msg::Odometry::ConstSharedPtr odom_; |
| 82 | + autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; |
| 83 | + tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr operation_mode_availability_; |
| 84 | + tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_pull_over_status_; |
| 85 | + tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_comfortable_stop_status_; |
| 86 | + tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_emergency_stop_status_; |
| 87 | + autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr operation_mode_state_; |
| 88 | + |
| 89 | + void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); |
| 90 | + void onControlMode(const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg); |
| 91 | + void onOperationModeAvailability( |
| 92 | + const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr msg); |
| 93 | + void onMrmPullOverStatus(const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); |
| 94 | + void onMrmComfortableStopStatus( |
| 95 | + const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); |
| 96 | + void onMrmEmergencyStopStatus( |
| 97 | + const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); |
| 98 | + void onOperationModeState( |
| 99 | + const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg); |
| 100 | + |
| 101 | + // Publisher |
| 102 | + |
| 103 | + // rclcpp::Publisher<tier4_vehicle_msgs::msg::ShiftStamped>::SharedPtr pub_shift_; |
| 104 | + // rclcpp::Publisher<tier4_vehicle_msgs::msg::TurnSignal>::SharedPtr pub_turn_signal_; |
| 105 | + rclcpp::Publisher<autoware_auto_vehicle_msgs::msg::HazardLightsCommand>::SharedPtr |
| 106 | + pub_hazard_cmd_; |
| 107 | + rclcpp::Publisher<autoware_auto_vehicle_msgs::msg::GearCommand>::SharedPtr pub_gear_cmd_; |
| 108 | + |
| 109 | + autoware_auto_vehicle_msgs::msg::HazardLightsCommand createHazardCmdMsg(); |
| 110 | + autoware_auto_vehicle_msgs::msg::GearCommand createGearCmdMsg(); |
| 111 | + void publishControlCommands(); |
| 112 | + |
| 113 | + rclcpp::Publisher<autoware_adapi_v1_msgs::msg::MrmState>::SharedPtr pub_mrm_state_; |
| 114 | + |
| 115 | + autoware_adapi_v1_msgs::msg::MrmState mrm_state_; |
| 116 | + void publishMrmState(); |
| 117 | + |
| 118 | + // Clients |
| 119 | + rclcpp::CallbackGroup::SharedPtr client_mrm_pull_over_group_; |
| 120 | + rclcpp::Client<tier4_system_msgs::srv::OperateMrm>::SharedPtr client_mrm_pull_over_; |
| 121 | + rclcpp::CallbackGroup::SharedPtr client_mrm_comfortable_stop_group_; |
| 122 | + rclcpp::Client<tier4_system_msgs::srv::OperateMrm>::SharedPtr client_mrm_comfortable_stop_; |
| 123 | + rclcpp::CallbackGroup::SharedPtr client_mrm_emergency_stop_group_; |
| 124 | + rclcpp::Client<tier4_system_msgs::srv::OperateMrm>::SharedPtr client_mrm_emergency_stop_; |
| 125 | + |
| 126 | + void callMrmBehavior( |
| 127 | + const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const; |
| 128 | + void cancelMrmBehavior( |
| 129 | + const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const; |
| 130 | + void logMrmCallingResult( |
| 131 | + const tier4_system_msgs::srv::OperateMrm::Response & result, const std::string & behavior, |
| 132 | + bool is_call) const; |
| 133 | + |
| 134 | + // Timer |
| 135 | + rclcpp::TimerBase::SharedPtr timer_; |
| 136 | + |
| 137 | + // Parameters |
| 138 | + Param param_; |
| 139 | + |
| 140 | + bool isDataReady(); |
| 141 | + void onTimer(); |
| 142 | + |
| 143 | + // Heartbeat |
| 144 | + rclcpp::Time stamp_operation_mode_availability_; |
| 145 | + std::optional<rclcpp::Time> stamp_autonomous_become_unavailable_ = std::nullopt; |
| 146 | + |
| 147 | + // Algorithm |
| 148 | + rclcpp::Time takeover_requested_time_; |
| 149 | + bool is_takeover_request_ = false; |
| 150 | + bool is_emergency_holding_ = false; |
| 151 | + void transitionTo(const int new_state); |
| 152 | + void updateMrmState(); |
| 153 | + void operateMrm(); |
| 154 | + autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior(); |
| 155 | + bool isStopped(); |
| 156 | + bool isEmergency() const; |
| 157 | + bool isArrivedAtGoal(); |
| 158 | +}; |
| 159 | + |
| 160 | +#endif // MRM_HANDLER__MRM_HANDLER_CORE_HPP_ |
![pre-commit-ci[bot]](https://avatars.githubusercontent.com/in/68672?v=4&size=40){kind=avatar}
0 commit comments