fix: set control_mode false before autoware engage (autowarefoundatio…

…n#232) * fix: set control_mode false before autoware engage Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * add input/engage remap in launch Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
badai-nguyen · Jan 6, 2022 · 7de6e2b · 7de6e2b
1 parent 6943ad5
commit 7de6e2b
Show file tree

Hide file tree

Showing 3 changed files with 24 additions and 2 deletions.
diff --git a/...e_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/...e_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp
@@ -38,6 +38,7 @@
 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
 #include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
 #include "autoware_auto_vehicle_msgs/msg/control_mode_report.hpp"
+#include "autoware_auto_vehicle_msgs/msg/engage.hpp"
 #include "autoware_auto_vehicle_msgs/msg/gear_command.hpp"
 #include "autoware_auto_vehicle_msgs/msg/gear_report.hpp"
 #include "autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp"
@@ -67,6 +68,7 @@ using autoware_auto_control_msgs::msg::AckermannControlCommand;
 using autoware_auto_geometry_msgs::msg::Complex32;
 using autoware_auto_planning_msgs::msg::Trajectory;
 using autoware_auto_vehicle_msgs::msg::ControlModeReport;
+using autoware_auto_vehicle_msgs::msg::Engage;
 using autoware_auto_vehicle_msgs::msg::GearCommand;
 using autoware_auto_vehicle_msgs::msg::GearReport;
 using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
@@ -140,6 +142,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
   rclcpp::Subscription<AckermannControlCommand>::SharedPtr sub_ackermann_cmd_;
   rclcpp::Subscription<PoseWithCovarianceStamped>::SharedPtr sub_init_pose_;
   rclcpp::Subscription<Trajectory>::SharedPtr sub_trajectory_;
+  rclcpp::Subscription<Engage>::SharedPtr sub_engage_;
 
   rclcpp::CallbackGroup::SharedPtr group_api_service_;
   tier4_api_utils::Service<InitializePose>::SharedPtr srv_set_pose_;
@@ -165,6 +168,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
   TurnIndicatorsCommand::ConstSharedPtr current_turn_indicators_cmd_ptr_;
   HazardLightsCommand::ConstSharedPtr current_hazard_lights_cmd_ptr_;
   Trajectory::ConstSharedPtr current_trajectory_ptr_;
+  bool current_engage_;
 
   /* frame_id */
   std::string simulated_frame_id_;  //!< @brief simulated vehicle frame id
@@ -239,6 +243,11 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
    */
   void on_trajectory(const Trajectory::ConstSharedPtr msg);
 
+  /**
+   * @brief subscribe autoware engage
+   */
+  void on_engage(const Engage::ConstSharedPtr msg);
+
   /**
    * @brief get z-position from trajectory
    * @param [in] x current x-position

diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py
@@ -62,6 +62,7 @@ def launch_setup(context, *args, **kwargs):
             ('input/turn_indicators_command', '/control/command/turn_indicators_cmd'),
             ('input/hazard_lights_command', '/control/command/hazard_lights_cmd'),
             ('input/trajectory', '/planning/scenario_planning/trajectory'),
+            ('input/engage', '/vehicle/engage'),
             ('output/twist', '/vehicle/status/velocity_status'),
             ('output/odometry', '/localization/kinematic_state'),
             ('output/steering', '/vehicle/status/steering_status'),

diff --git a/...imple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/...imple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
@@ -76,7 +76,8 @@ namespace simple_planning_simulator
 SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & options)
 : Node("simple_planning_simulator", options),
   tf_buffer_(get_clock()),
-  tf_listener_(tf_buffer_, std::shared_ptr<rclcpp::Node>(this, [](auto) {}), false)
+  tf_listener_(tf_buffer_, std::shared_ptr<rclcpp::Node>(this, [](auto) {}), false),
+  current_engage_(false)
 {
   simulated_frame_id_ = declare_parameter("simulated_frame_id", "base_link");
   origin_frame_id_ = declare_parameter("origin_frame_id", "odom");
@@ -101,6 +102,8 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt
     std::bind(&SimplePlanningSimulator::on_hazard_lights_cmd, this, _1));
   sub_trajectory_ = create_subscription<Trajectory>(
     "input/trajectory", QoS{1}, std::bind(&SimplePlanningSimulator::on_trajectory, this, _1));
+  sub_engage_ = create_subscription<Engage>(
+    "input/engage", rclcpp::QoS{1}, std::bind(&SimplePlanningSimulator::on_engage, this, _1));
 
   pub_control_mode_report_ =
     create_publisher<ControlModeReport>("output/control_mode_report", QoS{1});
@@ -357,6 +360,11 @@ void SimplePlanningSimulator::on_trajectory(const Trajectory::ConstSharedPtr msg
   current_trajectory_ptr_ = msg;
 }
 
+void SimplePlanningSimulator::on_engage(const Engage::ConstSharedPtr msg)
+{
+  current_engage_ = msg->engage;
+}
+
 void SimplePlanningSimulator::add_measurement_noise(
   Odometry & odom, VelocityReport & vel, SteeringReport & steer) const
 {
@@ -489,7 +497,11 @@ void SimplePlanningSimulator::publish_control_mode_report()
 {
   ControlModeReport msg;
   msg.stamp = get_clock()->now();
-  msg.mode = ControlModeReport::AUTONOMOUS;
+  if (current_engage_) {
+    msg.mode = ControlModeReport::AUTONOMOUS;
+  } else {
+    msg.mode = ControlModeReport::MANUAL;
+  }
   pub_control_mode_report_->publish(msg);
 }