pause added

control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp

@@ -15,6 +15,8 @@
 #ifndef PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_
 #define PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_
 
+#include <component_interface_specs/control.hpp>
+#include <component_interface_utils/rclcpp.hpp>
 #include <diagnostic_updater/diagnostic_updater.hpp>
 #include <motion_utils/trajectory/tmp_conversion.hpp>
 #include <motion_utils/trajectory/trajectory.hpp>
@@ -85,6 +87,8 @@ class PredictedPathCheckerNode : public rclcpp::Node
   explicit PredictedPathCheckerNode(const rclcpp::NodeOptions & node_options);
 
 private:
+  rclcpp::CallbackGroup::SharedPtr group_cli_;
+
   // Subscriber
   std::shared_ptr<tier4_autoware_utils::SelfPoseListener> self_pose_listener_;
   std::shared_ptr<tier4_autoware_utils::TransformListener> transform_listener_;
@@ -96,6 +100,12 @@ class PredictedPathCheckerNode : public rclcpp::Node
     sub_predicted_trajectory_;
   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
   rclcpp::Subscription<geometry_msgs::msg::AccelWithCovarianceStamped>::SharedPtr sub_accel_;
+  component_interface_utils::Subscription<control_interface::IsPaused>::SharedPtr sub_is_paused_;
+  component_interface_utils::Subscription<control_interface::IsStartRequested>::SharedPtr
+    sub_is_start_requested_;
+
+  // Service
+  component_interface_utils::Client<control_interface::SetPause>::SharedPtr cli_set_pause_;
 
   // Data Buffer
   geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_;
@@ -111,13 +121,19 @@ class PredictedPathCheckerNode : public rclcpp::Node
   // Variables
   State current_state_{State::DRIVE};
   vehicle_info_util::VehicleInfo vehicle_info_;
+  std::optional<bool> is_paused_;
+  std::optional<bool> is_start_requested_;
+  bool is_calling_set_pause_{false};
 
   // Callback
   void onObstaclePointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg);
   void onReferenceTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg);
   void onPredictedTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg);
   void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg);
   void onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg);
+  void on_is_start_requested(
+    const control_interface::IsStartRequested::Message::ConstSharedPtr msg);
+  void on_is_paused(const control_interface::IsPaused::Message::ConstSharedPtr msg);
 
   // Publisher
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr virtual_wall_publisher_;
@@ -137,6 +153,7 @@ class PredictedPathCheckerNode : public rclcpp::Node
   void publishVirtualWall(boost::optional<collision_checker::Output> & output);
   TrajectoryPoints trimTrajectoryFromSelfPose(
     const TrajectoryPoints & input, const Pose & self_pose);
+  void sendRequest(bool make_stop_vehicle);
 
   // Parameter
   collision_checker::collisionCheckerParam collision_checker_param_;

control/predicted_path_checker/package.xml

@@ -29,7 +29,10 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>tier4_autoware_utils</depend>
+  <depend>tier4_control_msgs</depend>
   <depend>vehicle_info_util</depend>
+  <depend>component_interface_specs</depend>
+  <depend>component_interface_utils</depend>
 
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>

control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp

@@ -32,6 +32,11 @@ PredictedPathCheckerNode::PredictedPathCheckerNode(const rclcpp::NodeOptions & n
 : Node("predicted_path_checker_node", node_options), updater_(this)
 {
   using std::placeholders::_1;
+  const auto adaptor = component_interface_utils::NodeAdaptor(this);
+  group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  adaptor.init_cli(cli_set_pause_, group_cli_);
+  adaptor.init_sub(sub_is_paused_, this, &PredictedPathCheckerNode::on_is_paused);
+  adaptor.init_sub(sub_is_start_requested_, this, &PredictedPathCheckerNode::on_is_start_requested);
 
   // Node Parameter
   node_param_.update_rate = declare_parameter("update_rate", 10.0);
@@ -167,6 +172,13 @@ bool PredictedPathCheckerNode::isDataReady()
     return false;
   }
 
+  if (!is_paused_ || !is_start_requested_ || !cli_set_pause_->service_is_ready()) {
+    RCLCPP_INFO_THROTTLE(
+      this->get_logger(), *this->get_clock(), 5000 /* ms */,
+      "waiting for pause interface service...");
+    return false;
+  }
+
   return true;
 }
 
@@ -220,6 +232,18 @@ void PredictedPathCheckerNode::onTimer()
   auto collision_checker_output = collision_checker_->run(collision_checker_input);
 
   updateState(collision_checker_output);
+
+  // send pause request
+  if (current_state_ == State::PAUSE) {
+    if (!is_paused_ || is_start_requested_) {
+      sendRequest(true);
+    }
+  } else if (current_state_ == State::DRIVE) {
+    if (is_paused_) {
+      sendRequest(false);
+    }
+  }
+
   publishVirtualWall(collision_checker_output);
 }
 
@@ -323,6 +347,27 @@ void PredictedPathCheckerNode::publishVirtualWall(
   virtual_wall_publisher_->publish(msg);
 }
 
+void PredictedPathCheckerNode::on_is_paused(
+  const control_interface::IsPaused::Message::ConstSharedPtr msg)
+{
+  is_paused_ = msg->data;
+}
+
+void PredictedPathCheckerNode::on_is_start_requested(
+  const control_interface::IsStartRequested::Message::ConstSharedPtr msg)
+{
+  is_start_requested_ = msg->data;
+}
+
+void PredictedPathCheckerNode::sendRequest(bool pause)
+{
+  if (!is_calling_set_pause_ && cli_set_pause_->service_is_ready()) {
+    const auto req = std::make_shared<control_interface::SetPause::Service::Request>();
+    req->pause = pause;
+    is_calling_set_pause_ = true;
+    cli_set_pause_->async_send_request(req, [this](auto) { is_calling_set_pause_ = false; });
+  }
+}
 }  // namespace predicted_path_checker
 
 #include <rclcpp_components/register_node_macro.hpp>