Skip to content

Commit

Permalink
heartbeat tests pass
Browse files Browse the repository at this point in the history 
Signed-off-by: M. Fatih Cırıt <[email protected]>
  • Loading branch information
M. Fatih Cırıt committed May 31, 2024
1 parent 4ba9eb6 commit b3978bc
Show file tree
Hide file tree
Showing 4 changed files with 58 additions and 37 deletions.
1 change: 1 addition & 0 deletions common/autoware_control_center/src/control_center_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -166,6 +166,7 @@ void ControlCenter::on_heartbeat(
node_info->report.stamp_last_heartbeat = heartbeat->stamp;
node_info->report.status_activity = heartbeat->status_activity;
node_info->report.status_operational = heartbeat->status_operational;
node_info->report.is_alive = true;
}

void ControlCenter::on_deadline_missed(
Expand Down
27 changes: 17 additions & 10 deletions common/autoware_control_center/test/test_cc_heartbeat.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ TEST_F(ControlCenterHeartbeatTest, HeartbeatHandling)
RCLCPP_INFO(rclcpp::get_logger("test"), "Test started");
const std::string node_name = "node_test_heartbeat";
const std::string ns;
const auto [uuid, node_fully_qualified_name] = test::register_node(node_name, ns);
const auto [uuid, node] = test::register_node(node_name, ns);

// Send a heartbeat and verify node status is updated
autoware_control_center_msgs::msg::NodeStatusActivity status_activity;
Expand All @@ -73,17 +73,17 @@ TEST_F(ControlCenterHeartbeatTest, HeartbeatHandling)
status_operational.status = autoware_control_center_msgs::msg::NodeStatusOperational::NORMAL;

RCLCPP_INFO(rclcpp::get_logger("test"), "Sending heartbeat");
test::send_heartbeat(node_name, ns, deadline_ms_, status_activity, status_operational);
auto pub_hb = test::send_first_heartbeat(node, deadline_ms_, status_activity, status_operational);
RCLCPP_INFO(rclcpp::get_logger("test"), "Sent heartbeat");
test::send_heartbeat(node_name, ns, deadline_ms_, status_activity, status_operational);
test::send_heartbeat(node, pub_hb, status_activity, status_operational);
RCLCPP_INFO(rclcpp::get_logger("test"), "Sent 2nd heartbeat");

// wait for the heartbeat to be processed
std::this_thread::sleep_for(std::chrono::duration<double, std::milli>(node_period_ms_ * 1.5));
RCLCPP_INFO(rclcpp::get_logger("test"), "Slept a bit");

autoware_control_center_msgs::msg::NodeReport report;
ASSERT_TRUE(test::wait_for_node_report(node_name, ns, report));
ASSERT_TRUE(test::wait_for_node_report(node->get_fully_qualified_name(), report));
ASSERT_EQ(report.status_activity.status, status_activity.status);
ASSERT_EQ(report.status_operational.status, status_operational.status);
}
Expand All @@ -92,14 +92,22 @@ TEST_F(ControlCenterHeartbeatTest, HeartbeatMissedDeadline)
{
const std::string node_name = "node_test_heartbeat";
const std::string ns;
const auto [uuid, node_fully_qualified_name] = test::register_node(node_name, ns);
const auto [uuid, node] = test::register_node(node_name, ns);

// Send a heartbeat
autoware_control_center_msgs::msg::NodeStatusActivity status_activity;
status_activity.status = autoware_control_center_msgs::msg::NodeStatusActivity::PROCESSING;
autoware_control_center_msgs::msg::NodeStatusOperational status_operational;
status_operational.status = autoware_control_center_msgs::msg::NodeStatusOperational::NORMAL;
auto pub_hb = test::send_first_heartbeat(node, deadline_ms_, status_activity, status_operational);

// Wait for the heartbeat deadline to be missed
std::this_thread::sleep_for(
std::chrono::duration<double, std::milli>(deadline_ms_ + node_period_ms_ * 1.5));

// Expect the node to be marked as dead
autoware_control_center_msgs::msg::NodeReport report;
ASSERT_TRUE(test::wait_for_node_report(node_name, ns, report));
ASSERT_TRUE(test::wait_for_node_report(node->get_fully_qualified_name(), report));
ASSERT_FALSE(report.is_alive);
ASSERT_EQ(
report.status_activity.status, autoware_control_center_msgs::msg::NodeStatusActivity::UNKNOWN);
Expand All @@ -108,14 +116,13 @@ TEST_F(ControlCenterHeartbeatTest, HeartbeatMissedDeadline)
autoware_control_center_msgs::msg::NodeStatusOperational::UNKNOWN);

// Send a heartbeat and verify node status is updated
autoware_control_center_msgs::msg::NodeStatusActivity status_activity;
status_activity.status = autoware_control_center_msgs::msg::NodeStatusActivity::PROCESSING;
autoware_control_center_msgs::msg::NodeStatusOperational status_operational;
status_operational.status = autoware_control_center_msgs::msg::NodeStatusOperational::NORMAL;
test::send_heartbeat(node_name, ns, deadline_ms_, status_activity, status_operational);
test::send_heartbeat(node, pub_hb, status_activity, status_operational);

// Wait shortly and expect the node to be alive
std::this_thread::sleep_for(std::chrono::duration<double, std::milli>(node_period_ms_ * 2.0));
ASSERT_TRUE(test::wait_for_node_report(node_name, ns, report));
ASSERT_TRUE(test::wait_for_node_report(node->get_fully_qualified_name(), report));
ASSERT_TRUE(report.is_alive);
ASSERT_EQ(report.status_activity.status, status_activity.status);
ASSERT_EQ(report.status_operational.status, status_operational.status);
Expand Down
14 changes: 7 additions & 7 deletions common/autoware_control_center/test/test_cc_registering.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,7 @@ TEST_F(ControlCenterRegisteringTest, NodeRegistration)
{
const std::string node_name = "node_test";
const std::string ns;
const auto [uuid, node_fully_qualified_name] = test::register_node(node_name, ns);
const auto [uuid, node] = test::register_node(node_name, ns);

// Make sure node is alive
ASSERT_EQ(
Expand All @@ -71,7 +71,7 @@ TEST_F(ControlCenterRegisteringTest, NodeRegistration)

// Make sure node is registered by subscribing to the node reports
autoware_control_center_msgs::msg::NodeReport report;
ASSERT_TRUE(test::wait_for_node_report(node_name, ns, report));
ASSERT_TRUE(test::wait_for_node_report(node->get_fully_qualified_name(), report));
ASSERT_EQ(report.uuid, uuid);
ASSERT_EQ(report.count_registered, 1);
ASSERT_TRUE(report.is_alive);
Expand All @@ -87,16 +87,16 @@ TEST_F(ControlCenterRegisteringTest, NodeDuplicateRegistration)
{
const std::string node_name = "node_test_duplicate";
const std::string ns;
const auto [uuid1, node_fully_qualified_name1] = test::register_node(node_name, ns);
const auto [uuid1, node1] = test::register_node(node_name, ns);

const auto [uuid2, node_fully_qualified_name2] = test::register_node(node_name, ns);
const auto [uuid2, node2] = test::register_node(node_name, ns);

// Ensure the UUID has changed
ASSERT_NE(uuid1, uuid2);

// Ensure the registration count incremented by subscribing to node reports
autoware_control_center_msgs::msg::NodeReport report;
ASSERT_TRUE(test::wait_for_node_report(node_name, ns, report));
ASSERT_TRUE(test::wait_for_node_report(node1->get_fully_qualified_name(), report));
ASSERT_EQ(report.count_registered, 2);
ASSERT_TRUE(report.is_alive);
ASSERT_EQ(
Expand All @@ -111,11 +111,11 @@ TEST_F(ControlCenterRegisteringTest, NodeDeregistration)
{
const std::string node_name = "node_test_deregister";
const std::string ns;
const auto [uuid, node_fully_qualified_name] = test::register_node(node_name, ns);
const auto [uuid, node] = test::register_node(node_name, ns);
ASSERT_TRUE(test::deregister_node(uuid));

// Ensure node is no longer registered by checking the node reports
ASSERT_FALSE(test::is_node_registered(node_name, ns));
ASSERT_FALSE(test::is_node_registered(node->get_fully_qualified_name()));
}

TEST_F(ControlCenterRegisteringTest, DeregisterNonExistentNode)
Expand Down
53 changes: 33 additions & 20 deletions common/autoware_control_center/test/test_utility.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@
namespace autoware::control_center::test
{

inline std::tuple<unique_identifier_msgs::msg::UUID, std::string> register_node(
inline std::tuple<unique_identifier_msgs::msg::UUID, rclcpp::Node::SharedPtr> register_node(
const std::string & node_name, const std::string & ns)
{
auto test_node = std::make_shared<rclcpp::Node>(node_name, ns);
Expand Down Expand Up @@ -61,7 +61,7 @@ inline std::tuple<unique_identifier_msgs::msg::UUID, std::string> register_node(
}
RCLCPP_INFO(rclcpp::get_logger("registering"), "Received the response");

return {result->uuid_node, request->node_name_with_namespace};
return {result->uuid_node, test_node};
}

inline bool deregister_node(const unique_identifier_msgs::msg::UUID & uuid)
Expand Down Expand Up @@ -92,22 +92,20 @@ inline bool deregister_node(const unique_identifier_msgs::msg::UUID & uuid)
}

inline bool wait_for_node_report(
const std::string & node_name, const std::string & ns,
autoware_control_center_msgs::msg::NodeReport & report)
const std::string & node_full_name, autoware_control_center_msgs::msg::NodeReport & report)
{
auto test_node = std::make_shared<rclcpp::Node>(node_name, ns);
std::promise<bool> report_received;
auto future_report = report_received.get_future();

const std::string fully_qualified_name = test_node->get_fully_qualified_name();
auto node_temp = std::make_shared<rclcpp::Node>("node_report_listener", "");

auto node_reports_sub =
test_node->create_subscription<autoware_control_center_msgs::msg::NodeReports>(
node_temp->create_subscription<autoware_control_center_msgs::msg::NodeReports>(
topic_node_reports, rclcpp::QoS(rclcpp::KeepLast(0)),
[&report_received, &report,
&fully_qualified_name](const autoware_control_center_msgs::msg::NodeReports::SharedPtr msg) {
&node_full_name](const autoware_control_center_msgs::msg::NodeReports::SharedPtr msg) {
for (const auto & node_report : msg->reports) {
if (node_report.name == fully_qualified_name) {
if (node_report.name == node_full_name) {
report = node_report;
report_received.set_value(true);
return;
Expand All @@ -117,42 +115,57 @@ inline bool wait_for_node_report(
});

if (
rclcpp::spin_until_future_complete(test_node, future_report, std::chrono::seconds(5)) !=
rclcpp::spin_until_future_complete(node_temp, future_report, std::chrono::seconds(5)) !=
rclcpp::FutureReturnCode::SUCCESS) {
return false;
}
return future_report.get();
}

inline bool is_node_registered(const std::string & node_name, const std::string & ns)
inline bool is_node_registered(const std::string & node_full_name)
{
autoware_control_center_msgs::msg::NodeReport report;
return wait_for_node_report(node_name, ns, report);
return wait_for_node_report(node_full_name, report);
}

inline void send_heartbeat(
const std::string & node_name, const std::string & ns, const double deadline_ms,
inline rclcpp::Publisher<autoware_control_center_msgs::msg::Heartbeat>::SharedPtr
send_first_heartbeat(
const rclcpp::Node::SharedPtr & node, const double deadline_ms,
const autoware_control_center_msgs::msg::NodeStatusActivity & status_activity,
const autoware_control_center_msgs::msg::NodeStatusOperational & status_operational)
{
auto test_node = std::make_shared<rclcpp::Node>(node_name, ns);

const std::string fully_qualified_name = test_node->get_fully_qualified_name();
const std::string fully_qualified_name = node->get_fully_qualified_name();

rclcpp::QoS qos_profile(1);
qos_profile.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC)
.liveliness_lease_duration(std::chrono::duration<double, std::milli>(deadline_ms))
.deadline(std::chrono::duration<double, std::milli>(deadline_ms));

auto pub = test_node->create_publisher<autoware_control_center_msgs::msg::Heartbeat>(
auto pub = node->create_publisher<autoware_control_center_msgs::msg::Heartbeat>(
fully_qualified_name + test::topic_heartbeat_suffix, qos_profile);
autoware_control_center_msgs::msg::Heartbeat heartbeat;
heartbeat.stamp = test_node->now();
heartbeat.stamp = node->now();
heartbeat.status_activity = status_activity;
heartbeat.status_operational = status_operational;

pub->publish(heartbeat);
rclcpp::spin_some(node);
return pub;
}

inline void send_heartbeat(
const rclcpp::Node::SharedPtr & node,
const rclcpp::Publisher<autoware_control_center_msgs::msg::Heartbeat>::SharedPtr & pub,
const autoware_control_center_msgs::msg::NodeStatusActivity & status_activity,
const autoware_control_center_msgs::msg::NodeStatusOperational & status_operational)
{
autoware_control_center_msgs::msg::Heartbeat heartbeat;
heartbeat.stamp = node->now();
heartbeat.status_activity = status_activity;
heartbeat.status_operational = status_operational;

pub->publish(heartbeat);
rclcpp::spin_some(test_node);
rclcpp::spin_some(node);
}

} // namespace autoware::control_center::test
Expand Down

0 comments on commit b3978bc

Please sign in to comment.