implemneted suggested changes compiling and ll tests pass

ros-controls · Feb 13, 2023 · 380233d · 380233d
1 parent 99a40a6
commit 380233d
Show file tree

Hide file tree

Showing 7 changed files with 87 additions and 129 deletions.
diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt
@@ -34,6 +34,7 @@ add_library(
   mecanum_drive_controller
   SHARED
   src/mecanum_drive_controller.cpp
+  src/odometry.cpp
 )
 target_compile_features(mecanum_drive_controller PUBLIC cxx_std_17)
 target_include_directories(mecanum_drive_controller PUBLIC

diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp
@@ -93,8 +93,6 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure(
     return CallbackReturn::FAILURE;
   }
 
-  reference_names_ = params_.reference_names;
-
   // Set wheel params for the odometry computation
   odometry_.setWheelsParams(
     params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis,
@@ -275,10 +273,12 @@ MecanumDriveController::on_export_reference_interfaces()
 
   reference_interfaces.reserve(reference_interfaces_.size());
 
+  std::vector<std::string> reference_interface_names = {"linear/x/velocity", "linear/y/velocity", "angular/z/velocity"};
+
   for (size_t i = 0; i < reference_interfaces_.size(); ++i)
   {
     reference_interfaces.push_back(hardware_interface::CommandInterface(
-      get_node()->get_name(), reference_names_[i] + "/" + params_.interface_name,
+      get_node()->get_name(), reference_interface_names[i],
       &reference_interfaces_[i]));
   }
 

diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml
@@ -19,13 +19,6 @@ mecanum_drive_controller:
     read_only: true,
   }
 
-  reference_names: {
-    type: string_array,
-    default_value: [],
-    description: " Names of the references, ex: high level vel commands from MoveIt, Nav2, etc.",
-    read_only: true,
-  }
-
   interface_name: {
     type: string,
     default_value: "",

diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml
@@ -5,8 +5,6 @@ test_mecanum_drive_controller:
 
     command_joint_names: ["front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", "front_right_wheel_joint"]
 
-    reference_names: ["linear_x", "linear_y", "angular_z"]
-
     interface_name: velocity
 
     kinematics:

diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp
diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp
@@ -49,21 +49,22 @@ constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR;
 // subclassing and friending so we can access member variables
 class TestableMecanumDriveController : public mecanum_drive_controller::MecanumDriveController
 {
-  FRIEND_TEST(MecanumDriveControllerTest, when_all_parameters_are_set_expect_them_in_storage);
-  FRIEND_TEST(MecanumDriveControllerTest, when_exported_all_interfaces_expect_them_in_storage);
-  FRIEND_TEST(MecanumDriveControllerTest, when_activated_expect_reference_msg_reset);
-  FRIEND_TEST(MecanumDriveControllerTest, when_update_successful_expect_return_type_success);
-  FRIEND_TEST(MecanumDriveControllerTest, when_deactivated_expect_return_type_success);
-  FRIEND_TEST(MecanumDriveControllerTest, when_reactivated_expect_reference_msg_reset);
-  FRIEND_TEST(MecanumDriveControllerTest, when_published_success_expect_in_storage);
-  FRIEND_TEST(MecanumDriveControllerTest, when_subscribed_msg_received_publish_succeeded_expect_value_in_storage);
-  FRIEND_TEST(MecanumDriveControllerTest, when_sending_too_old_message_expect_nan_in_reference_msg);
-  FRIEND_TEST(MecanumDriveControllerTest, when_time_stamp_zero_expect_setting_to_current);
-  FRIEND_TEST(MecanumDriveControllerTest, when_message_accepted_expect_reference_msg_in_rt_buffer);
-  FRIEND_TEST(MecanumDriveControllerTest, test_update_logic_not_chainable_mode);
-  FRIEND_TEST(MecanumDriveControllerTest, test_update_logic_chainable_mode);
-  FRIEND_TEST(MecanumDriveControllerTest, test_ref_timeout_zero_for_update);
-  FRIEND_TEST(MecanumDriveControllerTest, when_ref_timeout_zero_for_reference_callback_expect_reference_msg_in_rt_buffer);
+  FRIEND_TEST(MecanumDriveControllerTest, when_controller_is_configured_expect_all_parameters_set);
+  FRIEND_TEST(MecanumDriveControllerTest, when_controller_configured_expect_properly_exported_interfaces);
+  FRIEND_TEST(MecanumDriveControllerTest, when_controller_is_activated_expect_reference_reset);
+  FRIEND_TEST(MecanumDriveControllerTest, when_controller_active_and_update_called_expect_success);
+  FRIEND_TEST(MecanumDriveControllerTest, when_active_controller_is_deactivated_expect_success);
+  FRIEND_TEST(MecanumDriveControllerTest, when_controller_is_reactivated_expect_cmd_itfs_not_set_and_update_success);
+  FRIEND_TEST(MecanumDriveControllerTest, when_update_is_called_expect_status_message);
+  FRIEND_TEST(MecanumDriveControllerTest, when_reference_msg_received_expect_updated_commands_and_status_message);
+  FRIEND_TEST(MecanumDriveControllerTest, when_reference_msg_is_too_old_expect_unset_reference);
+  FRIEND_TEST(MecanumDriveControllerTest, when_reference_msg_has_timestamp_zero_expect_reference_set_and_timestamp_set_to_current_time);
+  FRIEND_TEST(MecanumDriveControllerTest, when_message_has_valid_timestamp_expect_reference_set);
+  FRIEND_TEST(MecanumDriveControllerTest, when_ref_msg_old_expect_cmnd_itfs_set_to_zero_otherwise_to_valid_cmnds);
+  FRIEND_TEST(MecanumDriveControllerTest, when_controller_in_chainable_mode_expect_receiving_commands_from_reference_interfaces_directly);
+  FRIEND_TEST(MecanumDriveControllerTest, when_reference_timeout_is_zero_expect_reference_msg_being_used_only_once);
+  FRIEND_TEST(MecanumDriveControllerTest, when_ref_timeout_zero_for_reference_callback_expect_reference_msg_being_used_only_once);
+
 
 public:
   controller_interface::CallbackReturn on_configure(
@@ -242,7 +243,7 @@ class MecanumDriveControllerFixture : public ::testing::Test
   }
 
 protected:
-  std::vector<std::string> reference_names_ = {"linear_x", "linear_y", "angular_z"};
+  std::vector<std::string> reference_interface_names = {"linear/x/velocity", "linear/y/velocity", "angular/z/velocity"};
   std::vector<std::string> command_joint_names_ = {
     "front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint",
     "front_right_wheel_joint"};

diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp
@@ -29,8 +29,7 @@ class MecanumDriveControllerTest
 {
 };
 
-// checking if all parameters are initialized and set as expected
-TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success)
+TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_parameters_set)
 {
   SetUpController();
 
@@ -51,7 +50,7 @@ TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success)
 }
 
 // checking if all interfaces, command and state interfaces are exported as expected
-TEST_F(MecanumDriveControllerTest, check_exported_intefaces)
+TEST_F(MecanumDriveControllerTest, when_controller_configured_expect_properly_exported_interfaces)
 {
   SetUpController();