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_