Skip to content

Commit 4b577b4

Browse files
christophfroehlichchristophfroehlich
committed
Use new update method for added tests
1 parent 49f3ec2 commit 4b577b4

2 files changed

Lines changed: 61 additions & 78 deletions

File tree

‎joint_trajectory_controller/test/test_trajectory_controller.cpp‎

Lines changed: 54 additions & 78 deletions
Original file line numberDiff line numberDiff line change
@@ -853,10 +853,8 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le
853853
constexpr double k_p = 10.0;
854854
std::vector<std::string> command_joint_names{joint_names_[0], joint_names_[1]};
855855
const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names);
856-
std::vector<rclcpp::Parameter> params = {
857-
rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true), command_joint_names_param};
856+
std::vector<rclcpp::Parameter> params = {command_joint_names_param};
858857
SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, false);
859-
subscribeToState();
860858

861859
size_t n_joints = joint_names_.size();
862860

@@ -872,66 +870,58 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le
872870
publish(time_from_start, points, rclcpp::Time(), {}, points_velocities);
873871
traj_controller_->wait_for_trajectory(executor);
874872

875-
// first update
876-
updateController(rclcpp::Duration(FIRST_POINT_TIME));
877-
878-
// Spin to receive latest state
879-
executor.spin_some();
880-
auto state_msg = getState();
881-
ASSERT_TRUE(state_msg);
873+
// trylock() has to succeed at least once to have current_command set
874+
updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME));
882875

883-
const auto allowed_delta = 0.1;
876+
// get states from class variables
877+
auto state_feedback = traj_controller_->get_state_feedback();
878+
auto state_reference = traj_controller_->get_state_reference();
879+
auto state_error = traj_controller_->get_state_error();
880+
auto current_command = traj_controller_->get_current_command();
884881

885882
// no update of state_interface
886-
EXPECT_EQ(state_msg->feedback.positions, INITIAL_POS_JOINTS);
883+
EXPECT_EQ(state_feedback.positions, INITIAL_POS_JOINTS);
887884

888885
// has the msg the correct vector sizes?
889-
EXPECT_EQ(n_joints, state_msg->reference.positions.size());
890-
EXPECT_EQ(n_joints, state_msg->feedback.positions.size());
891-
EXPECT_EQ(n_joints, state_msg->error.positions.size());
886+
EXPECT_EQ(n_joints, state_reference.positions.size());
887+
EXPECT_EQ(n_joints, state_feedback.positions.size());
888+
EXPECT_EQ(n_joints, state_error.positions.size());
892889

893890
// are the correct reference positions used?
894-
EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta);
895-
EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta);
896-
EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta);
891+
EXPECT_NEAR(points[0][0], state_reference.positions[0], COMMON_THRESHOLD);
892+
EXPECT_NEAR(points[0][1], state_reference.positions[1], COMMON_THRESHOLD);
893+
EXPECT_NEAR(points[0][2], state_reference.positions[2], COMMON_THRESHOLD);
897894

898895
// no normalization of position error
899-
EXPECT_NEAR(
900-
state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS);
901-
EXPECT_NEAR(
902-
state_msg->error.positions[1], state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1], EPS);
903-
EXPECT_NEAR(
904-
state_msg->error.positions[2], state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2], EPS);
896+
EXPECT_NEAR(state_error.positions[0], state_reference.positions[0] - INITIAL_POS_JOINTS[0], EPS);
897+
EXPECT_NEAR(state_error.positions[1], state_reference.positions[1] - INITIAL_POS_JOINTS[1], EPS);
898+
EXPECT_NEAR(state_error.positions[2], state_reference.positions[2] - INITIAL_POS_JOINTS[2], EPS);
905899

906900
if (traj_controller_->has_position_command_interface())
907901
{
908902
// check command interface
909-
EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta);
910-
EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta);
911-
EXPECT_NEAR(points[0][0], state_msg->output.positions[0], allowed_delta);
912-
EXPECT_NEAR(points[0][1], state_msg->output.positions[1], allowed_delta);
913-
EXPECT_TRUE(std::isnan(state_msg->output.positions[2]));
903+
EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD);
904+
EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD);
905+
EXPECT_TRUE(std::isnan(current_command.positions[2]));
914906
}
915907

916908
if (traj_controller_->has_velocity_command_interface())
917909
{
918910
// check command interface
919911
EXPECT_LT(0.0, joint_vel_[0]);
920912
EXPECT_LT(0.0, joint_vel_[1]);
921-
EXPECT_LT(0.0, state_msg->output.velocities[0]);
922-
EXPECT_LT(0.0, state_msg->output.velocities[1]);
923-
EXPECT_TRUE(std::isnan(state_msg->output.velocities[2]));
913+
EXPECT_TRUE(std::isnan(current_command.velocities[2]));
924914

925915
// use_closed_loop_pid_adapter_
926916
if (traj_controller_->use_closed_loop_pid_adapter())
927917
{
928918
// we expect u = k_p * (s_d-s)
929919
EXPECT_NEAR(
930-
k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
931-
k_p * allowed_delta);
920+
k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
921+
k_p * COMMON_THRESHOLD);
932922
EXPECT_NEAR(
933-
k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
934-
k_p * allowed_delta);
923+
k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
924+
k_p * COMMON_THRESHOLD);
935925
}
936926
}
937927

@@ -940,9 +930,7 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le
940930
// check command interface
941931
EXPECT_LT(0.0, joint_eff_[0]);
942932
EXPECT_LT(0.0, joint_eff_[1]);
943-
EXPECT_LT(0.0, state_msg->output.effort[0]);
944-
EXPECT_LT(0.0, state_msg->output.effort[1]);
945-
EXPECT_TRUE(std::isnan(state_msg->output.effort[2]));
933+
EXPECT_TRUE(std::isnan(current_command.effort[2]));
946934
}
947935

948936
executor.cancel();
@@ -958,10 +946,8 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le
958946
constexpr double k_p = 10.0;
959947
std::vector<std::string> command_joint_names{joint_names_[1], joint_names_[0]};
960948
const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names);
961-
std::vector<rclcpp::Parameter> params = {
962-
rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true), command_joint_names_param};
949+
std::vector<rclcpp::Parameter> params = {command_joint_names_param};
963950
SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, false);
964-
subscribeToState();
965951

966952
size_t n_joints = joint_names_.size();
967953

@@ -977,66 +963,58 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le
977963
publish(time_from_start, points, rclcpp::Time(), {}, points_velocities);
978964
traj_controller_->wait_for_trajectory(executor);
979965

980-
// first update
981-
updateController(rclcpp::Duration(FIRST_POINT_TIME));
982-
983-
// Spin to receive latest state
984-
executor.spin_some();
985-
auto state_msg = getState();
986-
ASSERT_TRUE(state_msg);
966+
// trylock() has to succeed at least once to have current_command set
967+
updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME));
987968

988-
const auto allowed_delta = 0.1;
969+
// get states from class variables
970+
auto state_feedback = traj_controller_->get_state_feedback();
971+
auto state_reference = traj_controller_->get_state_reference();
972+
auto state_error = traj_controller_->get_state_error();
973+
auto current_command = traj_controller_->get_current_command();
989974

990975
// no update of state_interface
991-
EXPECT_EQ(state_msg->feedback.positions, INITIAL_POS_JOINTS);
976+
EXPECT_EQ(state_feedback.positions, INITIAL_POS_JOINTS);
992977

993978
// has the msg the correct vector sizes?
994-
EXPECT_EQ(n_joints, state_msg->reference.positions.size());
995-
EXPECT_EQ(n_joints, state_msg->feedback.positions.size());
996-
EXPECT_EQ(n_joints, state_msg->error.positions.size());
979+
EXPECT_EQ(n_joints, state_reference.positions.size());
980+
EXPECT_EQ(n_joints, state_feedback.positions.size());
981+
EXPECT_EQ(n_joints, state_error.positions.size());
997982

998983
// are the correct reference positions used?
999-
EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta);
1000-
EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta);
1001-
EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta);
984+
EXPECT_NEAR(points[0][0], state_reference.positions[0], COMMON_THRESHOLD);
985+
EXPECT_NEAR(points[0][1], state_reference.positions[1], COMMON_THRESHOLD);
986+
EXPECT_NEAR(points[0][2], state_reference.positions[2], COMMON_THRESHOLD);
1002987

1003988
// no normalization of position error
1004-
EXPECT_NEAR(
1005-
state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS);
1006-
EXPECT_NEAR(
1007-
state_msg->error.positions[1], state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1], EPS);
1008-
EXPECT_NEAR(
1009-
state_msg->error.positions[2], state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2], EPS);
989+
EXPECT_NEAR(state_error.positions[0], state_reference.positions[0] - INITIAL_POS_JOINTS[0], EPS);
990+
EXPECT_NEAR(state_error.positions[1], state_reference.positions[1] - INITIAL_POS_JOINTS[1], EPS);
991+
EXPECT_NEAR(state_error.positions[2], state_reference.positions[2] - INITIAL_POS_JOINTS[2], EPS);
1010992

1011993
if (traj_controller_->has_position_command_interface())
1012994
{
1013995
// check command interface
1014-
EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta);
1015-
EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta);
1016-
EXPECT_NEAR(points[0][0], state_msg->output.positions[0], allowed_delta);
1017-
EXPECT_NEAR(points[0][1], state_msg->output.positions[1], allowed_delta);
1018-
EXPECT_TRUE(std::isnan(state_msg->output.positions[2]));
996+
EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD);
997+
EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD);
998+
EXPECT_TRUE(std::isnan(current_command.positions[2]));
1019999
}
10201000

10211001
if (traj_controller_->has_velocity_command_interface())
10221002
{
10231003
// check command interface
10241004
EXPECT_LT(0.0, joint_vel_[0]);
10251005
EXPECT_LT(0.0, joint_vel_[1]);
1026-
EXPECT_LT(0.0, state_msg->output.velocities[0]);
1027-
EXPECT_LT(0.0, state_msg->output.velocities[1]);
1028-
EXPECT_TRUE(std::isnan(state_msg->output.velocities[2]));
1006+
EXPECT_TRUE(std::isnan(current_command.velocities[2]));
10291007

10301008
// use_closed_loop_pid_adapter_
10311009
if (traj_controller_->use_closed_loop_pid_adapter())
10321010
{
10331011
// we expect u = k_p * (s_d-s)
10341012
EXPECT_NEAR(
1035-
k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
1036-
k_p * allowed_delta);
1013+
k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
1014+
k_p * COMMON_THRESHOLD);
10371015
EXPECT_NEAR(
1038-
k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
1039-
k_p * allowed_delta);
1016+
k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
1017+
k_p * COMMON_THRESHOLD);
10401018
}
10411019
}
10421020

@@ -1045,9 +1023,7 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le
10451023
// check command interface
10461024
EXPECT_LT(0.0, joint_eff_[0]);
10471025
EXPECT_LT(0.0, joint_eff_[1]);
1048-
EXPECT_LT(0.0, state_msg->output.effort[0]);
1049-
EXPECT_LT(0.0, state_msg->output.effort[1]);
1050-
EXPECT_TRUE(std::isnan(state_msg->output.effort[2]));
1026+
EXPECT_TRUE(std::isnan(current_command.effort[2]));
10511027
}
10521028

10531029
executor.cancel();

‎joint_trajectory_controller/test/test_trajectory_controller_utils.hpp‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323

2424
#include "gmock/gmock.h"
2525

26+
#include "control_msgs/msg/joint_trajectory_controller_state.hpp"
2627
#include "hardware_interface/types/hardware_interface_type_values.hpp"
2728
#include "joint_trajectory_controller/joint_trajectory_controller.hpp"
2829
#include "joint_trajectory_controller/trajectory.hpp"
@@ -154,6 +155,12 @@ class TestableJointTrajectoryController
154155
trajectory_msgs::msg::JointTrajectoryPoint get_state_feedback() { return state_current_; }
155156
trajectory_msgs::msg::JointTrajectoryPoint get_state_reference() { return state_desired_; }
156157
trajectory_msgs::msg::JointTrajectoryPoint get_state_error() { return state_error_; }
158+
trajectory_msgs::msg::JointTrajectoryPoint get_current_command() { return command_current_; }
159+
160+
control_msgs::msg::JointTrajectoryControllerState get_state_msg()
161+
{
162+
return state_publisher_->msg_;
163+
}
157164

158165
rclcpp::WaitSet joint_cmd_sub_wait_set_;
159166
};

0 commit comments

Comments
 (0)