diff --git a/system/redundant_autoware_state_checker/CMakeLists.txt b/system/redundant_autoware_state_checker/CMakeLists.txt index 02da221690b7c..0634e51ee4eb9 100644 --- a/system/redundant_autoware_state_checker/CMakeLists.txt +++ b/system/redundant_autoware_state_checker/CMakeLists.txt @@ -11,4 +11,4 @@ ament_auto_add_executable(redundant_autoware_state_checker ament_auto_package(INSTALL_TO_SHARE launch config -) \ No newline at end of file +) diff --git a/system/redundant_autoware_state_checker/README.md b/system/redundant_autoware_state_checker/README.md index 76b5f4ec2cc07..230a24ebcc00d 100644 --- a/system/redundant_autoware_state_checker/README.md +++ b/system/redundant_autoware_state_checker/README.md @@ -1 +1 @@ -# redundant_autoware_state_checker \ No newline at end of file +# redundant_autoware_state_checker diff --git a/system/redundant_autoware_state_checker/config/redundant_autoware_state_checker.param.yaml b/system/redundant_autoware_state_checker/config/redundant_autoware_state_checker.param.yaml index 89f82f1024545..1a3d3e2fb6d00 100644 --- a/system/redundant_autoware_state_checker/config/redundant_autoware_state_checker.param.yaml +++ b/system/redundant_autoware_state_checker/config/redundant_autoware_state_checker.param.yaml @@ -2,4 +2,4 @@ ros__parameters: states_equality_timeout: 1.0 update_rate_hz: 10.0 - pose_distance_threshold: 1.0 \ No newline at end of file + pose_distance_threshold: 1.0 diff --git a/system/redundant_autoware_state_checker/launch/redundant_autoware_state_checker.launch.xml b/system/redundant_autoware_state_checker/launch/redundant_autoware_state_checker.launch.xml index dcf53dfb8590b..14ff5756644c7 100644 --- a/system/redundant_autoware_state_checker/launch/redundant_autoware_state_checker.launch.xml +++ b/system/redundant_autoware_state_checker/launch/redundant_autoware_state_checker.launch.xml @@ -3,4 +3,4 @@ - \ No newline at end of file + diff --git a/system/redundant_autoware_state_checker/package.xml b/system/redundant_autoware_state_checker/package.xml index 0947e862175de..3f4ea7aefc4de 100644 --- a/system/redundant_autoware_state_checker/package.xml +++ b/system/redundant_autoware_state_checker/package.xml @@ -20,4 +20,4 @@ ament_cmake - \ No newline at end of file + diff --git a/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.cpp b/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.cpp index 6e37a504435b2..75fdcca09d5be 100644 --- a/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.cpp +++ b/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.cpp @@ -17,15 +17,16 @@ namespace redundant_autoware_state_checker { -RedundantAutowareStateChecker::RedundantAutowareStateChecker() : Node("RedundantAutowareStateChecker") +RedundantAutowareStateChecker::RedundantAutowareStateChecker() +: Node("RedundantAutowareStateChecker") { using std::placeholders::_1; // Params - states_equality_timeout = - this->declare_parameter("states_equality_timeout", 1.0); // 1.0s - update_rate_hz = this->declare_parameter("update_rate_hz", 10.0); // 10hz - pose_distance_threshold = this->declare_parameter("pose_distance_threshold", 1.0); // 1.0m + states_equality_timeout = this->declare_parameter("states_equality_timeout", 1.0); // 1.0s + update_rate_hz = this->declare_parameter("update_rate_hz", 10.0); // 10hz + pose_distance_threshold = + this->declare_parameter("pose_distance_threshold", 1.0); // 1.0m // Publishers pub_diagnostic = create_publisher( @@ -35,24 +36,31 @@ RedundantAutowareStateChecker::RedundantAutowareStateChecker() : Node("Redundant sub_overall_mrm_state = create_subscription( "/supervisor/fail_safe/overall/mrm_state", rclcpp::QoS(1), std::bind(&RedundantAutowareStateChecker::on_mrm_state, this, _1)); - sub_main_pose_with_covariance = create_subscription( - "/main/localization/pose_with_covariance", rclcpp::QoS(1), - std::bind(&RedundantAutowareStateChecker::on_main_pose_with_covariance, this, _1)); + sub_main_pose_with_covariance = + create_subscription( + "/main/localization/pose_with_covariance", rclcpp::QoS(1), + std::bind(&RedundantAutowareStateChecker::on_main_pose_with_covariance, this, _1)); sub_sub_pose_with_covariance = create_subscription( "/sub/localization/pose_with_covariance", rclcpp::QoS(1), std::bind(&RedundantAutowareStateChecker::on_sub_pose_with_covariance, this, _1)); - sub_main_operation_mode_state = create_subscription( - "/main/api/operation_mode/state", rclcpp::QoS(1), - std::bind(&RedundantAutowareStateChecker::on_main_operation_mode_state, this, _1)); - sub_sub_operation_mode_state = create_subscription( - "/sub/api/operation_mode/state", rclcpp::QoS(1), - std::bind(&RedundantAutowareStateChecker::on_sub_operation_mode_state, this, _1)); - sub_main_localization_initialization_state = create_subscription( - "/main/api/localization/initialization_state", rclcpp::QoS(1), - std::bind(&RedundantAutowareStateChecker::on_main_localization_initialization_state, this, _1)); - sub_sub_localization_initialization_state = create_subscription( - "/sub/api/localization/initialization_state", rclcpp::QoS(1), - std::bind(&RedundantAutowareStateChecker::on_sub_localization_initialization_state, this, _1)); + sub_main_operation_mode_state = + create_subscription( + "/main/api/operation_mode/state", rclcpp::QoS(1), + std::bind(&RedundantAutowareStateChecker::on_main_operation_mode_state, this, _1)); + sub_sub_operation_mode_state = + create_subscription( + "/sub/api/operation_mode/state", rclcpp::QoS(1), + std::bind(&RedundantAutowareStateChecker::on_sub_operation_mode_state, this, _1)); + sub_main_localization_initialization_state = + create_subscription( + "/main/api/localization/initialization_state", rclcpp::QoS(1), + std::bind( + &RedundantAutowareStateChecker::on_main_localization_initialization_state, this, _1)); + sub_sub_localization_initialization_state = + create_subscription( + "/sub/api/localization/initialization_state", rclcpp::QoS(1), + std::bind( + &RedundantAutowareStateChecker::on_sub_localization_initialization_state, this, _1)); sub_main_route_state = create_subscription( "/main/api/routing/state", rclcpp::QoS(1), std::bind(&RedundantAutowareStateChecker::on_main_route_state, this, _1)); @@ -68,7 +76,8 @@ RedundantAutowareStateChecker::RedundantAutowareStateChecker() : Node("Redundant // Timer const auto period_ns = rclcpp::Rate(update_rate_hz).period(); - timer_ = rclcpp::create_timer(this, get_clock(), period_ns, std::bind(&RedundantAutowareStateChecker::onTimer, this)); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&RedundantAutowareStateChecker::onTimer, this)); // Variables is_autonomous = true; @@ -82,9 +91,9 @@ RedundantAutowareStateChecker::RedundantAutowareStateChecker() : Node("Redundant void RedundantAutowareStateChecker::on_mrm_state( const autoware_adapi_v1_msgs::msg::MrmState::ConstSharedPtr msg) { - is_autonomous = ( - msg->state == autoware_adapi_v1_msgs::msg::MrmState::NORMAL && - msg->behavior == autoware_adapi_v1_msgs::msg::MrmState::NONE); + is_autonomous = + (msg->state == autoware_adapi_v1_msgs::msg::MrmState::NORMAL && + msg->behavior == autoware_adapi_v1_msgs::msg::MrmState::NONE); } void RedundantAutowareStateChecker::on_main_pose_with_covariance( @@ -197,9 +206,12 @@ void RedundantAutowareStateChecker::onTimer() if (is_equal_localization_initialization_state()) { last_time_localization_initialization_state_is_equal = now; } else { - if ((now - last_time_localization_initialization_state_is_equal).seconds() > states_equality_timeout) { + if ( + (now - last_time_localization_initialization_state_is_equal).seconds() > + states_equality_timeout) { diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - diag_status_msg.message += "Main and Sub ECUs' localization initialization states are different."; + diag_status_msg.message += + "Main and Sub ECUs' localization initialization states are different."; } } @@ -252,7 +264,10 @@ bool RedundantAutowareStateChecker::is_equal_operation_mode_stete() // Currently, following menbers are used to check the equality if (main_operation_mode_state->mode != sub_operation_mode_state->mode) return false; - if (main_operation_mode_state->is_autoware_control_enabled != sub_operation_mode_state->is_autoware_control_enabled) return false; + if ( + main_operation_mode_state->is_autoware_control_enabled != + sub_operation_mode_state->is_autoware_control_enabled) + return false; return true; } @@ -264,7 +279,9 @@ bool RedundantAutowareStateChecker::is_equal_localization_initialization_state() if (!has_subscribed_sub_localization_initialization_state) return true; // Currently, following menbers are used to check the equality - if (main_localization_initialization_state->state != main_localization_initialization_state->state) return false; + if ( + main_localization_initialization_state->state != main_localization_initialization_state->state) + return false; return true; } @@ -293,8 +310,7 @@ bool RedundantAutowareStateChecker::is_equal_route() return true; } -} // namespace redundant_autoware_state_checker - +} // namespace redundant_autoware_state_checker int main(int argc, char ** argv) { @@ -306,4 +322,4 @@ int main(int argc, char ** argv) executor.remove_node(node); rclcpp::shutdown(); return 0; -} \ No newline at end of file +} diff --git a/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.hpp b/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.hpp index 00988eae171c4..bf9d665436feb 100644 --- a/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.hpp +++ b/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef REDUNDANT_AUTOWARE_STATE_CHECKER_ -#define REDUNDANT_AUTOWARE_STATE_CHECKER_ +#ifndef REDUNDANT_AUTOWARE_STATE_CHECKER_HPP_ +#define REDUNDANT_AUTOWARE_STATE_CHECKER_HPP_ -#include +#include "rclcpp/rclcpp.hpp" + +#include #include -#include #include -#include -#include #include - -#include "rclcpp/rclcpp.hpp" +#include +#include +#include namespace redundant_autoware_state_checker { @@ -36,29 +36,41 @@ class RedundantAutowareStateChecker : public rclcpp::Node private: // Publishers rclcpp::Publisher::SharedPtr pub_diagnostic; - + // Subscribers rclcpp::Subscription::SharedPtr sub_overall_mrm_state; - rclcpp::Subscription::SharedPtr sub_main_pose_with_covariance; - rclcpp::Subscription::SharedPtr sub_sub_pose_with_covariance; - rclcpp::Subscription::SharedPtr sub_main_operation_mode_state; - rclcpp::Subscription::SharedPtr sub_sub_operation_mode_state; - rclcpp::Subscription::SharedPtr sub_main_localization_initialization_state; - rclcpp::Subscription::SharedPtr sub_sub_localization_initialization_state; + rclcpp::Subscription::SharedPtr + sub_main_pose_with_covariance; + rclcpp::Subscription::SharedPtr + sub_sub_pose_with_covariance; + rclcpp::Subscription::SharedPtr + sub_main_operation_mode_state; + rclcpp::Subscription::SharedPtr + sub_sub_operation_mode_state; + rclcpp::Subscription::SharedPtr + sub_main_localization_initialization_state; + rclcpp::Subscription::SharedPtr + sub_sub_localization_initialization_state; rclcpp::Subscription::SharedPtr sub_main_route_state; rclcpp::Subscription::SharedPtr sub_sub_route_state; rclcpp::Subscription::SharedPtr sub_main_route; rclcpp::Subscription::SharedPtr sub_sub_route; - + // Callbacks void onTimer(); - void on_main_pose_with_covariance(const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg); - void on_sub_pose_with_covariance(const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg); + void on_main_pose_with_covariance( + const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg); + void on_sub_pose_with_covariance( + const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg); void on_mrm_state(const autoware_adapi_v1_msgs::msg::MrmState::ConstSharedPtr msg); - void on_main_operation_mode_state(const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg); - void on_sub_operation_mode_state(const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg); - void on_main_localization_initialization_state(const autoware_adapi_v1_msgs::msg::LocalizationInitializationState::ConstSharedPtr msg); - void on_sub_localization_initialization_state(const autoware_adapi_v1_msgs::msg::LocalizationInitializationState::ConstSharedPtr msg); + void on_main_operation_mode_state( + const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg); + void on_sub_operation_mode_state( + const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg); + void on_main_localization_initialization_state( + const autoware_adapi_v1_msgs::msg::LocalizationInitializationState::ConstSharedPtr msg); + void on_sub_localization_initialization_state( + const autoware_adapi_v1_msgs::msg::LocalizationInitializationState::ConstSharedPtr msg); void on_main_route_state(const autoware_adapi_v1_msgs::msg::RouteState::ConstSharedPtr msg); void on_sub_route_state(const autoware_adapi_v1_msgs::msg::RouteState::ConstSharedPtr msg); void on_main_route(const autoware_adapi_v1_msgs::msg::Route::ConstSharedPtr msg); @@ -83,8 +95,10 @@ class RedundantAutowareStateChecker : public rclcpp::Node geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr sub_pose_with_covariance; autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr main_operation_mode_state; autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr sub_operation_mode_state; - autoware_adapi_v1_msgs::msg::LocalizationInitializationState::ConstSharedPtr main_localization_initialization_state; - autoware_adapi_v1_msgs::msg::LocalizationInitializationState::ConstSharedPtr sub_localization_initialization_state; + autoware_adapi_v1_msgs::msg::LocalizationInitializationState::ConstSharedPtr + main_localization_initialization_state; + autoware_adapi_v1_msgs::msg::LocalizationInitializationState::ConstSharedPtr + sub_localization_initialization_state; autoware_adapi_v1_msgs::msg::RouteState::ConstSharedPtr main_route_state; autoware_adapi_v1_msgs::msg::RouteState::ConstSharedPtr sub_route_state; autoware_adapi_v1_msgs::msg::Route::ConstSharedPtr main_route; @@ -109,6 +123,6 @@ class RedundantAutowareStateChecker : public rclcpp::Node bool is_equal_route(); }; -} // namespace redundant_autoware_state_checker +} // namespace redundant_autoware_state_checker -#endif // REDUNDANT_AUTOWARE_STATE_CHECKER_ \ No newline at end of file +#endif // REDUNDANT_AUTOWARE_STATE_CHECKER_HPP_