Merge branch 'ros-controls:master' into export/state_interfaces/pid_c…

…ontroller

ackermann_steering_controller/test/test_ackermann_steering_controller.cpp

@@ -14,10 +14,8 @@
 
 #include "test_ackermann_steering_controller.hpp"
 
-#include <limits>
 #include <memory>
 #include <string>
-#include <utility>
 #include <vector>
 
 class AckermannSteeringControllerTest
@@ -257,7 +255,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

@@ -15,22 +15,18 @@
 #ifndef TEST_ACKERMANN_STEERING_CONTROLLER_HPP_
 #define TEST_ACKERMANN_STEERING_CONTROLLER_HPP_
 
+#include <gmock/gmock.h>
+
 #include <chrono>
-#include <limits>
 #include <memory>
 #include <string>
-#include <tuple>
 #include <utility>
 #include <vector>
 
 #include "ackermann_steering_controller/ackermann_steering_controller.hpp"
-#include "gmock/gmock.h"
 #include "hardware_interface/loaned_command_interface.hpp"
 #include "hardware_interface/loaned_state_interface.hpp"
-#include "hardware_interface/types/hardware_interface_return_values.hpp"
-#include "rclcpp/parameter_value.hpp"
 #include "rclcpp/time.hpp"
-#include "rclcpp/utilities.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 
 using ControllerStateMsg =
@@ -76,14 +72,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 +86,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
@@ -214,36 +198,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)

ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp

@@ -14,10 +14,8 @@
 
 #include "test_ackermann_steering_controller.hpp"
 
-#include <limits>
 #include <memory>
 #include <string>
-#include <utility>
 #include <vector>
 
 class AckermannSteeringControllerTest

admittance_controller/include/admittance_controller/admittance_controller.hpp

@@ -17,7 +17,6 @@
 #ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_
 #define ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_
 
-#include <chrono>
 #include <memory>
 #include <string>
 #include <vector>
@@ -29,21 +28,14 @@
 #include "admittance_controller/visibility_control.h"
 #include "control_msgs/msg/admittance_controller_state.hpp"
 #include "controller_interface/chainable_controller_interface.hpp"
-#include "geometry_msgs/msg/pose_stamped.hpp"
-#include "geometry_msgs/msg/transform_stamped.hpp"
-#include "geometry_msgs/msg/wrench_stamped.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
-#include "pluginlib/class_loader.hpp"
 #include "rclcpp/duration.hpp"
 #include "rclcpp/time.hpp"
-#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "rclcpp_lifecycle/state.hpp"
 #include "realtime_tools/realtime_buffer.h"
 #include "realtime_tools/realtime_publisher.h"
 #include "semantic_components/force_torque_sensor.hpp"
 
-#include "trajectory_msgs/msg/joint_trajectory.hpp"
-
 namespace admittance_controller
 {
 using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState;

admittance_controller/include/admittance_controller/admittance_rule.hpp

@@ -18,23 +18,16 @@
 #define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_
 
 #include <Eigen/Core>
-#include <Eigen/Geometry>
-#include <map>
+
 #include <memory>
 #include <string>
 #include <vector>
 
+#include "admittance_controller_parameters.hpp"
 #include "control_msgs/msg/admittance_controller_state.hpp"
-#include "control_toolbox/filters.hpp"
-#include "controller_interface/controller_interface.hpp"
-#include "geometry_msgs/msg/wrench_stamped.hpp"
+#include "controller_interface/controller_interface_base.hpp"
 #include "kinematics_interface/kinematics_interface.hpp"
 #include "pluginlib/class_loader.hpp"
-#include "tf2_eigen/tf2_eigen.hpp"
-#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
-#include "tf2_kdl/tf2_kdl.hpp"
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
 #include "trajectory_msgs/msg/joint_trajectory_point.hpp"
 
 namespace admittance_controller

admittance_controller/include/admittance_controller/admittance_rule_impl.hpp

@@ -22,9 +22,10 @@
 #include <memory>
 #include <vector>
 
+#include <control_toolbox/filters.hpp>
+#include <tf2_eigen/tf2_eigen.hpp>
+
 #include "rclcpp/duration.hpp"
-#include "rclcpp/utilities.hpp"
-#include "tf2_ros/transform_listener.h"
 
 namespace admittance_controller
 {

admittance_controller/src/admittance_controller.cpp

@@ -16,17 +16,13 @@
 
 #include "admittance_controller/admittance_controller.hpp"
 
-#include <chrono>
 #include <cmath>
-#include <functional>
 #include <memory>
 #include <string>
 #include <vector>
 
 #include "admittance_controller/admittance_rule_impl.hpp"
 #include "geometry_msgs/msg/wrench.hpp"
-#include "rcutils/logging_macros.h"
-#include "tf2_ros/buffer.h"
 #include "trajectory_msgs/msg/joint_trajectory_point.hpp"
 
 namespace admittance_controller

admittance_controller/test/test_admittance_controller.cpp

@@ -16,9 +16,7 @@
 
 #include "test_admittance_controller.hpp"
 
-#include <limits>
 #include <memory>
-#include <utility>
 #include <vector>
 
 // Test on_init returns ERROR when a required parameter is missing
@@ -277,7 +275,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

@@ -17,6 +17,8 @@
 #ifndef TEST_ADMITTANCE_CONTROLLER_HPP_
 #define TEST_ADMITTANCE_CONTROLLER_HPP_
 
+#include <gmock/gmock.h>
+
 #include <chrono>
 #include <map>
 #include <memory>
@@ -25,21 +27,18 @@
 #include <utility>
 #include <vector>
 
-#include "gmock/gmock.h"
+#include "geometry_msgs/msg/pose_stamped.hpp"
 
 #include "admittance_controller/admittance_controller.hpp"
 #include "control_msgs/msg/admittance_controller_state.hpp"
 #include "geometry_msgs/msg/transform_stamped.hpp"
 #include "hardware_interface/loaned_command_interface.hpp"
 #include "hardware_interface/loaned_state_interface.hpp"
-#include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "rclcpp/parameter_value.hpp"
-#include "rclcpp/utilities.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "semantic_components/force_torque_sensor.hpp"
 #include "test_asset_6d_robot_description.hpp"
 #include "tf2_ros/transform_broadcaster.h"
-#include "trajectory_msgs/msg/joint_trajectory.hpp"
 
 // TODO(anyone): replace the state and command message types
 using ControllerCommandWrenchMsg = geometry_msgs::msg::WrenchStamped;
@@ -57,15 +56,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 +82,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 +257,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()