Update *states_has_offset* tests' criteria

ros-controls · Oct 10, 2023 · 25a6e39 · 25a6e39
1 parent 59f1116
commit 25a6e39
Showing 1 changed file with 10 additions and 8 deletions.
diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp
@@ -1675,7 +1675,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e
 TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_controller_start)
 {
   rclcpp::executors::SingleThreadedExecutor executor;
-  // default if false so it will not be actually set parameter
   rclcpp::Parameter is_open_loop_parameters("open_loop_control", true);
 
   // set command values to NaN
@@ -1700,7 +1699,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_co
         hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() &&
       traj_controller_->has_velocity_command_interface())
     {
-      EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]);
+      EXPECT_EQ(current_state_when_offset.velocities[i], joint_state_vel_[i]);
     }
 
     // check acceleration
@@ -1710,7 +1709,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_co
         hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() &&
       traj_controller_->has_acceleration_command_interface())
     {
-      EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]);
+      EXPECT_EQ(current_state_when_offset.accelerations[i], joint_state_acc_[i]);
     }
   }
 
@@ -1722,23 +1721,26 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_co
 TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_controller_start)
 {
   rclcpp::executors::SingleThreadedExecutor executor;
-  // default if false so it will not be actually set parameter
   rclcpp::Parameter is_open_loop_parameters("open_loop_control", true);
 
-  // set command values to NaN
+  // set command values to arbitrary values
+  std::vector<double> initial_pos_cmd, initial_vel_cmd, initial_acc_cmd;
   for (size_t i = 0; i < 3; ++i)
   {
     joint_pos_[i] = 3.1 + static_cast<double>(i);
+    initial_pos_cmd.push_back(joint_pos_[i]);
     joint_vel_[i] = 0.25 + static_cast<double>(i);
+    initial_vel_cmd.push_back(joint_vel_[i]);
     joint_acc_[i] = 0.02 + static_cast<double>(i) / 10.0;
+    initial_acc_cmd.push_back(joint_acc_[i]);
   }
   SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true);
 
   auto current_state_when_offset = traj_controller_->get_current_state_when_offset();
 
   for (size_t i = 0; i < 3; ++i)
   {
-    EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]);
+    EXPECT_EQ(current_state_when_offset.positions[i], initial_pos_cmd[i]);
 
     // check velocity
     if (
@@ -1747,7 +1749,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co
         hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() &&
       traj_controller_->has_velocity_command_interface())
     {
-      EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]);
+      EXPECT_EQ(current_state_when_offset.velocities[i], initial_vel_cmd[i]);
     }
 
     // check acceleration
@@ -1757,7 +1759,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co
         hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() &&
       traj_controller_->has_acceleration_command_interface())
     {
-      EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]);
+      EXPECT_EQ(current_state_when_offset.accelerations[i], initial_acc_cmd[i]);
     }
   }