Skip to content

Commit

Permalink
style(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
@pre-commit-ci
pre-commit-ci[bot] committed Jan 14, 2025
1 parent b57d637 commit ea35da2
Show file tree
Hide file tree
Showing 2 changed files with 40 additions and 25 deletions.
53 changes: 33 additions & 20 deletions system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,9 +20,7 @@
namespace autoware::redundancy_relay_manager
{
RedundancyRelayManager::RedundancyRelayManager(const rclcpp::NodeOptions & options)
: Node("redundancy_relay_manager", options),
is_relaying_(true),
is_stopped_by_main_(true)
: Node("redundancy_relay_manager", options), is_relaying_(true), is_stopped_by_main_(true)
{
// Params
node_param_.service_timeout_ms = declare_parameter<int>("service_timeout_ms");
Expand All @@ -37,17 +35,19 @@ RedundancyRelayManager::RedundancyRelayManager(const rclcpp::NodeOptions & optio
sub_sub_election_status_ = create_subscription<tier4_system_msgs::msg::ElectionStatus>(
"~/input/sub/election/status", rclcpp::QoS{1},
std::bind(&RedundancyRelayManager::onSubElectionStatus, this, std::placeholders::_1));

// Clients
client_relay_trajectory_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
client_relay_trajectory_group_ =
create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
client_relay_trajectory_ = create_client<tier4_system_msgs::srv::ChangeTopicRelayControl>(
"~/output/topic_relay_controller_trajectory/operate", rmw_qos_profile_services_default,
client_relay_trajectory_group_);
client_relay_pose_with_covariance_group_ =
create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
client_relay_pose_with_covariance_ = create_client<tier4_system_msgs::srv::ChangeTopicRelayControl>(
"~/output/topic_relay_controller_pose_with_covariance/operate", rmw_qos_profile_services_default,
client_relay_pose_with_covariance_group_);
client_relay_pose_with_covariance_ =
create_client<tier4_system_msgs::srv::ChangeTopicRelayControl>(
"~/output/topic_relay_controller_pose_with_covariance/operate",
rmw_qos_profile_services_default, client_relay_pose_with_covariance_group_);
}

void RedundancyRelayManager::onOperationModeState(
Expand All @@ -64,17 +64,21 @@ void RedundancyRelayManager::onMainElectionStatus(

if (is_relaying_) {
if (tmp_election_status == nullptr || operation_mode_state_ == nullptr) return;
if (operation_mode_state_->mode != autoware_adapi_v1_msgs::msg::OperationModeState::AUTONOMOUS) return;
if (((msg->path_info >> 3) & 0x01) == 1 || ((tmp_election_status->path_info >> 3) & 0x01) == 0) return;
if (operation_mode_state_->mode != autoware_adapi_v1_msgs::msg::OperationModeState::AUTONOMOUS)
return;
if (((msg->path_info >> 3) & 0x01) == 1 || ((tmp_election_status->path_info >> 3) & 0x01) == 0)
return;

requestTopicRelayControl(false, client_relay_trajectory_, "topic_relay_control_trajectory");
requestTopicRelayControl(false, client_relay_pose_with_covariance_, "topic_relay_control_pose_with_covariance");
requestTopicRelayControl(
false, client_relay_pose_with_covariance_, "topic_relay_control_pose_with_covariance");
is_relaying_ = false;
is_stopped_by_main_ = true;
} else {
if (((msg->path_info >> 3) & 0x01) == 1 && is_stopped_by_main_) {
requestTopicRelayControl(true, client_relay_trajectory_, "topic_relay_control_trajectory");
requestTopicRelayControl(true, client_relay_pose_with_covariance_, "topic_relay_control_pose_with_covariance");
requestTopicRelayControl(
true, client_relay_pose_with_covariance_, "topic_relay_control_pose_with_covariance");
is_relaying_ = true;
}
}
Expand All @@ -88,24 +92,28 @@ void RedundancyRelayManager::onSubElectionStatus(

if (is_relaying_) {
if (tmp_election_status == nullptr || operation_mode_state_ == nullptr) return;
if (operation_mode_state_->mode != autoware_adapi_v1_msgs::msg::OperationModeState::AUTONOMOUS) return;
if (((msg->path_info >> 3) & 0x01) == 1 || ((tmp_election_status->path_info >> 3) & 0x01) == 0) return;
if (operation_mode_state_->mode != autoware_adapi_v1_msgs::msg::OperationModeState::AUTONOMOUS)
return;
if (((msg->path_info >> 3) & 0x01) == 1 || ((tmp_election_status->path_info >> 3) & 0x01) == 0)
return;

requestTopicRelayControl(false, client_relay_trajectory_, "topic_relay_control_trajectory");
requestTopicRelayControl(false, client_relay_pose_with_covariance_, "topic_relay_control_pose_with_covariance");
requestTopicRelayControl(
false, client_relay_pose_with_covariance_, "topic_relay_control_pose_with_covariance");
is_relaying_ = false;
is_stopped_by_main_ = false;
} else {
if (((msg->path_info >> 3) & 0x01) == 1 && !is_stopped_by_main_) {
requestTopicRelayControl(true, client_relay_trajectory_, "topic_relay_control_trajectory");
requestTopicRelayControl(true, client_relay_pose_with_covariance_, "topic_relay_control_pose_with_covariance");
requestTopicRelayControl(
true, client_relay_pose_with_covariance_, "topic_relay_control_pose_with_covariance");
is_relaying_ = true;
}
}
}

void RedundancyRelayManager::requestTopicRelayControl(
const bool relay_on,
const bool relay_on,
rclcpp::Client<tier4_system_msgs::srv::ChangeTopicRelayControl>::SharedPtr client,
std::string srv_name)
{
Expand All @@ -118,12 +126,17 @@ void RedundancyRelayManager::requestTopicRelayControl(
if (future.wait_for(duration) == std::future_status::ready) {
auto response = future.get();
if (response->status.success) {
RCLCPP_INFO(get_logger(), "Changed %s relay control: %s", srv_name.c_str(), relay_on ? "ON" : "OFF");
RCLCPP_INFO(
get_logger(), "Changed %s relay control: %s", srv_name.c_str(), relay_on ? "ON" : "OFF");
} else {
RCLCPP_ERROR(get_logger(), "Failed to change %s relay control: %s", srv_name.c_str(), relay_on ? "ON" : "OFF");
RCLCPP_ERROR(
get_logger(), "Failed to change %s relay control: %s", srv_name.c_str(),
relay_on ? "ON" : "OFF");
}
} else {
RCLCPP_ERROR(get_logger(), "Service timeout %s relay control: %s", srv_name.c_str(), relay_on ? "ON" : "OFF");
RCLCPP_ERROR(
get_logger(), "Service timeout %s relay control: %s", srv_name.c_str(),
relay_on ? "ON" : "OFF");
}
}
} // namespace autoware::redundancy_relay_manager
Expand Down
12 changes: 7 additions & 5 deletions system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,13 +47,14 @@ class RedundancyRelayManager : public rclcpp::Node

// Clients
rclcpp::CallbackGroup::SharedPtr client_relay_trajectory_group_;
rclcpp::Client<tier4_system_msgs::srv::ChangeTopicRelayControl>::SharedPtr client_relay_trajectory_;
rclcpp::Client<tier4_system_msgs::srv::ChangeTopicRelayControl>::SharedPtr
client_relay_trajectory_;
rclcpp::CallbackGroup::SharedPtr client_relay_pose_with_covariance_group_;
rclcpp::Client<tier4_system_msgs::srv::ChangeTopicRelayControl>::SharedPtr client_relay_pose_with_covariance_;
rclcpp::Client<tier4_system_msgs::srv::ChangeTopicRelayControl>::SharedPtr
client_relay_pose_with_covariance_;

// Callbacks
void onOperationModeState(
const autoware_adapi_v1_msgs::msg::OperationModeState::SharedPtr msg);
void onOperationModeState(const autoware_adapi_v1_msgs::msg::OperationModeState::SharedPtr msg);
void onMainElectionStatus(const tier4_system_msgs::msg::ElectionStatus::SharedPtr msg);
void onSubElectionStatus(const tier4_system_msgs::msg::ElectionStatus::SharedPtr msg);

Expand All @@ -66,7 +67,8 @@ class RedundancyRelayManager : public rclcpp::Node

// Functions
void requestTopicRelayControl(
const bool relay_on, const rclcpp::Client<tier4_system_msgs::srv::ChangeTopicRelayControl>::SharedPtr client,
const bool relay_on,
const rclcpp::Client<tier4_system_msgs::srv::ChangeTopicRelayControl>::SharedPtr client,
std::string topic_name);
};
} // namespace autoware::redundancy_relay_manager
Expand Down

0 comments on commit ea35da2

Please sign in to comment.