[pose_broadcaster] Check for valid pose before attempting to publish …

…a tf for it (backport #1479) (#1483)

Co-authored-by: Felix Exner <[email protected]>

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 <cmath>
+#include <rclcpp/logging.hpp>
 
 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

pose_broadcaster/test/test_pose_broadcaster.cpp

@@ -13,6 +13,8 @@
 // limitations under the License.
 #include "test_pose_broadcaster.hpp"
 
+#include <cmath>
+#include <limits>
 #include <utility>
 #include <vector>
 
@@ -184,6 +186,61 @@ 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);
+
+  pose_values_[0] = std::numeric_limits<double>::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
+  pose_values_[0] = 0.0;
+  // but invalid quaternion
+  pose_values_[3] = 0.0;
+  pose_values_[4] = 0.0;
+  pose_values_[5] = 0.0;
+  pose_values_[6] = 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);

pose_broadcaster/test/test_pose_broadcaster.hpp

@@ -39,7 +39,8 @@ class PoseBroadcasterTest : public ::testing::Test
   const std::string frame_id_ = "pose_base_frame";
   const std::string tf_child_frame_id_ = "pose_frame";
 
-  std::array<double, 7> pose_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7};
+  std::array<double, 7> pose_values_ = {
+    {1.1, 2.2, 3.3, 0.39190382, 0.20056212, 0.53197575, 0.72331744}};
 
   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 +78,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));