change engage from bool to control msg (autowarefoundation#292)

Signed-off-by: Kosuke Murakami <[email protected]>

system/autoware_state_monitor/include/autoware_state_monitor/autoware_state_monitor_node.hpp

@@ -21,6 +21,7 @@
 
 #include "autoware_planning_msgs/msg/route.hpp"
 #include "autoware_planning_msgs/msg/trajectory.hpp"
+#include "autoware_control_msgs/msg/engage_mode.hpp"
 #include "autoware_system_msgs/msg/autoware_state.hpp"
 #include "autoware_vehicle_msgs/msg/control_mode.hpp"
 
@@ -62,14 +63,14 @@ class AutowareStateMonitorNode : public rclcpp::Node
   geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_;
 
   // Subscriber
-  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr sub_autoware_engage_;
+  rclcpp::Subscription<autoware_control_msgs::msg::EngageMode>::SharedPtr sub_autoware_engage_;
   rclcpp::Subscription<autoware_vehicle_msgs::msg::ControlMode>::SharedPtr
     sub_vehicle_control_mode_;
   rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr sub_is_emergency_;
   rclcpp::Subscription<autoware_planning_msgs::msg::Route>::SharedPtr sub_route_;
   rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr sub_twist_;
 
-  void onAutowareEngage(const std_msgs::msg::Bool::ConstSharedPtr msg);
+  void onAutowareEngage(const autoware_control_msgs::msg::EngageMode::ConstSharedPtr msg);
   void onVehicleControlMode(const autoware_vehicle_msgs::msg::ControlMode::ConstSharedPtr msg);
   void onIsEmergency(const std_msgs::msg::Bool::ConstSharedPtr msg);
   void onRoute(const autoware_planning_msgs::msg::Route::ConstSharedPtr msg);
@@ -86,7 +87,7 @@ class AutowareStateMonitorNode : public rclcpp::Node
 
   // Publisher
   rclcpp::Publisher<autoware_system_msgs::msg::AutowareState>::SharedPtr pub_autoware_state_;
-  rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr pub_autoware_engage_;
+  rclcpp::Publisher<autoware_control_msgs::msg::EngageMode>::SharedPtr pub_autoware_engage_;
 
   void setDisengage();
 

system/autoware_state_monitor/include/autoware_state_monitor/state_machine.hpp

@@ -21,6 +21,7 @@
 
 #include "autoware_planning_msgs/msg/route.hpp"
 #include "autoware_planning_msgs/msg/trajectory.hpp"
+#include "autoware_control_msgs/msg/engage_mode.hpp"
 #include "autoware_system_msgs/msg/autoware_state.hpp"
 #include "autoware_vehicle_msgs/msg/control_mode.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
@@ -42,7 +43,7 @@ struct StateInput
   geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose;
   geometry_msgs::msg::Pose::ConstSharedPtr goal_pose;
 
-  std_msgs::msg::Bool::ConstSharedPtr autoware_engage;
+  autoware_control_msgs::msg::EngageMode::ConstSharedPtr autoware_engage;
   autoware_vehicle_msgs::msg::ControlMode::ConstSharedPtr vehicle_control_mode;
   std_msgs::msg::Bool::ConstSharedPtr is_emergency;
   autoware_planning_msgs::msg::Route::ConstSharedPtr route;

system/autoware_state_monitor/package.xml

@@ -8,10 +8,11 @@
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
-  
+
   <depend>rclcpp</depend>
   <depend>rclcpp_generic</depend>
   <depend>autoware_planning_msgs</depend>
+  <depend>autoware_control_msgs</depend>
   <depend>autoware_system_msgs</depend>
   <depend>autoware_vehicle_msgs</depend>
   <depend>diagnostic_updater</depend>
@@ -24,7 +25,6 @@
   <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>autoware_perception_msgs</exec_depend>
-  <exec_depend>autoware_control_msgs</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_cmake_cppcheck</test_depend>

system/autoware_state_monitor/src/autoware_state_monitor_node/autoware_state_monitor_node.cpp

@@ -110,7 +110,7 @@ std::string getStateMessage(const AutowareState & state)
 
 }  // namespace
 
-void AutowareStateMonitorNode::onAutowareEngage(const std_msgs::msg::Bool::ConstSharedPtr msg)
+void AutowareStateMonitorNode::onAutowareEngage(const autoware_control_msgs::msg::EngageMode::ConstSharedPtr msg)
 {
   state_input_.autoware_engage = msg;
 }
@@ -333,8 +333,8 @@ TfStats AutowareStateMonitorNode::getTfStats() const
 
 void AutowareStateMonitorNode::setDisengage()
 {
-  std_msgs::msg::Bool msg;
-  msg.data = false;
+  autoware_control_msgs::msg::EngageMode msg;
+  msg.is_engaged = false;
   pub_autoware_engage_->publish(msg);
 }
 
@@ -372,7 +372,7 @@ AutowareStateMonitorNode::AutowareStateMonitorNode()
   }
 
   // Subscriber
-  sub_autoware_engage_ = this->create_subscription<std_msgs::msg::Bool>(
+  sub_autoware_engage_ = this->create_subscription<autoware_control_msgs::msg::EngageMode>(
     "input/autoware_engage", 1,
     std::bind(&AutowareStateMonitorNode::onAutowareEngage, this, std::placeholders::_1));
   sub_vehicle_control_mode_ = this->create_subscription<autoware_vehicle_msgs::msg::ControlMode>(
@@ -389,7 +389,7 @@ AutowareStateMonitorNode::AutowareStateMonitorNode()
   // Publisher
   pub_autoware_state_ =
     this->create_publisher<autoware_system_msgs::msg::AutowareState>("output/autoware_state", 1);
-  pub_autoware_engage_ = this->create_publisher<std_msgs::msg::Bool>("output/autoware_engage", 1);
+  pub_autoware_engage_ = this->create_publisher<autoware_control_msgs::msg::EngageMode>("output/autoware_engage", 1);
 
   // Diagnostic Updater
   setupDiagnosticUpdater();

system/autoware_state_monitor/src/autoware_state_monitor_node/state_machine.cpp

@@ -160,7 +160,7 @@ bool StateMachine::isEngaged() const
     return false;
   }
 
-  if (state_input_.autoware_engage->data != 1) {
+  if (state_input_.autoware_engage->is_engaged != 1) {
     return false;
   }