Merge branch 'iron' into iron

ackermann_steering_controller/CHANGELOG.rst

@@ -2,6 +2,14 @@
 Changelog for package ackermann_steering_controller
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+3.25.0 (2024-07-09)
+-------------------
+* Fix steering controllers library kinematics (`#1150 <https://github.com/ros-controls/ros2_controllers/issues/1150>`_) (`#1195 <https://github.com/ros-controls/ros2_controllers/issues/1195>`_)
+* [Steering controllers library] Reference interfaces are body twist (`#1168 <https://github.com/ros-controls/ros2_controllers/issues/1168>`_) (`#1174 <https://github.com/ros-controls/ros2_controllers/issues/1174>`_)
+* 🚀 Add PID controller 🎉 (backport `#434 <https://github.com/ros-controls/ros2_controllers/issues/434>`_, `#975 <https://github.com/ros-controls/ros2_controllers/issues/975>`_, `#899 <https://github.com/ros-controls/ros2_controllers/issues/899>`_, `#1084 <https://github.com/ros-controls/ros2_controllers/issues/1084>`_, `#951 <https://github.com/ros-controls/ros2_controllers/issues/951>`_) (`#1163 <https://github.com/ros-controls/ros2_controllers/issues/1163>`_)
+* Fix steering controllers library code documentation and naming (`#1149 <https://github.com/ros-controls/ros2_controllers/issues/1149>`_) (`#1165 <https://github.com/ros-controls/ros2_controllers/issues/1165>`_)
+* Contributors: mergify[bot]
+
 3.24.0 (2024-05-14)
 -------------------
 * Add parameter check for geometric values (`#1120 <https://github.com/ros-controls/ros2_controllers/issues/1120>`_) (`#1126 <https://github.com/ros-controls/ros2_controllers/issues/1126>`_)

ackermann_steering_controller/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>ackermann_steering_controller</name>
-  <version>3.24.0</version>
+  <version>3.25.0</version>
   <description>Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it.</description>
   <license>Apache License 2.0</license>
   <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>

ackermann_steering_controller/test/test_ackermann_steering_controller.cpp

@@ -257,7 +257,7 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat
   EXPECT_EQ(msg.steering_angle_command[1], 4.4);
 
   publish_commands();
-  ASSERT_TRUE(controller_->wait_for_commands(executor));
+  controller_->wait_for_commands(executor);
 
   ASSERT_EQ(
     controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),

ackermann_steering_controller/test/test_ackermann_steering_controller.hpp

@@ -76,14 +76,7 @@ class TestableAckermannSteeringController
   controller_interface::CallbackReturn on_configure(
     const rclcpp_lifecycle::State & previous_state) override
   {
-    auto ret =
-      ackermann_steering_controller::AckermannSteeringController::on_configure(previous_state);
-    // Only if on_configure is successful create subscription
-    if (ret == CallbackReturn::SUCCESS)
-    {
-      ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_);
-    }
-    return ret;
+    return ackermann_steering_controller::AckermannSteeringController::on_configure(previous_state);
   }
 
   controller_interface::CallbackReturn on_activate(
@@ -97,30 +90,25 @@ class TestableAckermannSteeringController
    * @brief wait_for_command blocks until a new ControllerReferenceMsg is received.
    * Requires that the executor is not spinned elsewhere between the
    *  message publication and the call to this function.
-   *
-   * @return true if new ControllerReferenceMsg msg was received, false if timeout.
    */
-  bool wait_for_command(
-    rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set,
+  void wait_for_command(
+    rclcpp::Executor & executor,
     const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500})
   {
-    bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready;
-    if (success)
+    auto until = get_node()->get_clock()->now() + timeout;
+    while (get_node()->get_clock()->now() < until)
     {
       executor.spin_some();
+      std::this_thread::sleep_for(std::chrono::microseconds(10));
     }
-    return success;
   }
 
-  bool wait_for_commands(
+  void wait_for_commands(
     rclcpp::Executor & executor,
     const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500})
   {
-    return wait_for_command(executor, ref_subscriber_wait_set_, timeout);
+    wait_for_command(executor, timeout);
   }
-
-private:
-  rclcpp::WaitSet ref_subscriber_wait_set_;
 };
 
 // We are using template class here for easier reuse of Fixture in specializations of controllers
@@ -212,36 +200,43 @@ class AckermannSteeringControllerFixture : public ::testing::Test
   void subscribe_and_get_messages(ControllerStateMsg & msg)
   {
     // create a new subscriber
+    ControllerStateMsg::SharedPtr received_msg;
     rclcpp::Node test_subscription_node("test_subscription_node");
-    auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {};
+    auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; };
     auto subscription = test_subscription_node.create_subscription<ControllerStateMsg>(
       "/test_ackermann_steering_controller/controller_state", 10, subs_callback);
+    rclcpp::executors::SingleThreadedExecutor executor;
+    executor.add_node(test_subscription_node.get_node_base_interface());
 
     // call update to publish the test value
     ASSERT_EQ(
       controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
       controller_interface::return_type::OK);
-
     // 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--)
     {
       controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01));
+      const auto timeout = std::chrono::milliseconds{1};
+      const auto until = test_subscription_node.get_clock()->now() + timeout;
+      while (!received_msg && test_subscription_node.get_clock()->now() < until)
+      {
+        executor.spin_some();
+        std::this_thread::sleep_for(std::chrono::microseconds(10));
+      }
       // check if message has been received
-      if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready)
+      if (received_msg.get())
       {
         break;
       }
     }
     ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through "
                                               "controller/broadcaster update loop";
+    ASSERT_TRUE(received_msg);
 
     // take message from subscription
-    rclcpp::MessageInfo msg_info;
-    ASSERT_TRUE(subscription->take(msg, msg_info));
+    msg = *received_msg;
   }
 
   void publish_commands(const double linear = 0.1, const double angular = 0.2)

admittance_controller/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package admittance_controller
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+3.25.0 (2024-07-09)
+-------------------
+
 3.24.0 (2024-05-14)
 -------------------
 

admittance_controller/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>admittance_controller</name>
-  <version>3.24.0</version>
+  <version>3.25.0</version>
   <description>Implementation of admittance controllers for different input and output interface.</description>
   <maintainer email="denis@stogl.de">Denis Štogl</maintainer>
   <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>

admittance_controller/test/test_admittance_controller.cpp

@@ -277,7 +277,7 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status)
   //   ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_);
 
   publish_commands();
-  ASSERT_TRUE(controller_->wait_for_commands(executor));
+  controller_->wait_for_commands(executor);
 
   broadcast_tfs();
   ASSERT_EQ(

admittance_controller/test/test_admittance_controller.hpp

@@ -57,15 +57,6 @@ constexpr auto NODE_FAILURE =
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE;
 constexpr auto NODE_ERROR =
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::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
 
 // subclassing and friending so we can access member variables
@@ -92,39 +83,27 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon
 
   CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override
   {
-    auto ret = admittance_controller::AdmittanceController::on_configure(previous_state);
-    // Only if on_configure is successful create subscription
-    if (ret == CallbackReturn::SUCCESS)
-    {
-      input_pose_command_subscriber_wait_set_.add_subscription(input_joint_command_subscriber_);
-    }
-    return ret;
+    return admittance_controller::AdmittanceController::on_configure(previous_state);
   }
 
   /**
    * @brief wait_for_commands blocks until a new ControllerCommandMsg is received.
    * Requires that the executor is not spinned elsewhere between the
    *  message publication and the call to this function.
-   *
-   * @return true if new ControllerCommandMsg msg was received, false if timeout.
    */
-  bool wait_for_commands(
+  void wait_for_commands(
     rclcpp::Executor & executor,
     const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500})
   {
-    bool success =
-      input_pose_command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready;
-
-    if (success)
+    auto until = get_node()->get_clock()->now() + timeout;
+    while (get_node()->get_clock()->now() < until)
     {
       executor.spin_some();
+      std::this_thread::sleep_for(std::chrono::microseconds(10));
     }
-    return success;
   }
 
 private:
-  rclcpp::WaitSet input_wrench_command_subscriber_wait_set_;
-  rclcpp::WaitSet input_pose_command_subscriber_wait_set_;
   const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf;
   const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf;
 };
@@ -279,21 +258,31 @@ class AdmittanceControllerTest : public ::testing::Test
   void subscribe_and_get_messages(ControllerStateMsg & msg)
   {
     // create a new subscriber
-    auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {};
+    ControllerStateMsg::SharedPtr received_msg;
+    auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; };
     auto subscription = test_subscription_node_->create_subscription<ControllerStateMsg>(
       "/test_admittance_controller/status", 10, subs_callback);
+    rclcpp::executors::SingleThreadedExecutor executor;
+    executor.add_node(test_subscription_node_->get_node_base_interface());
 
     // call update to publish the test value
     ASSERT_EQ(
       controller_->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);
+    const auto timeout = std::chrono::milliseconds{1};
+    const auto until = test_subscription_node_->get_clock()->now() + timeout;
+    while (!received_msg && test_subscription_node_->get_clock()->now() < until)
+    {
+      executor.spin_some();
+      std::this_thread::sleep_for(std::chrono::microseconds(10));
+    }
+
+    ASSERT_TRUE(received_msg);
 
     // take message from subscription
-    rclcpp::MessageInfo msg_info;
-    ASSERT_TRUE(subscription->take(msg, msg_info));
+    msg = *received_msg;
   }
 
   void publish_commands()

bicycle_steering_controller/CHANGELOG.rst

@@ -2,6 +2,14 @@
 Changelog for package bicycle_steering_controller
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+3.25.0 (2024-07-09)
+-------------------
+* Fix steering controllers library kinematics (`#1150 <https://github.com/ros-controls/ros2_controllers/issues/1150>`_) (`#1195 <https://github.com/ros-controls/ros2_controllers/issues/1195>`_)
+* [Steering controllers library] Reference interfaces are body twist (`#1168 <https://github.com/ros-controls/ros2_controllers/issues/1168>`_) (`#1174 <https://github.com/ros-controls/ros2_controllers/issues/1174>`_)
+* 🚀 Add PID controller 🎉 (backport `#434 <https://github.com/ros-controls/ros2_controllers/issues/434>`_, `#975 <https://github.com/ros-controls/ros2_controllers/issues/975>`_, `#899 <https://github.com/ros-controls/ros2_controllers/issues/899>`_, `#1084 <https://github.com/ros-controls/ros2_controllers/issues/1084>`_, `#951 <https://github.com/ros-controls/ros2_controllers/issues/951>`_) (`#1163 <https://github.com/ros-controls/ros2_controllers/issues/1163>`_)
+* Fix steering controllers library code documentation and naming (`#1149 <https://github.com/ros-controls/ros2_controllers/issues/1149>`_) (`#1165 <https://github.com/ros-controls/ros2_controllers/issues/1165>`_)
+* Contributors: mergify[bot]
+
 3.24.0 (2024-05-14)
 -------------------
 * Add parameter check for geometric values (`#1120 <https://github.com/ros-controls/ros2_controllers/issues/1120>`_) (`#1126 <https://github.com/ros-controls/ros2_controllers/issues/1126>`_)

bicycle_steering_controller/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>bicycle_steering_controller</name>
-  <version>3.24.0</version>
+  <version>3.25.0</version>
   <description>Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering.</description>
   <license>Apache License 2.0</license>
   <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>

bicycle_steering_controller/test/test_bicycle_steering_controller.cpp

@@ -238,7 +238,7 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status
   EXPECT_EQ(msg.steering_angle_command[0], 2.2);
 
   publish_commands(0.1, 0.2);
-  ASSERT_TRUE(controller_->wait_for_commands(executor));
+  controller_->wait_for_commands(executor);
 
   ASSERT_EQ(
     controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),

bicycle_steering_controller/test/test_bicycle_steering_controller.hpp

@@ -74,13 +74,7 @@ class TestableBicycleSteeringController
   controller_interface::CallbackReturn on_configure(
     const rclcpp_lifecycle::State & previous_state) override
   {
-    auto ret = bicycle_steering_controller::BicycleSteeringController::on_configure(previous_state);
-    // Only if on_configure is successful create subscription
-    if (ret == CallbackReturn::SUCCESS)
-    {
-      ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_);
-    }
-    return ret;
+    return bicycle_steering_controller::BicycleSteeringController::on_configure(previous_state);
   }
 
   controller_interface::CallbackReturn on_activate(
@@ -94,30 +88,25 @@ class TestableBicycleSteeringController
    * @brief wait_for_command blocks until a new ControllerReferenceMsg is received.
    * Requires that the executor is not spinned elsewhere between the
    *  message publication and the call to this function.
-   *
-   * @return true if new ControllerReferenceMsg msg was received, false if timeout.
    */
-  bool wait_for_command(
-    rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set,
+  void wait_for_command(
+    rclcpp::Executor & executor,
     const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500})
   {
-    bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready;
-    if (success)
+    auto until = get_node()->get_clock()->now() + timeout;
+    while (get_node()->get_clock()->now() < until)
     {
       executor.spin_some();
+      std::this_thread::sleep_for(std::chrono::microseconds(10));
     }
-    return success;
   }
 
-  bool wait_for_commands(
+  void wait_for_commands(
     rclcpp::Executor & executor,
     const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500})
   {
-    return wait_for_command(executor, ref_subscriber_wait_set_, timeout);
+    wait_for_command(executor, timeout);
   }
-
-private:
-  rclcpp::WaitSet ref_subscriber_wait_set_;
 };
 
 // We are using template class here for easier reuse of Fixture in specializations of controllers
@@ -185,34 +174,43 @@ class BicycleSteeringControllerFixture : public ::testing::Test
   void subscribe_and_get_messages(ControllerStateMsg & msg)
   {
     // create a new subscriber
+    ControllerStateMsg::SharedPtr received_msg;
     rclcpp::Node test_subscription_node("test_subscription_node");
-    auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {};
+    auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; };
     auto subscription = test_subscription_node.create_subscription<ControllerStateMsg>(
       "/test_bicycle_steering_controller/controller_state", 10, subs_callback);
+    rclcpp::executors::SingleThreadedExecutor executor;
+    executor.add_node(test_subscription_node.get_node_base_interface());
 
     // call update to publish the test value
     ASSERT_EQ(
       controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
       controller_interface::return_type::OK);
+    // 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 max_sub_check_loop_count
-    wait_set.add_subscription(subscription);
     while (max_sub_check_loop_count--)
     {
       controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01));
+      const auto timeout = std::chrono::milliseconds{1};
+      const auto until = test_subscription_node.get_clock()->now() + timeout;
+      while (!received_msg && test_subscription_node.get_clock()->now() < until)
+      {
+        executor.spin_some();
+        std::this_thread::sleep_for(std::chrono::microseconds(10));
+      }
       // check if message has been received
-      if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready)
+      if (received_msg.get())
       {
         break;
       }
     }
     ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through "
                                               "controller/broadcaster update loop";
+    ASSERT_TRUE(received_msg);
 
     // take message from subscription
-    rclcpp::MessageInfo msg_info;
-    ASSERT_TRUE(subscription->take(msg, msg_info));
+    msg = *received_msg;
   }
 
   void publish_commands(const double linear = 0.1, const double angular = 0.2)