diff --git a/system/redundant_autoware_state_checker/CMakeLists.txt b/system/redundant_autoware_state_checker/CMakeLists.txt new file mode 100644 index 0000000000000..0634e51ee4eb9 --- /dev/null +++ b/system/redundant_autoware_state_checker/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.14) +project(redundant_autoware_state_checker) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_executable(redundant_autoware_state_checker + src/redundant_autoware_state_checker.cpp +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/system/redundant_autoware_state_checker/README.md b/system/redundant_autoware_state_checker/README.md new file mode 100644 index 0000000000000..230a24ebcc00d --- /dev/null +++ b/system/redundant_autoware_state_checker/README.md @@ -0,0 +1 @@ +# 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 new file mode 100644 index 0000000000000..1a3d3e2fb6d00 --- /dev/null +++ b/system/redundant_autoware_state_checker/config/redundant_autoware_state_checker.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + states_equality_timeout: 1.0 + update_rate_hz: 10.0 + 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 new file mode 100644 index 0000000000000..9b11fd979dd8f --- /dev/null +++ b/system/redundant_autoware_state_checker/launch/redundant_autoware_state_checker.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/system/redundant_autoware_state_checker/package.xml b/system/redundant_autoware_state_checker/package.xml new file mode 100644 index 0000000000000..c78b5b2df38cb --- /dev/null +++ b/system/redundant_autoware_state_checker/package.xml @@ -0,0 +1,24 @@ + + + + redundant_autoware_state_checker + 0.1.0 + The redundant_autoware_state_checker ROS 2 package + Makoto Kurihara + Ryuta Kambe + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + diagnostic_msgs + rclcpp + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + 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 new file mode 100644 index 0000000000000..2395ada31223f --- /dev/null +++ b/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.cpp @@ -0,0 +1,332 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "redundant_autoware_state_checker.hpp" +#include + + +namespace redundant_autoware_state_checker +{ + +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 + + // Publishers + pub_diagnostic_ = create_publisher( + "/diagnostics/supervisor", rclcpp::QoS(1)); + + // Subscribers + 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_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_route_state_ = create_subscription( + "/main/api/routing/state", rclcpp::QoS(1), + std::bind(&RedundantAutowareStateChecker::on_main_route_state, this, _1)); + sub_sub_route_state_ = create_subscription( + "/sub/api/routing/state", rclcpp::QoS(1), + std::bind(&RedundantAutowareStateChecker::on_sub_route_state, this, _1)); + sub_main_route_ = create_subscription( + "/main/api/routing/route", rclcpp::QoS(1), + std::bind(&RedundantAutowareStateChecker::on_main_route, this, _1)); + sub_sub_route_ = create_subscription( + "/sub/api/routing/route", rclcpp::QoS(1), + std::bind(&RedundantAutowareStateChecker::on_sub_route, this, _1)); + + // 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)); + + // Variables + is_autonomous_ = false; + last_time_pose_with_covariance_is_equal_ = this->now(); + last_time_operation_mode_state_is_equal_ = this->now(); + last_time_localization_initialization_state_is_equal_ = this->now(); + last_time_route_state_is_equal_ = this->now(); + last_time_route_is_equal_ = this->now(); +} + +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); +} + +void RedundantAutowareStateChecker::on_main_pose_with_covariance( + const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg) +{ + has_subscribed_main_pose_with_covariance_ = true; + main_pose_with_covariance_ = msg; +} + +void RedundantAutowareStateChecker::on_sub_pose_with_covariance( + const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg) +{ + has_subscribed_sub_pose_with_covariance_ = true; + sub_pose_with_covariance_ = msg; +} + +void RedundantAutowareStateChecker::on_main_operation_mode_state( + const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg) +{ + has_subscribed_main_operation_mode_state_ = true; + main_operation_mode_state_ = msg; +} + +void RedundantAutowareStateChecker::on_sub_operation_mode_state( + const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg) +{ + has_subscribed_sub_operation_mode_state_ = true; + sub_operation_mode_state_ = msg; +} + +void RedundantAutowareStateChecker::on_main_localization_initialization_state( + const autoware_adapi_v1_msgs::msg::LocalizationInitializationState::ConstSharedPtr msg) +{ + has_subscribed_main_localization_initialization_state_ = true; + main_localization_initialization_state_ = msg; +} + +void RedundantAutowareStateChecker::on_sub_localization_initialization_state( + const autoware_adapi_v1_msgs::msg::LocalizationInitializationState::ConstSharedPtr msg) +{ + has_subscribed_sub_localization_initialization_state_ = true; + sub_localization_initialization_state_ = msg; +} + +void RedundantAutowareStateChecker::on_main_route_state( + const autoware_adapi_v1_msgs::msg::RouteState::ConstSharedPtr msg) +{ + has_subscribed_main_route_state_ = true; + main_route_state_ = msg; +} + +void RedundantAutowareStateChecker::on_sub_route_state( + const autoware_adapi_v1_msgs::msg::RouteState::ConstSharedPtr msg) +{ + has_subscribed_sub_route_state_ = true; + sub_route_state_ = msg; +} + +void RedundantAutowareStateChecker::on_main_route( + const autoware_adapi_v1_msgs::msg::Route::ConstSharedPtr msg) +{ + has_subscribed_main_route_ = true; + main_route_ = msg; +} + +void RedundantAutowareStateChecker::on_sub_route( + const autoware_adapi_v1_msgs::msg::Route::ConstSharedPtr msg) +{ + has_subscribed_sub_route_ = true; + sub_route_ = msg; +} + +void RedundantAutowareStateChecker::onTimer() +{ + const auto now = this->now(); + + diagnostic_msgs::msg::DiagnosticArray diag_msg; + diag_msg.header.stamp = now; + + diagnostic_msgs::msg::DiagnosticStatus diag_status_msg; + diag_status_msg.name = "redundant_autoware_state_checker"; + diag_status_msg.hardware_id = ""; + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diag_status_msg.message = ""; + + // check the equality only when is_autonomous is true + if (is_autonomous_) { + + // Check the pose with covariance equality + // Note that only this check raises WARN, not ERROR + if (is_equal_pose_with_covariance()) { + last_time_pose_with_covariance_is_equal_ = now; + } else { + if ((now - last_time_pose_with_covariance_is_equal_).seconds() > states_equality_timeout_) { + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + diag_status_msg.message += "Main and Sub ECUs' pose with covariance are different."; + } + } + + // Check the operation mode state equality + if (is_equal_operation_mode_state()) { + last_time_operation_mode_state_is_equal_ = now; + } else { + if ((now - last_time_operation_mode_state_is_equal_).seconds() > states_equality_timeout_) { + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + diag_status_msg.message += "Main and Sub ECUs' operation mode states are different."; + } + } + + // Check the localization initialization state equality + 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_) { + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + diag_status_msg.message += + "Main and Sub ECUs' localization initialization states are different."; + } + } + + // Check the route state equality + if (is_equal_route_state()) { + last_time_route_state_is_equal_ = now; + } else { + if ((now - last_time_route_state_is_equal_).seconds() > states_equality_timeout_) { + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + diag_status_msg.message += "Main and Sub ECUs' route states are different."; + } + } + + // Check the route equality + if (is_equal_route()) { + last_time_route_is_equal_ = now; + } else { + if ((now - last_time_route_is_equal_).seconds() > states_equality_timeout_) { + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + diag_status_msg.message += "Main and Sub ECUs' routes are different."; + } + } + } + + diag_msg.status.push_back(diag_status_msg); + pub_diagnostic_->publish(diag_msg); +} + +bool RedundantAutowareStateChecker::is_equal_pose_with_covariance() +{ + // If ether state is not subscribed, then assume the states are equal + if (!has_subscribed_main_pose_with_covariance_) return true; + if (!has_subscribed_sub_pose_with_covariance_) return true; + + // Currently, Main and Sub ECUs poses are used to check the equality + if (!is_close_to_each_other(main_pose_with_covariance_->pose.pose, sub_pose_with_covariance_->pose.pose)) return false; + + return true; +} + +bool RedundantAutowareStateChecker::is_equal_operation_mode_state() +{ + // If ether state is not subscribed, then assume the states are equal + if (!has_subscribed_main_operation_mode_state_) return true; + if (!has_subscribed_sub_operation_mode_state_) return true; + + // Currently, following members 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; + + return true; +} + +bool RedundantAutowareStateChecker::is_equal_localization_initialization_state() +{ + // If ether state is not subscribed, then assume the states are equal + if (!has_subscribed_main_localization_initialization_state_) return true; + if (!has_subscribed_sub_localization_initialization_state_) return true; + + // Currently, following members are used to check the equality + if ( + main_localization_initialization_state_->state != main_localization_initialization_state_->state) + return false; + + return true; +} + +bool RedundantAutowareStateChecker::is_equal_route_state() +{ + // If ether state is not subscribed, then assume the states are equal + if (!has_subscribed_main_route_state_) return true; + if (!has_subscribed_sub_route_state_) return true; + + // Currently, following members are used to check the equality + if (main_route_state_->state != sub_route_state_->state) return false; + + return true; +} + +bool RedundantAutowareStateChecker::is_equal_route() +{ + // If ether state is not subscribed, then assume the states are equal + if (!has_subscribed_main_route_) return true; + if (!has_subscribed_sub_route_) return true; + + // Currently, following members are used to check the equality + if (!is_close_to_each_other(main_route_->data[0].start, sub_route_->data[0].start)) return false; + if (!is_close_to_each_other(main_route_->data[0].goal, sub_route_->data[0].goal)) return false; + + return true; +} + +bool RedundantAutowareStateChecker::is_close_to_each_other(geometry_msgs::msg::Pose pose1, geometry_msgs::msg::Pose pose2) { + const auto squared_distance = + (pose1.position.x - pose2.position.x) * (pose1.position.x - pose2.position.x) + + (pose1.position.y - pose2.position.y) * (pose1.position.y - pose2.position.y); + return squared_distance <= pose_distance_threshold_ * pose_distance_threshold_; +} + +} // namespace redundant_autoware_state_checker + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::executors::SingleThreadedExecutor executor; + auto node = std::make_shared(); + executor.add_node(node); + executor.spin(); + executor.remove_node(node); + rclcpp::shutdown(); + return 0; +} 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 new file mode 100644 index 0000000000000..02615ad29b9c3 --- /dev/null +++ b/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.hpp @@ -0,0 +1,130 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef REDUNDANT_AUTOWARE_STATE_CHECKER_HPP_ +#define REDUNDANT_AUTOWARE_STATE_CHECKER_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace redundant_autoware_state_checker +{ + +class RedundantAutowareStateChecker : public rclcpp::Node +{ +public: + RedundantAutowareStateChecker(); + +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_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_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_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); + void on_sub_route(const autoware_adapi_v1_msgs::msg::Route::ConstSharedPtr msg); + + // Variables + bool is_autonomous_; + rclcpp::TimerBase::SharedPtr timer_; + + bool has_subscribed_main_pose_with_covariance_ = false; + bool has_subscribed_sub_pose_with_covariance_ = false; + bool has_subscribed_main_operation_mode_state_ = false; + bool has_subscribed_sub_operation_mode_state_ = false; + bool has_subscribed_main_localization_initialization_state_ = false; + bool has_subscribed_sub_localization_initialization_state_ = false; + bool has_subscribed_main_route_state_ = false; + bool has_subscribed_sub_route_state_ = false; + bool has_subscribed_main_route_ = false; + bool has_subscribed_sub_route_ = false; + + geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr main_pose_with_covariance_; + 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::RouteState::ConstSharedPtr main_route_state_; + autoware_adapi_v1_msgs::msg::RouteState::ConstSharedPtr sub_route_state_; + autoware_adapi_v1_msgs::msg::Route::ConstSharedPtr main_route_; + autoware_adapi_v1_msgs::msg::Route::ConstSharedPtr sub_route_; + + rclcpp::Time last_time_pose_with_covariance_is_equal_; + rclcpp::Time last_time_operation_mode_state_is_equal_; + rclcpp::Time last_time_localization_initialization_state_is_equal_; + rclcpp::Time last_time_route_state_is_equal_; + rclcpp::Time last_time_route_is_equal_; + + // Params + double states_equality_timeout_; + double update_rate_hz_; + double pose_distance_threshold_; + + // Functions + bool is_equal_pose_with_covariance(); + bool is_equal_operation_mode_state(); + bool is_equal_localization_initialization_state(); + bool is_equal_route_state(); + bool is_equal_route(); + + bool is_close_to_each_other(geometry_msgs::msg::Pose pose1, geometry_msgs::msg::Pose pose2); +}; + +} // namespace redundant_autoware_state_checker + +#endif // REDUNDANT_AUTOWARE_STATE_CHECKER_HPP_