diff --git a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp index 9c4c761fec7fd..f034e3b561d4a 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp +++ b/control/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 is_start_requested_; bool is_calling_set_pause_{false}; diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp index 56bffbab737b9..0f82a2b8aec61 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp +++ b/control/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(); } 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(); 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; }); }