Skip to content

Commit

Permalink
fix: 🐛 make force_torque_sensor_broadcaster wait for realtime_publish…
Browse files Browse the repository at this point in the history 
…er (ros-controls#327)

* fix: 🐛 make force_torque_sensor_broadcaster wait for realtime_publisher loop to be ready before finishing configure
* test: ✅ check for published message in test_force_torque_sensor_broadcaster
- create a loop that checks if force_torque_sensor_broadcaster published before checking subscribed message
* refactor: ♻️ remove unnecessary includes
* test: ✅ port realtime_publisher check to imu_sensor_broadcaster and joint_state_broadcaster
* refactor: ♻️ remove unnecessary wait_for code from tests
Co-authored-by: Denis Štogl <denis@stogl.de>
  • Loading branch information
@jaron-l @mamueluth
jaron-l authored and mamueluth committed Aug 26, 2022
1 parent 1baa09a commit acc3c93
Show file tree
Hide file tree
Showing 3 changed files with 62 additions and 48 deletions.
30 changes: 15 additions & 15 deletions force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,15 +37,6 @@ namespace
{
constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS;
constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR;

rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription)
{
rclcpp::WaitSet wait_set;
wait_set.add_subscription(subscription);
const auto timeout = std::chrono::seconds(20);
return wait_set.wait(timeout).kind();
}

} // namespace

void ForceTorqueSensorBroadcasterTest::SetUpTestCase() { rclcpp::init(0, nullptr); }
Expand Down Expand Up @@ -86,12 +77,21 @@ void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message(
"/test_force_torque_sensor_broadcaster/wrench", 10, subs_callback);

// call update to publish the test value
ASSERT_EQ(
fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
controller_interface::return_type::OK);

// wait for message to be passed
ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready);
// since update doesn't guarantee a published message, republish until received
int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop
rclcpp::WaitSet wait_set; // block used to wait on message
wait_set.add_subscription(subscription);
while (max_sub_check_loop_count--)
{
fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
// check if message has been received
if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready)
{
break;
}
}
ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through "
"controller/broadcaster update loop";

// take message from subscription
rclcpp::MessageInfo msg_info;
Expand Down
28 changes: 15 additions & 13 deletions imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,13 +38,6 @@ namespace
constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS;
constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR;

rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription)
{
rclcpp::WaitSet wait_set;
wait_set.add_subscription(subscription);
return wait_set.wait().kind();
}

} // namespace

void IMUSensorBroadcasterTest::SetUpTestCase() { rclcpp::init(0, nullptr); }
Expand Down Expand Up @@ -88,12 +81,21 @@ void IMUSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Imu &
"/test_imu_sensor_broadcaster/imu", 10, subs_callback);

// call update to publish the test value
ASSERT_EQ(
imu_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
controller_interface::return_type::OK);

// wait for message to be passed
ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready);
// since update doesn't guarantee a published message, republish until received
int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop
rclcpp::WaitSet wait_set; // block used to wait on message
wait_set.add_subscription(subscription);
while (max_sub_check_loop_count--)
{
imu_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
// check if message has been received
if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready)
{
break;
}
}
ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through "
"controller/broadcaster update loop";

// take message from subscription
rclcpp::MessageInfo msg_info;
Expand Down
52 changes: 32 additions & 20 deletions joint_state_broadcaster/test/test_joint_state_broadcaster.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,14 +45,6 @@ namespace
{
constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS;
constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR;

rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription)
{
rclcpp::WaitSet wait_set;
wait_set.add_subscription(subscription);
const auto timeout = std::chrono::seconds(10);
return wait_set.wait(timeout).kind();
}
} // namespace

void JointStateBroadcasterTest::SetUpTestCase() { rclcpp::init(0, nullptr); }
Expand Down Expand Up @@ -606,12 +598,22 @@ void JointStateBroadcasterTest::test_published_joint_state_message(const std::st
auto subscription =
test_node.create_subscription<sensor_msgs::msg::JointState>(topic, 10, subs_callback);

ASSERT_EQ(
state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
controller_interface::return_type::OK);

// wait for message to be passed
ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready);
// call update to publish the test value
// since update doesn't guarantee a published message, republish until received
int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop
rclcpp::WaitSet wait_set; // block used to wait on message
wait_set.add_subscription(subscription);
while (max_sub_check_loop_count--)
{
state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
// check if message has been received
if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready)
{
break;
}
}
ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through "
"controller/broadcaster update loop";

// take message from subscription
sensor_msgs::msg::JointState joint_state_msg;
Expand Down Expand Up @@ -658,12 +660,22 @@ void JointStateBroadcasterTest::test_published_dynamic_joint_state_message(
auto subscription =
test_node.create_subscription<control_msgs::msg::DynamicJointState>(topic, 10, subs_callback);

ASSERT_EQ(
state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
controller_interface::return_type::OK);

// wait for message to be passed
ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready);
// call update to publish the test value
// since update doesn't guarantee a published message, republish until received
int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop
rclcpp::WaitSet wait_set; // block used to wait on message
wait_set.add_subscription(subscription);
while (max_sub_check_loop_count--)
{
state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
// check if message has been received
if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready)
{
break;
}
}
ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through "
"controller/broadcaster update loop";

// take message from subscription
control_msgs::msg::DynamicJointState dynamic_joint_state_msg;
Expand Down

0 comments on commit acc3c93

Please sign in to comment.