Fixing the error for mecanum controller.

mecanum_drive_controller/CMakeLists.txt

@@ -58,12 +58,12 @@ if(BUILD_TESTING)
   find_package(hardware_interface REQUIRED)
   find_package(ros2_control_test_assets REQUIRED)
 
-  add_rostest_with_parameters_gmock(test_load_mecanum_drive_controller
-    test/test_load_mecanum_drive_controller.cpp
-    ${CMAKE_CURRENT_SOURCE_DIR}/test/mecanum_drive_controller_params.yaml
-  )
+
+  add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
+  ament_add_gmock(test_load_mecanum_drive_controller test/test_load_mecanum_drive_controller.cpp)
   ament_target_dependencies(test_load_mecanum_drive_controller
     controller_manager
+    hardware_interface
     ros2_control_test_assets
   )
 

mecanum_drive_controller/src/mecanum_drive_controller.cpp

@@ -204,23 +204,25 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure(
   controller_state_publisher_->msg_.header.frame_id = params_.odom_frame_id;
   controller_state_publisher_->unlock();
 
-  RCLCPP_INFO(get_node()->get_logger(), "configure successful");
+  RCLCPP_INFO(get_node()->get_logger(), "MecanumDriveController configured successfully");
+
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
 void MecanumDriveController::reference_callback(const std::shared_ptr<ControllerReferenceMsg> msg)
 {
-  // if no timestamp provided use current time for command timestamp
+  // If no timestamp provided, use current time for command timestamp
   if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0u)
   {
     RCLCPP_WARN(
       get_node()->get_logger(),
-      "Timestamp in header is missing, using current time as command "
-      "timestamp.");
+      "Timestamp in header is missing, using current time as command timestamp.");
     msg->header.stamp = get_node()->now();
   }
+
   const auto age_of_last_command = get_node()->now() - msg->header.stamp;
 
+  // Check the timeout condition
   if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_)
   {
     input_ref_.writeFromNonRT(msg);
@@ -229,10 +231,10 @@ void MecanumDriveController::reference_callback(const std::shared_ptr<Controller
   {
     RCLCPP_ERROR(
       get_node()->get_logger(),
-      "Received message has timestamp %.10f older for %.10f which is more then allowed timeout "
-      "(%.4f).",
+      "Received message has timestamp %.10f older by %.10f than allowed timeout (%.4f).",
       rclcpp::Time(msg->header.stamp).seconds(), age_of_last_command.seconds(),
       ref_timeout_.seconds());
+
     reset_controller_reference_msg(msg, get_node());
   }
 }
@@ -307,10 +309,21 @@ controller_interface::CallbackReturn MecanumDriveController::on_activate(
 controller_interface::CallbackReturn MecanumDriveController::on_deactivate(
   const rclcpp_lifecycle::State & /*previous_state*/)
 {
+  bool value_set_no_error = true;
   for (size_t i = 0; i < NR_CMD_ITFS; ++i)
   {
-    command_interfaces_[i].set_value(std::numeric_limits<double>::quiet_NaN());
+    value_set_no_error &=
+      command_interfaces_[i].set_value(std::numeric_limits<double>::quiet_NaN());
+  }
+  if (!value_set_no_error)
+  {
+    RCLCPP_ERROR(
+      get_node()->get_logger(),
+      "Setting values to command interfaces has failed! "
+      "This means that you are maybe blocking the interface in your hardware for too long.");
+    return controller_interface::CallbackReturn::FAILURE;
   }
+
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
@@ -430,17 +443,26 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma
 
     // 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);
+    const bool value_set_error =
+      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);
+    RCLCPP_ERROR_EXPRESSION(
+      get_node()->get_logger(), !value_set_error,
+      "Setting values to command interfaces has failed! "
+      "This means that you are maybe blocking the interface in your hardware for too long.");
   }
   else
   {
-    command_interfaces_[FRONT_LEFT].set_value(0.0);
-    command_interfaces_[FRONT_RIGHT].set_value(0.0);
-    command_interfaces_[REAR_RIGHT].set_value(0.0);
-    command_interfaces_[REAR_LEFT].set_value(0.0);
+    const bool value_set_error = command_interfaces_[FRONT_LEFT].set_value(0.0) &&
+                                 command_interfaces_[FRONT_RIGHT].set_value(0.0) &&
+                                 command_interfaces_[REAR_RIGHT].set_value(0.0) &&
+                                 command_interfaces_[REAR_LEFT].set_value(0.0);
+    RCLCPP_ERROR_EXPRESSION(
+      get_node()->get_logger(), !value_set_error,
+      "Setting values to command interfaces has failed! "
+      "This means that you are maybe blocking the interface in your hardware for too long.");
   }
 
   // Publish odometry message

mecanum_drive_controller/test/mecanum_drive_controller_params.yaml

@@ -1,6 +1,6 @@
 test_mecanum_drive_controller:
   ros__parameters:
-    reference_timeout: 0.1
+    reference_timeout: 0.9
 
     front_left_wheel_command_joint_name: "front_left_wheel_joint"
     front_right_wheel_command_joint_name: "front_right_wheel_joint"

mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp

@@ -30,10 +30,14 @@ TEST(TestLoadMecanumDriveController, load_controller)
   controller_manager::ControllerManager cm(
     executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager");
 
-  ASSERT_NE(
-    cm.load_controller(
-      "test_mecanum_drive_controller", "mecanum_drive_controller/MecanumDriveController"),
-    nullptr);
+  const std::string test_file_path =
+    std::string(TEST_FILES_DIRECTORY) + "/mecanum_drive_controller_params.yaml";
+
+  cm.set_parameter({"test_mecanum_drive_controller.params_file", test_file_path});
+  cm.set_parameter(
+    {"test_mecanum_drive_controller.type", "mecanum_drive_controller/MecanumDriveController"});
+
+  ASSERT_NE(cm.load_controller("test_mecanum_drive_controller"), nullptr);
 
   rclcpp::shutdown();
 }

mecanum_drive_controller/test/test_mecanum_drive_controller.cpp

@@ -60,7 +60,7 @@ TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_para
 
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
 
-  ASSERT_EQ(controller_->params_.reference_timeout, 0.1);
+  ASSERT_EQ(controller_->params_.reference_timeout, 0.9);
   ASSERT_EQ(controller_->params_.kinematics.wheels_radius, 0.5);
   ASSERT_EQ(controller_->params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis, 1.0);
 
@@ -212,7 +212,7 @@ TEST_F(
     controller_interface::return_type::OK);
   ControllerStateMsg msg;
   subscribe_to_controller_status_execute_update_and_get_messages(msg);
-  joint_command_values_[1] = command_lin_x;
+  joint_command_values_[controller_->get_rear_left_wheel_index()] = command_lin_x;
 
   EXPECT_TRUE(std::isnan(msg.reference_velocity.linear.x));
 
@@ -226,13 +226,13 @@ TEST_F(
     controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::OK);
 
-  // w_back_left_vel =
+  // REAR LEFT vel =
   // 1.0 / params_.kinematics.wheels_radius *
   // (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ -
   // params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis *
   // velocity_in_center_frame_angular_z_);
-  //  joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0)
-  EXPECT_EQ(joint_command_values_[1], 3.0);
+  //  joint_command_values_[REAR_LEFT] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0)
+  EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 3.0);
 
   subscribe_to_controller_status_execute_update_and_get_messages(msg);
 
@@ -356,7 +356,7 @@ TEST_F(
   }
 
   // set command statically
-  joint_command_values_[1] = command_lin_x;
+  joint_command_values_[controller_->get_rear_left_wheel_index()] = command_lin_x;
 
   std::shared_ptr<ControllerReferenceMsg> msg = std::make_shared<ControllerReferenceMsg>();
 
@@ -380,7 +380,7 @@ TEST_F(
     controller_interface::return_type::OK);
   EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0]));
 
-  EXPECT_EQ(joint_command_values_[1], 0);
+  EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 0);
   for (const auto & interface : controller_->reference_interfaces_)
   {
     EXPECT_TRUE(std::isnan(interface));
@@ -409,14 +409,15 @@ TEST_F(
     controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::OK);
 
-  EXPECT_NE(joint_command_values_[1], command_lin_x);
-  // w_back_left_vel =
+  EXPECT_NE(joint_command_values_[controller_->get_rear_left_wheel_index()], command_lin_x);
+  // BACK Left vel =
   // 1.0 / params_.kinematics.wheels_radius *
   // (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ -
   // params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis *
   // velocity_in_center_frame_angular_z_);
-  //  joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0)
-  EXPECT_EQ(joint_command_values_[1], 3.0);
+  //  joint_command_values_[controller_->get_rear_left_wheel_index()] = 1.0 / 0.5 * (1.5 - 0.0 - 1 *
+  //  0.0)
+  EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 3.0);
   ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X);
   for (const auto & interface : controller_->reference_interfaces_)
   {
@@ -452,7 +453,7 @@ TEST_F(
   }
 
   // set command statically
-  joint_command_values_[1] = command_lin_x;
+  joint_command_values_[controller_->get_rear_left_wheel_index()] = command_lin_x;
   // imitating preceding controllers command_interfaces setting reference_interfaces of chained
   // controller.
   controller_->reference_interfaces_[0] = 3.0;
@@ -468,16 +469,16 @@ TEST_F(
     controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::OK);
 
-  EXPECT_NE(joint_command_values_[1], command_lin_x);
+  EXPECT_NE(joint_command_values_[controller_->get_rear_left_wheel_index()], command_lin_x);
 
-  // w_back_left_vel =
+  // REAR LEFT vel =
   // 1.0 / params_.kinematics.wheels_radius *
   // (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ -
   // params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis *
   // velocity_in_center_frame_angular_z_);
 
-  //  joint_command_values_[1] = 1.0 / 0.5 * (3.0 - 0.0 - 1 * 0.0)
-  EXPECT_EQ(joint_command_values_[1], 6.0);
+  //  joint_command_values_[REAR_LEFT] = 1.0 / 0.5 * (3.0 - 0.0 - 1 * 0.0)
+  EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 6.0);
   for (const auto & interface : controller_->reference_interfaces_)
   {
     EXPECT_TRUE(std::isnan(interface));
@@ -501,7 +502,7 @@ TEST_F(
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
 
   // set command statically
-  joint_command_values_[1] = command_lin_x;
+  joint_command_values_[controller_->get_rear_left_wheel_index()] = command_lin_x;
 
   controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0);
   std::shared_ptr<ControllerReferenceMsg> msg = std::make_shared<ControllerReferenceMsg>();
@@ -524,14 +525,14 @@ TEST_F(
     controller_interface::return_type::OK);
 
   EXPECT_FALSE(std::isnan(joint_command_values_[1]));
-  EXPECT_NE(joint_command_values_[1], command_lin_x);
-  // w_back_left_vel =
+  EXPECT_NE(joint_command_values_[controller_->get_rear_left_wheel_index()], command_lin_x);
+  // REAR LEFT vel =
   // 1.0 / params_.kinematics.wheels_radius *
   // (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ -
   // params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis *
   // velocity_in_center_frame_angular_z_);
-  //  joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0)
-  EXPECT_EQ(joint_command_values_[1], 3.0);
+  //  joint_command_values_[REAR_LEFT] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0)
+  EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 3.0);
   ASSERT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x));
 }
 

mecanum_drive_controller/test/test_mecanum_drive_controller.hpp

@@ -120,6 +120,14 @@ class TestableMecanumDriveController : public mecanum_drive_controller::MecanumD
   {
     wait_for_command(executor, timeout);
   }
+
+  size_t get_front_left_wheel_index() { return WheelIndex::FRONT_LEFT; }
+
+  size_t get_front_right_wheel_index() { return WheelIndex::FRONT_RIGHT; }
+
+  size_t get_rear_right_wheel_index() { return WheelIndex::REAR_RIGHT; }
+
+  size_t get_rear_left_wheel_index() { return WheelIndex::REAR_LEFT; }
 };
 
 // We are using template class here for easier reuse of Fixture in specializations of controllers
@@ -241,7 +249,7 @@ class MecanumDriveControllerFixture : public ::testing::Test
             std::string("publishing to ") + topic_name + " but no node subscribes to it";
           throw std::runtime_error(error_msg);
         }
-        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
         ++wait_count;
       }
     };