Merge branch 'master' into make-diff-drive-chainable2-1457

diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp

@@ -76,9 +76,6 @@ class DiffDriveController : public controller_interface::ChainableControllerInte
   controller_interface::CallbackReturn on_error(
     const rclcpp_lifecycle::State & previous_state) override;
 
-  controller_interface::CallbackReturn on_shutdown(
-    const rclcpp_lifecycle::State & previous_state) override;
-
 protected:
   bool on_set_chained_mode(bool chained_mode) override;
 

diff_drive_controller/src/diff_drive_controller.cpp

@@ -648,12 +648,6 @@ bool DiffDriveController::reset()
   return true;
 }
 
-controller_interface::CallbackReturn DiffDriveController::on_shutdown(
-  const rclcpp_lifecycle::State &)
-{
-  return controller_interface::CallbackReturn::SUCCESS;
-}
-
 void DiffDriveController::halt()
 {
   const auto halt_wheels = [](auto & wheel_handles)

joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp

@@ -26,7 +26,13 @@
 #include "joint_state_broadcaster_parameters.hpp"
 #include "realtime_tools/realtime_publisher.hpp"
 #include "sensor_msgs/msg/joint_state.hpp"
+
+#include "rclcpp/version.h"
+#if RCLCPP_VERSION_GTE(29, 0, 0)
+#include "urdf/model.hpp"
+#else
 #include "urdf/model.h"
+#endif
 
 namespace joint_state_broadcaster
 {

joint_state_broadcaster/src/joint_state_broadcaster.cpp

@@ -26,13 +26,6 @@
 #include "rclcpp/time.hpp"
 #include "std_msgs/msg/header.hpp"
 
-#include "rclcpp/version.h"
-#if RCLCPP_VERSION_GTE(29, 0, 0)
-#include "urdf/model.hpp"
-#else
-#include "urdf/model.h"
-#endif
-
 namespace rclcpp_lifecycle
 {
 class State;

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -84,9 +84,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   controller_interface::CallbackReturn on_error(
     const rclcpp_lifecycle::State & previous_state) override;
 
-  controller_interface::CallbackReturn on_shutdown(
-    const rclcpp_lifecycle::State & previous_state) override;
-
 protected:
   // To reduce number of variables and to make the code shorter the interfaces are ordered in types
   // as the following constants

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -1077,14 +1077,6 @@ bool JointTrajectoryController::reset()
   return true;
 }
 
-controller_interface::CallbackReturn JointTrajectoryController::on_shutdown(
-  const rclcpp_lifecycle::State &)
-{
-  // TODO(karsten1987): what to do?
-
-  return CallbackReturn::SUCCESS;
-}
-
 void JointTrajectoryController::publish_state(
   const rclcpp::Time & time, const JointTrajectoryPoint & desired_state,
   const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error)

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>
 
@@ -186,6 +188,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<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 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);

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));
 

tricycle_controller/include/tricycle_controller/tricycle_controller.hpp

@@ -76,8 +76,6 @@ class TricycleController : public controller_interface::ControllerInterface
 
   CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override;
 
-  CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override;
-
 protected:
   struct TractionHandle
   {

tricycle_controller/src/tricycle_controller.cpp

@@ -440,11 +440,6 @@ bool TricycleController::reset()
   return true;
 }
 
-CallbackReturn TricycleController::on_shutdown(const rclcpp_lifecycle::State &)
-{
-  return CallbackReturn::SUCCESS;
-}
-
 void TricycleController::halt()
 {
   traction_joint_[0].velocity_command.get().set_value(0.0);