Skip to content

Commit

Permalink
select JointTrajectoryControllerState fields based on 'control_msgs' …
Browse files Browse the repository at this point in the history 
…version
  • Loading branch information
@christian-rauch
christian-rauch committed Sep 10, 2024
1 parent 0c7d786 commit 4bf1065
Show file tree
Hide file tree
Showing 4 changed files with 123 additions and 33 deletions.
3 changes: 3 additions & 0 deletions joint_trajectory_controller/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,9 @@ ament_target_dependencies(joint_trajectory_controller PUBLIC ${THIS_PACKAGE_INCL
target_compile_definitions(joint_trajectory_controller PRIVATE "JOINT_TRAJECTORY_CONTROLLER_BUILDING_DLL" "_USE_MATH_DEFINES")
pluginlib_export_plugin_description_file(controller_interface joint_trajectory_plugin.xml)

# forward 'control_msgs' major version to handle different API
target_compile_definitions(joint_trajectory_controller PUBLIC control_msgs_VERSION_MAJOR=${control_msgs_VERSION_MAJOR})

if(BUILD_TESTING)
find_package(ament_cmake_gmock REQUIRED)
find_package(controller_manager REQUIRED)
Expand Down
36 changes: 36 additions & 0 deletions joint_trajectory_controller/src/joint_trajectory_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -681,6 +681,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(

state_publisher_->lock();
state_publisher_->msg_.joint_names = params_.joints;
#if control_msgs_VERSION_MAJOR == 4
state_publisher_->msg_.desired.positions.resize(dof_);
state_publisher_->msg_.desired.velocities.resize(dof_);
state_publisher_->msg_.desired.accelerations.resize(dof_);
Expand All @@ -696,6 +697,23 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
state_publisher_->msg_.actual.accelerations.resize(dof_);
state_publisher_->msg_.error.accelerations.resize(dof_);
}
#elif control_msgs_VERSION_MAJOR >= 5
state_publisher_->msg_.reference.positions.resize(dof_);
state_publisher_->msg_.reference.velocities.resize(dof_);
state_publisher_->msg_.reference.accelerations.resize(dof_);
state_publisher_->msg_.feedback.positions.resize(dof_);
state_publisher_->msg_.error.positions.resize(dof_);
if (has_velocity_state_interface_)
{
state_publisher_->msg_.feedback.velocities.resize(dof_);
state_publisher_->msg_.error.velocities.resize(dof_);
}
if (has_acceleration_state_interface_)
{
state_publisher_->msg_.feedback.accelerations.resize(dof_);
state_publisher_->msg_.error.accelerations.resize(dof_);
}
#endif
state_publisher_->unlock();

last_state_publish_time_ = get_node()->now();
Expand Down Expand Up @@ -908,6 +926,7 @@ void JointTrajectoryController::publish_state(
{
last_state_publish_time_ = get_node()->now();
state_publisher_->msg_.header.stamp = last_state_publish_time_;
#if control_msgs_VERSION_MAJOR == 4
state_publisher_->msg_.desired.positions = desired_state.positions;
state_publisher_->msg_.desired.velocities = desired_state.velocities;
state_publisher_->msg_.desired.accelerations = desired_state.accelerations;
Expand All @@ -923,6 +942,23 @@ void JointTrajectoryController::publish_state(
state_publisher_->msg_.actual.accelerations = current_state.accelerations;
state_publisher_->msg_.error.accelerations = state_error.accelerations;
}
#elif control_msgs_VERSION_MAJOR >= 5
state_publisher_->msg_.reference.positions = desired_state.positions;
state_publisher_->msg_.reference.velocities = desired_state.velocities;
state_publisher_->msg_.reference.accelerations = desired_state.accelerations;
state_publisher_->msg_.feedback.positions = current_state.positions;
state_publisher_->msg_.error.positions = state_error.positions;
if (has_velocity_state_interface_)
{
state_publisher_->msg_.feedback.velocities = current_state.velocities;
state_publisher_->msg_.error.velocities = state_error.velocities;
}
if (has_acceleration_state_interface_)
{
state_publisher_->msg_.feedback.accelerations = current_state.accelerations;
state_publisher_->msg_.error.accelerations = state_error.accelerations;
}
#endif

state_publisher_->unlockAndPublish();
}
Expand Down
109 changes: 76 additions & 33 deletions joint_trajectory_controller/test/test_trajectory_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -470,32 +470,51 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency)
EXPECT_EQ(joint_names_[i], state->joint_names[i]);
}

#if control_msgs_VERSION_MAJOR == 4
// desired | reference
const std::vector<double> &ref_pos = state->desired.positions;
const std::vector<double> &ref_vel = state->desired.velocities;
const std::vector<double> &ref_acc = state->desired.accelerations;
// actual | feedback
const std::vector<double> &fb_pos = state->actual.positions;
const std::vector<double> &fb_vel = state->actual.velocities;
const std::vector<double> &fb_acc = state->actual.accelerations;
#elif control_msgs_VERSION_MAJOR >= 5
// desired | reference
const std::vector<double> &ref_pos = state->reference.positions;
const std::vector<double> &ref_vel = state->reference.velocities;
const std::vector<double> &ref_acc = state->reference.accelerations;
// actual | feedback
const std::vector<double> &fb_pos = state->feedback.positions;
const std::vector<double> &fb_vel = state->feedback.velocities;
const std::vector<double> &fb_acc = state->feedback.accelerations;
#endif

// No trajectory by default, no desired state or error
EXPECT_TRUE(state->desired.positions.empty() || state->desired.positions == INITIAL_POS_JOINTS);
EXPECT_TRUE(state->desired.velocities.empty() || state->desired.velocities == INITIAL_VEL_JOINTS);
EXPECT_TRUE(
state->desired.accelerations.empty() || state->desired.accelerations == INITIAL_EFF_JOINTS);
EXPECT_TRUE(ref_pos.empty() || ref_pos == INITIAL_POS_JOINTS);
EXPECT_TRUE(ref_vel.empty() || ref_vel == INITIAL_VEL_JOINTS);
EXPECT_TRUE(ref_acc.empty() || ref_acc == INITIAL_EFF_JOINTS);

EXPECT_EQ(n_joints, state->actual.positions.size());
EXPECT_EQ(n_joints, fb_pos.size());
if (
std::find(state_interface_types_.begin(), state_interface_types_.end(), "velocity") ==
state_interface_types_.end())
{
EXPECT_TRUE(state->actual.velocities.empty());
EXPECT_TRUE(fb_vel.empty());
}
else
{
EXPECT_EQ(n_joints, state->actual.velocities.size());
EXPECT_EQ(n_joints, fb_vel.size());
}
if (
std::find(state_interface_types_.begin(), state_interface_types_.end(), "acceleration") ==
state_interface_types_.end())
{
EXPECT_TRUE(state->actual.accelerations.empty());
EXPECT_TRUE(fb_acc.empty());
}
else
{
EXPECT_EQ(n_joints, state->actual.accelerations.size());
EXPECT_EQ(n_joints, fb_acc.size());
}

std::vector<double> zeros(3, 0);
Expand Down Expand Up @@ -542,26 +561,38 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized)

const auto allowed_delta = 0.1;

#if control_msgs_VERSION_MAJOR == 4
// desired | reference
const std::vector<double> &ref_pos = state_msg->desired.positions;
// actual | feedback
const std::vector<double> &fb_pos = state_msg->actual.positions;
#elif control_msgs_VERSION_MAJOR >= 5
// desired | reference
const std::vector<double> &ref_pos = state_msg->reference.positions;
// actual | feedback
const std::vector<double> &fb_pos = state_msg->feedback.positions;
#endif

// no update of state_interface
EXPECT_EQ(state_msg->actual.positions, INITIAL_POS_JOINTS);
EXPECT_EQ(fb_pos, INITIAL_POS_JOINTS);

// has the msg the correct vector sizes?
EXPECT_EQ(n_joints, state_msg->desired.positions.size());
EXPECT_EQ(n_joints, state_msg->actual.positions.size());
EXPECT_EQ(n_joints, ref_pos.size());
EXPECT_EQ(n_joints, fb_pos.size());
EXPECT_EQ(n_joints, state_msg->error.positions.size());

// are the correct desired positions used?
EXPECT_NEAR(points[0][0], state_msg->desired.positions[0], allowed_delta);
EXPECT_NEAR(points[0][1], state_msg->desired.positions[1], allowed_delta);
EXPECT_NEAR(points[0][2], state_msg->desired.positions[2], 3 * allowed_delta);
EXPECT_NEAR(points[0][0], ref_pos[0], allowed_delta);
EXPECT_NEAR(points[0][1], ref_pos[1], allowed_delta);
EXPECT_NEAR(points[0][2], ref_pos[2], 3 * allowed_delta);

// no normalization of position error
EXPECT_NEAR(
state_msg->error.positions[0], state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0], EPS);
state_msg->error.positions[0], ref_pos[0] - INITIAL_POS_JOINTS[0], EPS);
EXPECT_NEAR(
state_msg->error.positions[1], state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1], EPS);
state_msg->error.positions[1], ref_pos[1] - INITIAL_POS_JOINTS[1], EPS);
EXPECT_NEAR(
state_msg->error.positions[2], state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2], EPS);
state_msg->error.positions[2], ref_pos[2] - INITIAL_POS_JOINTS[2], EPS);

if (traj_controller_->has_position_command_interface())
{
Expand All @@ -583,14 +614,14 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized)
{
// we expect u = k_p * (s_d-s)
EXPECT_NEAR(
k_p * (state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
k_p * (ref_pos[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
k_p * allowed_delta);
EXPECT_NEAR(
k_p * (state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
k_p * (ref_pos[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
k_p * allowed_delta);
// no normalization of position error
EXPECT_NEAR(
k_p * (state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2],
k_p * (ref_pos[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2],
k_p * allowed_delta);
}
}
Expand Down Expand Up @@ -642,27 +673,39 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized)

const auto allowed_delta = 0.1;

#if control_msgs_VERSION_MAJOR == 4
// desired | reference
const std::vector<double> &ref_pos = state_msg->desired.positions;
// actual | feedback
const std::vector<double> &fb_pos = state_msg->actual.positions;
#elif control_msgs_VERSION_MAJOR >= 5
// desired | reference
const std::vector<double> &ref_pos = state_msg->reference.positions;
// actual | feedback
const std::vector<double> &fb_pos = state_msg->feedback.positions;
#endif

// no update of state_interface
EXPECT_EQ(state_msg->actual.positions, INITIAL_POS_JOINTS);
EXPECT_EQ(fb_pos, INITIAL_POS_JOINTS);

// has the msg the correct vector sizes?
EXPECT_EQ(n_joints, state_msg->desired.positions.size());
EXPECT_EQ(n_joints, state_msg->actual.positions.size());
EXPECT_EQ(n_joints, ref_pos.size());
EXPECT_EQ(n_joints, fb_pos.size());
EXPECT_EQ(n_joints, state_msg->error.positions.size());

// are the correct desired positions used?
EXPECT_NEAR(points[0][0], state_msg->desired.positions[0], allowed_delta);
EXPECT_NEAR(points[0][1], state_msg->desired.positions[1], allowed_delta);
EXPECT_NEAR(points[0][2], state_msg->desired.positions[2], 3 * allowed_delta);
EXPECT_NEAR(points[0][0], ref_pos[0], allowed_delta);
EXPECT_NEAR(points[0][1], ref_pos[1], allowed_delta);
EXPECT_NEAR(points[0][2], ref_pos[2], 3 * allowed_delta);

// is error.positions[2] normalized?
EXPECT_NEAR(
state_msg->error.positions[0], state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0], EPS);
state_msg->error.positions[0], ref_pos[0] - INITIAL_POS_JOINTS[0], EPS);
EXPECT_NEAR(
state_msg->error.positions[1], state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1], EPS);
state_msg->error.positions[1], ref_pos[1] - INITIAL_POS_JOINTS[1], EPS);
EXPECT_NEAR(
state_msg->error.positions[2],
state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS);
ref_pos[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS);

if (traj_controller_->has_position_command_interface())
{
Expand All @@ -684,14 +727,14 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized)
EXPECT_GE(0.0, joint_vel_[2]);
// we expect u = k_p * (s_d-s) for positions[0] and positions[1]
EXPECT_NEAR(
k_p * (state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
k_p * (ref_pos[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
k_p * allowed_delta);
EXPECT_NEAR(
k_p * (state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
k_p * (ref_pos[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
k_p * allowed_delta);
// is error of positions[2] normalized?
EXPECT_NEAR(
k_p * (state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2],
k_p * (ref_pos[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2],
k_p * allowed_delta);
}
else
Expand Down
8 changes: 8 additions & 0 deletions joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -388,7 +388,11 @@ class TrajectoryControllerTest : public ::testing::Test
// TODO(anyone): add checking for velocties and accelerations
if (traj_controller_->has_position_command_interface())
{
#if control_msgs_VERSION_MAJOR == 4
EXPECT_NEAR(expected_actual.positions[i], state_msg->actual.positions[i], allowed_delta);
#elif control_msgs_VERSION_MAJOR >= 5
EXPECT_NEAR(expected_actual.positions[i], state_msg->feedback.positions[i], allowed_delta);
#endif
}
}

Expand All @@ -398,7 +402,11 @@ class TrajectoryControllerTest : public ::testing::Test
// TODO(anyone): add checking for velocties and accelerations
if (traj_controller_->has_position_command_interface())
{
#if control_msgs_VERSION_MAJOR == 4
EXPECT_NEAR(expected_desired.positions[i], state_msg->desired.positions[i], allowed_delta);
#elif control_msgs_VERSION_MAJOR >= 5
EXPECT_NEAR(expected_desired.positions[i], state_msg->reference.positions[i], allowed_delta);
#endif
}
}
}
Expand Down

0 comments on commit 4bf1065

Please sign in to comment.