Skip to content

Commit

Permalink
simplify the tests
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 b3978bc commit 7fa05bc
Show file tree
Hide file tree
Showing 2 changed files with 26 additions and 34 deletions.
19 changes: 7 additions & 12 deletions common/autoware_control_center/test/test_cc_heartbeat.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -73,9 +73,9 @@ TEST_F(ControlCenterHeartbeatTest, HeartbeatHandling)
status_operational.status = autoware_control_center_msgs::msg::NodeStatusOperational::NORMAL;

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

// wait for the heartbeat to be processed
Expand All @@ -95,11 +95,7 @@ TEST_F(ControlCenterHeartbeatTest, HeartbeatMissedDeadline)
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);
auto pub_hb = test::send_first_heartbeat(node, deadline_ms_, test::generate_hb_healthy());

// Wait for the heartbeat deadline to be missed
std::this_thread::sleep_for(
Expand All @@ -116,16 +112,15 @@ TEST_F(ControlCenterHeartbeatTest, HeartbeatMissedDeadline)
autoware_control_center_msgs::msg::NodeStatusOperational::UNKNOWN);

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

// 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->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);
ASSERT_EQ(report.status_activity.status, hb_healthy->status_activity.status);
ASSERT_EQ(report.status_operational.status, hb_healthy->status_operational.status);
}

int main(int argc, char ** argv)
Expand Down
41 changes: 19 additions & 22 deletions common/autoware_control_center/test/test_utility.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -128,11 +128,19 @@ inline bool is_node_registered(const std::string & node_full_name)
return wait_for_node_report(node_full_name, report);
}

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::Heartbeat::SharedPtr & heartbeat)
{
pub->publish(*heartbeat);
rclcpp::spin_some(node);
}

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)
const autoware_control_center_msgs::msg::Heartbeat::SharedPtr & heartbeat)
{
const std::string fully_qualified_name = node->get_fully_qualified_name();

Expand All @@ -143,31 +151,20 @@ send_first_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 = node->now();
heartbeat.status_activity = status_activity;
heartbeat.status_operational = status_operational;

pub->publish(heartbeat);
rclcpp::spin_some(node);
send_heartbeat(node, pub, heartbeat);
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::SharedPtr generate_hb_healthy()
{
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(node);
auto heartbeat = std::make_shared<autoware_control_center_msgs::msg::Heartbeat>();
heartbeat->stamp = rclcpp::Clock().now();
heartbeat->status_activity.status =
autoware_control_center_msgs::msg::NodeStatusActivity::PROCESSING;
heartbeat->status_operational.status =
autoware_control_center_msgs::msg::NodeStatusOperational::NORMAL;
return heartbeat;
}

} // namespace autoware::control_center::test

#endif // TEST_UTILITY_HPP_

0 comments on commit 7fa05bc

Please sign in to comment.