From 07e0047fc5f13ae1b69b3fe7a553bee359d92c61 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 10 Jan 2025 00:32:02 +0100 Subject: [PATCH] [pose_broadcaster] Check for valid pose before attempting to publish a tf for it (#1479) (cherry picked from commit 67096bdb9b99cf0d010019728bf1ff46aad76dc9) # Conflicts: # pose_broadcaster/test/test_pose_broadcaster.hpp --- pose_broadcaster/src/pose_broadcaster.cpp | 26 ++++++++- .../test/test_pose_broadcaster.cpp | 56 +++++++++++++++++++ .../test/test_pose_broadcaster.hpp | 10 +++- 3 files changed, 89 insertions(+), 3 deletions(-) diff --git a/pose_broadcaster/src/pose_broadcaster.cpp b/pose_broadcaster/src/pose_broadcaster.cpp index 7e3aeaddf3..535a12f419 100644 --- a/pose_broadcaster/src/pose_broadcaster.cpp +++ b/pose_broadcaster/src/pose_broadcaster.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. #include "pose_broadcaster/pose_broadcaster.hpp" +#include +#include namespace { @@ -24,6 +26,19 @@ constexpr auto DEFAULT_TF_TOPIC = "/tf"; namespace pose_broadcaster { +bool is_pose_valid(const geometry_msgs::msg::Pose & pose) +{ + return std::isfinite(pose.position.x) && std::isfinite(pose.position.y) && + std::isfinite(pose.position.z) && std::isfinite(pose.orientation.x) && + std::isfinite(pose.orientation.y) && std::isfinite(pose.orientation.z) && + std::isfinite(pose.orientation.w) && + + std::abs( + pose.orientation.x * pose.orientation.x + pose.orientation.y * pose.orientation.y + + pose.orientation.z * pose.orientation.z + pose.orientation.w * pose.orientation.w - + 1.0) <= 10e-3; +} + controller_interface::InterfaceConfiguration PoseBroadcaster::command_interface_configuration() const { @@ -147,8 +162,15 @@ controller_interface::return_type PoseBroadcaster::update( realtime_publisher_->msg_.pose = pose; realtime_publisher_->unlockAndPublish(); } - - if (realtime_tf_publisher_ && realtime_tf_publisher_->trylock()) + if (!is_pose_valid(pose)) + { + RCLCPP_ERROR_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 1000, + "Invalid pose [%f, %f, %f], [%f, %f, %f, %f]", pose.position.x, pose.position.y, + pose.position.z, pose.orientation.x, pose.orientation.y, pose.orientation.z, + pose.orientation.w); + } + else if (realtime_tf_publisher_ && realtime_tf_publisher_->trylock()) { bool do_publish = false; // rlcpp::Time comparisons throw if clock types are not the same diff --git a/pose_broadcaster/test/test_pose_broadcaster.cpp b/pose_broadcaster/test/test_pose_broadcaster.cpp index bcd764c026..472ad5136d 100644 --- a/pose_broadcaster/test/test_pose_broadcaster.cpp +++ b/pose_broadcaster/test/test_pose_broadcaster.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include "test_pose_broadcaster.hpp" +#include +#include #include #include @@ -184,6 +186,60 @@ TEST_F(PoseBroadcasterTest, PublishSuccess) EXPECT_EQ(tf_msg.transforms[0].transform.rotation.w, pose_values_[6]); } +TEST_F(PoseBroadcasterTest, invalid_pose_no_tf_published) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Set 'tf.enable' and 'tf.child_frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"tf.enable", true}); + pose_broadcaster_->get_node()->set_parameter({"tf.child_frame_id", tf_child_frame_id_}); + + // Configure and activate controller + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + pose_broadcaster_->on_activate(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + ASSERT_TRUE(pose_position_x_.set_value(std::numeric_limits::quiet_NaN())); + + // Subscribe to pose topic + geometry_msgs::msg::PoseStamped pose_msg; + subscribe_and_get_message("/test_pose_broadcaster/pose", pose_msg); + + // Verify content of pose message + EXPECT_EQ(pose_msg.header.frame_id, frame_id_); + EXPECT_TRUE(std::isnan(pose_msg.pose.position.x)); // We set that to NaN above + EXPECT_EQ(pose_msg.pose.position.y, pose_values_[1]); + EXPECT_EQ(pose_msg.pose.position.z, pose_values_[2]); + EXPECT_EQ(pose_msg.pose.orientation.x, pose_values_[3]); + EXPECT_EQ(pose_msg.pose.orientation.y, pose_values_[4]); + EXPECT_EQ(pose_msg.pose.orientation.z, pose_values_[5]); + EXPECT_EQ(pose_msg.pose.orientation.w, pose_values_[6]); + + // Subscribe to tf topic + tf2_msgs::msg::TFMessage tf_msg; + EXPECT_THROW(subscribe_and_get_message("/tf", tf_msg), std::runtime_error); + // Verify that no tf message was sent + ASSERT_EQ(tf_msg.transforms.size(), 0lu); + + // Set valid position but invalid quaternion + ASSERT_TRUE(pose_position_x_.set_value(0.0)); + ASSERT_TRUE(pose_orientation_x_.set_value(0.0)); + ASSERT_TRUE(pose_orientation_y_.set_value(0.0)); + ASSERT_TRUE(pose_orientation_z_.set_value(0.0)); + ASSERT_TRUE(pose_orientation_w_.set_value(0.0)); + + EXPECT_THROW(subscribe_and_get_message("/tf", tf_msg), std::runtime_error); + // Verify that no tf message was sent + ASSERT_EQ(tf_msg.transforms.size(), 0lu); +} + int main(int argc, char * argv[]) { ::testing::InitGoogleMock(&argc, argv); diff --git a/pose_broadcaster/test/test_pose_broadcaster.hpp b/pose_broadcaster/test/test_pose_broadcaster.hpp index a164b2c6ac..9d28ce10c5 100644 --- a/pose_broadcaster/test/test_pose_broadcaster.hpp +++ b/pose_broadcaster/test/test_pose_broadcaster.hpp @@ -39,7 +39,12 @@ class PoseBroadcasterTest : public ::testing::Test const std::string frame_id_ = "pose_base_frame"; const std::string tf_child_frame_id_ = "pose_frame"; +<<<<<<< HEAD std::array pose_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7}; +======= + std::array pose_values_ = { + {1.1, 2.2, 3.3, 0.39190382, 0.20056212, 0.53197575, 0.72331744}}; +>>>>>>> 67096bd ([pose_broadcaster] Check for valid pose before attempting to publish a tf for it (#1479)) hardware_interface::StateInterface pose_position_x_{pose_name_, "position.x", &pose_values_[0]}; hardware_interface::StateInterface pose_position_y_{pose_name_, "position.x", &pose_values_[1]}; @@ -77,7 +82,10 @@ void PoseBroadcasterTest::subscribe_and_get_message(const std::string & topic, T constexpr size_t max_sub_check_loop_count = 5; for (size_t i = 0; !received_msg; ++i) { - ASSERT_LT(i, max_sub_check_loop_count); + if (i >= max_sub_check_loop_count) + { + throw std::runtime_error("Failed to receive message on topic: " + topic); + } pose_broadcaster_->update(rclcpp::Time{0}, rclcpp::Duration::from_seconds(0.01));