Skip to content

Commit

Permalink
update
Browse files Browse the repository at this point in the history
  • Loading branch information
brkay54 committed Dec 24, 2022
1 parent 9de0ad6 commit ee5e59a
Show file tree
Hide file tree
Showing 2 changed files with 11 additions and 7 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -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};

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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...");
Expand Down Expand Up @@ -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);
}
}
Expand Down Expand Up @@ -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(
Expand All @@ -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; });
}
Expand Down

0 comments on commit ee5e59a

Please sign in to comment.