update

autowarefoundation · Feb 1, 2023 · 5d94ee6 · 5d94ee6
1 parent 83b713c
commit 5d94ee6
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 7 deletions.
diff --git a/...rol/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp b/...rol/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp
@@ -121,7 +121,7 @@ class PredictedPathCheckerNode : public rclcpp::Node
   // Variables
   State current_state_{State::DRIVE};
   vehicle_info_util::VehicleInfo vehicle_info_;
-  tier4_control_msgs::msg::PauseState pause_state_;
+  tier4_control_msgs::msg::PauseState::SharedPtr current_pause_state_;
   std::optional<bool> is_start_requested_;
   bool is_calling_set_pause_{false};
 

diff --git a/...ol/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp b/...ol/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp
@@ -35,7 +35,7 @@ PredictedPathCheckerNode::PredictedPathCheckerNode(const rclcpp::NodeOptions & n
   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_pause_state_, this, &PredictedPathCheckerNode::on_is_paused);
   adaptor.init_sub(sub_is_start_requested_, this, &PredictedPathCheckerNode::on_is_start_requested);
 
   // Node Parameter
@@ -99,7 +99,7 @@ PredictedPathCheckerNode::PredictedPathCheckerNode(const rclcpp::NodeOptions & n
   // Timer
   initTimer(1.0 / node_param_.update_rate);
 
-  pause_state_.state = tier4_control_msgs::msg::PauseState::DRIVE;
+  current_pause_state_ = std::make_shared<tier4_control_msgs::msg::PauseState>();
 }
 
 void PredictedPathCheckerNode::onObstaclePointcloud(
@@ -174,7 +174,7 @@ bool PredictedPathCheckerNode::isDataReady()
     return false;
   }
 
-  if (!is_paused_ || !is_start_requested_ || !cli_set_pause_->service_is_ready()) {
+  if (!current_pause_state_ || !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...");
@@ -237,11 +237,14 @@ void PredictedPathCheckerNode::onTimer()
 
   // send pause request
   if (current_state_ == State::PAUSE) {
-    if (!*is_paused_ || *is_start_requested_) {
+    if (
+      current_pause_state_->state == tier4_control_msgs::msg::PauseState::DRIVE) {
       sendRequest(true);
     }
   } else if (current_state_ == State::DRIVE) {
-    if (*is_paused_ && *is_start_requested_) {
+    if (
+      current_pause_state_->state != tier4_control_msgs::msg::PauseState::DRIVE &&
+      *is_start_requested_) {
       sendRequest(false);
     }
   }
@@ -352,7 +355,7 @@ void PredictedPathCheckerNode::publishVirtualWall(
 void PredictedPathCheckerNode::on_is_paused(
   const control_interface::PauseState::Message::ConstSharedPtr msg)
 {
-  is_paused_ = msg->pause_state.state;
+  *current_pause_state_ = msg->pause_state;
 }
 
 void PredictedPathCheckerNode::on_is_start_requested(
@@ -366,6 +369,7 @@ 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;
+    req->pause_type.type = tier4_control_msgs::msg::SetPauseType::URGENT;
     is_calling_set_pause_ = true;
     cli_set_pause_->async_send_request(req, [this](auto) { is_calling_set_pause_ = false; });
   }