Fix mecanum

mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp

@@ -48,8 +48,8 @@ class Odometry
   /// \param time      Current time
   /// \return true if the odometry is actually updated
   bool update(
-    const double wheel_front_left_vel, const double wheel_rear_left_vel, const double wheel_rear_right_vel,
-    const double wheel_front_right_vel, const double dt);
+    const double wheel_front_left_vel, const double wheel_rear_left_vel,
+    const double wheel_rear_right_vel, const double wheel_front_right_vel, const double dt);
 
   /// \return position (x component) [m]
   double getX() const { return position_x_in_base_frame_; }
@@ -72,7 +72,8 @@ class Odometry
   /// \param sum_of_robot_center_projection_on_X_Y_axis Wheels geometric param
   /// (used in mecanum wheels' ik) [m]
   /// \param wheels_radius  Wheels radius [m]
-  void setWheelsParams(const double sum_of_robot_center_projection_on_X_Y_axis, const double wheels_radius);
+  void setWheelsParams(
+    const double sum_of_robot_center_projection_on_X_Y_axis, const double wheels_radius);
 
 private:
   /// Current timestamp:

mecanum_drive_controller/src/mecanum_drive_controller.cpp

@@ -74,7 +74,10 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure(
 {
   params_ = param_listener_->get_params();
 
-  auto prepare_lists_with_joint_names = [&command_joints = this->command_joint_names_, &state_joints = this->state_joint_names_](const std::size_t index, const std::string & command_joint_name, const std::string & state_joint_name)
+  auto prepare_lists_with_joint_names =
+    [&command_joints = this->command_joint_names_, &state_joints = this->state_joint_names_](
+      const std::size_t index, const std::string & command_joint_name,
+      const std::string & state_joint_name)
   {
     command_joints[index] = command_joint_name;
     if (state_joint_name.empty())
@@ -91,10 +94,18 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure(
   state_joint_names_.resize(4);
 
   // The joint names are sorted according to the order documented in the header file!
-  prepare_lists_with_joint_names(FRONT_LEFT, params_.front_left_wheel_command_joint_name, params_.front_left_wheel_state_joint_name);
-  prepare_lists_with_joint_names(FRONT_RIGHT, params_.front_right_wheel_command_joint_name, params_.front_right_wheel_state_joint_name);
-  prepare_lists_with_joint_names(REAR_RIGHT, params_.rear_right_wheel_command_joint_name, params_.rear_right_wheel_state_joint_name);
-  prepare_lists_with_joint_names(REAR_LEFT, params_.rear_left_wheel_command_joint_name, params_.rear_left_wheel_state_joint_name);
+  prepare_lists_with_joint_names(
+    FRONT_LEFT, params_.front_left_wheel_command_joint_name,
+    params_.front_left_wheel_state_joint_name);
+  prepare_lists_with_joint_names(
+    FRONT_RIGHT, params_.front_right_wheel_command_joint_name,
+    params_.front_right_wheel_state_joint_name);
+  prepare_lists_with_joint_names(
+    REAR_RIGHT, params_.rear_right_wheel_command_joint_name,
+    params_.rear_right_wheel_state_joint_name);
+  prepare_lists_with_joint_names(
+    REAR_LEFT, params_.rear_left_wheel_command_joint_name,
+    params_.rear_left_wheel_state_joint_name);
 
   // Set wheel params for the odometry computation
   odometry_.setWheelsParams(
@@ -362,8 +373,8 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma
   {
     // Estimate twist (using joint information) and integrate
     odometry_.update(
-      wheel_front_left_state_vel, wheel_rear_left_state_vel, wheel_rear_right_state_vel, wheel_front_right_state_vel,
-      period.seconds());
+      wheel_front_left_state_vel, wheel_rear_left_state_vel, wheel_rear_right_state_vel,
+      wheel_front_right_state_vel, period.seconds());
   }
 
   // INVERSE KINEMATICS (move robot).
@@ -417,11 +428,12 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma
        params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis *
          velocity_in_center_frame_angular_z_);
 
-      // Set wheels velocities - The joint names are sorted accoring to the order documented in the header file!
-      command_interfaces_[FRONT_LEFT].set_value(wheel_front_left_vel);
-      command_interfaces_[FRONT_RIGHT].set_value(wheel_front_right_vel);
-      command_interfaces_[REAR_RIGHT].set_value(wheel_rear_right_vel);
-      command_interfaces_[REAR_LEFT].set_value(wheel_rear_left_vel);
+    // Set wheels velocities - The joint names are sorted according to the order documented in the
+    // header file!
+    command_interfaces_[FRONT_LEFT].set_value(wheel_front_left_vel);
+    command_interfaces_[FRONT_RIGHT].set_value(wheel_front_right_vel);
+    command_interfaces_[REAR_RIGHT].set_value(wheel_rear_right_vel);
+    command_interfaces_[REAR_LEFT].set_value(wheel_rear_left_vel);
   }
   else
   {
@@ -463,10 +475,14 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma
   if (controller_state_publisher_->trylock())
   {
     controller_state_publisher_->msg_.header.stamp = get_node()->now();
-    controller_state_publisher_->msg_.front_left_wheel_velocity = state_interfaces_[FRONT_LEFT].get_value();
-    controller_state_publisher_->msg_.front_right_wheel_velocity = state_interfaces_[FRONT_RIGHT].get_value();
-    controller_state_publisher_->msg_.back_right_wheel_velocity = state_interfaces_[REAR_RIGHT].get_value();
-    controller_state_publisher_->msg_.back_left_wheel_velocity = state_interfaces_[REAR_LEFT].get_value();
+    controller_state_publisher_->msg_.front_left_wheel_velocity =
+      state_interfaces_[FRONT_LEFT].get_value();
+    controller_state_publisher_->msg_.front_right_wheel_velocity =
+      state_interfaces_[FRONT_RIGHT].get_value();
+    controller_state_publisher_->msg_.back_right_wheel_velocity =
+      state_interfaces_[REAR_RIGHT].get_value();
+    controller_state_publisher_->msg_.back_left_wheel_velocity =
+      state_interfaces_[REAR_LEFT].get_value();
     controller_state_publisher_->msg_.reference_velocity.linear.x = reference_interfaces_[0];
     controller_state_publisher_->msg_.reference_velocity.linear.y = reference_interfaces_[1];
     controller_state_publisher_->msg_.reference_velocity.angular.z = reference_interfaces_[2];

mecanum_drive_controller/src/odometry.cpp

@@ -45,8 +45,8 @@ void Odometry::init(
 }
 
 bool Odometry::update(
-  const double wheel_front_left_vel, const double wheel_rear_left_vel, const double wheel_rear_right_vel,
-  const double wheel_front_right_vel, const double dt)
+  const double wheel_front_left_vel, const double wheel_rear_left_vel,
+  const double wheel_rear_right_vel, const double wheel_front_right_vel, const double dt)
 {
   /// We cannot estimate the speed with very small time intervals:
   // const double dt = (time - timestamp_).toSec();

mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp

@@ -28,9 +28,7 @@ TEST(TestLoadMecanumDriveController, load_controller)
     std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
 
   controller_manager::ControllerManager cm(
-    std::make_unique<hardware_interface::ResourceManager>(
-      ros2_control_test_assets::minimal_robot_urdf),
-    executor, "test_controller_manager");
+    executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager");
 
   ASSERT_NE(
     cm.load_controller(

mecanum_drive_controller/test/test_mecanum_drive_controller.cpp

@@ -220,7 +220,7 @@ TEST_F(
   // reference_msg is published with provided time stamp when publish_commands( time_stamp)
   // is called
   publish_commands(controller_->get_node()->now());
-  ASSERT_TRUE(controller_->wait_for_commands(executor));
+  controller_->wait_for_commands(executor);
 
   ASSERT_EQ(
     controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)),
@@ -263,7 +263,7 @@ TEST_F(MecanumDriveControllerTest, when_reference_msg_is_too_old_expect_unset_re
   publish_commands(
     controller_->get_node()->now() - controller_->ref_timeout_ -
     rclcpp::Duration::from_seconds(0.1));
-  ASSERT_TRUE(controller_->wait_for_commands(executor));
+  controller_->wait_for_commands(executor);
   ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp);
   EXPECT_TRUE(std::isnan((reference)->twist.linear.x));
   EXPECT_TRUE(std::isnan((reference)->twist.linear.y));
@@ -293,7 +293,7 @@ TEST_F(
   // is called
   publish_commands(rclcpp::Time(0));
 
-  ASSERT_TRUE(controller_->wait_for_commands(executor));
+  controller_->wait_for_commands(executor);
   ASSERT_EQ(old_timestamp.sec, (*(controller_->input_ref_.readFromNonRT()))->header.stamp.sec);
   EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x));
   EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z));
@@ -324,7 +324,7 @@ TEST_F(MecanumDriveControllerTest, when_message_has_valid_timestamp_expect_refer
   // is called
   publish_commands(controller_->get_node()->now());
 
-  ASSERT_TRUE(controller_->wait_for_commands(executor));
+  controller_->wait_for_commands(executor);
   EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x));
   EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z));
   EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 1.5);
@@ -555,7 +555,7 @@ TEST_F(
   // is called
   publish_commands(controller_->get_node()->now());
 
-  ASSERT_TRUE(controller_->wait_for_commands(executor));
+  controller_->wait_for_commands(executor);
 
   EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x));
   EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y));

mecanum_drive_controller/test/test_mecanum_drive_controller.hpp

@@ -29,6 +29,8 @@
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "mecanum_drive_controller/mecanum_drive_controller.hpp"
+#include "rclcpp/executor.hpp"
+#include "rclcpp/executors.hpp"
 #include "rclcpp/parameter_value.hpp"
 #include "rclcpp/time.hpp"
 #include "rclcpp/utilities.hpp"
@@ -85,13 +87,7 @@ class TestableMecanumDriveController : public mecanum_drive_controller::MecanumD
   controller_interface::CallbackReturn on_configure(
     const rclcpp_lifecycle::State & previous_state) override
   {
-    auto ret = mecanum_drive_controller::MecanumDriveController::on_configure(previous_state);
-    // Only if on_configure is successful create subscription
-    if (ret == CallbackReturn::SUCCESS)
-    {
-      ref_subscriber_wait_set_.add_subscription(ref_subscriber_);
-    }
-    return ret;
+    return mecanum_drive_controller::MecanumDriveController::on_configure(previous_state);
   }
 
   controller_interface::CallbackReturn on_activate(
@@ -105,30 +101,25 @@ class TestableMecanumDriveController : public mecanum_drive_controller::MecanumD
    * @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
@@ -197,34 +188,42 @@ class MecanumDriveControllerFixture : public ::testing::Test
   void subscribe_to_controller_status_execute_update_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_mecanum_drive_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(controller_->get_node()->now(), 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(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01));
+      controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01));
+      const auto timeout = std::chrono::milliseconds{5};
+      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(