fix tests

cartesian_admittance_controller/package.xml

@@ -32,11 +32,12 @@
 
   <depend>cartesian_control_msgs</depend>
 
+  <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>controller_manager</test_depend>
   <test_depend>kinematics_interface_kdl</test_depend>
-  <test_depend>ros2_control_test_assets</test_depend>
   <test_depend>hardware_interface_testing</test_depend>
+  <test_depend>ros2_control_test_assets</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

cartesian_admittance_controller/test/test_cartesian_admittance_controller.cpp

@@ -33,7 +33,7 @@ INSTANTIATE_TEST_SUITE_P(
   AdmittanceControllerTestParameterizedMissingParameters,
   ::testing::Values(
     "admittance.inertia", "admittance.selected_axes", "admittance.stiffness",
-    "chainable_command_interfaces", "command_interfaces", "control.frame.id", "admittance.frame.id",
+    "command_interfaces", "control.frame.id", "admittance.frame.id",
     "fixed_world_frame.frame.id", "ft_sensor.frame.id", "ft_sensor.name",
     "gravity_compensation.CoG.pos", "gravity_compensation.frame.id", "joints", "kinematics.base",
     "kinematics.plugin_name", "kinematics.plugin_package", "kinematics.tip", "state_interfaces"));
@@ -170,11 +170,13 @@ TEST_F(CartesianAdmittanceControllerTest, check_interfaces)
   ASSERT_EQ(command_interfaces.names.size(), joint_command_values_.size());
 
   ASSERT_EQ(
-    controller_->command_interfaces_.size(), command_interface_types_.size() * joint_names_.size());
+    controller_->command_interfaces_.size(),
+    command_interface_types_.size() * joint_names_.size());
 
   auto state_interfaces = controller_->state_interface_configuration();
-  ASSERT_EQ(state_interfaces.names.size(), joint_state_values_.size() + fts_state_values_.size());
-
+  ASSERT_EQ(
+    state_interfaces.names.size(),
+    state_interface_types_.size() * joint_state_position_values_.size() + fts_state_values_.size());
   ASSERT_EQ(
     controller_->state_interfaces_.size(),
     state_interface_types_.size() * joint_names_.size() + fts_state_values_.size());
@@ -274,8 +276,8 @@ TEST_F(CartesianAdmittanceControllerTest, receive_message_and_publish_updated_st
     controller_interface::return_type::OK);
 
   // After first update state, commanded position should be near the start state
-  for (auto i = 0ul; i < joint_state_values_.size(); i++) {
-    ASSERT_NEAR(joint_state_values_[i], joint_command_values_[i], COMMON_THRESHOLD);
+  for (auto i = 0ul; i < joint_state_position_values_.size(); i++) {
+    ASSERT_NEAR(joint_state_position_values_[i], joint_command_values_[i], COMMON_THRESHOLD);
   }
   /*
   //TODO(tpoignonec)

cartesian_admittance_controller/test/test_cartesian_admittance_controller.hpp

@@ -213,14 +213,21 @@ class CartesianAdmittanceControllerTest : public ::testing::Test
     fts_state_names_ = sc_fts.get_state_interface_names();
     std::vector<hardware_interface::LoanedStateInterface> state_ifs;
 
-    const size_t num_state_ifs = joint_state_values_.size() + fts_state_names_.size();
+    const size_t num_state_ifs = 2 * joint_state_position_values_.size() + fts_state_names_.size();
     state_itfs_.reserve(num_state_ifs);
     state_ifs.reserve(num_state_ifs);
 
-    for (auto i = 0u; i < joint_state_values_.size(); ++i) {
+    for (auto i = 0u; i < joint_state_position_values_.size(); ++i) {
       state_itfs_.emplace_back(
         hardware_interface::StateInterface(
-          joint_names_[i], state_interface_types_[0], &joint_state_values_[i]));
+          joint_names_[i], state_interface_types_[0], &joint_state_position_values_[i]));
+      state_ifs.emplace_back(state_itfs_.back());
+    }
+
+    for (auto i = 0u; i < joint_state_velocity_values_.size(); ++i) {
+      state_itfs_.emplace_back(
+        hardware_interface::StateInterface(
+          joint_names_[i], state_interface_types_[1], &joint_state_velocity_values_[i]));
       state_ifs.emplace_back(state_itfs_.back());
     }
 
@@ -350,7 +357,8 @@ class CartesianAdmittanceControllerTest : public ::testing::Test
     Eigen::Isometry3d robot_reference_pose;
     Eigen::Matrix<double, 6, 1> robot_reference_velocity = Eigen::Matrix<double, 6, 1>::Zero();
 
-    std::vector<double> joint_state_values(joint_state_values_.begin(), joint_state_values_.end());
+    std::vector<double> joint_state_values(joint_state_position_values_.begin(),
+      joint_state_position_values_.end());
     kinematics_->calculate_link_transform(
       joint_state_values,
       tcp_frame,
@@ -402,10 +410,12 @@ class CartesianAdmittanceControllerTest : public ::testing::Test
   // TODO(anyone): adjust the members as needed
 
   // Controller-related parameters
-  const std::vector<std::string> joint_names_ = {"joint1", "joint2", "joint3",
-    "joint4", "joint5", "joint6"};
+  const std::vector<std::string> joint_names_ = {
+    "joint_a1", "joint_a2", "joint_a3",
+    "joint_a4", "joint_a5", "joint_a6"
+  };
   const std::vector<std::string> command_interface_types_ = {"position"};
-  const std::vector<std::string> state_interface_types_ = {"position"};
+  const std::vector<std::string> state_interface_types_ = {"position", "velocity"};
   const std::string ft_sensor_name_ = "ft_sensor_name";
 
   bool hardware_state_has_offset_ = false;
@@ -428,7 +438,8 @@ class CartesianAdmittanceControllerTest : public ::testing::Test
   std::array<double, 6> admittance_stiffness_ = {214.1, 214.2, 214.3, 214.4, 214.5, 214.6};
 
   std::array<double, 6> joint_command_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
-  std::array<double, 6> joint_state_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6};
+  std::array<double, 6> joint_state_position_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6};
+  std::array<double, 6> joint_state_velocity_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
   std::array<double, 6> fts_state_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
   std::vector<std::string> fts_state_names_;
 

cartesian_admittance_controller/test/test_load_controller.cpp

@@ -13,7 +13,6 @@
 // limitations under the License.
 
 #include <gmock/gmock.h>
-
 #include <memory>
 
 #include "controller_manager/controller_manager.hpp"
@@ -35,7 +34,7 @@ TEST(TestLoadAdmittanceController, load_controller)
 
   ASSERT_EQ(
     cm.load_controller(
-      "load_admittance_controller",
+      "test_cartesian_admittance_controller",
       "cartesian_admittance_controller/CartesianAdmittanceController"),
     nullptr);
 }

cartesian_admittance_controller/test/test_params.yaml

@@ -1,27 +1,13 @@
-load_admittance_controller:
-  # contains minimal parameters that need to be set to load controller
-  ros__parameters:
-    joints:
-      - joint1
-      - joint2
-
-    command_interfaces:
-      - velocity
-
-    state_interfaces:
-      - position
-      - velocity
-
 test_cartesian_admittance_controller:
   # contains minimal needed parameters for kuka_kr6
   ros__parameters:
     joints:
-      - joint1
-      - joint2
-      - joint3
-      - joint4
-      - joint5
-      - joint6
+      - joint_a1
+      - joint_a2
+      - joint_a3
+      - joint_a4
+      - joint_a5
+      - joint_a6
 
     command_interfaces:
       - position