feat: apply mrm state to autoware state (#81)

feat: apply mrm state Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
tier4 · Nov 30, 2022 · 1fc2957 · 1fc2957
1 parent 65624d8
commit 1fc2957
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 13 deletions.
diff --git a/autoware_iv_internal_api_adaptor/package.xml b/autoware_iv_internal_api_adaptor/package.xml
@@ -11,6 +11,7 @@
 
   <build_depend>autoware_cmake</build_depend>
 
+  <depend>autoware_adapi_v1_msgs</depend>
   <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
   <depend>autoware_auto_system_msgs</depend>

diff --git a/autoware_iv_internal_api_adaptor/src/iv_msgs.cpp b/autoware_iv_internal_api_adaptor/src/iv_msgs.cpp
@@ -26,7 +26,7 @@ IVMsgs::IVMsgs(const rclcpp::NodeOptions & options) : Node("external_api_iv_msgs
   sub_state_ = create_subscription<AutowareStateAuto>(
     "/autoware/state", rclcpp::QoS(1), std::bind(&IVMsgs::onState, this, _1));
   sub_emergency_ = create_subscription<EmergencyStateAuto>(
-    "/system/emergency/emergency_state", rclcpp::QoS(1), std::bind(&IVMsgs::onEmergency, this, _1));
+    "/system/fail_safe/mrm_state", rclcpp::QoS(1), std::bind(&IVMsgs::onEmergency, this, _1));
 
   pub_control_mode_ =
     create_publisher<ControlModeAuto>("/api/iv_msgs/vehicle/status/control_mode", rclcpp::QoS(1));
@@ -44,24 +44,23 @@ IVMsgs::IVMsgs(const rclcpp::NodeOptions & options) : Node("external_api_iv_msgs
   sub_tracked_objects_ = create_subscription<TrackedObjectsAuto>(
     "/perception/object_recognition/tracking/objects", rclcpp::QoS(1),
     std::bind(&IVMsgs::onTrackedObjects, this, _1));
+
+  is_emergency_ = false;
 }
 
 void IVMsgs::onState(const AutowareStateAuto::ConstSharedPtr message)
 {
   auto state = tier4_auto_msgs_converter::convert(*message);
-  if (emergency_) {
-    switch (emergency_->state) {
-      case EmergencyStateAuto::MRM_OPERATING:
-      case EmergencyStateAuto::MRM_SUCCEEDED:
-      case EmergencyStateAuto::MRM_FAILED:
-        state.state = AutowareStateIV::EMERGENCY;
-        break;
-    }
+  if (is_emergency_) {
+    state.state = AutowareStateIV::EMERGENCY;
   }
   pub_state_->publish(state);
 }
 
-void IVMsgs::onEmergency(const EmergencyStateAuto::ConstSharedPtr message) { emergency_ = message; }
+void IVMsgs::onEmergency(const EmergencyStateAuto::ConstSharedPtr message)
+{
+  is_emergency_ = message->state != EmergencyStateAuto::NORMAL;
+}
 
 void IVMsgs::onControlMode(const ControlModeAuto::ConstSharedPtr message)
 {

diff --git a/autoware_iv_internal_api_adaptor/src/iv_msgs.hpp b/autoware_iv_internal_api_adaptor/src/iv_msgs.hpp
@@ -17,10 +17,10 @@
 
 #include <rclcpp/rclcpp.hpp>
 
+#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
 #include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
 #include <autoware_auto_planning_msgs/msg/trajectory.hpp>
 #include <autoware_auto_system_msgs/msg/autoware_state.hpp>
-#include <autoware_auto_system_msgs/msg/emergency_state.hpp>
 #include <autoware_auto_vehicle_msgs/msg/control_mode_report.hpp>
 #include <tier4_perception_msgs/msg/dynamic_object_array.hpp>
 #include <tier4_planning_msgs/msg/trajectory.hpp>
@@ -34,7 +34,7 @@ class IVMsgs : public rclcpp::Node
   explicit IVMsgs(const rclcpp::NodeOptions & options);
 
 private:
-  using EmergencyStateAuto = autoware_auto_system_msgs::msg::EmergencyState;
+  using EmergencyStateAuto = autoware_adapi_v1_msgs::msg::MrmState;
   using AutowareStateAuto = autoware_auto_system_msgs::msg::AutowareState;
   using AutowareStateIV = tier4_system_msgs::msg::AutowareState;
   rclcpp::Subscription<EmergencyStateAuto>::SharedPtr sub_emergency_;
@@ -61,7 +61,7 @@ class IVMsgs : public rclcpp::Node
   void onTrajectory(const TrajectoryAuto::ConstSharedPtr message);
   void onTrackedObjects(const TrackedObjectsAuto::ConstSharedPtr message);
 
-  EmergencyStateAuto::ConstSharedPtr emergency_;
+  bool is_emergency_;
 };
 
 }  // namespace internal_api