From 09fd49ab04095789c57d810998de8417635f2fd3 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Tue, 28 Nov 2023 08:34:40 +0000
Subject: [PATCH 01/36] First changes to JTC accepting num_cmd_joints<dof
---
.../joint_trajectory_controller.hpp | 2 +
.../trajectory.hpp | 12 +
.../src/joint_trajectory_controller.cpp | 82 +++--
.../test/test_trajectory_controller.cpp | 294 +++++++++++++++++-
4 files changed, 358 insertions(+), 32 deletions(-)
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
index d16e4f8267..d4fc3a8ce1 100644
--- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
@@ -131,6 +131,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
// Degrees of freedom
size_t dof_;
+ size_t num_cmd_joints_;
+ std::vector<size_t> map_cmd_to_joints_;
// Storing command joint names for interfaces
std::vector<std::string> command_joint_names_;
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp
index 3bd4873a31..7164d973ca 100644
--- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp
@@ -15,6 +15,7 @@
#ifndef JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_
#define JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_
+#include <algorithm>
#include <memory>
#include <vector>
@@ -189,6 +190,17 @@ inline std::vector<size_t> mapping(const T & t1, const T & t2)
return mapping_vector;
}
+/**
+ * \return True if \p B is a subset of \p A, false otherwise.
+ */
+template <typename T>
+bool is_subset(std::vector<T> A, std::vector<T> B)
+{
+ std::sort(A.begin(), A.end());
+ std::sort(B.begin(), B.end());
+ return std::includes(A.begin(), A.end(), B.begin(), B.end());
+}
+
} // namespace joint_trajectory_controller
#endif // JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
index 59e2c408c5..5216ddcd83 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp
+++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -46,7 +46,7 @@
namespace joint_trajectory_controller
{
JointTrajectoryController::JointTrajectoryController()
-: controller_interface::ControllerInterface(), dof_(0)
+: controller_interface::ControllerInterface(), dof_(0), num_cmd_joints_(0)
{
}
@@ -72,16 +72,16 @@ JointTrajectoryController::command_interface_configuration() const
{
controller_interface::InterfaceConfiguration conf;
conf.type = controller_interface::interface_configuration_type::INDIVIDUAL;
- if (dof_ == 0)
+ if (num_cmd_joints_ == 0)
{
fprintf(
stderr,
- "During ros2_control interface configuration, degrees of freedom is not valid;"
- " it should be positive. Actual DOF is %zu\n",
- dof_);
+ "During ros2_control interface configuration, number of command interfaces is not valid;"
+ " it should be positive. Actual number is %zu\n",
+ num_cmd_joints_);
std::exit(EXIT_FAILURE);
}
- conf.names.reserve(dof_ * params_.command_interfaces.size());
+ conf.names.reserve(num_cmd_joints_ * params_.command_interfaces.size());
for (const auto & joint_name : command_joint_names_)
{
for (const auto & interface_type : params_.command_interfaces)
@@ -179,9 +179,9 @@ controller_interface::return_type JointTrajectoryController::update(
auto assign_interface_from_point =
[&](auto & joint_interface, const std::vector<double> & trajectory_point_interface)
{
- for (size_t index = 0; index < dof_; ++index)
+ for (size_t index = 0; index < num_cmd_joints_; ++index)
{
- joint_interface[index].get().set_value(trajectory_point_interface[index]);
+ joint_interface[index].get().set_value(trajectory_point_interface[map_cmd_to_joints_[index]]);
}
};
@@ -280,12 +280,13 @@ controller_interface::return_type JointTrajectoryController::update(
if (use_closed_loop_pid_adapter_)
{
// Update PIDs
- for (auto i = 0ul; i < dof_; ++i)
+ for (auto i = 0ul; i < num_cmd_joints_; ++i)
{
- tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) +
- pids_[i]->computeCommand(
- state_error_.positions[i], state_error_.velocities[i],
- (uint64_t)period.nanoseconds());
+ size_t index = map_cmd_to_joints_[i];
+ tmp_command_[index] = (state_desired_.velocities[index] * ff_velocity_scale_[i]) +
+ pids_[i]->computeCommand(
+ state_error_.positions[index], state_error_.velocities[index],
+ (uint64_t)period.nanoseconds());
}
}
@@ -453,9 +454,10 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto
auto assign_point_from_interface =
[&](std::vector<double> & trajectory_point_interface, const auto & joint_interface)
{
- for (size_t index = 0; index < dof_; ++index)
+ for (size_t index = 0; index < num_cmd_joints_; ++index)
{
- trajectory_point_interface[index] = joint_interface[index].get().get_value();
+ trajectory_point_interface[map_cmd_to_joints_[index]] =
+ joint_interface[index].get().get_value();
}
};
@@ -524,7 +526,7 @@ bool JointTrajectoryController::read_commands_from_command_interfaces(
auto assign_point_from_interface =
[&](std::vector<double> & trajectory_point_interface, const auto & joint_interface)
{
- for (size_t index = 0; index < dof_; ++index)
+ for (size_t index = 0; index < num_cmd_joints_; ++index)
{
trajectory_point_interface[index] = joint_interface[index].get().get_value();
}
@@ -665,8 +667,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
if (params_.joints.empty())
{
- // TODO(destogl): is this correct? Can we really move-on if no joint names are not provided?
RCLCPP_WARN(logger, "'joints' parameter is empty.");
+ return CallbackReturn::FAILURE;
}
command_joint_names_ = params_.command_joints;
@@ -677,12 +679,34 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
RCLCPP_INFO(
logger, "No specific joint names are used for command interfaces. Using 'joints' parameter.");
}
- else if (command_joint_names_.size() != params_.joints.size())
+ num_cmd_joints_ = command_joint_names_.size();
+
+ if (num_cmd_joints_ > dof_)
{
RCLCPP_ERROR(
- logger, "'command_joints' parameter has to have the same size as 'joints' parameter.");
+ logger, "'command_joints' parameter must not have greater size as 'joints' parameter.");
return CallbackReturn::FAILURE;
}
+ else if (num_cmd_joints_ < dof_)
+ {
+ if (is_subset(params_.joints, command_joint_names_) == false)
+ {
+ RCLCPP_ERROR(
+ logger,
+ "'command_joints' parameter must be a subset of 'joints' parameter, if their size is not "
+ "equal.");
+ return CallbackReturn::FAILURE;
+ }
+ }
+ // create a map for the command joints, trivial if they are the same
+ map_cmd_to_joints_ = mapping(command_joint_names_, params_.joints);
+ for (size_t i = 0; i < command_joint_names_.size(); i++)
+ {
+ RCLCPP_INFO(
+ logger, "Command joint %lu: '%s' maps to joint %lu: '%s'.", i,
+ command_joint_names_[i].c_str(), map_cmd_to_joints_[i],
+ params_.joints[map_cmd_to_joints_[i]].c_str());
+ }
if (params_.command_interfaces.empty())
{
@@ -712,9 +736,9 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
if (use_closed_loop_pid_adapter_)
{
- pids_.resize(dof_);
- ff_velocity_scale_.resize(dof_);
- tmp_command_.resize(dof_, 0.0);
+ pids_.resize(num_cmd_joints_);
+ ff_velocity_scale_.resize(num_cmd_joints_);
+ tmp_command_.resize(dof_, std::numeric_limits<double>::quiet_NaN());
update_pids();
}
@@ -902,7 +926,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
command_interfaces_, command_joint_names_, interface, joint_command_interface_[index]))
{
RCLCPP_ERROR(
- get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", dof_,
+ get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", num_cmd_joints_,
interface.c_str(), joint_command_interface_[index].size());
return CallbackReturn::ERROR;
}
@@ -976,7 +1000,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
controller_interface::CallbackReturn JointTrajectoryController::on_deactivate(
const rclcpp_lifecycle::State &)
{
- for (size_t index = 0; index < dof_; ++index)
+ for (size_t index = 0; index < num_cmd_joints_; ++index)
{
if (has_position_command_interface_)
{
@@ -1491,14 +1515,14 @@ bool JointTrajectoryController::contains_interface_type(
void JointTrajectoryController::resize_joint_trajectory_point(
trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size)
{
- point.positions.resize(size, 0.0);
+ point.positions.resize(size, std::numeric_limits<double>::quiet_NaN());
if (has_velocity_state_interface_)
{
- point.velocities.resize(size, 0.0);
+ point.velocities.resize(size, std::numeric_limits<double>::quiet_NaN());
}
if (has_acceleration_state_interface_)
{
- point.accelerations.resize(size, 0.0);
+ point.accelerations.resize(size, std::numeric_limits<double>::quiet_NaN());
}
}
@@ -1530,9 +1554,9 @@ bool JointTrajectoryController::has_active_trajectory() const
void JointTrajectoryController::update_pids()
{
- for (size_t i = 0; i < dof_; ++i)
+ for (size_t i = 0; i < num_cmd_joints_; ++i)
{
- const auto & gains = params_.gains.joints_map.at(params_.joints[i]);
+ const auto & gains = params_.gains.joints_map.at(command_joint_names_[i]);
if (pids_[i])
{
// update PIDs with gains from ROS parameters
diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp
index 05010c562c..4c7aa95684 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller.cpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp
@@ -58,6 +58,10 @@ using test_trajectory_controllers::TrajectoryControllerTestParameterized;
void spin(rclcpp::executors::MultiThreadedExecutor * exe) { exe->spin(); }
+// Floating-point value comparison threshold
+const double EPS = 1e-6;
+
+#if 0
TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands)
{
rclcpp::executors::MultiThreadedExecutor executor;
@@ -116,7 +120,81 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command
compare_joints(joint_names_, command_joint_names_);
}
+#endif
+
+TEST_P(
+ TrajectoryControllerTestParameterized, check_interface_names_with_command_joints_less_than_dof)
+{
+ rclcpp::executors::MultiThreadedExecutor executor;
+ // set command_joints parameter
+ std::vector<std::string> command_joint_names{joint_names_[0], joint_names_[1]};
+ const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names);
+ SetUpTrajectoryController(executor, {command_joint_names_param});
+ const auto state = traj_controller_->get_node()->configure();
+ ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE);
+
+ std::vector<std::string> state_interface_names;
+ state_interface_names.reserve(joint_names_.size() * state_interface_types_.size());
+ for (const auto & joint : joint_names_)
+ {
+ for (const auto & interface : state_interface_types_)
+ {
+ state_interface_names.push_back(joint + "/" + interface);
+ }
+ }
+ auto state_interfaces = traj_controller_->state_interface_configuration();
+ EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL);
+ EXPECT_EQ(state_interfaces.names.size(), joint_names_.size() * state_interface_types_.size());
+ ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names));
+
+ std::vector<std::string> command_interface_names;
+ command_interface_names.reserve(command_joint_names.size() * command_interface_types_.size());
+ for (const auto & joint : command_joint_names)
+ {
+ for (const auto & interface : command_interface_types_)
+ {
+ command_interface_names.push_back(joint + "/" + interface);
+ }
+ }
+ auto command_interfaces = traj_controller_->command_interface_configuration();
+ EXPECT_EQ(
+ command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL);
+ EXPECT_EQ(
+ command_interfaces.names.size(), command_joint_names.size() * command_interface_types_.size());
+ ASSERT_THAT(
+ command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names));
+}
+
+TEST_P(
+ TrajectoryControllerTestParameterized, check_interface_names_with_command_joints_greater_than_dof)
+{
+ rclcpp::executors::MultiThreadedExecutor executor;
+ // set command_joints parameter
+ std::vector<std::string> command_joint_names{
+ joint_names_[0], joint_names_[1], joint_names_[2], "joint_4"};
+ const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names);
+ SetUpTrajectoryController(executor, {command_joint_names_param});
+
+ const auto state = traj_controller_->get_node()->configure();
+ ASSERT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED);
+}
+
+TEST_P(
+ TrajectoryControllerTestParameterized,
+ check_interface_names_with_command_joints_different_than_dof)
+{
+ rclcpp::executors::MultiThreadedExecutor executor;
+ // set command_joints parameter
+ std::vector<std::string> command_joint_names{joint_names_[0], "joint_4"};
+ const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names);
+ SetUpTrajectoryController(executor, {command_joint_names_param});
+
+ const auto state = traj_controller_->get_node()->configure();
+ ASSERT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED);
+}
+
+#if 0
TEST_P(TrajectoryControllerTestParameterized, activate)
{
rclcpp::executors::MultiThreadedExecutor executor;
@@ -354,7 +432,107 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency)
EXPECT_EQ(n_joints, state->output.effort.size());
}
}
+#endif
+
+TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency_command_joints_less_than_dof)
+{
+ rclcpp::executors::SingleThreadedExecutor executor;
+ std::vector<std::string> command_joint_names{joint_names_[0], joint_names_[1]};
+ const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names);
+ SetUpAndActivateTrajectoryController(executor, {command_joint_names_param});
+ subscribeToState();
+ updateController();
+
+ // Spin to receive latest state
+ executor.spin_some();
+ auto state = getState();
+
+ size_t n_joints = joint_names_.size();
+
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ EXPECT_EQ(joint_names_[i], state->joint_names[i]);
+ }
+ // No trajectory by default, no reference state or error
+ EXPECT_TRUE(
+ state->reference.positions.empty() || state->reference.positions == INITIAL_POS_JOINTS);
+ EXPECT_TRUE(
+ state->reference.velocities.empty() || state->reference.velocities == INITIAL_VEL_JOINTS);
+ EXPECT_TRUE(
+ state->reference.accelerations.empty() || state->reference.accelerations == INITIAL_EFF_JOINTS);
+
+ std::vector<double> zeros(3, 0);
+ EXPECT_EQ(state->error.positions, zeros);
+ EXPECT_TRUE(state->error.velocities.empty() || state->error.velocities == zeros);
+ EXPECT_TRUE(state->error.accelerations.empty() || state->error.accelerations == zeros);
+
+ // expect feedback including all state_interfaces
+ EXPECT_EQ(n_joints, state->feedback.positions.size());
+ if (
+ std::find(state_interface_types_.begin(), state_interface_types_.end(), "velocity") ==
+ state_interface_types_.end())
+ {
+ EXPECT_TRUE(state->feedback.velocities.empty());
+ }
+ else
+ {
+ EXPECT_EQ(n_joints, state->feedback.velocities.size());
+ }
+ if (
+ std::find(state_interface_types_.begin(), state_interface_types_.end(), "acceleration") ==
+ state_interface_types_.end())
+ {
+ EXPECT_TRUE(state->feedback.accelerations.empty());
+ }
+ else
+ {
+ EXPECT_EQ(n_joints, state->feedback.accelerations.size());
+ }
+
+ // expect output including all command_interfaces
+ if (
+ std::find(command_interface_types_.begin(), command_interface_types_.end(), "position") ==
+ command_interface_types_.end())
+ {
+ EXPECT_TRUE(state->output.positions.empty());
+ }
+ else
+ {
+ EXPECT_EQ(n_joints, state->output.positions.size());
+ }
+ if (
+ std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") ==
+ command_interface_types_.end())
+ {
+ EXPECT_TRUE(state->output.velocities.empty());
+ }
+ else
+ {
+ EXPECT_EQ(n_joints, state->output.velocities.size());
+ }
+ if (
+ std::find(command_interface_types_.begin(), command_interface_types_.end(), "acceleration") ==
+ command_interface_types_.end())
+ {
+ EXPECT_TRUE(state->output.accelerations.empty());
+ }
+ else
+ {
+ EXPECT_EQ(n_joints, state->output.accelerations.size());
+ }
+ if (
+ std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") ==
+ command_interface_types_.end())
+ {
+ EXPECT_TRUE(state->output.effort.empty());
+ }
+ else
+ {
+ EXPECT_EQ(n_joints, state->output.effort.size());
+ }
+}
+#if 0
/**
* @brief check if dynamic parameters are updated
*/
@@ -464,8 +642,6 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup)
executor.cancel();
}
-// Floating-point value comparison threshold
-const double EPS = 1e-6;
/**
* @brief check if position error of revolute joints are angle_wraparound if not configured so
*/
@@ -694,7 +870,119 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound)
executor.cancel();
}
+#endif
+
+/**
+ * @brief check if position error of revolute joints are normalized if not configured so
+ */
+TEST_P(TrajectoryControllerTestParameterized, position_error_command_joints_less_than_dof)
+{
+ rclcpp::executors::MultiThreadedExecutor executor;
+ constexpr double k_p = 10.0;
+ std::vector<std::string> command_joint_names{joint_names_[0], joint_names_[1]};
+ const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names);
+ std::vector<rclcpp::Parameter> params = {
+ rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true), command_joint_names_param};
+ SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, false);
+ subscribeToState();
+
+ size_t n_joints = joint_names_.size();
+
+ // send msg for all joints
+ constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
+ builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)};
+ // *INDENT-OFF*
+ std::vector<std::vector<double>> points{
+ {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}};
+ std::vector<std::vector<double>> points_velocities{
+ {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
+ // *INDENT-ON*
+ publish(time_from_start, points, rclcpp::Time(), {}, points_velocities);
+ traj_controller_->wait_for_trajectory(executor);
+ // first update
+ updateController(rclcpp::Duration(FIRST_POINT_TIME));
+
+ // Spin to receive latest state
+ executor.spin_some();
+ auto state_msg = getState();
+ ASSERT_TRUE(state_msg);
+
+ const auto allowed_delta = 0.1;
+
+ // no update of state_interface
+ EXPECT_EQ(state_msg->feedback.positions, INITIAL_POS_JOINTS);
+
+ // has the msg the correct vector sizes?
+ EXPECT_EQ(n_joints, state_msg->reference.positions.size());
+ EXPECT_EQ(n_joints, state_msg->feedback.positions.size());
+ EXPECT_EQ(n_joints, state_msg->error.positions.size());
+
+ // are the correct reference positions used?
+ EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta);
+ EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta);
+ EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta);
+
+ // no normalization of position error
+ EXPECT_NEAR(
+ state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS);
+ EXPECT_NEAR(
+ state_msg->error.positions[1], state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1], EPS);
+ EXPECT_NEAR(
+ state_msg->error.positions[2], state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2], EPS);
+
+ if (traj_controller_->has_position_command_interface())
+ {
+ // check command interface
+ EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta);
+ EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta);
+ EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta);
+ EXPECT_NEAR(points[0][0], state_msg->output.positions[0], allowed_delta);
+ EXPECT_NEAR(points[0][1], state_msg->output.positions[1], allowed_delta);
+ EXPECT_NEAR(points[0][2], state_msg->output.positions[2], allowed_delta);
+ }
+
+ if (traj_controller_->has_velocity_command_interface())
+ {
+ // check command interface
+ EXPECT_LT(0.0, joint_vel_[0]);
+ EXPECT_LT(0.0, joint_vel_[1]);
+ EXPECT_LT(0.0, joint_vel_[2]);
+ EXPECT_LT(0.0, state_msg->output.velocities[0]);
+ EXPECT_LT(0.0, state_msg->output.velocities[1]);
+ EXPECT_LT(0.0, state_msg->output.velocities[2]);
+
+ // use_closed_loop_pid_adapter_
+ if (traj_controller_->use_closed_loop_pid_adapter())
+ {
+ // we expect u = k_p * (s_d-s)
+ EXPECT_NEAR(
+ k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
+ k_p * allowed_delta);
+ EXPECT_NEAR(
+ k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
+ k_p * allowed_delta);
+ // no normalization of position error
+ EXPECT_NEAR(
+ k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2],
+ k_p * allowed_delta);
+ }
+ }
+
+ if (traj_controller_->has_effort_command_interface())
+ {
+ // check command interface
+ EXPECT_LT(0.0, joint_eff_[0]);
+ EXPECT_LT(0.0, joint_eff_[1]);
+ EXPECT_LT(0.0, joint_eff_[2]);
+ EXPECT_LT(0.0, state_msg->output.effort[0]);
+ EXPECT_LT(0.0, state_msg->output.effort[1]);
+ EXPECT_LT(0.0, state_msg->output.effort[2]);
+ }
+
+ executor.cancel();
+}
+#if 0
/**
* @brief cmd_timeout must be greater than constraints.goal_time
*/
@@ -1836,7 +2124,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail)
// it should be still holding the old point
expectCommandPoint(hold_position);
}
-
+#endif
// position controllers
INSTANTIATE_TEST_SUITE_P(
PositionTrajectoryControllers, TrajectoryControllerTestParameterized,
From eb64893a22f7a0ad91fae0744e746f86afa39087 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Tue, 28 Nov 2023 08:34:40 +0000
Subject: [PATCH 02/36] Map command joints to dof
---
.../joint_trajectory_controller.hpp | 4 +-
.../trajectory.hpp | 13 +-
.../src/joint_trajectory_controller.cpp | 54 +++++---
.../test/test_trajectory_controller.cpp | 128 +++++++++++++++---
4 files changed, 145 insertions(+), 54 deletions(-)
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
index d4fc3a8ce1..1df3a7eb8e 100644
--- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
@@ -294,9 +294,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
void init_hold_position_msg();
void resize_joint_trajectory_point(
- trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size);
+ trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double vale = 0.0);
void resize_joint_trajectory_point_command(
- trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size);
+ trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double vale = 0.0);
};
} // namespace joint_trajectory_controller
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp
index 7164d973ca..e7995efb66 100644
--- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp
@@ -161,7 +161,7 @@ class Trajectory
/**
* \return The map between \p t1 indices (implicitly encoded in return vector indices) to \p t2
* indices. If \p t1 is <tt>"{C, B}"</tt> and \p t2 is <tt>"{A, B, C, D}"</tt>, the associated
- * mapping vector is <tt>"{2, 1}"</tt>.
+ * mapping vector is <tt>"{2, 1}"</tt>. return empty vector if \p t1 is not a subset of \p t2.
*/
template <class T>
inline std::vector<size_t> mapping(const T & t1, const T & t2)
@@ -190,17 +190,6 @@ inline std::vector<size_t> mapping(const T & t1, const T & t2)
return mapping_vector;
}
-/**
- * \return True if \p B is a subset of \p A, false otherwise.
- */
-template <typename T>
-bool is_subset(std::vector<T> A, std::vector<T> B)
-{
- std::sort(A.begin(), A.end());
- std::sort(B.begin(), B.end());
- return std::includes(A.begin(), A.end(), B.begin(), B.end());
-}
-
} // namespace joint_trajectory_controller
#endif // JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
index 5216ddcd83..f076b09931 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp
+++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -18,6 +18,7 @@
#include <chrono>
#include <functional>
#include <memory>
+#include <numeric>
#include <ostream>
#include <ratio>
#include <string>
@@ -528,7 +529,8 @@ bool JointTrajectoryController::read_commands_from_command_interfaces(
{
for (size_t index = 0; index < num_cmd_joints_; ++index)
{
- trajectory_point_interface[index] = joint_interface[index].get().get_value();
+ trajectory_point_interface[map_cmd_to_joints_[index]] =
+ joint_interface[index].get().get_value();
}
};
@@ -689,7 +691,9 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
}
else if (num_cmd_joints_ < dof_)
{
- if (is_subset(params_.joints, command_joint_names_) == false)
+ // create a map for the command joints
+ map_cmd_to_joints_ = mapping(command_joint_names_, params_.joints);
+ if (map_cmd_to_joints_.size() != num_cmd_joints_)
{
RCLCPP_ERROR(
logger,
@@ -697,15 +701,19 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
"equal.");
return CallbackReturn::FAILURE;
}
+ for (size_t i = 0; i < command_joint_names_.size(); i++)
+ {
+ RCLCPP_DEBUG(
+ logger, "Command joint %lu: '%s' maps to joint %lu: '%s'.", i,
+ command_joint_names_[i].c_str(), map_cmd_to_joints_[i],
+ params_.joints.at(map_cmd_to_joints_[i]).c_str());
+ }
}
- // create a map for the command joints, trivial if they are the same
- map_cmd_to_joints_ = mapping(command_joint_names_, params_.joints);
- for (size_t i = 0; i < command_joint_names_.size(); i++)
+ else
{
- RCLCPP_INFO(
- logger, "Command joint %lu: '%s' maps to joint %lu: '%s'.", i,
- command_joint_names_[i].c_str(), map_cmd_to_joints_[i],
- params_.joints[map_cmd_to_joints_[i]].c_str());
+ // create a map for the command joints, trivial if the size is the same
+ map_cmd_to_joints_.resize(num_cmd_joints_);
+ std::iota(map_cmd_to_joints_.begin(), map_cmd_to_joints_.end(), 0);
}
if (params_.command_interfaces.empty())
@@ -892,10 +900,12 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
std::bind(&JointTrajectoryController::goal_accepted_callback, this, _1));
resize_joint_trajectory_point(state_current_, dof_);
- resize_joint_trajectory_point_command(command_current_, dof_);
+ resize_joint_trajectory_point_command(
+ command_current_, dof_, std::numeric_limits<double>::quiet_NaN());
resize_joint_trajectory_point(state_desired_, dof_);
resize_joint_trajectory_point(state_error_, dof_);
- resize_joint_trajectory_point(last_commanded_state_, dof_);
+ resize_joint_trajectory_point(
+ last_commanded_state_, dof_, std::numeric_limits<double>::quiet_NaN());
query_state_srv_ = get_node()->create_service<control_msgs::srv::QueryTrajectoryState>(
std::string(get_node()->get_name()) + "/query_state",
@@ -956,7 +966,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
// running already)
trajectory_msgs::msg::JointTrajectoryPoint state;
resize_joint_trajectory_point(state, dof_);
- if (read_state_from_command_interfaces(state))
+ if (dof_ == num_cmd_joints_ && read_state_from_command_interfaces(state))
{
state_current_ = state;
last_commanded_state_ = state;
@@ -1513,37 +1523,37 @@ bool JointTrajectoryController::contains_interface_type(
}
void JointTrajectoryController::resize_joint_trajectory_point(
- trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size)
+ trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double value)
{
- point.positions.resize(size, std::numeric_limits<double>::quiet_NaN());
+ point.positions.resize(size, value);
if (has_velocity_state_interface_)
{
- point.velocities.resize(size, std::numeric_limits<double>::quiet_NaN());
+ point.velocities.resize(size, value);
}
if (has_acceleration_state_interface_)
{
- point.accelerations.resize(size, std::numeric_limits<double>::quiet_NaN());
+ point.accelerations.resize(size, value);
}
}
void JointTrajectoryController::resize_joint_trajectory_point_command(
- trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size)
+ trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double value)
{
if (has_position_command_interface_)
{
- point.positions.resize(size, 0.0);
+ point.positions.resize(size, value);
}
if (has_velocity_command_interface_)
{
- point.velocities.resize(size, 0.0);
+ point.velocities.resize(size, value);
}
if (has_acceleration_command_interface_)
{
- point.accelerations.resize(size, 0.0);
+ point.accelerations.resize(size, value);
}
if (has_effort_command_interface_)
{
- point.effort.resize(size, 0.0);
+ point.effort.resize(size, value);
}
}
@@ -1556,7 +1566,7 @@ void JointTrajectoryController::update_pids()
{
for (size_t i = 0; i < num_cmd_joints_; ++i)
{
- const auto & gains = params_.gains.joints_map.at(command_joint_names_[i]);
+ const auto & gains = params_.gains.joints_map.at(params_.joints.at(map_cmd_to_joints_[i]));
if (pids_[i])
{
// update PIDs with gains from ROS parameters
diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp
index 4c7aa95684..839acf894c 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller.cpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp
@@ -61,7 +61,6 @@ void spin(rclcpp::executors::MultiThreadedExecutor * exe) { exe->spin(); }
// Floating-point value comparison threshold
const double EPS = 1e-6;
-#if 0
TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands)
{
rclcpp::executors::MultiThreadedExecutor executor;
@@ -120,7 +119,6 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command
compare_joints(joint_names_, command_joint_names_);
}
-#endif
TEST_P(
TrajectoryControllerTestParameterized, check_interface_names_with_command_joints_less_than_dof)
@@ -194,7 +192,6 @@ TEST_P(
ASSERT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED);
}
-#if 0
TEST_P(TrajectoryControllerTestParameterized, activate)
{
rclcpp::executors::MultiThreadedExecutor executor;
@@ -432,7 +429,6 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency)
EXPECT_EQ(n_joints, state->output.effort.size());
}
}
-#endif
TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency_command_joints_less_than_dof)
{
@@ -532,7 +528,7 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency_command_jo
EXPECT_EQ(n_joints, state->output.effort.size());
}
}
-#if 0
+
/**
* @brief check if dynamic parameters are updated
*/
@@ -870,12 +866,11 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound)
executor.cancel();
}
-#endif
/**
- * @brief check if position error of revolute joints are normalized if not configured so
+ * @brief check if trajectory error is calculated correctly in case #command-joints < #dof
*/
-TEST_P(TrajectoryControllerTestParameterized, position_error_command_joints_less_than_dof)
+TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_less_than_dof)
{
rclcpp::executors::MultiThreadedExecutor executor;
constexpr double k_p = 10.0;
@@ -936,10 +931,9 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_command_joints_less
// check command interface
EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta);
EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta);
- EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta);
EXPECT_NEAR(points[0][0], state_msg->output.positions[0], allowed_delta);
EXPECT_NEAR(points[0][1], state_msg->output.positions[1], allowed_delta);
- EXPECT_NEAR(points[0][2], state_msg->output.positions[2], allowed_delta);
+ EXPECT_TRUE(std::isnan(state_msg->output.positions[2]));
}
if (traj_controller_->has_velocity_command_interface())
@@ -947,10 +941,9 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_command_joints_less
// check command interface
EXPECT_LT(0.0, joint_vel_[0]);
EXPECT_LT(0.0, joint_vel_[1]);
- EXPECT_LT(0.0, joint_vel_[2]);
EXPECT_LT(0.0, state_msg->output.velocities[0]);
EXPECT_LT(0.0, state_msg->output.velocities[1]);
- EXPECT_LT(0.0, state_msg->output.velocities[2]);
+ EXPECT_TRUE(std::isnan(state_msg->output.velocities[2]));
// use_closed_loop_pid_adapter_
if (traj_controller_->use_closed_loop_pid_adapter())
@@ -962,9 +955,109 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_command_joints_less
EXPECT_NEAR(
k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
k_p * allowed_delta);
- // no normalization of position error
+ }
+ }
+
+ if (traj_controller_->has_effort_command_interface())
+ {
+ // check command interface
+ EXPECT_LT(0.0, joint_eff_[0]);
+ EXPECT_LT(0.0, joint_eff_[1]);
+ EXPECT_LT(0.0, state_msg->output.effort[0]);
+ EXPECT_LT(0.0, state_msg->output.effort[1]);
+ EXPECT_TRUE(std::isnan(state_msg->output.effort[2]));
+ }
+
+ executor.cancel();
+}
+
+/**
+ * @brief check if trajectory error is calculated correctly in case #command-joints < #dof
+ */
+TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_less_than_dof_jumbled)
+{
+ rclcpp::executors::MultiThreadedExecutor executor;
+ constexpr double k_p = 10.0;
+ std::vector<std::string> command_joint_names{joint_names_[1], joint_names_[0]};
+ const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names);
+ std::vector<rclcpp::Parameter> params = {
+ rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true), command_joint_names_param};
+ SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, false);
+ subscribeToState();
+
+ size_t n_joints = joint_names_.size();
+
+ // send msg for all joints
+ constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
+ builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)};
+ // *INDENT-OFF*
+ std::vector<std::vector<double>> points{
+ {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}};
+ std::vector<std::vector<double>> points_velocities{
+ {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
+ // *INDENT-ON*
+ publish(time_from_start, points, rclcpp::Time(), {}, points_velocities);
+ traj_controller_->wait_for_trajectory(executor);
+
+ // first update
+ updateController(rclcpp::Duration(FIRST_POINT_TIME));
+
+ // Spin to receive latest state
+ executor.spin_some();
+ auto state_msg = getState();
+ ASSERT_TRUE(state_msg);
+
+ const auto allowed_delta = 0.1;
+
+ // no update of state_interface
+ EXPECT_EQ(state_msg->feedback.positions, INITIAL_POS_JOINTS);
+
+ // has the msg the correct vector sizes?
+ EXPECT_EQ(n_joints, state_msg->reference.positions.size());
+ EXPECT_EQ(n_joints, state_msg->feedback.positions.size());
+ EXPECT_EQ(n_joints, state_msg->error.positions.size());
+
+ // are the correct reference positions used?
+ EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta);
+ EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta);
+ EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta);
+
+ // no normalization of position error
+ EXPECT_NEAR(
+ state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS);
+ EXPECT_NEAR(
+ state_msg->error.positions[1], state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1], EPS);
+ EXPECT_NEAR(
+ state_msg->error.positions[2], state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2], EPS);
+
+ if (traj_controller_->has_position_command_interface())
+ {
+ // check command interface
+ EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta);
+ EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta);
+ EXPECT_NEAR(points[0][0], state_msg->output.positions[0], allowed_delta);
+ EXPECT_NEAR(points[0][1], state_msg->output.positions[1], allowed_delta);
+ EXPECT_TRUE(std::isnan(state_msg->output.positions[2]));
+ }
+
+ if (traj_controller_->has_velocity_command_interface())
+ {
+ // check command interface
+ EXPECT_LT(0.0, joint_vel_[0]);
+ EXPECT_LT(0.0, joint_vel_[1]);
+ EXPECT_LT(0.0, state_msg->output.velocities[0]);
+ EXPECT_LT(0.0, state_msg->output.velocities[1]);
+ EXPECT_TRUE(std::isnan(state_msg->output.velocities[2]));
+
+ // use_closed_loop_pid_adapter_
+ if (traj_controller_->use_closed_loop_pid_adapter())
+ {
+ // we expect u = k_p * (s_d-s)
EXPECT_NEAR(
- k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2],
+ k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
+ k_p * allowed_delta);
+ EXPECT_NEAR(
+ k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
k_p * allowed_delta);
}
}
@@ -974,15 +1067,14 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_command_joints_less
// check command interface
EXPECT_LT(0.0, joint_eff_[0]);
EXPECT_LT(0.0, joint_eff_[1]);
- EXPECT_LT(0.0, joint_eff_[2]);
EXPECT_LT(0.0, state_msg->output.effort[0]);
EXPECT_LT(0.0, state_msg->output.effort[1]);
- EXPECT_LT(0.0, state_msg->output.effort[2]);
+ EXPECT_TRUE(std::isnan(state_msg->output.effort[2]));
}
executor.cancel();
}
-#if 0
+
/**
* @brief cmd_timeout must be greater than constraints.goal_time
*/
@@ -2124,7 +2216,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail)
// it should be still holding the old point
expectCommandPoint(hold_position);
}
-#endif
+
// position controllers
INSTANTIATE_TEST_SUITE_P(
PositionTrajectoryControllers, TrajectoryControllerTestParameterized,
From d1d55d3820cff1a4d9be0a65f6af6b87850070de Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Tue, 28 Nov 2023 08:34:40 +0000
Subject: [PATCH 03/36] Remove not needed included
---
.../include/joint_trajectory_controller/trajectory.hpp | 1 -
1 file changed, 1 deletion(-)
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp
index e7995efb66..189e2a82fd 100644
--- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp
@@ -15,7 +15,6 @@
#ifndef JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_
#define JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_
-#include <algorithm>
#include <memory>
#include <vector>
From 1327fd9dcbbd40ad7595070b1399a86001548f0e Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Tue, 28 Nov 2023 08:36:25 +0000
Subject: [PATCH 04/36] Add some comments to the tests
---
.../test/test_trajectory_controller.cpp | 12 ++++++++++--
1 file changed, 10 insertions(+), 2 deletions(-)
diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp
index 839acf894c..2227e03ec0 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller.cpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp
@@ -120,6 +120,9 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command
compare_joints(joint_names_, command_joint_names_);
}
+/**
+ * \brief same as check_interface_names_with_command_joints but with #command-joints < #dof
+ */
TEST_P(
TrajectoryControllerTestParameterized, check_interface_names_with_command_joints_less_than_dof)
{
@@ -329,7 +332,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param
}
/**
- * @brief test if correct topic is received
+ * @brief test if correct topic is received, consistent with parameters
*
* this test doesn't use class variables but subscribes to the state topic
*/
@@ -430,6 +433,10 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency)
}
}
+/**
+ * @brief same as state_topic_consistency but with #command-joints < #dof
+ */
+
TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency_command_joints_less_than_dof)
{
rclcpp::executors::SingleThreadedExecutor executor;
@@ -972,7 +979,8 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le
}
/**
- * @brief check if trajectory error is calculated correctly in case #command-joints < #dof
+ * @brief check if trajectory error is calculated correctly in case #command-joints < #dof, but with
+ * jumbled order of command joints
*/
TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_less_than_dof_jumbled)
{
From 474b04a7f661776ee045e18d675d36eef811eeb3 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Tue, 28 Nov 2023 08:36:25 +0000
Subject: [PATCH 05/36] Fix typos
---
.../joint_trajectory_controller.hpp | 4 ++--
.../src/joint_trajectory_controller.cpp | 11 ++++++-----
2 files changed, 8 insertions(+), 7 deletions(-)
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
index 1df3a7eb8e..8f3fba93f6 100644
--- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
@@ -294,9 +294,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
void init_hold_position_msg();
void resize_joint_trajectory_point(
- trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double vale = 0.0);
+ trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double value = 0.0);
void resize_joint_trajectory_point_command(
- trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double vale = 0.0);
+ trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double value = 0.0);
};
} // namespace joint_trajectory_controller
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
index f076b09931..e701737d84 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp
+++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -283,11 +283,12 @@ controller_interface::return_type JointTrajectoryController::update(
// Update PIDs
for (auto i = 0ul; i < num_cmd_joints_; ++i)
{
- size_t index = map_cmd_to_joints_[i];
- tmp_command_[index] = (state_desired_.velocities[index] * ff_velocity_scale_[i]) +
- pids_[i]->computeCommand(
- state_error_.positions[index], state_error_.velocities[index],
- (uint64_t)period.nanoseconds());
+ size_t index_cmd_joint = map_cmd_to_joints_[i];
+ tmp_command_[index_cmd_joint] =
+ (state_desired_.velocities[index_cmd_joint] * ff_velocity_scale_[index_cmd_joint]) +
+ pids_[i]->computeCommand(
+ state_error_.positions[index_cmd_joint], state_error_.velocities[index_cmd_joint],
+ (uint64_t)period.nanoseconds());
}
}
From 28bfa80126ffa4f9aa2f19c0972ee6d547602beb Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Tue, 28 Nov 2023 08:36:25 +0000
Subject: [PATCH 06/36] Update comments
---
.../src/joint_trajectory_controller.cpp | 4 ++++
.../test/test_trajectory_controller.cpp | 1 -
2 files changed, 4 insertions(+), 1 deletion(-)
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
index e701737d84..a0b63f2a0a 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp
+++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -967,6 +967,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
// running already)
trajectory_msgs::msg::JointTrajectoryPoint state;
resize_joint_trajectory_point(state, dof_);
+ // read from cmd joints only if all joints have command interface
+ // otherwise it leaves the entries of joints without command interface NaN.
+ // if no open_loop control, state_current_ is then used for `set_point_before_trajectory_msg` and
+ // future trajectory sampling will always give NaN for these joints
if (dof_ == num_cmd_joints_ && read_state_from_command_interfaces(state))
{
state_current_ = state;
diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp
index 2227e03ec0..25a748eb67 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller.cpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp
@@ -436,7 +436,6 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency)
/**
* @brief same as state_topic_consistency but with #command-joints < #dof
*/
-
TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency_command_joints_less_than_dof)
{
rclcpp::executors::SingleThreadedExecutor executor;
From aa67b0bc8b4ef30cf7d9ed9cddffacff87b9f040 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Tue, 28 Nov 2023 08:36:25 +0000
Subject: [PATCH 07/36] Use a function to reuse for tests
---
.../test/test_trajectory_controller.cpp | 33 ++-----------------
1 file changed, 2 insertions(+), 31 deletions(-)
diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp
index 25a748eb67..27d52720ce 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller.cpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp
@@ -127,7 +127,7 @@ TEST_P(
TrajectoryControllerTestParameterized, check_interface_names_with_command_joints_less_than_dof)
{
rclcpp::executors::MultiThreadedExecutor executor;
- // set command_joints parameter
+ // set command_joints parameter to a subset of joint_names_
std::vector<std::string> command_joint_names{joint_names_[0], joint_names_[1]};
const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names);
SetUpTrajectoryController(executor, {command_joint_names_param});
@@ -135,36 +135,7 @@ TEST_P(
const auto state = traj_controller_->get_node()->configure();
ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE);
- std::vector<std::string> state_interface_names;
- state_interface_names.reserve(joint_names_.size() * state_interface_types_.size());
- for (const auto & joint : joint_names_)
- {
- for (const auto & interface : state_interface_types_)
- {
- state_interface_names.push_back(joint + "/" + interface);
- }
- }
- auto state_interfaces = traj_controller_->state_interface_configuration();
- EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL);
- EXPECT_EQ(state_interfaces.names.size(), joint_names_.size() * state_interface_types_.size());
- ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names));
-
- std::vector<std::string> command_interface_names;
- command_interface_names.reserve(command_joint_names.size() * command_interface_types_.size());
- for (const auto & joint : command_joint_names)
- {
- for (const auto & interface : command_interface_types_)
- {
- command_interface_names.push_back(joint + "/" + interface);
- }
- }
- auto command_interfaces = traj_controller_->command_interface_configuration();
- EXPECT_EQ(
- command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL);
- EXPECT_EQ(
- command_interfaces.names.size(), command_joint_names.size() * command_interface_types_.size());
- ASSERT_THAT(
- command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names));
+ compare_joints(joint_names_, command_joint_names);
}
TEST_P(
From 7d62e83df883bd0e38c80171a3e10347cebbaacb Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Tue, 28 Nov 2023 08:36:25 +0000
Subject: [PATCH 08/36] Fix includes
---
.../test/test_trajectory_actions.cpp | 6 +++---
.../test/test_trajectory_controller.cpp | 2 +-
2 files changed, 4 insertions(+), 4 deletions(-)
diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp
index 87d557df1b..bc9d9719aa 100644
--- a/joint_trajectory_controller/test/test_trajectory_actions.cpp
+++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp
@@ -30,9 +30,7 @@
#include "action_msgs/msg/goal_status_array.hpp"
#include "control_msgs/action/detail/follow_joint_trajectory__struct.hpp"
#include "controller_interface/controller_interface.hpp"
-#include "gtest/gtest.h"
#include "hardware_interface/resource_manager.hpp"
-#include "joint_trajectory_controller/joint_trajectory_controller.hpp"
#include "rclcpp/clock.hpp"
#include "rclcpp/duration.hpp"
#include "rclcpp/executors/multi_threaded_executor.hpp"
@@ -45,10 +43,12 @@
#include "rclcpp_action/client_goal_handle.hpp"
#include "rclcpp_action/create_client.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
-#include "test_trajectory_controller_utils.hpp"
#include "trajectory_msgs/msg/joint_trajectory.hpp"
#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
+#include "joint_trajectory_controller/joint_trajectory_controller.hpp"
+#include "test_trajectory_controller_utils.hpp"
+
using std::placeholders::_1;
using std::placeholders::_2;
using test_trajectory_controllers::TestableJointTrajectoryController;
diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp
index 27d52720ce..14d130d93a 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller.cpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp
@@ -30,7 +30,6 @@
#include "builtin_interfaces/msg/time.hpp"
#include "controller_interface/controller_interface.hpp"
#include "hardware_interface/resource_manager.hpp"
-#include "joint_trajectory_controller/joint_trajectory_controller.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/clock.hpp"
#include "rclcpp/duration.hpp"
@@ -50,6 +49,7 @@
#include "trajectory_msgs/msg/joint_trajectory.hpp"
#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
+#include "joint_trajectory_controller/joint_trajectory_controller.hpp"
#include "test_trajectory_controller_utils.hpp"
using lifecycle_msgs::msg::State;
From 141f686eeabc36f57e2a633591acc39902272e22 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?=
<christophfroehlich@users.noreply.github.com>
Date: Tue, 28 Nov 2023 08:36:25 +0000
Subject: [PATCH 09/36] Use correct ff_velocity_scale parameter
---
joint_trajectory_controller/src/joint_trajectory_controller.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
index a0b63f2a0a..1fca832c82 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp
+++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -285,7 +285,7 @@ controller_interface::return_type JointTrajectoryController::update(
{
size_t index_cmd_joint = map_cmd_to_joints_[i];
tmp_command_[index_cmd_joint] =
- (state_desired_.velocities[index_cmd_joint] * ff_velocity_scale_[index_cmd_joint]) +
+ (state_desired_.velocities[index_cmd_joint] * ff_velocity_scale_[i]) +
pids_[i]->computeCommand(
state_error_.positions[index_cmd_joint], state_error_.velocities[index_cmd_joint],
(uint64_t)period.nanoseconds());
From 88ddb8f1a004e80e0f61f96e070e69a5a0a7528c Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Tue, 28 Nov 2023 08:37:12 +0000
Subject: [PATCH 10/36] Fix tests due to allow_nonzero..
---
.../test/test_trajectory_controller_utils.hpp | 1 +
1 file changed, 1 insertion(+)
diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
index 4a65b4ad51..8eef7f8a8e 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
@@ -261,6 +261,7 @@ class TrajectoryControllerTest : public ::testing::Test
// set pid parameters before configure
SetPidParameters(k_p, ff, angle_wraparound);
+
traj_controller_->get_node()->configure();
ActivateTrajectoryController(
From c5c2f0105427bd63ec446036cb81cadc676a4efe Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Tue, 28 Nov 2023 08:37:12 +0000
Subject: [PATCH 11/36] Remove unnecessary warning
---
.../src/joint_trajectory_controller.cpp | 9 ---------
1 file changed, 9 deletions(-)
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
index 1fca832c82..0a037ab9ee 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp
+++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -73,15 +73,6 @@ JointTrajectoryController::command_interface_configuration() const
{
controller_interface::InterfaceConfiguration conf;
conf.type = controller_interface::interface_configuration_type::INDIVIDUAL;
- if (num_cmd_joints_ == 0)
- {
- fprintf(
- stderr,
- "During ros2_control interface configuration, number of command interfaces is not valid;"
- " it should be positive. Actual number is %zu\n",
- num_cmd_joints_);
- std::exit(EXIT_FAILURE);
- }
conf.names.reserve(num_cmd_joints_ * params_.command_interfaces.size());
for (const auto & joint_name : command_joint_names_)
{
From 23146d84881813783ffc60d02ba948614fc89038 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Tue, 28 Nov 2023 09:43:04 +0000
Subject: [PATCH 12/36] Use new update method for added tests
---
.../test/test_trajectory_controller.cpp | 132 +++++++-----------
.../test/test_trajectory_controller_utils.hpp | 7 +
2 files changed, 61 insertions(+), 78 deletions(-)
diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp
index 14d130d93a..c7a219ea4c 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller.cpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp
@@ -853,10 +853,8 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le
constexpr double k_p = 10.0;
std::vector<std::string> command_joint_names{joint_names_[0], joint_names_[1]};
const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names);
- std::vector<rclcpp::Parameter> params = {
- rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true), command_joint_names_param};
+ std::vector<rclcpp::Parameter> params = {command_joint_names_param};
SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, false);
- subscribeToState();
size_t n_joints = joint_names_.size();
@@ -872,45 +870,39 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le
publish(time_from_start, points, rclcpp::Time(), {}, points_velocities);
traj_controller_->wait_for_trajectory(executor);
- // first update
- updateController(rclcpp::Duration(FIRST_POINT_TIME));
-
- // Spin to receive latest state
- executor.spin_some();
- auto state_msg = getState();
- ASSERT_TRUE(state_msg);
+ // trylock() has to succeed at least once to have current_command set
+ updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME));
- const auto allowed_delta = 0.1;
+ // get states from class variables
+ auto state_feedback = traj_controller_->get_state_feedback();
+ auto state_reference = traj_controller_->get_state_reference();
+ auto state_error = traj_controller_->get_state_error();
+ auto current_command = traj_controller_->get_current_command();
// no update of state_interface
- EXPECT_EQ(state_msg->feedback.positions, INITIAL_POS_JOINTS);
+ EXPECT_EQ(state_feedback.positions, INITIAL_POS_JOINTS);
// has the msg the correct vector sizes?
- EXPECT_EQ(n_joints, state_msg->reference.positions.size());
- EXPECT_EQ(n_joints, state_msg->feedback.positions.size());
- EXPECT_EQ(n_joints, state_msg->error.positions.size());
+ EXPECT_EQ(n_joints, state_reference.positions.size());
+ EXPECT_EQ(n_joints, state_feedback.positions.size());
+ EXPECT_EQ(n_joints, state_error.positions.size());
// are the correct reference positions used?
- EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta);
- EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta);
- EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta);
+ EXPECT_NEAR(points[0][0], state_reference.positions[0], COMMON_THRESHOLD);
+ EXPECT_NEAR(points[0][1], state_reference.positions[1], COMMON_THRESHOLD);
+ EXPECT_NEAR(points[0][2], state_reference.positions[2], COMMON_THRESHOLD);
// no normalization of position error
- EXPECT_NEAR(
- state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS);
- EXPECT_NEAR(
- state_msg->error.positions[1], state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1], EPS);
- EXPECT_NEAR(
- state_msg->error.positions[2], state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2], EPS);
+ EXPECT_NEAR(state_error.positions[0], state_reference.positions[0] - INITIAL_POS_JOINTS[0], EPS);
+ EXPECT_NEAR(state_error.positions[1], state_reference.positions[1] - INITIAL_POS_JOINTS[1], EPS);
+ EXPECT_NEAR(state_error.positions[2], state_reference.positions[2] - INITIAL_POS_JOINTS[2], EPS);
if (traj_controller_->has_position_command_interface())
{
// check command interface
- EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta);
- EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta);
- EXPECT_NEAR(points[0][0], state_msg->output.positions[0], allowed_delta);
- EXPECT_NEAR(points[0][1], state_msg->output.positions[1], allowed_delta);
- EXPECT_TRUE(std::isnan(state_msg->output.positions[2]));
+ EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD);
+ EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD);
+ EXPECT_TRUE(std::isnan(current_command.positions[2]));
}
if (traj_controller_->has_velocity_command_interface())
@@ -918,20 +910,18 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le
// check command interface
EXPECT_LT(0.0, joint_vel_[0]);
EXPECT_LT(0.0, joint_vel_[1]);
- EXPECT_LT(0.0, state_msg->output.velocities[0]);
- EXPECT_LT(0.0, state_msg->output.velocities[1]);
- EXPECT_TRUE(std::isnan(state_msg->output.velocities[2]));
+ EXPECT_TRUE(std::isnan(current_command.velocities[2]));
// use_closed_loop_pid_adapter_
if (traj_controller_->use_closed_loop_pid_adapter())
{
// we expect u = k_p * (s_d-s)
EXPECT_NEAR(
- k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
- k_p * allowed_delta);
+ k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
+ k_p * COMMON_THRESHOLD);
EXPECT_NEAR(
- k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
- k_p * allowed_delta);
+ k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
+ k_p * COMMON_THRESHOLD);
}
}
@@ -940,9 +930,7 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le
// check command interface
EXPECT_LT(0.0, joint_eff_[0]);
EXPECT_LT(0.0, joint_eff_[1]);
- EXPECT_LT(0.0, state_msg->output.effort[0]);
- EXPECT_LT(0.0, state_msg->output.effort[1]);
- EXPECT_TRUE(std::isnan(state_msg->output.effort[2]));
+ EXPECT_TRUE(std::isnan(current_command.effort[2]));
}
executor.cancel();
@@ -958,10 +946,8 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le
constexpr double k_p = 10.0;
std::vector<std::string> command_joint_names{joint_names_[1], joint_names_[0]};
const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names);
- std::vector<rclcpp::Parameter> params = {
- rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true), command_joint_names_param};
+ std::vector<rclcpp::Parameter> params = {command_joint_names_param};
SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, false);
- subscribeToState();
size_t n_joints = joint_names_.size();
@@ -977,45 +963,39 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le
publish(time_from_start, points, rclcpp::Time(), {}, points_velocities);
traj_controller_->wait_for_trajectory(executor);
- // first update
- updateController(rclcpp::Duration(FIRST_POINT_TIME));
-
- // Spin to receive latest state
- executor.spin_some();
- auto state_msg = getState();
- ASSERT_TRUE(state_msg);
+ // trylock() has to succeed at least once to have current_command set
+ updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME));
- const auto allowed_delta = 0.1;
+ // get states from class variables
+ auto state_feedback = traj_controller_->get_state_feedback();
+ auto state_reference = traj_controller_->get_state_reference();
+ auto state_error = traj_controller_->get_state_error();
+ auto current_command = traj_controller_->get_current_command();
// no update of state_interface
- EXPECT_EQ(state_msg->feedback.positions, INITIAL_POS_JOINTS);
+ EXPECT_EQ(state_feedback.positions, INITIAL_POS_JOINTS);
// has the msg the correct vector sizes?
- EXPECT_EQ(n_joints, state_msg->reference.positions.size());
- EXPECT_EQ(n_joints, state_msg->feedback.positions.size());
- EXPECT_EQ(n_joints, state_msg->error.positions.size());
+ EXPECT_EQ(n_joints, state_reference.positions.size());
+ EXPECT_EQ(n_joints, state_feedback.positions.size());
+ EXPECT_EQ(n_joints, state_error.positions.size());
// are the correct reference positions used?
- EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta);
- EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta);
- EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta);
+ EXPECT_NEAR(points[0][0], state_reference.positions[0], COMMON_THRESHOLD);
+ EXPECT_NEAR(points[0][1], state_reference.positions[1], COMMON_THRESHOLD);
+ EXPECT_NEAR(points[0][2], state_reference.positions[2], COMMON_THRESHOLD);
// no normalization of position error
- EXPECT_NEAR(
- state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS);
- EXPECT_NEAR(
- state_msg->error.positions[1], state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1], EPS);
- EXPECT_NEAR(
- state_msg->error.positions[2], state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2], EPS);
+ EXPECT_NEAR(state_error.positions[0], state_reference.positions[0] - INITIAL_POS_JOINTS[0], EPS);
+ EXPECT_NEAR(state_error.positions[1], state_reference.positions[1] - INITIAL_POS_JOINTS[1], EPS);
+ EXPECT_NEAR(state_error.positions[2], state_reference.positions[2] - INITIAL_POS_JOINTS[2], EPS);
if (traj_controller_->has_position_command_interface())
{
// check command interface
- EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta);
- EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta);
- EXPECT_NEAR(points[0][0], state_msg->output.positions[0], allowed_delta);
- EXPECT_NEAR(points[0][1], state_msg->output.positions[1], allowed_delta);
- EXPECT_TRUE(std::isnan(state_msg->output.positions[2]));
+ EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD);
+ EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD);
+ EXPECT_TRUE(std::isnan(current_command.positions[2]));
}
if (traj_controller_->has_velocity_command_interface())
@@ -1023,20 +1003,18 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le
// check command interface
EXPECT_LT(0.0, joint_vel_[0]);
EXPECT_LT(0.0, joint_vel_[1]);
- EXPECT_LT(0.0, state_msg->output.velocities[0]);
- EXPECT_LT(0.0, state_msg->output.velocities[1]);
- EXPECT_TRUE(std::isnan(state_msg->output.velocities[2]));
+ EXPECT_TRUE(std::isnan(current_command.velocities[2]));
// use_closed_loop_pid_adapter_
if (traj_controller_->use_closed_loop_pid_adapter())
{
// we expect u = k_p * (s_d-s)
EXPECT_NEAR(
- k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
- k_p * allowed_delta);
+ k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
+ k_p * COMMON_THRESHOLD);
EXPECT_NEAR(
- k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
- k_p * allowed_delta);
+ k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
+ k_p * COMMON_THRESHOLD);
}
}
@@ -1045,9 +1023,7 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le
// check command interface
EXPECT_LT(0.0, joint_eff_[0]);
EXPECT_LT(0.0, joint_eff_[1]);
- EXPECT_LT(0.0, state_msg->output.effort[0]);
- EXPECT_LT(0.0, state_msg->output.effort[1]);
- EXPECT_TRUE(std::isnan(state_msg->output.effort[2]));
+ EXPECT_TRUE(std::isnan(current_command.effort[2]));
}
executor.cancel();
diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
index 8eef7f8a8e..19c563e284 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
@@ -23,6 +23,7 @@
#include "gmock/gmock.h"
+#include "control_msgs/msg/joint_trajectory_controller_state.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "joint_trajectory_controller/joint_trajectory_controller.hpp"
#include "joint_trajectory_controller/trajectory.hpp"
@@ -154,6 +155,12 @@ class TestableJointTrajectoryController
trajectory_msgs::msg::JointTrajectoryPoint get_state_feedback() { return state_current_; }
trajectory_msgs::msg::JointTrajectoryPoint get_state_reference() { return state_desired_; }
trajectory_msgs::msg::JointTrajectoryPoint get_state_error() { return state_error_; }
+ trajectory_msgs::msg::JointTrajectoryPoint get_current_command() { return command_current_; }
+
+ control_msgs::msg::JointTrajectoryControllerState get_state_msg()
+ {
+ return state_publisher_->msg_;
+ }
rclcpp::WaitSet joint_cmd_sub_wait_set_;
};
From ea50f0f014b4c9f0969144e8ecdff4863347370b Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Sun, 10 Dec 2023 18:13:21 +0000
Subject: [PATCH 13/36] Fix rqt_jtc with num_cmd<dof
---
.../joint_trajectory_controller.py | 5 +----
1 file changed, 1 insertion(+), 4 deletions(-)
diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py
index 162977cdfe..9e9c071002 100644
--- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py
+++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py
@@ -457,12 +457,9 @@ def _joint_widgets(self): # TODO: Cache instead of compute every time?
def _jtc_joint_names(jtc_info):
- # NOTE: We assume that there is at least one hardware interface that
- # claims resources (there should be), and the resource list is fetched
- # from the first available interface
joint_names = []
- for interface in jtc_info.claimed_interfaces:
+ for interface in jtc_info.required_state_interfaces:
name = interface.split("/")[-2]
if name not in joint_names:
joint_names.append(name)
From fefa49408cca3ada3f89fc63f14d5d2db6561dc5 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Thu, 7 Mar 2024 11:58:35 +0000
Subject: [PATCH 14/36] Merge branch 'jtc/controller_plugin'
---
joint_trajectory_controller/CMakeLists.txt | 2 +
.../joint_trajectory_controller.hpp | 38 +--
joint_trajectory_controller/package.xml | 3 +-
.../src/joint_trajectory_controller.cpp | 214 ++++++++------
...oint_trajectory_controller_parameters.yaml | 266 +++++++++---------
.../test/test_trajectory_controller.cpp | 195 +++++++------
.../test/test_trajectory_controller_utils.hpp | 33 ++-
.../CMakeLists.txt | 77 +++++
.../pid_trajectory_plugin.hpp | 91 ++++++
.../trajectory_controller_base.hpp | 177 ++++++++++++
.../visibility_control.h | 49 ++++
.../package.xml | 23 ++
.../plugins.xml | 6 +
.../src/pid_trajectory_plugin.cpp | 137 +++++++++
.../src/pid_trajectory_plugin_parameters.yaml | 37 +++
.../test/test_load_pid_trajectory.cpp | 49 ++++
.../test/test_pid_trajectory.cpp | 107 +++++++
.../test/test_pid_trajectory.hpp | 72 +++++
18 files changed, 1232 insertions(+), 344 deletions(-)
create mode 100644 joint_trajectory_controller_plugins/CMakeLists.txt
create mode 100644 joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp
create mode 100644 joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp
create mode 100644 joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/visibility_control.h
create mode 100644 joint_trajectory_controller_plugins/package.xml
create mode 100644 joint_trajectory_controller_plugins/plugins.xml
create mode 100644 joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp
create mode 100644 joint_trajectory_controller_plugins/src/pid_trajectory_plugin_parameters.yaml
create mode 100644 joint_trajectory_controller_plugins/test/test_load_pid_trajectory.cpp
create mode 100644 joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp
create mode 100644 joint_trajectory_controller_plugins/test/test_pid_trajectory.hpp
diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt
index 4b0ca82df8..b644bb4110 100644
--- a/joint_trajectory_controller/CMakeLists.txt
+++ b/joint_trajectory_controller/CMakeLists.txt
@@ -20,6 +20,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
tl_expected
trajectory_msgs
urdf
+ joint_trajectory_controller_plugins
)
find_package(ament_cmake REQUIRED)
@@ -107,4 +108,5 @@ install(TARGETS
ament_export_targets(export_joint_trajectory_controller HAS_LIBRARY_TARGET)
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+
ament_package()
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
index d51c592d44..e5eac8421a 100644
--- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
@@ -24,13 +24,9 @@
#include "control_msgs/action/follow_joint_trajectory.hpp"
#include "control_msgs/msg/joint_trajectory_controller_state.hpp"
#include "control_msgs/srv/query_trajectory_state.hpp"
-#include "control_toolbox/pid.hpp"
#include "controller_interface/controller_interface.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
-#include "joint_trajectory_controller/interpolation_methods.hpp"
-#include "joint_trajectory_controller/tolerances.hpp"
-#include "joint_trajectory_controller/trajectory.hpp"
-#include "joint_trajectory_controller/visibility_control.h"
+#include "pluginlib/class_loader.hpp"
#include "rclcpp/duration.hpp"
#include "rclcpp/subscription.hpp"
#include "rclcpp/time.hpp"
@@ -45,8 +41,12 @@
#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
#include "urdf/model.h"
-// auto-generated by generate_parameter_library
-#include "joint_trajectory_controller_parameters.hpp"
+#include "joint_trajectory_controller/interpolation_methods.hpp"
+#include "joint_trajectory_controller/tolerances.hpp"
+#include "joint_trajectory_controller/trajectory.hpp"
+#include "joint_trajectory_controller/visibility_control.h"
+#include "joint_trajectory_controller_parameters.hpp" // auto-generated by generate_parameter_library
+#include "joint_trajectory_controller_plugins/trajectory_controller_base.hpp"
using namespace std::chrono_literals; // NOLINT
@@ -125,7 +125,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
// Storing command joint names for interfaces
std::vector<std::string> command_joint_names_;
-
+#if RCLCPP_VERSION_MAJOR >= 17
+ // TODO(anyone) remove this if there is another way to lock command_joints parameter
+ rclcpp_lifecycle::LifecycleNode::PreSetParametersCallbackHandle::SharedPtr lock_cmd_joint_names;
+#endif
// Parameters from ROS for joint_trajectory_controller
std::shared_ptr<ParamListener> param_listener_;
Params params_;
@@ -153,11 +156,13 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
bool has_effort_command_interface_ = false;
/// If true, a velocity feedforward term plus corrective PID term is used
- bool use_closed_loop_pid_adapter_ = false;
- using PidPtr = std::shared_ptr<control_toolbox::Pid>;
- std::vector<PidPtr> pids_;
- // Feed-forward velocity weight factor when calculating closed loop pid adapter's command
- std::vector<double> ff_velocity_scale_;
+ bool use_external_control_law_ = false;
+ // class loader for actual trajectory controller
+ std::shared_ptr<
+ pluginlib::ClassLoader<joint_trajectory_controller_plugins::TrajectoryControllerBase>>
+ traj_controller_loader_;
+ // The actual trajectory controller
+ std::shared_ptr<joint_trajectory_controller_plugins::TrajectoryControllerBase> traj_contr_;
// Configuration for every joint if it wraps around (ie. is continuous, position error is
// normalized)
std::vector<bool> joints_angle_wraparound_;
@@ -239,7 +244,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const;
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
- void add_new_trajectory_msg(
+ void add_new_trajectory_msg_nonRT(
+ const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & traj_msg);
+ JOINT_TRAJECTORY_CONTROLLER_PUBLIC
+ void add_new_trajectory_msg_RT(
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & traj_msg);
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
bool validate_trajectory_point_field(
@@ -290,8 +298,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
std::shared_ptr<control_msgs::srv::QueryTrajectoryState::Response> response);
private:
- void update_pids();
-
bool contains_interface_type(
const std::vector<std::string> & interface_type_list, const std::string & interface_type);
diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml
index 8457c02aeb..a5161d5a85 100644
--- a/joint_trajectory_controller/package.xml
+++ b/joint_trajectory_controller/package.xml
@@ -26,6 +26,7 @@
<depend>tl_expected</depend>
<depend>trajectory_msgs</depend>
<depend>urdf</depend>
+ <depend>joint_trajectory_controller_plugins</depend>
<test_depend>ament_cmake_gmock</test_depend>
<test_depend>controller_manager</test_depend>
@@ -35,4 +36,4 @@
<export>
<build_type>ament_cmake</build_type>
</export>
-</package>
+</package>
\ No newline at end of file
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
index ef3917d291..860e8d23af 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp
+++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -27,7 +27,6 @@
#include "controller_interface/helpers.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
-#include "joint_trajectory_controller/trajectory.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/qos.hpp"
@@ -37,10 +36,18 @@
#include "rclcpp_lifecycle/state.hpp"
#include "urdf/model.h"
+#include "joint_trajectory_controller/trajectory.hpp"
+
namespace joint_trajectory_controller
{
JointTrajectoryController::JointTrajectoryController()
-: controller_interface::ControllerInterface(), dof_(0), num_cmd_joints_(0)
+: controller_interface::ControllerInterface(),
+ dof_(0), num_cmd_joints_(0),
+ traj_controller_loader_(
+ std::make_shared<
+ pluginlib::ClassLoader<joint_trajectory_controller_plugins::TrajectoryControllerBase>>(
+ "joint_trajectory_controller_plugins",
+ "joint_trajectory_controller_plugins::TrajectoryControllerBase"))
{
}
@@ -122,11 +129,12 @@ controller_interface::return_type JointTrajectoryController::update(
{
params_ = param_listener_->get_params();
default_tolerances_ = get_segment_tolerances(params_);
- // update the PID gains
- // variable use_closed_loop_pid_adapter_ is updated in on_configure only
- if (use_closed_loop_pid_adapter_)
- {
- update_pids();
+ // update gains of controller
+ if (traj_contr_) {
+ if (traj_contr_->updateGainsRT() == false) {
+ RCLCPP_ERROR(get_node()->get_logger(), "Could not update gains of controller");
+ return controller_interface::return_type::ERROR;
+ }
}
}
@@ -136,10 +144,13 @@ controller_interface::return_type JointTrajectoryController::update(
// Check if a new external message has been received from nonRT threads
auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg();
auto new_external_msg = traj_msg_external_point_ptr_.readFromRT();
- // Discard, if a goal is pending but still not active (somewhere stuck in goal_handle_timer_)
+ // Discard,
+ // if a goal is pending but still not active (somewhere stuck in goal_handle_timer_)
+ // and if traj_contr_: wait until control law is computed by the traj_contr_
if (
current_external_msg != *new_external_msg &&
- (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false)
+ (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false &&
+ (traj_contr_ == nullptr || traj_contr_->is_ready()))
{
fill_partial_goal(*new_external_msg);
sort_to_local_joint_order(*new_external_msg);
@@ -169,6 +180,10 @@ controller_interface::return_type JointTrajectoryController::update(
traj_external_point_ptr_->set_point_before_trajectory_msg(
time, state_current_, joints_angle_wraparound_);
}
+ if (traj_contr_) {
+ // switch RT buffer of traj_contr_
+ traj_contr_->start();
+ }
}
// find segment for current timestamp
@@ -200,8 +215,7 @@ controller_interface::return_type JointTrajectoryController::update(
{
RCLCPP_WARN(get_node()->get_logger(), "Aborted due to command timeout");
- traj_msg_external_point_ptr_.reset();
- traj_msg_external_point_ptr_.initRT(set_hold_position());
+ add_new_trajectory_msg_RT(set_hold_position());
}
// Check state/goal tolerance
@@ -244,20 +258,11 @@ controller_interface::return_type JointTrajectoryController::update(
}
// set values for next hardware write() if tolerance is met
- if (!tolerance_violated_while_moving && within_goal_time)
- {
- if (use_closed_loop_pid_adapter_)
- {
- // Update PIDs
- for (auto i = 0ul; i < num_cmd_joints_; ++i)
- {
- size_t index_cmd_joint = map_cmd_to_joints_[i];
- tmp_command_[index_cmd_joint] =
- (state_desired_.velocities[index_cmd_joint] * ff_velocity_scale_[i]) +
- pids_[i]->computeCommand(
- state_error_.positions[index_cmd_joint], state_error_.velocities[index_cmd_joint],
- (uint64_t)period.nanoseconds());
- }
+ if (!tolerance_violated_while_moving && within_goal_time) {
+ if (traj_contr_) {
+ traj_contr_->computeCommands(
+ tmp_command_, state_current_, state_error_, state_desired_,
+ time - traj_external_point_ptr_->time_from_start(), period);
}
// set values for next hardware write()
@@ -265,10 +270,8 @@ controller_interface::return_type JointTrajectoryController::update(
{
assign_interface_from_point(joint_command_interface_[0], state_desired_.positions);
}
- if (has_velocity_command_interface_)
- {
- if (use_closed_loop_pid_adapter_)
- {
+ if (has_velocity_command_interface_) {
+ if (use_external_control_law_) {
assign_interface_from_point(joint_command_interface_[1], tmp_command_);
}
else
@@ -315,8 +318,7 @@ controller_interface::return_type JointTrajectoryController::update(
RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation");
- traj_msg_external_point_ptr_.reset();
- traj_msg_external_point_ptr_.initRT(set_hold_position());
+ add_new_trajectory_msg_RT(set_hold_position());
}
// check goal tolerance
else if (!before_last_point)
@@ -334,11 +336,8 @@ controller_interface::return_type JointTrajectoryController::update(
RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!");
- traj_msg_external_point_ptr_.reset();
- traj_msg_external_point_ptr_.initRT(set_success_trajectory_point());
- }
- else if (!within_goal_time)
- {
+ add_new_trajectory_msg_RT(set_success_trajectory_point());
+ } else if (!within_goal_time) {
const std::string error_string = "Aborted due to goal_time_tolerance exceeding by " +
std::to_string(time_difference) + " seconds";
@@ -353,8 +352,7 @@ controller_interface::return_type JointTrajectoryController::update(
RCLCPP_WARN(get_node()->get_logger(), error_string.c_str());
- traj_msg_external_point_ptr_.reset();
- traj_msg_external_point_ptr_.initRT(set_hold_position());
+ add_new_trajectory_msg_RT(set_hold_position());
}
}
}
@@ -363,16 +361,13 @@ controller_interface::return_type JointTrajectoryController::update(
// we need to ensure that there is no pending goal -> we get a race condition otherwise
RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation");
- traj_msg_external_point_ptr_.reset();
- traj_msg_external_point_ptr_.initRT(set_hold_position());
- }
- else if (
+ add_new_trajectory_msg_RT(set_hold_position());
+ } else if (
!before_last_point && !within_goal_time && *(rt_has_pending_goal_.readFromRT()) == false)
{
RCLCPP_ERROR(get_node()->get_logger(), "Exceeded goal_time_tolerance: holding position...");
- traj_msg_external_point_ptr_.reset();
- traj_msg_external_point_ptr_.initRT(set_hold_position());
+ add_new_trajectory_msg_RT(set_hold_position());
}
// else, run another cycle while waiting for outside_goal_tolerance
// to be satisfied (will stay in this state until new message arrives)
@@ -709,19 +704,43 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
contains_interface_type(params_.command_interfaces, hardware_interface::HW_IF_EFFORT);
// if there is only velocity or if there is effort command interface
- // then use also PID adapter
- use_closed_loop_pid_adapter_ =
+ // then use external control law
+ use_external_control_law_ =
(has_velocity_command_interface_ && params_.command_interfaces.size() == 1 &&
!params_.open_loop_control) ||
has_effort_command_interface_;
- if (use_closed_loop_pid_adapter_)
- {
- pids_.resize(num_cmd_joints_);
- ff_velocity_scale_.resize(num_cmd_joints_);
- tmp_command_.resize(dof_, std::numeric_limits<double>::quiet_NaN());
+ if (use_external_control_law_) {
+ try {
+ traj_contr_ =
+ traj_controller_loader_->createSharedInstance(params_.controller_plugin.c_str());
+ } catch (pluginlib::PluginlibException & ex) {
+ RCLCPP_FATAL(
+ logger, "The trajectory controller plugin `%s` failed to load for some reason. Error: %s\n",
+ params_.controller_plugin.c_str(), ex.what());
+ return CallbackReturn::FAILURE;
+ }
+ if (traj_contr_->initialize(get_node()) == false) {
+ RCLCPP_FATAL(
+ logger,
+ "The trajectory controller plugin `%s` failed to initialize for some reason. Aborting.",
+ params_.controller_plugin.c_str());
+ return CallbackReturn::FAILURE;
+ } else {
+ if (traj_contr_->configure() == false) {
+ RCLCPP_FATAL(
+ logger,
+ "The trajectory controller plugin `%s` failed to configure for some reason. Aborting.",
+ params_.controller_plugin.c_str());
+ return CallbackReturn::FAILURE;
+ } else {
+ RCLCPP_INFO(
+ logger, "The trajectory controller plugin `%s` was loaded and configured.",
+ params_.controller_plugin.c_str());
+ }
+ }
- update_pids();
+ tmp_command_.resize(dof_, 0.0);
}
// Configure joint position error normalization (angle_wraparound)
@@ -977,7 +996,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
}
// The controller should start by holding position at the beginning of active state
- add_new_trajectory_msg(set_hold_position());
+ add_new_trajectory_msg_nonRT(set_hold_position());
rt_is_holding_.writeFromNonRT(true);
// parse timeout parameter
@@ -1002,6 +1021,18 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
cmd_timeout_ = 0.0;
}
+ // activate traj_contr_, e.g., update gains
+ if (traj_contr_ && traj_contr_->activate() == false) {
+ RCLCPP_ERROR(get_node()->get_logger(), "Error during trajectory controller activation.");
+ return CallbackReturn::ERROR;
+ }
+
+ // activate traj_contr_, e.g., update gains
+ if (traj_contr_ && traj_contr_->activate() == false) {
+ RCLCPP_ERROR(get_node()->get_logger(), "Error during trajectory controller activation.");
+ return CallbackReturn::ERROR;
+ }
+
return CallbackReturn::SUCCESS;
}
@@ -1009,8 +1040,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate(
const rclcpp_lifecycle::State &)
{
const auto active_goal = *rt_active_goal_.readFromNonRT();
- if (active_goal)
- {
+ if (active_goal) {
rt_has_pending_goal_.writeFromNonRT(false);
auto action_res = std::make_shared<FollowJTrajAction::Result>();
action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL);
@@ -1019,10 +1049,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate(
rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
}
- for (size_t index = 0; index < num_cmd_joints_; ++index)
- {
- if (has_position_command_interface_)
- {
+ for (size_t index = 0; index < num_cmd_joints_; ++index) {
+ if (has_position_command_interface_) {
joint_command_interface_[0][index].get().set_value(
joint_command_interface_[0][index].get().get_value());
}
@@ -1079,12 +1107,8 @@ bool JointTrajectoryController::reset()
subscriber_is_active_ = false;
joint_command_subscriber_.reset();
- for (const auto & pid : pids_)
- {
- if (pid)
- {
- pid->reset();
- }
+ if (traj_contr_) {
+ traj_contr_->reset();
}
traj_external_point_ptr_.reset();
@@ -1140,9 +1164,8 @@ void JointTrajectoryController::topic_callback(
}
// http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement
// always replace old msg with new one for now
- if (subscriber_is_active_)
- {
- add_new_trajectory_msg(msg);
+ if (subscriber_is_active_) {
+ add_new_trajectory_msg_nonRT(msg);
rt_is_holding_.writeFromNonRT(false);
}
};
@@ -1188,7 +1211,7 @@ rclcpp_action::CancelResponse JointTrajectoryController::goal_cancelled_callback
rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
// Enter hold current position mode
- add_new_trajectory_msg(set_hold_position());
+ add_new_trajectory_msg_nonRT(set_hold_position());
}
return rclcpp_action::CancelResponse::ACCEPT;
}
@@ -1205,7 +1228,7 @@ void JointTrajectoryController::goal_accepted_callback(
auto traj_msg =
std::make_shared<trajectory_msgs::msg::JointTrajectory>(goal_handle->get_goal()->trajectory);
- add_new_trajectory_msg(traj_msg);
+ add_new_trajectory_msg_nonRT(traj_msg);
rt_is_holding_.writeFromNonRT(false);
}
@@ -1512,17 +1535,42 @@ bool JointTrajectoryController::validate_trajectory_msg(
return true;
}
-void JointTrajectoryController::add_new_trajectory_msg(
+void JointTrajectoryController::add_new_trajectory_msg_nonRT(
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & traj_msg)
{
traj_msg_external_point_ptr_.writeFromNonRT(traj_msg);
+
+ // compute control law
+ if (traj_contr_) {
+ // this can take some time; trajectory won't start until control law is computed
+ if (traj_contr_->computeControlLawNonRT(traj_msg) == false) {
+ RCLCPP_ERROR(get_node()->get_logger(), "Failed to compute gains for trajectory.");
+ }
+ }
+}
+
+void JointTrajectoryController::add_new_trajectory_msg_RT(
+ const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & traj_msg)
+{
+ // TODO(christophfroehlich): Need a lock-free write here
+ // See https://github.com/ros-controls/ros2_controllers/issues/168
+ traj_msg_external_point_ptr_.reset();
+ traj_msg_external_point_ptr_.initRT(traj_msg);
+
+ // compute control law
+ if (traj_contr_) {
+ // this is used for set_hold_position() only -> this should (and must) not take a long time
+ if (traj_contr_->computeControlLawRT(traj_msg) == false) {
+ RCLCPP_ERROR(get_node()->get_logger(), "Failed to compute gains for trajectory.");
+ }
+ }
}
void JointTrajectoryController::preempt_active_goal()
{
const auto active_goal = *rt_active_goal_.readFromNonRT();
- if (active_goal)
- {
+ if (active_goal) {
+ add_new_trajectory_msg_nonRT(set_hold_position());
auto action_res = std::make_shared<FollowJTrajAction::Result>();
action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL);
action_res->set__error_string("Current goal cancelled due to new incoming action.");
@@ -1604,26 +1652,6 @@ bool JointTrajectoryController::has_active_trajectory() const
return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg();
}
-void JointTrajectoryController::update_pids()
-{
- for (size_t i = 0; i < num_cmd_joints_; ++i)
- {
- const auto & gains = params_.gains.joints_map.at(params_.joints.at(map_cmd_to_joints_[i]));
- if (pids_[i])
- {
- // update PIDs with gains from ROS parameters
- pids_[i]->setGains(gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp);
- }
- else
- {
- // Init PIDs with gains from ROS parameters
- pids_[i] = std::make_shared<control_toolbox::Pid>(
- gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp);
- }
- ff_velocity_scale_[i] = gains.ff_velocity_scale;
- }
-}
-
void JointTrajectoryController::init_hold_position_msg()
{
hold_position_msg_ptr_ = std::make_shared<trajectory_msgs::msg::JointTrajectory>();
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml
index b17229cab8..f191544d4d 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml
+++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml
@@ -1,156 +1,146 @@
joint_trajectory_controller:
- joints: {
- type: string_array,
- default_value: [],
- description: "Joint names to control and listen to",
- read_only: true,
- validation: {
- unique<>: null,
+ joints:
+ {
+ type: string_array,
+ default_value: [],
+ description: "Joint names to control and listen to",
+ read_only: true,
+ validation: { unique<>: null },
}
- }
command_joints: {
- type: string_array,
- default_value: [],
- description: "Joint names to control. This parameters is used if JTC is used in a controller chain where command and state interfaces don't have same names.",
- read_only: true,
- validation: {
- unique<>: null,
+ type: string_array,
+ default_value: [],
+ description: "Joint names to control. This parameters is used if JTC is used in a controller chain where command and state interfaces don't have same names.",
+ read_only: false, # should be set to true after configuration of the controller
+ validation: { unique<>: null },
}
- }
- command_interfaces: {
- type: string_array,
- default_value: [],
- description: "Command interfaces provided by the hardware interface for all joints",
- read_only: true,
- validation: {
- unique<>: null,
- subset_of<>: [["position", "velocity", "acceleration", "effort",]],
- "joint_trajectory_controller::command_interface_type_combinations": null,
+ command_interfaces:
+ {
+ type: string_array,
+ default_value: [],
+ description: "Command interfaces provided by the hardware interface for all joints",
+ read_only: true,
+ validation:
+ {
+ unique<>: null,
+ subset_of<>: [["position", "velocity", "acceleration", "effort"]],
+ "joint_trajectory_controller::command_interface_type_combinations": null,
+ },
}
- }
- state_interfaces: {
- type: string_array,
- default_value: [],
- description: "State interfaces provided by the hardware for all joints.",
- read_only: true,
- validation: {
- unique<>: null,
- subset_of<>: [["position", "velocity", "acceleration",]],
- "joint_trajectory_controller::state_interface_type_combinations": null,
+ state_interfaces:
+ {
+ type: string_array,
+ default_value: [],
+ description: "State interfaces provided by the hardware for all joints.",
+ read_only: true,
+ validation:
+ {
+ unique<>: null,
+ subset_of<>: [["position", "velocity", "acceleration"]],
+ "joint_trajectory_controller::state_interface_type_combinations": null,
+ },
}
- }
- allow_partial_joints_goal: {
- type: bool,
- default_value: false,
- description: "Allow joint goals defining trajectory for only some joints.",
- }
- open_loop_control: {
- type: bool,
- default_value: false,
- description: "Use controller in open-loop control mode
- \n\n
- * The controller ignores the states provided by hardware interface but using last commands as states for starting the trajectory interpolation.\n
- * It deactivates the feedback control, see the ``gains`` structure.
- \n\n
- This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators).
- \n\n
- If this flag is set, the controller tries to read the values from the command interfaces on activation.
- If they have real numeric values, those will be used instead of state interfaces.
- Therefore it is important set command interfaces to NaN (i.e., ``std::numeric_limits<double>::quiet_NaN()``) or state values when the hardware is started.\n",
- read_only: true,
- }
- allow_integration_in_goal_trajectories: {
- type: bool,
- default_value: false,
- description: "Allow integration in goal trajectories to accept goals without position or velocity specified",
- }
- action_monitor_rate: {
- type: double,
- default_value: 20.0,
- description: "Rate to monitor status changes when the controller is executing action (control_msgs::action::FollowJointTrajectory)",
- read_only: true,
- validation: {
- gt_eq: [0.1]
+ allow_partial_joints_goal:
+ {
+ type: bool,
+ default_value: false,
+ description: "Allow joint goals defining trajectory for only some joints.",
}
- }
- interpolation_method: {
- type: string,
- default_value: "splines",
- description: "The type of interpolation to use, if any",
- read_only: true,
- validation: {
- one_of<>: [["splines", "none"]],
+ open_loop_control:
+ {
+ type: bool,
+ default_value: false,
+ description: "Use controller in open-loop control mode
+ \n\n
+ * The controller ignores the states provided by hardware interface but using last commands as states for starting the trajectory interpolation.\n
+ * It deactivates the feedback control, see the ``gains`` structure.
+ \n\n
+ This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators).
+ \n\n
+ If this flag is set, the controller tries to read the values from the command interfaces on activation.
+ If they have real numeric values, those will be used instead of state interfaces.
+ Therefore it is important set command interfaces to NaN (i.e., ``std::numeric_limits<double>::quiet_NaN()``) or state values when the hardware is started.\n",
+ read_only: true,
}
- }
- allow_nonzero_velocity_at_trajectory_end: {
- type: bool,
- default_value: false,
- description: "If false, the last velocity point has to be zero or the goal will be rejected",
- }
- cmd_timeout: {
- type: double,
- default_value: 0.0, # seconds
- description: "Timeout after which the input command is considered stale.
- Timeout is counted from the end of the trajectory (the last point).
- ``cmd_timeout`` must be greater than ``constraints.goal_time``, otherwise ignored.
- If zero, timeout is deactivated",
- }
- gains:
- __map_joints:
- p: {
- type: double,
- default_value: 0.0,
- description: "Proportional gain :math:`k_p` for PID"
- }
- i: {
- type: double,
- default_value: 0.0,
- description: "Integral gain :math:`k_i` for PID"
- }
- d: {
- type: double,
- default_value: 0.0,
- description: "Derivative gain :math:`k_d` for PID"
- }
- i_clamp: {
- type: double,
- default_value: 0.0,
- description: "Integral clamp. Symmetrical in both positive and negative direction."
- }
- ff_velocity_scale: {
- type: double,
- default_value: 0.0,
- description: "Feed-forward scaling :math:`k_{ff}` of velocity"
- }
- angle_wraparound: {
- type: bool,
- default_value: false,
- description: "[deprecated] For joints that wrap around (ie. are continuous).
- Normalizes position-error to -pi to pi."
- }
- constraints:
- stopped_velocity_tolerance: {
+ allow_integration_in_goal_trajectories:
+ {
+ type: bool,
+ default_value: false,
+ description: "Allow integration in goal trajectories to accept goals without position or velocity specified",
+ }
+ action_monitor_rate:
+ {
type: double,
- default_value: 0.01,
- description: "Velocity tolerance for at the end of the trajectory that indicates that controlled system is stopped.",
+ default_value: 20.0,
+ description: "Rate to monitor status changes when the controller is executing action (control_msgs::action::FollowJointTrajectory)",
+ read_only: true,
+ validation: { gt_eq: [0.1] },
+ }
+ interpolation_method:
+ {
+ type: string,
+ default_value: "splines",
+ description: "The type of interpolation to use, if any",
+ read_only: true,
+ validation: { one_of<>: [["splines", "none"]] },
+ }
+ allow_nonzero_velocity_at_trajectory_end:
+ {
+ type: bool,
+ default_value: false,
+ description: "If false, the last velocity point has to be zero or the goal will be rejected",
}
- goal_time: {
+ cmd_timeout: {
type: double,
- default_value: 0.0,
- description: "Time tolerance for achieving trajectory goal before or after commanded time.
- If set to zero, the controller will wait a potentially infinite amount of time.",
- validation: {
- gt_eq: [0.0],
- }
+ default_value: 0.0, # seconds
+ description: "Timeout after which the input command is considered stale.
+ Timeout is counted from the end of the trajectory (the last point).
+ ``cmd_timeout`` must be greater than ``constraints.goal_time``, otherwise ignored.
+ If zero, timeout is deactivated",
}
+ controller_plugin:
+ {
+ type: string,
+ default_value: "joint_trajectory_controller_plugins::PidTrajectoryPlugin",
+ description: "Type of the plugin for the trajectory controller",
+ read_only: true,
+ }
+ gains:
__map_joints:
- trajectory: {
+ angle_wraparound:
+ {
+ type: bool,
+ default_value: false,
+ description:
+ "[deprecated] For joints that wrap around (ie. are continuous).
+ Normalizes position-error to -pi to pi.",
+ }
+ constraints:
+ stopped_velocity_tolerance:
+ {
type: double,
- default_value: 0.0,
- description: "Per-joint trajectory offset tolerance during movement.",
+ default_value: 0.01,
+ description: "Velocity tolerance for at the end of the trajectory that indicates that controlled system is stopped.",
}
- goal: {
+ goal_time:
+ {
type: double,
default_value: 0.0,
- description: "Per-joint trajectory offset tolerance at the goal position.",
+ description:
+ "Time tolerance for achieving trajectory goal before or after commanded time.
+ If set to zero, the controller will wait a potentially infinite amount of time.",
+ validation: { gt_eq: [0.0] },
}
+ __map_joints:
+ trajectory:
+ {
+ type: double,
+ default_value: 0.0,
+ description: "Per-joint trajectory offset tolerance during movement.",
+ }
+ goal:
+ {
+ type: double,
+ default_value: 0.0,
+ description: "Per-joint trajectory offset tolerance at the goal position.",
+ }
diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp
index 84c0287d79..0383a5cb84 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller.cpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp
@@ -509,30 +509,35 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters)
{
rclcpp::executors::MultiThreadedExecutor executor;
+ // with kp = 0.0
SetUpAndActivateTrajectoryController(executor);
updateControllerAsync();
- auto pids = traj_controller_->get_pids();
-
- if (traj_controller_->use_closed_loop_pid_adapter())
- {
- EXPECT_EQ(pids.size(), 3);
- auto gain_0 = pids.at(0)->getGains();
- EXPECT_EQ(gain_0.p_gain_, 0.0);
+ auto pids = traj_controller_->get_traj_contr();
+
+ if (traj_controller_->use_external_control_law()) {
+ std::vector<double> tmp_command{0.0, 0.0, 0.0};
+ trajectory_msgs::msg::JointTrajectoryPoint error;
+ error.positions = {1.0, 0.0, 0.0};
+ error.velocities = {0.0, 0.0, 0.0};
+ trajectory_msgs::msg::JointTrajectoryPoint current;
+ trajectory_msgs::msg::JointTrajectoryPoint desired;
+ desired.velocities = {0.0, 0.0, 0.0};
+ rclcpp::Duration duration_since_start(std::chrono::milliseconds(250));
+ rclcpp::Duration period(std::chrono::milliseconds(100));
+
+ pids->computeCommands(tmp_command, current, error, desired, duration_since_start, period);
+ EXPECT_EQ(tmp_command.at(0), 0.0);
double kp = 1.0;
SetPidParameters(kp);
updateControllerAsync();
- pids = traj_controller_->get_pids();
- EXPECT_EQ(pids.size(), 3);
- gain_0 = pids.at(0)->getGains();
- EXPECT_EQ(gain_0.p_gain_, kp);
- }
- else
- {
+ pids->computeCommands(tmp_command, current, error, desired, duration_since_start, period);
+ EXPECT_EQ(tmp_command.at(0), 1.0);
+ } else {
// nothing to check here, skip further test
- EXPECT_EQ(pids.size(), 0);
+ EXPECT_EQ(pids, nullptr);
}
executor.cancel();
@@ -844,11 +849,9 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun
EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD);
}
- if (traj_controller_->has_velocity_command_interface())
- {
- // use_closed_loop_pid_adapter_
- if (traj_controller_->use_closed_loop_pid_adapter())
- {
+ if (traj_controller_->has_velocity_command_interface()) {
+ // use_external_control_law_
+ if (traj_controller_->use_external_control_law()) {
// we expect u = k_p * (s_d-s) for positions
EXPECT_NEAR(
k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
@@ -870,19 +873,25 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun
}
}
- if (traj_controller_->has_effort_command_interface())
- {
- // with effort command interface, use_closed_loop_pid_adapter is always true
- // we expect u = k_p * (s_d-s) for positions
- EXPECT_NEAR(
- k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0],
- k_p * COMMON_THRESHOLD);
- EXPECT_NEAR(
- k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1],
- k_p * COMMON_THRESHOLD);
- EXPECT_NEAR(
- k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_eff_[2],
- k_p * COMMON_THRESHOLD);
+ if (traj_controller_->has_effort_command_interface()) {
+ if (traj_controller_->use_external_control_law()) {
+ // we expect u = k_p * (s_d-s) for positions
+ EXPECT_NEAR(
+ k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0],
+ k_p * COMMON_THRESHOLD);
+ EXPECT_NEAR(
+ k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1],
+ k_p * COMMON_THRESHOLD);
+ EXPECT_NEAR(
+ k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_eff_[2],
+ k_p * COMMON_THRESHOLD);
+ } else {
+ // interpolated points_velocities only
+ // check command interface
+ EXPECT_LT(0.0, joint_eff_[0]);
+ EXPECT_LT(0.0, joint_eff_[1]);
+ EXPECT_LT(0.0, joint_eff_[2]);
+ }
}
executor.cancel();
@@ -948,11 +957,9 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound)
EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD);
}
- if (traj_controller_->has_velocity_command_interface())
- {
- // use_closed_loop_pid_adapter_
- if (traj_controller_->use_closed_loop_pid_adapter())
- {
+ if (traj_controller_->has_velocity_command_interface()) {
+ // use_external_control_law_
+ if (traj_controller_->use_external_control_law()) {
// we expect u = k_p * (s_d-s) for joint0 and joint1
EXPECT_NEAR(
k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
@@ -976,21 +983,28 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound)
}
}
- if (traj_controller_->has_effort_command_interface())
- {
- // with effort command interface, use_closed_loop_pid_adapter is always true
- // we expect u = k_p * (s_d-s) for joint0 and joint1
- EXPECT_NEAR(
- k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0],
- k_p * COMMON_THRESHOLD);
- EXPECT_NEAR(
- k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1],
- k_p * COMMON_THRESHOLD);
- // is error of positions[2] wrapped around?
- EXPECT_GT(0.0, joint_eff_[2]);
- EXPECT_NEAR(
- k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2],
- k_p * COMMON_THRESHOLD);
+ if (traj_controller_->has_effort_command_interface()) {
+ // use_external_control_law_
+ if (traj_controller_->use_external_control_law()) {
+ // we expect u = k_p * (s_d-s) for positions[0] and positions[1]
+ EXPECT_NEAR(
+ k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0],
+ k_p * COMMON_THRESHOLD);
+ EXPECT_NEAR(
+ k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1],
+ k_p * COMMON_THRESHOLD);
+ // is error of positions[2] angle_wraparound?
+ EXPECT_GT(0.0, joint_eff_[2]);
+ EXPECT_NEAR(
+ k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2],
+ k_p * COMMON_THRESHOLD);
+ } else {
+ // interpolated points_velocities only
+ // check command interface
+ EXPECT_LT(0.0, joint_eff_[0]);
+ EXPECT_LT(0.0, joint_eff_[1]);
+ EXPECT_LT(0.0, joint_eff_[2]);
+ }
}
executor.cancel();
@@ -1064,9 +1078,8 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le
EXPECT_LT(0.0, joint_vel_[1]);
EXPECT_TRUE(std::isnan(current_command.velocities[2]));
- // use_closed_loop_pid_adapter_
- if (traj_controller_->use_closed_loop_pid_adapter())
- {
+ // use_external_control_law_
+ if (traj_controller_->use_external_control_law()) {
// we expect u = k_p * (s_d-s)
EXPECT_NEAR(
k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
@@ -1150,16 +1163,14 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le
EXPECT_TRUE(std::isnan(current_command.positions[2]));
}
- if (traj_controller_->has_velocity_command_interface())
- {
+ if (traj_controller_->has_velocity_command_interface()) {
// check command interface
EXPECT_LT(0.0, joint_vel_[0]);
EXPECT_LT(0.0, joint_vel_[1]);
EXPECT_TRUE(std::isnan(current_command.velocities[2]));
// use_closed_loop_pid_adapter_
- if (traj_controller_->use_closed_loop_pid_adapter())
- {
+ if (traj_controller_->use_external_control_law()) {
// we expect u = k_p * (s_d-s)
EXPECT_NEAR(
k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
@@ -1170,8 +1181,14 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le
}
}
- if (traj_controller_->has_effort_command_interface())
- {
+ if (traj_controller_->has_effort_command_interface()) {
+ // check command interface
+ EXPECT_LT(0.0, joint_eff_[0]);
+ EXPECT_LT(0.0, joint_eff_[1]);
+ EXPECT_TRUE(std::isnan(current_command.effort[2]));
+ }
+
+ if (traj_controller_->has_effort_command_interface()) {
// check command interface
EXPECT_LT(0.0, joint_eff_[0]);
EXPECT_LT(0.0, joint_eff_[1]);
@@ -1317,9 +1334,9 @@ TEST_P(TrajectoryControllerTestParameterized, timeout)
}
/**
- * @brief check if use_closed_loop_pid is active
+ * @brief check if use_external_control_law is set
*/
-TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid)
+TEST_P(TrajectoryControllerTestParameterized, set_external_control_law)
{
rclcpp::executors::MultiThreadedExecutor executor;
@@ -1333,7 +1350,7 @@ TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid)
!traj_controller_->is_open_loop()) ||
traj_controller_->has_effort_command_interface())
{
- EXPECT_TRUE(traj_controller_->use_closed_loop_pid_adapter());
+ EXPECT_TRUE(traj_controller_->use_external_control_law());
}
}
@@ -1390,9 +1407,9 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error)
}
// is the velocity error correct?
if (
- traj_controller_->use_closed_loop_pid_adapter() // always needed for PID controller
- || (traj_controller_->has_velocity_state_interface() &&
- traj_controller_->has_velocity_command_interface()))
+ traj_controller_->use_external_control_law() || // always needed for PID controller
+ (traj_controller_->has_velocity_state_interface() &&
+ traj_controller_->has_velocity_command_interface()))
{
// don't check against a value, because spline interpolation might overshoot depending on
// interface combinations
@@ -1453,10 +1470,9 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order)
EXPECT_NEAR(points_positions.at(2), joint_pos_[2], COMMON_THRESHOLD);
}
- if (traj_controller_->has_velocity_command_interface())
- {
- // if use_closed_loop_pid_adapter_==false: we expect desired velocities from direct sampling
- // if use_closed_loop_pid_adapter_==true: we expect desired velocities, because we use PID with
+ if (traj_controller_->has_velocity_command_interface()) {
+ // if use_external_control_law_==false: we expect desired velocities from direct sampling
+ // if use_external_control_law_==true: we expect desired velocities, because we use PID with
// feedforward term only
EXPECT_GT(0.0, joint_vel_[0]);
EXPECT_GT(0.0, joint_vel_[1]);
@@ -1839,10 +1855,13 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace)
expected_actual.positions = {points_old[0].begin(), points_old[0].end()};
expected_desired.positions = {points_old[0].begin(), points_old[0].end()};
expected_actual.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()};
- expected_desired.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()};
+ expected_desired.velocities =
+ {points_old_velocities[0].begin(), points_old_velocities[0].end()};
// Check that we reached end of points_old trajectory
auto end_time =
- waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1);
+ waitAndCompareState(
+ expected_actual, expected_desired, executor, rclcpp::Duration(delay),
+ 0.1);
RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory");
points_partial_new_velocities[0][0] =
@@ -1882,7 +1901,9 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory)
expected_desired = expected_actual;
// Check that we reached end of points_old[0] trajectory
auto end_time =
- waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1);
+ waitAndCompareState(
+ expected_actual, expected_desired, executor, rclcpp::Duration(delay),
+ 0.1);
RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the past");
// New trajectory will end before current time
@@ -1912,7 +1933,9 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory
expected_desired = expected_actual;
// Check that we reached end of points_old[0]trajectory
auto end_time =
- waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1);
+ waitAndCompareState(
+ expected_actual, expected_desired, executor, rclcpp::Duration(delay),
+ 0.1);
// send points_new before the old trajectory is finished
RCLCPP_INFO(
@@ -1957,7 +1980,9 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur
expected_desired = expected_actual;
// Check that we reached end of points_old[0]trajectory and are starting points_old[1]
auto end_time =
- waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1);
+ waitAndCompareState(
+ expected_actual, expected_desired, executor, rclcpp::Duration(delay),
+ 0.1);
// Send partial trajectory starting after full trajecotry is complete
RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the future");
@@ -1966,11 +1991,13 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur
partial_traj_velocities);
// Wait until the end start and end of partial traj
- expected_actual.positions = {partial_traj.back()[0], partial_traj.back()[1], full_traj.back()[2]};
+ expected_actual.positions =
+ {partial_traj.back()[0], partial_traj.back()[1], full_traj.back()[2]};
expected_desired = expected_actual;
waitAndCompareState(
- expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1, end_time);
+ expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1,
+ end_time);
}
TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_error_updated)
@@ -2016,7 +2043,9 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro
// Move joint further in the same direction as before (to the second goal)
points = {{second_goal}};
- publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, second_goal_velocities);
+ publish(
+ time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {},
+ second_goal_velocities);
traj_controller_->wait_for_trajectory(executor);
// One the first update(s) there should be a "jump" in opposite direction from command
@@ -2302,7 +2331,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail)
rclcpp::Parameter("constraints.joint3.trajectory", state_tol)};
rclcpp::executors::MultiThreadedExecutor executor;
- double kp = 1.0; // activate feedback control for testing velocity/effort PID
+ double kp = 1.0; // activate feedback control for testing velocity/effort PID
SetUpAndActivateTrajectoryController(executor, params, true, kp);
// send msg
@@ -2365,7 +2394,9 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail)
INSTANTIATE_TEST_SUITE_P(
PositionTrajectoryControllers, TrajectoryControllerTestParameterized,
::testing::Values(
- std::make_tuple(std::vector<std::string>({"position"}), std::vector<std::string>({"position"})),
+ std::make_tuple(
+ std::vector<std::string>({"position"}),
+ std::vector<std::string>({"position"})),
std::make_tuple(
std::vector<std::string>({"position"}), std::vector<std::string>({"position", "velocity"})),
std::make_tuple(
diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
index 77151c6fac..955b2fcced 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
@@ -88,11 +88,6 @@ class TestableJointTrajectoryController
return success;
}
- void set_joint_names(const std::vector<std::string> & joint_names)
- {
- params_.joints = joint_names;
- }
-
void set_command_joint_names(const std::vector<std::string> & command_joint_names)
{
command_joint_names_ = command_joint_names;
@@ -108,7 +103,7 @@ class TestableJointTrajectoryController
params_.state_interfaces = state_interfaces;
}
- void trigger_declare_parameters() { param_listener_->declare_params(); }
+ void trigger_declare_parameters() {param_listener_->declare_params();}
void testable_compute_error_for_joint(
JointTrajectoryPoint & error, const size_t index, const JointTrajectoryPoint & current,
@@ -135,11 +130,15 @@ class TestableJointTrajectoryController
bool has_effort_command_interface() const { return has_effort_command_interface_; }
- bool use_closed_loop_pid_adapter() const { return use_closed_loop_pid_adapter_; }
+ bool use_external_control_law() const {return use_external_control_law_;}
bool is_open_loop() const { return params_.open_loop_control; }
- std::vector<PidPtr> get_pids() const { return pids_; }
+ std::shared_ptr<joint_trajectory_controller_plugins::TrajectoryControllerBase> get_traj_contr()
+ const
+ {
+ return traj_contr_;
+ }
joint_trajectory_controller::SegmentTolerances get_tolerances() const
{
@@ -252,6 +251,10 @@ class TrajectoryControllerTest : public ::testing::Test
controller_name_, urdf, 0, "", traj_controller_->define_custom_node_options());
}
+ /**
+ * @brief set PIDs for every entry in joint_names_
+ * be aware to update if PIDs should be configured for different command_joints than joint_names
+ */
void SetPidParameters(double p_value = 0.0, double ff_value = 1.0)
{
traj_controller_->trigger_declare_parameters();
@@ -294,10 +297,14 @@ class TrajectoryControllerTest : public ::testing::Test
// read-only parameters have to be set before init -> won't be read otherwise
SetUpTrajectoryController(executor, parameters_local, urdf);
- // set pid parameters before configure
- SetPidParameters(k_p, ff);
traj_controller_->get_node()->configure();
+ // set pid parameters before activate. The PID plugin has to be loaded already, otherwise
+ // parameters are not declared yet
+ if (traj_controller_->use_external_control_law()) {
+ SetPidParameters(k_p, ff);
+ }
+
ActivateTrajectoryController(
separate_cmd_and_state_values, initial_pos_joints, initial_vel_joints, initial_acc_joints,
initial_eff_joints);
@@ -563,10 +570,8 @@ class TrajectoryControllerTest : public ::testing::Test
// i.e., active but trivial trajectory (one point only)
EXPECT_TRUE(traj_controller_->has_trivial_traj());
- if (traj_controller_->use_closed_loop_pid_adapter() == false)
- {
- if (traj_controller_->has_position_command_interface())
- {
+ if (traj_controller_->use_external_control_law() == false) {
+ if (traj_controller_->has_position_command_interface()) {
EXPECT_NEAR(position.at(0), joint_pos_[0], COMMON_THRESHOLD);
EXPECT_NEAR(position.at(1), joint_pos_[1], COMMON_THRESHOLD);
EXPECT_NEAR(position.at(2), joint_pos_[2], COMMON_THRESHOLD);
diff --git a/joint_trajectory_controller_plugins/CMakeLists.txt b/joint_trajectory_controller_plugins/CMakeLists.txt
new file mode 100644
index 0000000000..5ef16e7b54
--- /dev/null
+++ b/joint_trajectory_controller_plugins/CMakeLists.txt
@@ -0,0 +1,77 @@
+cmake_minimum_required(VERSION 3.8)
+project(joint_trajectory_controller_plugins LANGUAGES CXX)
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+ add_compile_options(-Wall -Wextra -Wconversion)
+endif()
+
+# find dependencies
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+ control_toolbox
+ generate_parameter_library
+ pluginlib
+ rclcpp
+ rclcpp_lifecycle
+ trajectory_msgs
+)
+
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
+find_package(backward_ros REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+ find_package(${Dependency} REQUIRED)
+endforeach()
+
+generate_parameter_library(pid_trajectory_plugin_parameters
+ src/pid_trajectory_plugin_parameters.yaml
+)
+
+add_library(pid_trajectory_plugin SHARED src/pid_trajectory_plugin.cpp)
+add_library(joint_trajectory_controller_plugins::pid_trajectory_plugin ALIAS pid_trajectory_plugin)
+target_compile_features(pid_trajectory_plugin PUBLIC cxx_std_17)
+target_include_directories(pid_trajectory_plugin PUBLIC
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+ $<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
+ament_target_dependencies(pid_trajectory_plugin PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
+target_link_libraries(pid_trajectory_plugin PUBLIC
+ pid_trajectory_plugin_parameters
+)
+pluginlib_export_plugin_description_file(joint_trajectory_controller_plugins plugins.xml)
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(pid_trajectory_plugin PRIVATE "JOINT_TRAJECTORY_CONTROLLER_PLUGINS_BUILDING_LIBRARY")
+
+install(
+ DIRECTORY include/
+ DESTINATION include/${PROJECT_NAME}
+)
+install(
+ TARGETS pid_trajectory_plugin pid_trajectory_plugin_parameters
+ EXPORT export_${PROJECT_NAME}
+ ARCHIVE DESTINATION lib
+ LIBRARY DESTINATION lib
+ RUNTIME DESTINATION bin
+)
+
+if(BUILD_TESTING)
+ ament_add_gmock(test_load_pid_trajectory test/test_load_pid_trajectory.cpp)
+ target_link_libraries(test_load_pid_trajectory pid_trajectory_plugin pid_trajectory_plugin_parameters)
+ ament_target_dependencies(test_load_pid_trajectory ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+ ament_add_gmock(test_pid_trajectory test/test_pid_trajectory.cpp)
+ target_link_libraries(test_pid_trajectory pid_trajectory_plugin pid_trajectory_plugin_parameters)
+ ament_target_dependencies(test_pid_trajectory ${THIS_PACKAGE_INCLUDE_DEPENDS})
+endif()
+
+ament_export_include_directories(
+ "include/${PROJECT_NAME}"
+)
+ament_export_libraries(
+ pid_trajectory_plugin
+)
+ament_export_targets(
+ export_${PROJECT_NAME}
+)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()
diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp
new file mode 100644
index 0000000000..3718f04847
--- /dev/null
+++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp
@@ -0,0 +1,91 @@
+// Copyright 2023 AIT Austrian Institute of Technology
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef JOINT_TRAJECTORY_CONTROLLER_PLUGINS__PID_TRAJECTORY_PLUGIN_HPP_
+#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS__PID_TRAJECTORY_PLUGIN_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "control_toolbox/pid.hpp"
+
+#include "joint_trajectory_controller_plugins/trajectory_controller_base.hpp"
+#include "joint_trajectory_controller_plugins/visibility_control.h"
+#include "pid_trajectory_plugin_parameters.hpp" // auto-generated by generate_parameter_library
+
+using PidPtr = std::shared_ptr<control_toolbox::Pid>;
+
+namespace joint_trajectory_controller_plugins
+{
+
+class PidTrajectoryPlugin : public TrajectoryControllerBase
+{
+public:
+ bool initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) override;
+
+ bool configure() override;
+
+ bool activate() override;
+
+ bool computeControlLawNonRT_impl(
+ const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /*trajectory*/) override
+ {
+ // nothing to do
+ return true;
+ }
+
+ bool computeControlLawRT_impl(
+ const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /*trajectory*/) override
+ {
+ // nothing to do
+ return true;
+ }
+
+ bool updateGainsRT() override;
+
+ void computeCommands(
+ std::vector<double> & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current,
+ const trajectory_msgs::msg::JointTrajectoryPoint error,
+ const trajectory_msgs::msg::JointTrajectoryPoint desired,
+ const rclcpp::Duration & duration_since_start, const rclcpp::Duration & period) override;
+
+ void reset() override;
+
+ void start() override
+ {
+ // nothing to do
+ }
+
+protected:
+ /**
+ * @brief parse PID gains from parameter struct
+ */
+ void parseGains();
+
+ // number of command joints
+ size_t num_cmd_joints_;
+ // PID controllers
+ std::vector<PidPtr> pids_;
+ // Feed-forward velocity weight factor when calculating closed loop pid adapter's command
+ std::vector<double> ff_velocity_scale_;
+
+ // Parameters from ROS for joint_trajectory_controller_plugins
+ std::shared_ptr<ParamListener> param_listener_;
+ Params params_;
+};
+
+} // namespace joint_trajectory_controller_plugins
+
+#endif // JOINT_TRAJECTORY_CONTROLLER_PLUGINS__PID_TRAJECTORY_PLUGIN_HPP_
diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp
new file mode 100644
index 0000000000..9814ddfeb6
--- /dev/null
+++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp
@@ -0,0 +1,177 @@
+// Copyright (c) 2023 AIT Austrian Institute of Technology
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef JOINT_TRAJECTORY_CONTROLLER_PLUGINS__TRAJECTORY_CONTROLLER_BASE_HPP_
+#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS__TRAJECTORY_CONTROLLER_BASE_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/time.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "realtime_tools/realtime_buffer.h"
+#include "trajectory_msgs/msg/joint_trajectory.hpp"
+#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
+
+#include "joint_trajectory_controller_plugins/visibility_control.h"
+
+namespace joint_trajectory_controller_plugins
+{
+class TrajectoryControllerBase
+{
+public:
+ JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
+ TrajectoryControllerBase() = default;
+
+ JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
+ virtual ~TrajectoryControllerBase() = default;
+
+ /**
+ * @brief initialize the controller plugin.
+ * declare parameters
+ */
+ JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
+ virtual bool initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) = 0;
+
+ /**
+ * @brief configure the controller plugin.
+ * parse read-only parameters, pre-allocate memory for the controller
+ */
+ JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
+ virtual bool configure() = 0;
+
+ /**
+ * @brief activate the controller plugin.
+ * parse parameters
+ */
+ JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
+ virtual bool activate() = 0;
+
+ /**
+ * @brief compute the control law for the given trajectory
+ *
+ * this method can take more time to compute the control law. Hence, it will block the execution
+ * of the trajectory until it finishes
+ *
+ * this method is not virtual, any overrides won't be called by JTC. Instead, override
+ * computeControlLawNonRT_impl for your implementation
+ *
+ * @return true if the gains were computed, false otherwise
+ */
+ JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
+ bool computeControlLawNonRT(
+ const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory)
+ {
+ rt_control_law_ready_.writeFromNonRT(false);
+ auto ret = computeControlLawNonRT_impl(trajectory);
+ rt_control_law_ready_.writeFromNonRT(true);
+ return ret;
+ }
+
+ /**
+ * @brief compute the control law for the given trajectory
+ *
+ * this method must finish quickly (within one controller-update rate)
+ *
+ * this method is not virtual, any overrides won't be called by JTC. Instead, override
+ * computeControlLawRT_impl for your implementation
+ *
+ * @return true if the gains were computed, false otherwise
+ */
+ JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
+ bool computeControlLawRT(
+ const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory)
+ {
+ // TODO(christophfroehlich): Need a lock-free write here
+ rt_control_law_ready_.writeFromNonRT(false);
+ auto ret = computeControlLawRT_impl(trajectory);
+ rt_control_law_ready_.writeFromNonRT(true);
+ return ret;
+ }
+
+ /**
+ * @brief called when the current trajectory started in update loop
+ *
+ * use this to implement switching of real-time buffers for updating the control law
+ */
+ virtual void start(void) = 0;
+
+ /**
+ * @brief update the gains from a RT thread
+ *
+ * this method must finish quickly (within one controller-update rate)
+ *
+ * @return true if the gains were updated, false otherwise
+ */
+ JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
+ virtual bool updateGainsRT(void) = 0;
+
+ /**
+ * @brief compute the commands with the precalculated control law
+ *
+ * @param[out] tmp_command the output command
+ * @param[in] current the current state
+ * @param[in] error the error between the current state and the desired state
+ * @param[in] desired the desired state
+ * @param[in] duration_since_start the duration since the start of the trajectory
+ * can be negative if the trajectory-start is in the future
+ * @param[in] period the period since the last update
+ */
+ JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
+ virtual void computeCommands(
+ std::vector<double> & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current,
+ const trajectory_msgs::msg::JointTrajectoryPoint error,
+ const trajectory_msgs::msg::JointTrajectoryPoint desired,
+ const rclcpp::Duration & duration_since_start, const rclcpp::Duration & period) = 0;
+
+ /**
+ * @brief reset internal states
+ */
+ JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
+ virtual void reset() = 0;
+
+ /**
+ * @return true if the control law is ready (updated with the trajectory)
+ */
+ JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
+ bool is_ready() { return rt_control_law_ready_.readFromRT(); }
+
+protected:
+ // the node handle for parameter handling
+ rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
+ // Are we computing the control law or is it valid?
+ realtime_tools::RealtimeBuffer<bool> rt_control_law_ready_;
+
+ /**
+ * @brief compute the control law from the given trajectory (in the non-RT loop)
+ * @return true if the gains were computed, false otherwise
+ */
+ JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
+ virtual bool computeControlLawNonRT_impl(
+ const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory) = 0;
+
+ /**
+ * @brief compute the control law for a single point (in the RT loop)
+ * @return true if the gains were computed, false otherwise
+ */
+ JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
+ virtual bool computeControlLawRT_impl(
+ const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory) = 0;
+};
+
+} // namespace joint_trajectory_controller_plugins
+
+#endif // JOINT_TRAJECTORY_CONTROLLER_PLUGINS__TRAJECTORY_CONTROLLER_BASE_HPP_
diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/visibility_control.h b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/visibility_control.h
new file mode 100644
index 0000000000..71b56114c6
--- /dev/null
+++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/visibility_control.h
@@ -0,0 +1,49 @@
+// Copyright 2023 AIT Austrian Institute of Technology
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef JOINT_TRAJECTORY_CONTROLLER_PLUGINS__VISIBILITY_CONTROL_H_
+#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS__VISIBILITY_CONTROL_H_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+// https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+#ifdef __GNUC__
+#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_EXPORT __attribute__((dllexport))
+#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_IMPORT __attribute__((dllimport))
+#else
+#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_EXPORT __declspec(dllexport)
+#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_IMPORT __declspec(dllimport)
+#endif
+#ifdef JOINT_TRAJECTORY_CONTROLLER_PLUGINS_BUILDING_LIBRARY
+#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC JOINT_TRAJECTORY_CONTROLLER_PLUGINS_EXPORT
+#else
+#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC JOINT_TRAJECTORY_CONTROLLER_PLUGINS_IMPORT
+#endif
+#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC_TYPE JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
+#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_LOCAL
+#else
+#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_EXPORT __attribute__((visibility("default")))
+#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_IMPORT
+#if __GNUC__ >= 4
+#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC __attribute__((visibility("default")))
+#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_LOCAL __attribute__((visibility("hidden")))
+#else
+#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
+#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_LOCAL
+#endif
+#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC_TYPE
+#endif
+
+#endif // JOINT_TRAJECTORY_CONTROLLER_PLUGINS__VISIBILITY_CONTROL_H_
diff --git a/joint_trajectory_controller_plugins/package.xml b/joint_trajectory_controller_plugins/package.xml
new file mode 100644
index 0000000000..b0812ed8f8
--- /dev/null
+++ b/joint_trajectory_controller_plugins/package.xml
@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+ <name>joint_trajectory_controller_plugins</name>
+ <version>0.0.0</version>
+ <description>TODO: Package description</description>
+ <maintainer email="christoph.froehlich@ait.ac.at">vscode</maintainer>
+ <license>Apache-2.0</license>
+
+ <buildtool_depend>ament_cmake_ros</buildtool_depend>
+
+ <depend>control_toolbox</depend>
+ <depend>pluginlib</depend>
+ <depend>trajectory_msgs</depend>
+ <depend>generate_parameter_library</depend>
+
+ <test_depend>ament_lint_auto</test_depend>
+ <test_depend>ament_lint_common</test_depend>
+
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
+</package>
diff --git a/joint_trajectory_controller_plugins/plugins.xml b/joint_trajectory_controller_plugins/plugins.xml
new file mode 100644
index 0000000000..af91db2a73
--- /dev/null
+++ b/joint_trajectory_controller_plugins/plugins.xml
@@ -0,0 +1,6 @@
+<library path="pid_trajectory_plugin">
+ <class
+ type="joint_trajectory_controller_plugins::PidTrajectoryPlugin" base_class_type="joint_trajectory_controller_plugins::TrajectoryControllerBase">
+ <description>A trajectory controller consisting of a PID controller per joint.</description>
+ </class>
+</library>
diff --git a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp
new file mode 100644
index 0000000000..7ca9b8b4a9
--- /dev/null
+++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp
@@ -0,0 +1,137 @@
+// Copyright 2023 AIT Austrian Institute of Technology
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp"
+
+namespace joint_trajectory_controller_plugins
+{
+
+bool PidTrajectoryPlugin::initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node)
+{
+ node_ = node;
+
+ try
+ {
+ // Create the parameter listener and get the parameters
+ param_listener_ = std::make_shared<ParamListener>(node_);
+ }
+ catch (const std::exception & e)
+ {
+ fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
+ return false;
+ }
+
+ return true;
+}
+
+bool PidTrajectoryPlugin::configure()
+{
+ try
+ {
+ params_ = param_listener_->get_params();
+ }
+ catch (const std::exception & e)
+ {
+ fprintf(stderr, "Exception thrown during configure stage with message: %s \n", e.what());
+ return false;
+ }
+
+ // parse read-only params
+ num_cmd_joints_ = params_.command_joints.size();
+ if (num_cmd_joints_ == 0)
+ {
+ RCLCPP_ERROR(node_->get_logger(), "[PidTrajectoryPlugin] No command joints specified.");
+ return false;
+ }
+ pids_.resize(num_cmd_joints_);
+ ff_velocity_scale_.resize(num_cmd_joints_);
+
+ return true;
+};
+
+bool PidTrajectoryPlugin::activate()
+{
+ params_ = param_listener_->get_params();
+ parseGains();
+ return true;
+};
+
+bool PidTrajectoryPlugin::updateGainsRT()
+{
+ if (param_listener_->is_old(params_))
+ {
+ params_ = param_listener_->get_params();
+ parseGains();
+ }
+
+ return true;
+}
+
+void PidTrajectoryPlugin::parseGains()
+{
+ for (size_t i = 0; i < num_cmd_joints_; ++i)
+ {
+ RCLCPP_DEBUG(
+ node_->get_logger(), "[PidTrajectoryPlugin] params_.command_joints %lu : %s", i,
+ params_.command_joints[i].c_str());
+
+ const auto & gains = params_.gains.command_joints_map.at(params_.command_joints[i]);
+ pids_[i] = std::make_shared<control_toolbox::Pid>(
+ gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp);
+ ff_velocity_scale_[i] = gains.ff_velocity_scale;
+
+ RCLCPP_DEBUG(node_->get_logger(), "[PidTrajectoryPlugin] gains.p: %f", gains.p);
+ RCLCPP_DEBUG(
+ node_->get_logger(), "[PidTrajectoryPlugin] ff_velocity_scale_: %f", ff_velocity_scale_[i]);
+ }
+
+ RCLCPP_INFO(
+ node_->get_logger(),
+ "[PidTrajectoryPlugin] Loaded PID gains from ROS parameters for %lu joint(s).",
+ num_cmd_joints_);
+}
+
+void PidTrajectoryPlugin::computeCommands(
+ std::vector<double> & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint /*current*/,
+ const trajectory_msgs::msg::JointTrajectoryPoint error,
+ const trajectory_msgs::msg::JointTrajectoryPoint desired,
+ const rclcpp::Duration & /*duration_since_start*/, const rclcpp::Duration & period)
+{
+ // Update PIDs
+ for (auto i = 0ul; i < num_cmd_joints_; ++i)
+ {
+ tmp_command[i] = (desired.velocities[i] * ff_velocity_scale_[i]) +
+ pids_[i]->computeCommand(
+ error.positions[i], error.velocities[i], (uint64_t)period.nanoseconds());
+ }
+}
+
+void PidTrajectoryPlugin::reset()
+{
+ for (const auto & pid : pids_)
+ {
+ if (pid)
+ {
+ pid->reset();
+ }
+ }
+}
+
+} // namespace joint_trajectory_controller_plugins
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(
+ joint_trajectory_controller_plugins::PidTrajectoryPlugin,
+ joint_trajectory_controller_plugins::TrajectoryControllerBase)
diff --git a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin_parameters.yaml b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin_parameters.yaml
new file mode 100644
index 0000000000..f934fc9bdf
--- /dev/null
+++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin_parameters.yaml
@@ -0,0 +1,37 @@
+joint_trajectory_controller_plugins:
+ command_joints: {
+ type: string_array,
+ default_value: [],
+ description: "Names of joints used by the controller plugin",
+ read_only: true,
+ validation: {
+ size_gt<>: [0],
+ }
+ }
+ gains:
+ __map_command_joints:
+ p: {
+ type: double,
+ default_value: 0.0,
+ description: "Proportional gain for PID"
+ }
+ i: {
+ type: double,
+ default_value: 0.0,
+ description: "Integral gain for PID"
+ }
+ d: {
+ type: double,
+ default_value: 0.0,
+ description: "Derivative gain for PID"
+ }
+ i_clamp: {
+ type: double,
+ default_value: 0.0,
+ description: "Integral clamp. Symmetrical in both positive and negative direction."
+ }
+ ff_velocity_scale: {
+ type: double,
+ default_value: 0.0,
+ description: "Feed-forward scaling of velocity."
+ }
diff --git a/joint_trajectory_controller_plugins/test/test_load_pid_trajectory.cpp b/joint_trajectory_controller_plugins/test/test_load_pid_trajectory.cpp
new file mode 100644
index 0000000000..09a08d6cee
--- /dev/null
+++ b/joint_trajectory_controller_plugins/test/test_load_pid_trajectory.cpp
@@ -0,0 +1,49 @@
+// Copyright 2023 AIT Austrian Institute of Technology
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <string>
+
+#include "gmock/gmock.h"
+
+#include "joint_trajectory_controller_plugins/trajectory_controller_base.hpp"
+#include "pluginlib/class_loader.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+
+TEST(TestLoadPidController, load_controller)
+{
+ rclcpp::init(0, nullptr);
+
+ pluginlib::ClassLoader<joint_trajectory_controller_plugins::TrajectoryControllerBase>
+ traj_controller_loader(
+ "joint_trajectory_controller_plugins",
+ "joint_trajectory_controller_plugins::TrajectoryControllerBase");
+
+ std::shared_ptr<joint_trajectory_controller_plugins::TrajectoryControllerBase> traj_contr;
+
+ auto available_classes = traj_controller_loader.getDeclaredClasses();
+
+ std::cout << "available filters:" << std::endl;
+ for (const auto & available_class : available_classes)
+ {
+ std::cout << " " << available_class << std::endl;
+ }
+
+ std::string controller_type = "joint_trajectory_controller_plugins::PidTrajectoryPlugin";
+ ASSERT_TRUE(traj_controller_loader.isClassAvailable(controller_type));
+ ASSERT_NO_THROW(traj_contr = traj_controller_loader.createSharedInstance(controller_type));
+
+ rclcpp::shutdown();
+}
diff --git a/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp b/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp
new file mode 100644
index 0000000000..ef8227a332
--- /dev/null
+++ b/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp
@@ -0,0 +1,107 @@
+// Copyright 2023 AIT Austrian Institute of Technology
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "test_pid_trajectory.hpp"
+
+TEST_F(PidTrajectoryTest, TestEmptySetup)
+{
+ std::shared_ptr<TestableJointTrajectoryControllerPlugin> traj_contr =
+ std::make_shared<TestableJointTrajectoryControllerPlugin>();
+
+ ASSERT_FALSE(traj_contr->initialize(node_));
+}
+
+TEST_F(PidTrajectoryTest, TestSingleJoint)
+{
+ std::shared_ptr<TestableJointTrajectoryControllerPlugin> traj_contr =
+ std::make_shared<TestableJointTrajectoryControllerPlugin>();
+
+ std::vector<std::string> joint_names = {"joint1"};
+ auto joint_names_paramv = rclcpp::ParameterValue(joint_names);
+
+ // override parameter
+ node_->declare_parameter("command_joints", joint_names_paramv);
+
+ ASSERT_TRUE(traj_contr->initialize(node_));
+ node_->set_parameter(rclcpp::Parameter("gains.joint1.p", 1.0));
+ ASSERT_TRUE(traj_contr->configure());
+ ASSERT_TRUE(traj_contr->activate());
+ ASSERT_TRUE(
+ traj_contr->computeControlLawNonRT(std::make_shared<trajectory_msgs::msg::JointTrajectory>()));
+ ASSERT_TRUE(traj_contr->updateGainsRT());
+
+ trajectory_msgs::msg::JointTrajectoryPoint traj_msg;
+ traj_msg.positions.push_back(0.0);
+ traj_msg.velocities.push_back(0.0);
+ std::vector<double> tmp_command(1, 0.0);
+ const rclcpp::Duration time_since_start(1, 0);
+ const rclcpp::Duration period(1, 0);
+
+ ASSERT_NO_THROW(traj_contr->computeCommands(
+ tmp_command, traj_msg, traj_msg, traj_msg, time_since_start, period));
+}
+
+TEST_F(PidTrajectoryTest, TestMultipleJoints)
+{
+ std::shared_ptr<TestableJointTrajectoryControllerPlugin> traj_contr =
+ std::make_shared<TestableJointTrajectoryControllerPlugin>();
+
+ std::vector<std::string> joint_names = {"joint1", "joint2", "joint3"};
+ auto joint_names_paramv = rclcpp::ParameterValue(joint_names);
+
+ // override parameter
+ node_->declare_parameter("command_joints", joint_names_paramv);
+
+ ASSERT_TRUE(traj_contr->initialize(node_));
+ // set dynamic parameters
+ node_->set_parameter(rclcpp::Parameter("gains.joint1.p", 1.0));
+ node_->set_parameter(rclcpp::Parameter("gains.joint2.p", 2.0));
+ node_->set_parameter(rclcpp::Parameter("gains.joint3.p", 3.0));
+ ASSERT_TRUE(traj_contr->configure());
+ ASSERT_TRUE(traj_contr->activate());
+ ASSERT_TRUE(
+ traj_contr->computeControlLawNonRT(std::make_shared<trajectory_msgs::msg::JointTrajectory>()));
+ ASSERT_TRUE(traj_contr->updateGainsRT());
+
+ ASSERT_TRUE(
+ traj_contr->computeControlLawNonRT(std::make_shared<trajectory_msgs::msg::JointTrajectory>()));
+ ASSERT_TRUE(traj_contr->updateGainsRT());
+
+ trajectory_msgs::msg::JointTrajectoryPoint traj_msg;
+ traj_msg.positions.push_back(1.0);
+ traj_msg.positions.push_back(1.0);
+ traj_msg.positions.push_back(1.0);
+ traj_msg.velocities.push_back(0.0);
+ traj_msg.velocities.push_back(0.0);
+ traj_msg.velocities.push_back(0.0);
+ std::vector<double> tmp_command(3, 0.0);
+ const rclcpp::Duration time_since_start(1, 0);
+ const rclcpp::Duration period(1, 0);
+
+ ASSERT_NO_THROW(traj_contr->computeCommands(
+ tmp_command, traj_msg, traj_msg, traj_msg, time_since_start, period));
+
+ EXPECT_EQ(tmp_command[0], 1.0);
+ EXPECT_EQ(tmp_command[1], 2.0);
+ EXPECT_EQ(tmp_command[2], 3.0);
+}
+
+int main(int argc, char ** argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ rclcpp::init(argc, argv);
+ int result = RUN_ALL_TESTS();
+ rclcpp::shutdown();
+ return result;
+}
diff --git a/joint_trajectory_controller_plugins/test/test_pid_trajectory.hpp b/joint_trajectory_controller_plugins/test/test_pid_trajectory.hpp
new file mode 100644
index 0000000000..ebe2b95a98
--- /dev/null
+++ b/joint_trajectory_controller_plugins/test/test_pid_trajectory.hpp
@@ -0,0 +1,72 @@
+// Copyright 2023 AIT Austrian Institute of Technology
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef TEST_PID_TRAJECTORY_HPP_
+#define TEST_PID_TRAJECTORY_HPP_
+
+#include <memory>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include "gmock/gmock.h"
+
+#include "joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "trajectory_msgs/msg/joint_trajectory.hpp"
+
+namespace
+{
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_pid_trajectory");
+} // namespace
+
+class TestableJointTrajectoryControllerPlugin
+: public joint_trajectory_controller_plugins::PidTrajectoryPlugin
+{
+public:
+ // updates mapped parameters, i.e., should be called after setting the joint names
+ void trigger_declare_parameters() { param_listener_->declare_params(); }
+};
+
+class PidTrajectoryTest : public ::testing::Test
+{
+public:
+ void SetUp() override
+ {
+ auto testname = ::testing::UnitTest::GetInstance()->current_test_info()->name();
+ node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(testname);
+ executor_->add_node(node_->get_node_base_interface());
+ executor_thread_ = std::thread([this]() { executor_->spin(); });
+ }
+
+ PidTrajectoryTest() { executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(); }
+
+ void TearDown() override
+ {
+ executor_->cancel();
+ if (executor_thread_.joinable())
+ {
+ executor_thread_.join();
+ }
+ node_.reset();
+ }
+
+protected:
+ rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
+ rclcpp::Executor::SharedPtr executor_;
+ std::thread executor_thread_;
+};
+
+#endif // TEST_PID_TRAJECTORY_HPP_
From 6a0921a42b7cd787fdcd32706f74ba3f8270d947 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Thu, 7 Mar 2024 12:01:46 +0000
Subject: [PATCH 15/36] Fix PID controller for less cmd_ifs than dofs
---
.../src/joint_trajectory_controller.cpp | 2 +-
.../test/test_trajectory_controller.cpp | 2 --
.../test/test_trajectory_controller_utils.hpp | 6 +++---
.../pid_trajectory_plugin.hpp | 6 +++++-
.../trajectory_controller_base.hpp | 6 +++++-
.../src/pid_trajectory_plugin.cpp | 19 +++++++++++++++----
.../test/test_pid_trajectory.cpp | 9 ++++++---
7 files changed, 35 insertions(+), 15 deletions(-)
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
index 860e8d23af..6240ef6d57 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp
+++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -720,7 +720,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
params_.controller_plugin.c_str(), ex.what());
return CallbackReturn::FAILURE;
}
- if (traj_contr_->initialize(get_node()) == false) {
+ if (traj_contr_->initialize(get_node(), map_cmd_to_joints_) == false) {
RCLCPP_FATAL(
logger,
"The trajectory controller plugin `%s` failed to initialize for some reason. Aborting.",
diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp
index 0383a5cb84..2068dae6dd 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller.cpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp
@@ -1078,7 +1078,6 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le
EXPECT_LT(0.0, joint_vel_[1]);
EXPECT_TRUE(std::isnan(current_command.velocities[2]));
- // use_external_control_law_
if (traj_controller_->use_external_control_law()) {
// we expect u = k_p * (s_d-s)
EXPECT_NEAR(
@@ -1169,7 +1168,6 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le
EXPECT_LT(0.0, joint_vel_[1]);
EXPECT_TRUE(std::isnan(current_command.velocities[2]));
- // use_closed_loop_pid_adapter_
if (traj_controller_->use_external_control_law()) {
// we expect u = k_p * (s_d-s)
EXPECT_NEAR(
diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
index 955b2fcced..f6a055ecee 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
@@ -253,15 +253,15 @@ class TrajectoryControllerTest : public ::testing::Test
/**
* @brief set PIDs for every entry in joint_names_
- * be aware to update if PIDs should be configured for different command_joints than joint_names
*/
void SetPidParameters(double p_value = 0.0, double ff_value = 1.0)
{
traj_controller_->trigger_declare_parameters();
auto node = traj_controller_->get_node();
- for (size_t i = 0; i < joint_names_.size(); ++i)
- {
+ // if command_joints were not set manually, it was done in the on_configure() method
+ auto command_joint_names = node->get_parameter("command_joints").as_string_array();
+ for (size_t i = 0; i < command_joint_names.size(); ++i) {
const std::string prefix = "gains." + joint_names_[i];
const rclcpp::Parameter k_p(prefix + ".p", p_value);
const rclcpp::Parameter k_i(prefix + ".i", 0.0);
diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp
index 3718f04847..8d66837c71 100644
--- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp
+++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp
@@ -33,7 +33,9 @@ namespace joint_trajectory_controller_plugins
class PidTrajectoryPlugin : public TrajectoryControllerBase
{
public:
- bool initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) override;
+ bool initialize(
+ rclcpp_lifecycle::LifecycleNode::SharedPtr node,
+ std::vector<size_t> map_cmd_to_joints) override;
bool configure() override;
@@ -76,6 +78,8 @@ class PidTrajectoryPlugin : public TrajectoryControllerBase
// number of command joints
size_t num_cmd_joints_;
+ // map from joints in the message to command joints
+ std::vector<size_t> map_cmd_to_joints_;
// PID controllers
std::vector<PidPtr> pids_;
// Feed-forward velocity weight factor when calculating closed loop pid adapter's command
diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp
index 9814ddfeb6..b062c54aa9 100644
--- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp
+++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp
@@ -42,9 +42,13 @@ class TrajectoryControllerBase
/**
* @brief initialize the controller plugin.
* declare parameters
+ * @param node the node handle to use for parameter handling
+ * @param map_cmd_to_joints a mapping from the joint names in the trajectory messages to the
+ * command joints
*/
JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
- virtual bool initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) = 0;
+ virtual bool initialize(
+ rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::vector<size_t> map_cmd_to_joints) = 0;
/**
* @brief configure the controller plugin.
diff --git a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp
index 7ca9b8b4a9..ded0572fbe 100644
--- a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp
+++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp
@@ -17,9 +17,11 @@
namespace joint_trajectory_controller_plugins
{
-bool PidTrajectoryPlugin::initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node)
+bool PidTrajectoryPlugin::initialize(
+ rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::vector<size_t> map_cmd_to_joints)
{
node_ = node;
+ map_cmd_to_joints_ = map_cmd_to_joints;
try
{
@@ -54,6 +56,13 @@ bool PidTrajectoryPlugin::configure()
RCLCPP_ERROR(node_->get_logger(), "[PidTrajectoryPlugin] No command joints specified.");
return false;
}
+ if (num_cmd_joints_ != map_cmd_to_joints_.size())
+ {
+ RCLCPP_ERROR(
+ node_->get_logger(),
+ "[PidTrajectoryPlugin] map_cmd_to_joints has to be of size num_cmd_joints.");
+ return false;
+ }
pids_.resize(num_cmd_joints_);
ff_velocity_scale_.resize(num_cmd_joints_);
@@ -111,9 +120,11 @@ void PidTrajectoryPlugin::computeCommands(
// Update PIDs
for (auto i = 0ul; i < num_cmd_joints_; ++i)
{
- tmp_command[i] = (desired.velocities[i] * ff_velocity_scale_[i]) +
- pids_[i]->computeCommand(
- error.positions[i], error.velocities[i], (uint64_t)period.nanoseconds());
+ tmp_command[map_cmd_to_joints_[i]] =
+ (desired.velocities[map_cmd_to_joints_[i]] * ff_velocity_scale_[i]) +
+ pids_[i]->computeCommand(
+ error.positions[map_cmd_to_joints_[i]], error.velocities[map_cmd_to_joints_[i]],
+ (uint64_t)period.nanoseconds());
}
}
diff --git a/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp b/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp
index ef8227a332..bb10b62a06 100644
--- a/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp
+++ b/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp
@@ -19,7 +19,8 @@ TEST_F(PidTrajectoryTest, TestEmptySetup)
std::shared_ptr<TestableJointTrajectoryControllerPlugin> traj_contr =
std::make_shared<TestableJointTrajectoryControllerPlugin>();
- ASSERT_FALSE(traj_contr->initialize(node_));
+ std::vector<size_t> map_cmd_to_joints{};
+ ASSERT_FALSE(traj_contr->initialize(node_, map_cmd_to_joints));
}
TEST_F(PidTrajectoryTest, TestSingleJoint)
@@ -33,7 +34,8 @@ TEST_F(PidTrajectoryTest, TestSingleJoint)
// override parameter
node_->declare_parameter("command_joints", joint_names_paramv);
- ASSERT_TRUE(traj_contr->initialize(node_));
+ std::vector<size_t> map_cmd_to_joints{0};
+ ASSERT_TRUE(traj_contr->initialize(node_, map_cmd_to_joints));
node_->set_parameter(rclcpp::Parameter("gains.joint1.p", 1.0));
ASSERT_TRUE(traj_contr->configure());
ASSERT_TRUE(traj_contr->activate());
@@ -63,7 +65,8 @@ TEST_F(PidTrajectoryTest, TestMultipleJoints)
// override parameter
node_->declare_parameter("command_joints", joint_names_paramv);
- ASSERT_TRUE(traj_contr->initialize(node_));
+ std::vector<size_t> map_cmd_to_joints{0, 1, 2};
+ ASSERT_TRUE(traj_contr->initialize(node_, map_cmd_to_joints));
// set dynamic parameters
node_->set_parameter(rclcpp::Parameter("gains.joint1.p", 1.0));
node_->set_parameter(rclcpp::Parameter("gains.joint2.p", 2.0));
From 20dbdfd09459a88eda4094d64dacac8425937b4e Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Thu, 7 Mar 2024 12:02:59 +0000
Subject: [PATCH 16/36] Parse traj_control gains before sending hold position
---
.../src/joint_trajectory_controller.cpp | 18 ++++++------------
1 file changed, 6 insertions(+), 12 deletions(-)
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
index 6240ef6d57..9a709a970f 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp
+++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -995,6 +995,12 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
read_state_from_state_interfaces(last_commanded_state_);
}
+ // activate traj_contr_, e.g., update gains
+ if (traj_contr_ && traj_contr_->activate() == false) {
+ RCLCPP_ERROR(get_node()->get_logger(), "Error during trajectory controller activation.");
+ return CallbackReturn::ERROR;
+ }
+
// The controller should start by holding position at the beginning of active state
add_new_trajectory_msg_nonRT(set_hold_position());
rt_is_holding_.writeFromNonRT(true);
@@ -1021,18 +1027,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
cmd_timeout_ = 0.0;
}
- // activate traj_contr_, e.g., update gains
- if (traj_contr_ && traj_contr_->activate() == false) {
- RCLCPP_ERROR(get_node()->get_logger(), "Error during trajectory controller activation.");
- return CallbackReturn::ERROR;
- }
-
- // activate traj_contr_, e.g., update gains
- if (traj_contr_ && traj_contr_->activate() == false) {
- RCLCPP_ERROR(get_node()->get_logger(), "Error during trajectory controller activation.");
- return CallbackReturn::ERROR;
- }
-
return CallbackReturn::SUCCESS;
}
From a0d113a1c3099ade561b3b006f0659d4eaf7ecc2 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Thu, 7 Mar 2024 12:06:20 +0000
Subject: [PATCH 17/36] add_pre_set_parameters_callback
---
.../src/joint_trajectory_controller.cpp | 29 +++++++++++++++++++
1 file changed, 29 insertions(+)
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
index 9a709a970f..c072a742bf 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp
+++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -648,6 +648,35 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
command_joint_names_ = params_.joints;
RCLCPP_INFO(
logger, "No specific joint names are used for command interfaces. Using 'joints' parameter.");
+
+ // set the parameter for the controller plugin
+ auto result =
+ get_node()->set_parameter(rclcpp::Parameter("command_joints", command_joint_names_));
+ if (result.successful == false) {
+ RCLCPP_ERROR(logger, "Failed to set 'command_joints' parameter");
+ return CallbackReturn::FAILURE;
+ }
+#if RCLCPP_VERSION_MAJOR >= 17
+ // TODO(christophfroehlich) how to lock the parameter (set read_only to false)?
+ // Setting it to read_only but override is not supported
+ // https://github.com/ros2/rclcpp/issues/1762 get_node()->undeclare_parameter("command_joints");
+ // rcl_interfaces::msg::ParameterDescriptor parameter_descriptor;
+ // parameter_descriptor.read_only = true;
+ // get_node()->declare_parameter("command_joints",
+ // rclcpp::ParameterValue(command_joint_names_), parameter_descriptor);
+ lock_cmd_joint_names = get_node()->add_pre_set_parameters_callback(
+ [this](std::vector<rclcpp::Parameter> & parameters)
+ {
+ for (auto & parameter : parameters) {
+ if (parameter.get_name() == "command_joints") {
+ RCLCPP_ERROR(
+ get_node()->get_logger(),
+ "The parameter 'command_joints' is read-only. You can't change it.");
+ parameter = rclcpp::Parameter("command_joints", command_joint_names_);
+ }
+ }
+ });
+#endif
}
num_cmd_joints_ = command_joint_names_.size();
From 8578b890ebc0315d1958644067f6f0ea231a2eb0 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Wed, 14 Feb 2024 11:42:57 +0000
Subject: [PATCH 18/36] Fix wrong text in test
---
.../test/test_load_pid_trajectory.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/joint_trajectory_controller_plugins/test/test_load_pid_trajectory.cpp b/joint_trajectory_controller_plugins/test/test_load_pid_trajectory.cpp
index 09a08d6cee..c53c35c64a 100644
--- a/joint_trajectory_controller_plugins/test/test_load_pid_trajectory.cpp
+++ b/joint_trajectory_controller_plugins/test/test_load_pid_trajectory.cpp
@@ -35,7 +35,7 @@ TEST(TestLoadPidController, load_controller)
auto available_classes = traj_controller_loader.getDeclaredClasses();
- std::cout << "available filters:" << std::endl;
+ std::cout << "available plugins:" << std::endl;
for (const auto & available_class : available_classes)
{
std::cout << " " << available_class << std::endl;
From 38e43ca0ce9c1dd542809a07dba3baca9695532a Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Fri, 24 May 2024 20:17:12 +0000
Subject: [PATCH 19/36] Properly initialize and update PIDs
---
.../trajectory_controller_base.hpp | 2 ++
.../src/pid_trajectory_plugin.cpp | 16 ++++++++++------
2 files changed, 12 insertions(+), 6 deletions(-)
diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp
index b062c54aa9..28e81aa559 100644
--- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp
+++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp
@@ -143,6 +143,8 @@ class TrajectoryControllerBase
/**
* @brief reset internal states
+ *
+ * is called in on_deactivate() of JTC
*/
JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
virtual void reset() = 0;
diff --git a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp
index ded0572fbe..1a8982fd9c 100644
--- a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp
+++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp
@@ -63,18 +63,23 @@ bool PidTrajectoryPlugin::configure()
"[PidTrajectoryPlugin] map_cmd_to_joints has to be of size num_cmd_joints.");
return false;
}
- pids_.resize(num_cmd_joints_);
- ff_velocity_scale_.resize(num_cmd_joints_);
+ pids_.resize(num_cmd_joints_); // memory for the shared pointers, will be nullptr
+ // create the objects with default values
+ for (size_t i = 0; i < num_cmd_joints_; ++i)
+ {
+ pids_[i] = std::make_shared<control_toolbox::Pid>();
+ }
+ ff_velocity_scale_.resize(num_cmd_joints_, 0.0);
return true;
-};
+}
bool PidTrajectoryPlugin::activate()
{
params_ = param_listener_->get_params();
parseGains();
return true;
-};
+}
bool PidTrajectoryPlugin::updateGainsRT()
{
@@ -96,8 +101,7 @@ void PidTrajectoryPlugin::parseGains()
params_.command_joints[i].c_str());
const auto & gains = params_.gains.command_joints_map.at(params_.command_joints[i]);
- pids_[i] = std::make_shared<control_toolbox::Pid>(
- gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp);
+ pids_[i]->setGains(gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp, true);
ff_velocity_scale_[i] = gains.ff_velocity_scale;
RCLCPP_DEBUG(node_->get_logger(), "[PidTrajectoryPlugin] gains.p: %f", gains.p);
From 8aadf613f22450db7d70ddab749c5acee758ab35 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Mon, 24 Jun 2024 21:37:31 +0000
Subject: [PATCH 20/36] Reset traj_contrl in on_deactivate
---
.../src/joint_trajectory_controller.cpp | 604 ++++++------------
1 file changed, 213 insertions(+), 391 deletions(-)
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
index c072a742bf..885d03c25d 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp
+++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -53,31 +53,22 @@ JointTrajectoryController::JointTrajectoryController()
controller_interface::CallbackReturn JointTrajectoryController::on_init()
{
- if (!urdf_.empty())
- {
- if (!model_.initString(urdf_))
- {
+ if (!urdf_.empty()) {
+ if (!model_.initString(urdf_)) {
RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse URDF file");
- }
- else
- {
+ } else {
RCLCPP_DEBUG(get_node()->get_logger(), "Successfully parsed URDF file");
}
- }
- else
- {
+ } else {
// empty URDF is used for some tests
RCLCPP_DEBUG(get_node()->get_logger(), "No URDF file given");
}
- try
- {
+ try {
// Create the parameter listener and get the parameters
param_listener_ = std::make_shared<ParamListener>(get_node());
params_ = param_listener_->get_params();
- }
- catch (const std::exception & e)
- {
+ } catch (const std::exception & e) {
fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
return CallbackReturn::ERROR;
}
@@ -91,10 +82,8 @@ JointTrajectoryController::command_interface_configuration() const
controller_interface::InterfaceConfiguration conf;
conf.type = controller_interface::interface_configuration_type::INDIVIDUAL;
conf.names.reserve(num_cmd_joints_ * params_.command_interfaces.size());
- for (const auto & joint_name : command_joint_names_)
- {
- for (const auto & interface_type : params_.command_interfaces)
- {
+ for (const auto & joint_name : command_joint_names_) {
+ for (const auto & interface_type : params_.command_interfaces) {
conf.names.push_back(joint_name + "/" + interface_type);
}
}
@@ -107,10 +96,8 @@ JointTrajectoryController::state_interface_configuration() const
controller_interface::InterfaceConfiguration conf;
conf.type = controller_interface::interface_configuration_type::INDIVIDUAL;
conf.names.reserve(dof_ * params_.state_interfaces.size());
- for (const auto & joint_name : params_.joints)
- {
- for (const auto & interface_type : params_.state_interfaces)
- {
+ for (const auto & joint_name : params_.joints) {
+ for (const auto & interface_type : params_.state_interfaces) {
conf.names.push_back(joint_name + "/" + interface_type);
}
}
@@ -120,13 +107,11 @@ JointTrajectoryController::state_interface_configuration() const
controller_interface::return_type JointTrajectoryController::update(
const rclcpp::Time & time, const rclcpp::Duration & period)
{
- if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
- {
+ if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {
return controller_interface::return_type::OK;
}
// update dynamic parameters
- if (param_listener_->is_old(params_))
- {
+ if (param_listener_->is_old(params_)) {
params_ = param_listener_->get_params();
default_tolerances_ = get_segment_tolerances(params_);
// update gains of controller
@@ -163,20 +148,15 @@ controller_interface::return_type JointTrajectoryController::update(
read_state_from_state_interfaces(state_current_);
// currently carrying out a trajectory
- if (has_active_trajectory())
- {
+ if (has_active_trajectory()) {
bool first_sample = false;
// if sampling the first time, set the point before you sample
- if (!traj_external_point_ptr_->is_sampled_already())
- {
+ if (!traj_external_point_ptr_->is_sampled_already()) {
first_sample = true;
- if (params_.open_loop_control)
- {
+ if (params_.open_loop_control) {
traj_external_point_ptr_->set_point_before_trajectory_msg(
time, last_commanded_state_, joints_angle_wraparound_);
- }
- else
- {
+ } else {
traj_external_point_ptr_->set_point_before_trajectory_msg(
time, state_current_, joints_angle_wraparound_);
}
@@ -191,8 +171,7 @@ controller_interface::return_type JointTrajectoryController::update(
const bool valid_point = traj_external_point_ptr_->sample(
time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr);
- if (valid_point)
- {
+ if (valid_point) {
const rclcpp::Time traj_start = traj_external_point_ptr_->time_from_start();
// this is the time instance
// - started with the first segment: when the first point will be reached (in the future)
@@ -219,8 +198,7 @@ controller_interface::return_type JointTrajectoryController::update(
}
// Check state/goal tolerance
- for (size_t index = 0; index < dof_; ++index)
- {
+ for (size_t index = 0; index < dof_; ++index) {
compute_error_for_joint(state_error_, index, state_current_, state_desired_);
// Always check the state tolerance on the first sample in case the first sample
@@ -243,10 +221,8 @@ controller_interface::return_type JointTrajectoryController::update(
{
outside_goal_tolerance = true;
- if (default_tolerances_.goal_time_tolerance != 0.0)
- {
- if (time_difference > default_tolerances_.goal_time_tolerance)
- {
+ if (default_tolerances_.goal_time_tolerance != 0.0) {
+ if (time_difference > default_tolerances_.goal_time_tolerance) {
within_goal_time = false;
// print once, goal will be aborted afterwards
check_state_tolerance_per_joint(
@@ -266,25 +242,20 @@ controller_interface::return_type JointTrajectoryController::update(
}
// set values for next hardware write()
- if (has_position_command_interface_)
- {
+ if (has_position_command_interface_) {
assign_interface_from_point(joint_command_interface_[0], state_desired_.positions);
}
if (has_velocity_command_interface_) {
if (use_external_control_law_) {
assign_interface_from_point(joint_command_interface_[1], tmp_command_);
- }
- else
- {
+ } else {
assign_interface_from_point(joint_command_interface_[1], state_desired_.velocities);
}
}
- if (has_acceleration_command_interface_)
- {
+ if (has_acceleration_command_interface_) {
assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations);
}
- if (has_effort_command_interface_)
- {
+ if (has_effort_command_interface_) {
assign_interface_from_point(joint_command_interface_[3], tmp_command_);
}
@@ -292,8 +263,7 @@ controller_interface::return_type JointTrajectoryController::update(
last_commanded_state_ = state_desired_;
}
- if (active_goal)
- {
+ if (active_goal) {
// send feedback
auto feedback = std::make_shared<FollowJTrajAction::Feedback>();
feedback->header.stamp = time;
@@ -305,8 +275,7 @@ controller_interface::return_type JointTrajectoryController::update(
active_goal->setFeedback(feedback);
// check abort
- if (tolerance_violated_while_moving)
- {
+ if (tolerance_violated_while_moving) {
auto result = std::make_shared<FollowJTrajAction::Result>();
result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED);
result->set__error_string("Aborted due to path tolerance violation");
@@ -321,10 +290,8 @@ controller_interface::return_type JointTrajectoryController::update(
add_new_trajectory_msg_RT(set_hold_position());
}
// check goal tolerance
- else if (!before_last_point)
- {
- if (!outside_goal_tolerance)
- {
+ else if (!before_last_point) {
+ if (!outside_goal_tolerance) {
auto result = std::make_shared<FollowJTrajAction::Result>();
result->set__error_code(FollowJTrajAction::Result::SUCCESSFUL);
result->set__error_string("Goal successfully reached!");
@@ -339,7 +306,7 @@ controller_interface::return_type JointTrajectoryController::update(
add_new_trajectory_msg_RT(set_success_trajectory_point());
} else if (!within_goal_time) {
const std::string error_string = "Aborted due to goal_time_tolerance exceeding by " +
- std::to_string(time_difference) + " seconds";
+ std::to_string(time_difference) + " seconds";
auto result = std::make_shared<FollowJTrajAction::Result>();
result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED);
@@ -355,9 +322,7 @@ controller_interface::return_type JointTrajectoryController::update(
add_new_trajectory_msg_RT(set_hold_position());
}
}
- }
- else if (tolerance_violated_while_moving && *(rt_has_pending_goal_.readFromRT()) == false)
- {
+ } else if (tolerance_violated_while_moving && *(rt_has_pending_goal_.readFromRT()) == false) {
// we need to ensure that there is no pending goal -> we get a race condition otherwise
RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation");
@@ -383,33 +348,26 @@ void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectory
{
auto assign_point_from_interface =
[&](std::vector<double> & trajectory_point_interface, const auto & joint_interface)
- {
- for (size_t index = 0; index < dof_; ++index)
{
- trajectory_point_interface[index] = joint_interface[index].get().get_value();
- }
- };
+ for (size_t index = 0; index < dof_; ++index) {
+ trajectory_point_interface[index] = joint_interface[index].get().get_value();
+ }
+ };
// Assign values from the hardware
// Position states always exist
assign_point_from_interface(state.positions, joint_state_interface_[0]);
// velocity and acceleration states are optional
- if (has_velocity_state_interface_)
- {
+ if (has_velocity_state_interface_) {
assign_point_from_interface(state.velocities, joint_state_interface_[1]);
// Acceleration is used only in combination with velocity
- if (has_acceleration_state_interface_)
- {
+ if (has_acceleration_state_interface_) {
assign_point_from_interface(state.accelerations, joint_state_interface_[2]);
- }
- else
- {
+ } else {
// Make empty so the property is ignored during interpolation
state.accelerations.clear();
}
- }
- else
- {
+ } else {
// Make empty so the property is ignored during interpolation
state.velocities.clear();
state.accelerations.clear();
@@ -422,65 +380,49 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto
auto assign_point_from_interface =
[&](std::vector<double> & trajectory_point_interface, const auto & joint_interface)
- {
- for (size_t index = 0; index < num_cmd_joints_; ++index)
{
- trajectory_point_interface[map_cmd_to_joints_[index]] =
- joint_interface[index].get().get_value();
- }
- };
+ for (size_t index = 0; index < num_cmd_joints_; ++index) {
+ trajectory_point_interface[map_cmd_to_joints_[index]] =
+ joint_interface[index].get().get_value();
+ }
+ };
auto interface_has_values = [](const auto & joint_interface)
- {
- return std::find_if(
- joint_interface.begin(), joint_interface.end(),
- [](const auto & interface)
- { return std::isnan(interface.get().get_value()); }) == joint_interface.end();
- };
+ {
+ return std::find_if(
+ joint_interface.begin(), joint_interface.end(),
+ [](const auto & interface)
+ {return std::isnan(interface.get().get_value());}) == joint_interface.end();
+ };
// Assign values from the command interfaces as state. Therefore needs check for both.
// Position state interface has to exist always
- if (has_position_command_interface_ && interface_has_values(joint_command_interface_[0]))
- {
+ if (has_position_command_interface_ && interface_has_values(joint_command_interface_[0])) {
assign_point_from_interface(state.positions, joint_command_interface_[0]);
- }
- else
- {
+ } else {
state.positions.clear();
has_values = false;
}
// velocity and acceleration states are optional
- if (has_velocity_state_interface_)
- {
- if (has_velocity_command_interface_ && interface_has_values(joint_command_interface_[1]))
- {
+ if (has_velocity_state_interface_) {
+ if (has_velocity_command_interface_ && interface_has_values(joint_command_interface_[1])) {
assign_point_from_interface(state.velocities, joint_command_interface_[1]);
- }
- else
- {
+ } else {
state.velocities.clear();
has_values = false;
}
- }
- else
- {
+ } else {
state.velocities.clear();
}
// Acceleration is used only in combination with velocity
- if (has_acceleration_state_interface_)
- {
- if (has_acceleration_command_interface_ && interface_has_values(joint_command_interface_[2]))
- {
+ if (has_acceleration_state_interface_) {
+ if (has_acceleration_command_interface_ && interface_has_values(joint_command_interface_[2])) {
assign_point_from_interface(state.accelerations, joint_command_interface_[2]);
- }
- else
- {
+ } else {
state.accelerations.clear();
has_values = false;
}
- }
- else
- {
+ } else {
state.accelerations.clear();
}
@@ -494,67 +436,50 @@ bool JointTrajectoryController::read_commands_from_command_interfaces(
auto assign_point_from_interface =
[&](std::vector<double> & trajectory_point_interface, const auto & joint_interface)
- {
- for (size_t index = 0; index < num_cmd_joints_; ++index)
{
- trajectory_point_interface[map_cmd_to_joints_[index]] =
- joint_interface[index].get().get_value();
- }
- };
+ for (size_t index = 0; index < num_cmd_joints_; ++index) {
+ trajectory_point_interface[map_cmd_to_joints_[index]] =
+ joint_interface[index].get().get_value();
+ }
+ };
auto interface_has_values = [](const auto & joint_interface)
- {
- return std::find_if(
- joint_interface.begin(), joint_interface.end(),
- [](const auto & interface)
- { return std::isnan(interface.get().get_value()); }) == joint_interface.end();
- };
+ {
+ return std::find_if(
+ joint_interface.begin(), joint_interface.end(),
+ [](const auto & interface)
+ {return std::isnan(interface.get().get_value());}) == joint_interface.end();
+ };
// Assign values from the command interfaces as command.
- if (has_position_command_interface_)
- {
- if (interface_has_values(joint_command_interface_[0]))
- {
+ if (has_position_command_interface_) {
+ if (interface_has_values(joint_command_interface_[0])) {
assign_point_from_interface(commands.positions, joint_command_interface_[0]);
- }
- else
- {
+ } else {
commands.positions.clear();
has_values = false;
}
}
- if (has_velocity_command_interface_)
- {
- if (interface_has_values(joint_command_interface_[1]))
- {
+ if (has_velocity_command_interface_) {
+ if (interface_has_values(joint_command_interface_[1])) {
assign_point_from_interface(commands.velocities, joint_command_interface_[1]);
- }
- else
- {
+ } else {
commands.velocities.clear();
has_values = false;
}
}
- if (has_acceleration_command_interface_)
- {
- if (interface_has_values(joint_command_interface_[2]))
- {
+ if (has_acceleration_command_interface_) {
+ if (interface_has_values(joint_command_interface_[2])) {
assign_point_from_interface(commands.accelerations, joint_command_interface_[2]);
- }
- else
- {
+ } else {
commands.accelerations.clear();
has_values = false;
}
}
- if (has_effort_command_interface_)
- {
- if (interface_has_values(joint_command_interface_[3]))
- {
+ if (has_effort_command_interface_) {
+ if (interface_has_values(joint_command_interface_[3])) {
assign_point_from_interface(commands.effort, joint_command_interface_[3]);
- }
- else
- {
+ } else {
commands.effort.clear();
has_values = false;
}
@@ -569,8 +494,7 @@ void JointTrajectoryController::query_state_service(
{
const auto logger = get_node()->get_logger();
// Preconditions
- if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
- {
+ if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
RCLCPP_ERROR(logger, "Can't sample trajectory. Controller is not active.");
response->success = false;
return;
@@ -578,29 +502,22 @@ void JointTrajectoryController::query_state_service(
const auto active_goal = *rt_active_goal_.readFromRT();
response->name = params_.joints;
trajectory_msgs::msg::JointTrajectoryPoint state_requested = state_current_;
- if (has_active_trajectory())
- {
+ if (has_active_trajectory()) {
TrajectoryPointConstIter start_segment_itr, end_segment_itr;
response->success = traj_external_point_ptr_->sample(
static_cast<rclcpp::Time>(request->time), interpolation_method_, state_requested,
start_segment_itr, end_segment_itr);
// If the requested sample time precedes the trajectory finish time respond as failure
- if (response->success)
- {
- if (end_segment_itr == traj_external_point_ptr_->end())
- {
+ if (response->success) {
+ if (end_segment_itr == traj_external_point_ptr_->end()) {
RCLCPP_ERROR(logger, "Requested sample time precedes the current trajectory end time.");
response->success = false;
}
- }
- else
- {
+ } else {
RCLCPP_ERROR(
logger, "Requested sample time is earlier than the current trajectory start time.");
}
- }
- else
- {
+ } else {
RCLCPP_ERROR(logger, "Currently there is no valid trajectory instance.");
response->success = false;
}
@@ -614,8 +531,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
{
const auto logger = get_node()->get_logger();
- if (!param_listener_)
- {
+ if (!param_listener_) {
RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init");
return controller_interface::CallbackReturn::ERROR;
}
@@ -630,21 +546,18 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
dof_ = params_.joints.size();
// TODO(destogl): why is this here? Add comment or move
- if (!reset())
- {
+ if (!reset()) {
return CallbackReturn::FAILURE;
}
- if (params_.joints.empty())
- {
+ if (params_.joints.empty()) {
RCLCPP_WARN(logger, "'joints' parameter is empty.");
return CallbackReturn::FAILURE;
}
command_joint_names_ = params_.command_joints;
- if (command_joint_names_.empty())
- {
+ if (command_joint_names_.empty()) {
command_joint_names_ = params_.joints;
RCLCPP_INFO(
logger, "No specific joint names are used for command interfaces. Using 'joints' parameter.");
@@ -680,41 +593,33 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
}
num_cmd_joints_ = command_joint_names_.size();
- if (num_cmd_joints_ > dof_)
- {
+ if (num_cmd_joints_ > dof_) {
RCLCPP_ERROR(
logger, "'command_joints' parameter must not have greater size as 'joints' parameter.");
return CallbackReturn::FAILURE;
- }
- else if (num_cmd_joints_ < dof_)
- {
+ } else if (num_cmd_joints_ < dof_) {
// create a map for the command joints
map_cmd_to_joints_ = mapping(command_joint_names_, params_.joints);
- if (map_cmd_to_joints_.size() != num_cmd_joints_)
- {
+ if (map_cmd_to_joints_.size() != num_cmd_joints_) {
RCLCPP_ERROR(
logger,
"'command_joints' parameter must be a subset of 'joints' parameter, if their size is not "
"equal.");
return CallbackReturn::FAILURE;
}
- for (size_t i = 0; i < command_joint_names_.size(); i++)
- {
+ for (size_t i = 0; i < command_joint_names_.size(); i++) {
RCLCPP_DEBUG(
logger, "Command joint %lu: '%s' maps to joint %lu: '%s'.", i,
command_joint_names_[i].c_str(), map_cmd_to_joints_[i],
params_.joints.at(map_cmd_to_joints_[i]).c_str());
}
- }
- else
- {
+ } else {
// create a map for the command joints, trivial if the size is the same
map_cmd_to_joints_.resize(num_cmd_joints_);
std::iota(map_cmd_to_joints_.begin(), map_cmd_to_joints_.end(), 0);
}
- if (params_.command_interfaces.empty())
- {
+ if (params_.command_interfaces.empty()) {
RCLCPP_ERROR(logger, "'command_interfaces' parameter is empty.");
return CallbackReturn::FAILURE;
}
@@ -736,7 +641,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
// then use external control law
use_external_control_law_ =
(has_velocity_command_interface_ && params_.command_interfaces.size() == 1 &&
- !params_.open_loop_control) ||
+ !params_.open_loop_control) ||
has_effort_command_interface_;
if (use_external_control_law_) {
@@ -774,11 +679,9 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
// Configure joint position error normalization (angle_wraparound)
joints_angle_wraparound_.resize(dof_);
- for (size_t i = 0; i < dof_; ++i)
- {
+ for (size_t i = 0; i < dof_; ++i) {
const auto & gains = params_.gains.joints_map.at(params_.joints[i]);
- if (gains.angle_wraparound)
- {
+ if (gains.angle_wraparound) {
// TODO(christophfroehlich): remove this warning in a future release (ROS-J)
RCLCPP_WARN(
logger,
@@ -787,11 +690,9 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
joints_angle_wraparound_[i] = true;
}
- if (!urdf_.empty())
- {
+ if (!urdf_.empty()) {
auto urdf_joint = model_.getJoint(params_.joints[i]);
- if (urdf_joint && urdf_joint->type == urdf::Joint::CONTINUOUS)
- {
+ if (urdf_joint && urdf_joint->type == urdf::Joint::CONTINUOUS) {
RCLCPP_DEBUG(
logger, "joint '%s' is of type continuous, use angle_wraparound.",
params_.joints[i].c_str());
@@ -801,8 +702,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
}
}
- if (params_.state_interfaces.empty())
- {
+ if (params_.state_interfaces.empty()) {
RCLCPP_ERROR(logger, "'state_interfaces' parameter is empty.");
return CallbackReturn::FAILURE;
}
@@ -846,18 +746,16 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
}
auto get_interface_list = [](const std::vector<std::string> & interface_types)
- {
- std::stringstream ss_interfaces;
- for (size_t index = 0; index < interface_types.size(); ++index)
{
- if (index != 0)
- {
- ss_interfaces << " ";
+ std::stringstream ss_interfaces;
+ for (size_t index = 0; index < interface_types.size(); ++index) {
+ if (index != 0) {
+ ss_interfaces << " ";
+ }
+ ss_interfaces << interface_types[index];
}
- ss_interfaces << interface_types[index];
- }
- return ss_interfaces.str();
- };
+ return ss_interfaces.str();
+ };
// Print output so users can be sure the interface setup is correct
RCLCPP_INFO(
@@ -879,8 +777,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
// create subscriber and publishers
joint_command_subscriber_ =
get_node()->create_subscription<trajectory_msgs::msg::JointTrajectory>(
- "~/joint_trajectory", rclcpp::SystemDefaultsQoS(),
- std::bind(&JointTrajectoryController::topic_callback, this, std::placeholders::_1));
+ "~/joint_trajectory", rclcpp::SystemDefaultsQoS(),
+ std::bind(&JointTrajectoryController::topic_callback, this, std::placeholders::_1));
publisher_ = get_node()->create_publisher<ControllerStateMsg>(
"~/controller_state", rclcpp::SystemDefaultsQoS());
@@ -893,38 +791,31 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
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_)
- {
+ if (has_velocity_state_interface_) {
state_publisher_->msg_.feedback.velocities.resize(dof_);
state_publisher_->msg_.error.velocities.resize(dof_);
}
- if (has_acceleration_state_interface_)
- {
+ if (has_acceleration_state_interface_) {
state_publisher_->msg_.feedback.accelerations.resize(dof_);
state_publisher_->msg_.error.accelerations.resize(dof_);
}
- if (has_position_command_interface_)
- {
+ if (has_position_command_interface_) {
state_publisher_->msg_.output.positions.resize(dof_);
}
- if (has_velocity_command_interface_)
- {
+ if (has_velocity_command_interface_) {
state_publisher_->msg_.output.velocities.resize(dof_);
}
- if (has_acceleration_command_interface_)
- {
+ if (has_acceleration_command_interface_) {
state_publisher_->msg_.output.accelerations.resize(dof_);
}
- if (has_effort_command_interface_)
- {
+ if (has_effort_command_interface_) {
state_publisher_->msg_.output.effort.resize(dof_);
}
state_publisher_->unlock();
// action server configuration
- if (params_.allow_partial_joints_goal)
- {
+ if (params_.allow_partial_joints_goal) {
RCLCPP_INFO(logger, "Goals with partial set of joints are allowed");
}
@@ -969,13 +860,12 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
default_tolerances_ = get_segment_tolerances(params_);
// order all joints in the storage
- for (const auto & interface : params_.command_interfaces)
- {
+ for (const auto & interface : params_.command_interfaces) {
auto it =
std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface);
auto index = std::distance(allowed_interface_types_.begin(), it);
if (!controller_interface::get_ordered_interfaces(
- command_interfaces_, command_joint_names_, interface, joint_command_interface_[index]))
+ command_interfaces_, command_joint_names_, interface, joint_command_interface_[index]))
{
RCLCPP_ERROR(
get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", num_cmd_joints_,
@@ -983,13 +873,12 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
return CallbackReturn::ERROR;
}
}
- for (const auto & interface : params_.state_interfaces)
- {
+ for (const auto & interface : params_.state_interfaces) {
auto it =
std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface);
auto index = std::distance(allowed_interface_types_.begin(), it);
if (!controller_interface::get_ordered_interfaces(
- state_interfaces_, params_.joints, interface, joint_state_interface_[index]))
+ state_interfaces_, params_.joints, interface, joint_state_interface_[index]))
{
RCLCPP_ERROR(
get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", dof_,
@@ -1012,13 +901,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
// otherwise it leaves the entries of joints without command interface NaN.
// if no open_loop control, state_current_ is then used for `set_point_before_trajectory_msg` and
// future trajectory sampling will always give NaN for these joints
- if (dof_ == num_cmd_joints_ && read_state_from_command_interfaces(state))
- {
+ if (dof_ == num_cmd_joints_ && read_state_from_command_interfaces(state)) {
state_current_ = state;
last_commanded_state_ = state;
- }
- else
- {
+ } else {
// Initialize current state storage from hardware
read_state_from_state_interfaces(state_current_);
read_state_from_state_interfaces(last_commanded_state_);
@@ -1035,14 +921,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
rt_is_holding_.writeFromNonRT(true);
// parse timeout parameter
- if (params_.cmd_timeout > 0.0)
- {
- if (params_.cmd_timeout > default_tolerances_.goal_time_tolerance)
- {
+ if (params_.cmd_timeout > 0.0) {
+ if (params_.cmd_timeout > default_tolerances_.goal_time_tolerance) {
cmd_timeout_ = params_.cmd_timeout;
- }
- else
- {
+ } else {
// deactivate timeout
RCLCPP_WARN(
get_node()->get_logger(),
@@ -1050,9 +932,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
default_tolerances_.goal_time_tolerance);
cmd_timeout_ = 0.0;
}
- }
- else
- {
+ } else {
cmd_timeout_ = 0.0;
}
@@ -1078,25 +958,21 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate(
joint_command_interface_[0][index].get().get_value());
}
- if (has_velocity_command_interface_)
- {
+ if (has_velocity_command_interface_) {
joint_command_interface_[1][index].get().set_value(0.0);
}
- if (has_acceleration_command_interface_)
- {
+ if (has_acceleration_command_interface_) {
joint_command_interface_[2][index].get().set_value(0.0);
}
// TODO(anyone): How to halt when using effort commands?
- if (has_effort_command_interface_)
- {
+ if (has_effort_command_interface_) {
joint_command_interface_[3][index].get().set_value(0.0);
}
}
- for (size_t index = 0; index < allowed_interface_types_.size(); ++index)
- {
+ for (size_t index = 0; index < allowed_interface_types_.size(); ++index) {
joint_command_interface_[index].clear();
joint_state_interface_[index].clear();
}
@@ -1106,6 +982,11 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate(
traj_external_point_ptr_.reset();
+ // e.g., reset integral states
+ if (traj_contr_) {
+ traj_contr_->reset();
+ }
+
return CallbackReturn::SUCCESS;
}
@@ -1118,8 +999,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_cleanup(
controller_interface::CallbackReturn JointTrajectoryController::on_error(
const rclcpp_lifecycle::State &)
{
- if (!reset())
- {
+ if (!reset()) {
return CallbackReturn::ERROR;
}
return CallbackReturn::SUCCESS;
@@ -1130,10 +1010,6 @@ bool JointTrajectoryController::reset()
subscriber_is_active_ = false;
joint_command_subscriber_.reset();
- if (traj_contr_) {
- traj_contr_->reset();
- }
-
traj_external_point_ptr_.reset();
return true;
@@ -1151,26 +1027,22 @@ void JointTrajectoryController::publish_state(
const rclcpp::Time & time, const JointTrajectoryPoint & desired_state,
const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error)
{
- if (state_publisher_->trylock())
- {
+ if (state_publisher_->trylock()) {
state_publisher_->msg_.header.stamp = time;
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_)
- {
+ 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_)
- {
+ if (has_acceleration_state_interface_) {
state_publisher_->msg_.feedback.accelerations = current_state.accelerations;
state_publisher_->msg_.error.accelerations = state_error.accelerations;
}
- if (read_commands_from_command_interfaces(command_current_))
- {
+ if (read_commands_from_command_interfaces(command_current_)) {
state_publisher_->msg_.output = command_current_;
}
@@ -1181,8 +1053,7 @@ void JointTrajectoryController::publish_state(
void JointTrajectoryController::topic_callback(
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> msg)
{
- if (!validate_trajectory_msg(*msg))
- {
+ if (!validate_trajectory_msg(*msg)) {
return;
}
// http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement
@@ -1191,7 +1062,7 @@ void JointTrajectoryController::topic_callback(
add_new_trajectory_msg_nonRT(msg);
rt_is_holding_.writeFromNonRT(false);
}
-};
+}
rclcpp_action::GoalResponse JointTrajectoryController::goal_received_callback(
const rclcpp_action::GoalUUID &, std::shared_ptr<const FollowJTrajAction::Goal> goal)
@@ -1199,15 +1070,13 @@ rclcpp_action::GoalResponse JointTrajectoryController::goal_received_callback(
RCLCPP_INFO(get_node()->get_logger(), "Received new action goal");
// Precondition: Running controller
- if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
- {
+ if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {
RCLCPP_ERROR(
get_node()->get_logger(), "Can't accept new action goals. Controller is not running.");
return rclcpp_action::GoalResponse::REJECT;
}
- if (!validate_trajectory_msg(goal->trajectory))
- {
+ if (!validate_trajectory_msg(goal->trajectory)) {
return rclcpp_action::GoalResponse::REJECT;
}
@@ -1222,8 +1091,7 @@ rclcpp_action::CancelResponse JointTrajectoryController::goal_cancelled_callback
// Check that cancel request refers to currently active goal (if any)
const auto active_goal = *rt_active_goal_.readFromNonRT();
- if (active_goal && active_goal->gh_ == goal_handle)
- {
+ if (active_goal && active_goal->gh_ == goal_handle) {
RCLCPP_INFO(
get_node()->get_logger(), "Canceling active action goal because cancel callback received.");
@@ -1275,15 +1143,12 @@ void JointTrajectoryController::compute_error_for_joint(
const JointTrajectoryPoint & desired) const
{
// error defined as the difference between current and desired
- if (joints_angle_wraparound_[index])
- {
+ if (joints_angle_wraparound_[index]) {
// if desired, the shortest_angular_distance is calculated, i.e., the error is
// normalized between -pi<error<pi
error.positions[index] =
angles::shortest_angular_distance(current.positions[index], desired.positions[index]);
- }
- else
- {
+ } else {
error.positions[index] = desired.positions[index] - current.positions[index];
}
if (
@@ -1292,8 +1157,7 @@ void JointTrajectoryController::compute_error_for_joint(
{
error.velocities[index] = desired.velocities[index] - current.velocities[index];
}
- if (has_acceleration_state_interface_ && has_acceleration_command_interface_)
- {
+ if (has_acceleration_state_interface_ && has_acceleration_command_interface_) {
error.accelerations[index] = desired.accelerations[index] - current.accelerations[index];
}
}
@@ -1303,15 +1167,13 @@ void JointTrajectoryController::fill_partial_goal(
{
// joint names in the goal are a subset of existing joints, as checked in goal_callback
// so if the size matches, the goal contains all controller joints
- if (dof_ == trajectory_msg->joint_names.size())
- {
+ if (dof_ == trajectory_msg->joint_names.size()) {
return;
}
trajectory_msg->joint_names.reserve(dof_);
- for (size_t index = 0; index < dof_; ++index)
- {
+ for (size_t index = 0; index < dof_; ++index) {
{
if (
std::find(
@@ -1323,34 +1185,27 @@ void JointTrajectoryController::fill_partial_goal(
}
trajectory_msg->joint_names.push_back(params_.joints[index]);
- for (auto & it : trajectory_msg->points)
- {
+ for (auto & it : trajectory_msg->points) {
// Assume hold position with 0 velocity and acceleration for missing joints
- if (!it.positions.empty())
- {
+ if (!it.positions.empty()) {
if (
has_position_command_interface_ &&
!std::isnan(joint_command_interface_[0][index].get().get_value()))
{
// copy last command if cmd interface exists
it.positions.push_back(joint_command_interface_[0][index].get().get_value());
- }
- else if (has_position_state_interface_)
- {
+ } else if (has_position_state_interface_) {
// copy current state if state interface exists
it.positions.push_back(joint_state_interface_[0][index].get().get_value());
}
}
- if (!it.velocities.empty())
- {
+ if (!it.velocities.empty()) {
it.velocities.push_back(0.0);
}
- if (!it.accelerations.empty())
- {
+ if (!it.accelerations.empty()) {
it.accelerations.push_back(0.0);
}
- if (!it.effort.empty())
- {
+ if (!it.effort.empty()) {
it.effort.push_back(0.0);
}
}
@@ -1364,35 +1219,30 @@ void JointTrajectoryController::sort_to_local_joint_order(
// rearrange all points in the trajectory message based on mapping
std::vector<size_t> mapping_vector = mapping(trajectory_msg->joint_names, params_.joints);
auto remap = [this](
- const std::vector<double> & to_remap,
- const std::vector<size_t> & mapping) -> std::vector<double>
- {
- if (to_remap.empty())
+ const std::vector<double> & to_remap,
+ const std::vector<size_t> & mapping) -> std::vector<double>
{
- return to_remap;
- }
- if (to_remap.size() != mapping.size())
- {
- RCLCPP_WARN(
- get_node()->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size());
- return to_remap;
- }
- static std::vector<double> output(dof_, 0.0);
- // Only resize if necessary since it's an expensive operation
- if (output.size() != mapping.size())
- {
- output.resize(mapping.size(), 0.0);
- }
- for (size_t index = 0; index < mapping.size(); ++index)
- {
- auto map_index = mapping[index];
- output[map_index] = to_remap[index];
- }
- return output;
- };
+ if (to_remap.empty()) {
+ return to_remap;
+ }
+ if (to_remap.size() != mapping.size()) {
+ RCLCPP_WARN(
+ get_node()->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size());
+ return to_remap;
+ }
+ static std::vector<double> output(dof_, 0.0);
+ // Only resize if necessary since it's an expensive operation
+ if (output.size() != mapping.size()) {
+ output.resize(mapping.size(), 0.0);
+ }
+ for (size_t index = 0; index < mapping.size(); ++index) {
+ auto map_index = mapping[index];
+ output[map_index] = to_remap[index];
+ }
+ return output;
+ };
- for (size_t index = 0; index < trajectory_msg->points.size(); ++index)
- {
+ for (size_t index = 0; index < trajectory_msg->points.size(); ++index) {
trajectory_msg->points[index].positions =
remap(trajectory_msg->points[index].positions, mapping_vector);
@@ -1411,12 +1261,10 @@ bool JointTrajectoryController::validate_trajectory_point_field(
size_t joint_names_size, const std::vector<double> & vector_field,
const std::string & string_for_vector_field, size_t i, bool allow_empty) const
{
- if (allow_empty && vector_field.empty())
- {
+ if (allow_empty && vector_field.empty()) {
return true;
}
- if (joint_names_size != vector_field.size())
- {
+ if (joint_names_size != vector_field.size()) {
RCLCPP_ERROR(
get_node()->get_logger(),
"Mismatch between joint_names size (%zu) and %s (%zu) at point #%zu.", joint_names_size,
@@ -1430,10 +1278,8 @@ bool JointTrajectoryController::validate_trajectory_msg(
const trajectory_msgs::msg::JointTrajectory & trajectory) const
{
// If partial joints goals are not allowed, goal should specify all controller joints
- if (!params_.allow_partial_joints_goal)
- {
- if (trajectory.joint_names.size() != dof_)
- {
+ if (!params_.allow_partial_joints_goal) {
+ if (trajectory.joint_names.size() != dof_) {
RCLCPP_ERROR(
get_node()->get_logger(),
"Joints on incoming trajectory don't match the controller joints.");
@@ -1441,8 +1287,7 @@ bool JointTrajectoryController::validate_trajectory_msg(
}
}
- if (trajectory.joint_names.empty())
- {
+ if (trajectory.joint_names.empty()) {
RCLCPP_ERROR(get_node()->get_logger(), "Empty joint names on incoming trajectory.");
return false;
}
@@ -1451,15 +1296,12 @@ bool JointTrajectoryController::validate_trajectory_msg(
// If the starting time it set to 0.0, it means the controller should start it now.
// Otherwise we check if the trajectory ends before the current time,
// in which case it can be ignored.
- if (trajectory_start_time.seconds() != 0.0)
- {
+ if (trajectory_start_time.seconds() != 0.0) {
auto trajectory_end_time = trajectory_start_time;
- for (const auto & p : trajectory.points)
- {
+ for (const auto & p : trajectory.points) {
trajectory_end_time += p.time_from_start;
}
- if (trajectory_end_time < get_node()->now())
- {
+ if (trajectory_end_time < get_node()->now()) {
RCLCPP_ERROR(
get_node()->get_logger(),
"Received trajectory with non-zero start time (%f) that ends in the past (%f)",
@@ -1468,13 +1310,11 @@ bool JointTrajectoryController::validate_trajectory_msg(
}
}
- for (size_t i = 0; i < trajectory.joint_names.size(); ++i)
- {
+ for (size_t i = 0; i < trajectory.joint_names.size(); ++i) {
const std::string & incoming_joint_name = trajectory.joint_names[i];
auto it = std::find(params_.joints.begin(), params_.joints.end(), incoming_joint_name);
- if (it == params_.joints.end())
- {
+ if (it == params_.joints.end()) {
RCLCPP_ERROR(
get_node()->get_logger(), "Incoming joint %s doesn't match the controller's joints.",
incoming_joint_name.c_str());
@@ -1482,18 +1322,14 @@ bool JointTrajectoryController::validate_trajectory_msg(
}
}
- if (trajectory.points.empty())
- {
+ if (trajectory.points.empty()) {
RCLCPP_ERROR(get_node()->get_logger(), "Empty trajectory received.");
return false;
}
- if (!params_.allow_nonzero_velocity_at_trajectory_end)
- {
- for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i)
- {
- if (fabs(trajectory.points.back().velocities.at(i)) > std::numeric_limits<float>::epsilon())
- {
+ if (!params_.allow_nonzero_velocity_at_trajectory_end) {
+ for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i) {
+ if (fabs(trajectory.points.back().velocities.at(i)) > std::numeric_limits<float>::epsilon()) {
RCLCPP_ERROR(
get_node()->get_logger(),
"Velocity of last trajectory point of joint %s is not zero: %.15f",
@@ -1504,10 +1340,8 @@ bool JointTrajectoryController::validate_trajectory_msg(
}
rclcpp::Duration previous_traj_time(0ms);
- for (size_t i = 0; i < trajectory.points.size(); ++i)
- {
- if ((i > 0) && (rclcpp::Duration(trajectory.points[i].time_from_start) <= previous_traj_time))
- {
+ for (size_t i = 0; i < trajectory.points.size(); ++i) {
+ if ((i > 0) && (rclcpp::Duration(trajectory.points[i].time_from_start) <= previous_traj_time)) {
RCLCPP_ERROR(
get_node()->get_logger(),
"Time between points %zu and %zu is not strictly increasing, it is %f and %f respectively",
@@ -1520,10 +1354,9 @@ bool JointTrajectoryController::validate_trajectory_msg(
const size_t joint_count = trajectory.joint_names.size();
const auto & points = trajectory.points;
// This currently supports only position, velocity and acceleration inputs
- if (params_.allow_integration_in_goal_trajectories)
- {
+ if (params_.allow_integration_in_goal_trajectories) {
const bool all_empty = points[i].positions.empty() && points[i].velocities.empty() &&
- points[i].accelerations.empty();
+ points[i].accelerations.empty();
const bool position_error =
!points[i].positions.empty() &&
!validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false);
@@ -1533,13 +1366,11 @@ bool JointTrajectoryController::validate_trajectory_msg(
const bool acceleration_error =
!points[i].accelerations.empty() &&
!validate_trajectory_point_field(
- joint_count, points[i].accelerations, "accelerations", i, false);
- if (all_empty || position_error || velocity_error || acceleration_error)
- {
+ joint_count, points[i].accelerations, "accelerations", i, false);
+ if (all_empty || position_error || velocity_error || acceleration_error) {
return false;
}
- }
- else if (
+ } else if (
!validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false) ||
!validate_trajectory_point_field(joint_count, points[i].velocities, "velocities", i, true) ||
!validate_trajectory_point_field(
@@ -1548,8 +1379,7 @@ bool JointTrajectoryController::validate_trajectory_msg(
return false;
}
// reject effort entries
- if (!points[i].effort.empty())
- {
+ if (!points[i].effort.empty()) {
RCLCPP_ERROR(
get_node()->get_logger(), "Trajectories with effort fields are currently not supported.");
return false;
@@ -1639,12 +1469,10 @@ void JointTrajectoryController::resize_joint_trajectory_point(
trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double value)
{
point.positions.resize(size, value);
- if (has_velocity_state_interface_)
- {
+ if (has_velocity_state_interface_) {
point.velocities.resize(size, value);
}
- if (has_acceleration_state_interface_)
- {
+ if (has_acceleration_state_interface_) {
point.accelerations.resize(size, value);
}
}
@@ -1652,20 +1480,16 @@ void JointTrajectoryController::resize_joint_trajectory_point(
void JointTrajectoryController::resize_joint_trajectory_point_command(
trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double value)
{
- if (has_position_command_interface_)
- {
+ if (has_position_command_interface_) {
point.positions.resize(size, value);
}
- if (has_velocity_command_interface_)
- {
+ if (has_velocity_command_interface_) {
point.velocities.resize(size, value);
}
- if (has_acceleration_command_interface_)
- {
+ if (has_acceleration_command_interface_) {
point.accelerations.resize(size, value);
}
- if (has_effort_command_interface_)
- {
+ if (has_effort_command_interface_) {
point.effort.resize(size, value);
}
}
@@ -1685,13 +1509,11 @@ void JointTrajectoryController::init_hold_position_msg()
hold_position_msg_ptr_->points[0].velocities.clear();
hold_position_msg_ptr_->points[0].accelerations.clear();
hold_position_msg_ptr_->points[0].effort.clear();
- if (has_velocity_command_interface_ || has_acceleration_command_interface_)
- {
+ if (has_velocity_command_interface_ || has_acceleration_command_interface_) {
// add velocity, so that trajectory sampling returns velocity points in any case
hold_position_msg_ptr_->points[0].velocities.resize(dof_, 0.0);
}
- if (has_acceleration_command_interface_)
- {
+ if (has_acceleration_command_interface_) {
// add velocity, so that trajectory sampling returns acceleration points in any case
hold_position_msg_ptr_->points[0].accelerations.resize(dof_, 0.0);
}
From 3f48f9b3fee3ab9f9003e30570b196bd5a34add3 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Tue, 16 Jul 2024 11:38:23 +0000
Subject: [PATCH 21/36] Add missing backward_ros dependency
---
joint_trajectory_controller_plugins/package.xml | 5 +++--
1 file changed, 3 insertions(+), 2 deletions(-)
diff --git a/joint_trajectory_controller_plugins/package.xml b/joint_trajectory_controller_plugins/package.xml
index b0812ed8f8..aaeb07abf9 100644
--- a/joint_trajectory_controller_plugins/package.xml
+++ b/joint_trajectory_controller_plugins/package.xml
@@ -9,10 +9,11 @@
<buildtool_depend>ament_cmake_ros</buildtool_depend>
+ <depend>backward_ros</depend>
<depend>control_toolbox</depend>
+ <depend>generate_parameter_library</depend>
<depend>pluginlib</depend>
<depend>trajectory_msgs</depend>
- <depend>generate_parameter_library</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
@@ -20,4 +21,4 @@
<export>
<build_type>ament_cmake</build_type>
</export>
-</package>
+</package>
\ No newline at end of file
From cc34e67c89e0d43a68a0668607a12df38090a3c0 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Mon, 29 Jul 2024 10:07:51 +0000
Subject: [PATCH 22/36] Fix the test code
---
joint_trajectory_controller/test/test_trajectory_controller.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp
index 9767fb3709..c201a9ee2d 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller.cpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp
@@ -401,7 +401,7 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency_command_jo
std::vector<std::string> command_joint_names{joint_names_[0], joint_names_[1]};
const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names);
SetUpAndActivateTrajectoryController(executor, {command_joint_names_param});
- subscribeToState();
+ subscribeToState(executor);
updateController();
// Spin to receive latest state
From 10eb5e605cd530db531222716e49441026817a1d Mon Sep 17 00:00:00 2001
From: Manuel Muth <manuel.muth@stoglrobotics.de>
Date: Mon, 26 Aug 2024 14:51:49 +0200
Subject: [PATCH 23/36] rename get/set_state to get/set_lifecylce_state (#1250)
---
diff_drive_controller/src/diff_drive_controller.cpp | 2 +-
.../src/joint_trajectory_controller.cpp | 6 +++---
tricycle_controller/src/tricycle_controller.cpp | 2 +-
3 files changed, 5 insertions(+), 5 deletions(-)
diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp
index a64f210503..66da6d6738 100644
--- a/diff_drive_controller/src/diff_drive_controller.cpp
+++ b/diff_drive_controller/src/diff_drive_controller.cpp
@@ -101,7 +101,7 @@ controller_interface::return_type DiffDriveController::update(
const rclcpp::Time & time, const rclcpp::Duration & period)
{
auto logger = get_node()->get_logger();
- if (get_state().id() == State::PRIMARY_STATE_INACTIVE)
+ if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE)
{
if (!is_halted)
{
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
index 1cdfeacccd..b217142435 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp
+++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -136,7 +136,7 @@ JointTrajectoryController::state_interface_configuration() const
controller_interface::return_type JointTrajectoryController::update(
const rclcpp::Time & time, const rclcpp::Duration & period)
{
- if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
+ if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
{
return controller_interface::return_type::OK;
}
@@ -594,7 +594,7 @@ void JointTrajectoryController::query_state_service(
{
const auto logger = get_node()->get_logger();
// Preconditions
- if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+ if (get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
RCLCPP_ERROR(logger, "Can't sample trajectory. Controller is not active.");
response->success = false;
@@ -1218,7 +1218,7 @@ rclcpp_action::GoalResponse JointTrajectoryController::goal_received_callback(
RCLCPP_INFO(get_node()->get_logger(), "Received new action goal");
// Precondition: Running controller
- if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
+ if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
{
RCLCPP_ERROR(
get_node()->get_logger(), "Can't accept new action goals. Controller is not running.");
diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp
index 94ff63e659..ec7ca7bd5e 100644
--- a/tricycle_controller/src/tricycle_controller.cpp
+++ b/tricycle_controller/src/tricycle_controller.cpp
@@ -86,7 +86,7 @@ InterfaceConfiguration TricycleController::state_interface_configuration() const
controller_interface::return_type TricycleController::update(
const rclcpp::Time & time, const rclcpp::Duration & period)
{
- if (get_state().id() == State::PRIMARY_STATE_INACTIVE)
+ if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE)
{
if (!is_halted)
{
From 55f43ac8991c252427991cb03a9265187aca3cc5 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Wed, 5 Feb 2025 11:39:16 +0000
Subject: [PATCH 24/36] Use global cmake macros
---
ackermann_steering_controller/CMakeLists.txt | 12 ++---
admittance_controller/CMakeLists.txt | 12 ++---
bicycle_steering_controller/CMakeLists.txt | 12 ++---
diff_drive_controller/CMakeLists.txt | 12 ++---
effort_controllers/CMakeLists.txt | 12 ++---
.../CMakeLists.txt | 12 ++---
forward_command_controller/CMakeLists.txt | 12 ++---
gpio_controllers/CMakeLists.txt | 12 ++---
gripper_controllers/CMakeLists.txt | 12 ++---
imu_sensor_broadcaster/CMakeLists.txt | 12 ++---
joint_state_broadcaster/CMakeLists.txt | 12 ++---
joint_trajectory_controller/CMakeLists.txt | 12 ++---
mecanum_drive_controller/CMakeLists.txt | 12 ++---
parallel_gripper_controller/CMakeLists.txt | 8 ++--
pid_controller/CMakeLists.txt | 12 ++---
pose_broadcaster/CMakeLists.txt | 12 ++---
position_controllers/CMakeLists.txt | 12 ++---
range_sensor_broadcaster/CMakeLists.txt | 12 ++---
ros2_controllers.cmake | 44 +++++++++++++++++++
steering_controllers_library/CMakeLists.txt | 12 ++---
tricycle_controller/CMakeLists.txt | 12 ++---
tricycle_steering_controller/CMakeLists.txt | 12 ++---
velocity_controllers/CMakeLists.txt | 12 ++---
23 files changed, 110 insertions(+), 194 deletions(-)
create mode 100644 ros2_controllers.cmake
diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt
index 2f160fd4c5..8f1887ba23 100644
--- a/ackermann_steering_controller/CMakeLists.txt
+++ b/ackermann_steering_controller/CMakeLists.txt
@@ -1,15 +1,9 @@
cmake_minimum_required(VERSION 3.16)
project(ackermann_steering_controller)
-if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
- add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable
- -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct
- -Werror=missing-braces)
-endif()
-
-# using this instead of visibility macros
-# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053
-set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+include(../ros2_controllers.cmake)
+set_compiler_options()
+export_windows_symbols()
# find dependencies
set(THIS_PACKAGE_INCLUDE_DEPENDS
diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt
index a068239c17..d57d2e1665 100644
--- a/admittance_controller/CMakeLists.txt
+++ b/admittance_controller/CMakeLists.txt
@@ -1,15 +1,9 @@
cmake_minimum_required(VERSION 3.16)
project(admittance_controller)
-if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
- add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable
- -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct
- -Werror=missing-braces)
-endif()
-
-# using this instead of visibility macros
-# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053
-set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+include(../ros2_controllers.cmake)
+set_compiler_options()
+export_windows_symbols()
set(THIS_PACKAGE_INCLUDE_DEPENDS
angles
diff --git a/bicycle_steering_controller/CMakeLists.txt b/bicycle_steering_controller/CMakeLists.txt
index 2e88e9e70a..24e4925d22 100644
--- a/bicycle_steering_controller/CMakeLists.txt
+++ b/bicycle_steering_controller/CMakeLists.txt
@@ -1,15 +1,9 @@
cmake_minimum_required(VERSION 3.16)
project(bicycle_steering_controller)
-if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
- add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable
- -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct
- -Werror=missing-braces)
-endif()
-
-# using this instead of visibility macros
-# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053
-set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+include(../ros2_controllers.cmake)
+set_compiler_options()
+export_windows_symbols()
# find dependencies
set(THIS_PACKAGE_INCLUDE_DEPENDS
diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt
index c5f4ade8a6..036d782dda 100644
--- a/diff_drive_controller/CMakeLists.txt
+++ b/diff_drive_controller/CMakeLists.txt
@@ -1,15 +1,9 @@
cmake_minimum_required(VERSION 3.16)
project(diff_drive_controller)
-if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
- add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable
- -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct
- -Werror=missing-braces)
-endif()
-
-# using this instead of visibility macros
-# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053
-set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+include(../ros2_controllers.cmake)
+set_compiler_options()
+export_windows_symbols()
set(THIS_PACKAGE_INCLUDE_DEPENDS
control_toolbox
diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt
index 60cce95e28..7c5f1b027f 100644
--- a/effort_controllers/CMakeLists.txt
+++ b/effort_controllers/CMakeLists.txt
@@ -1,15 +1,9 @@
cmake_minimum_required(VERSION 3.16)
project(effort_controllers)
-if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
- add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable
- -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct
- -Werror=missing-braces)
-endif()
-
-# using this instead of visibility macros
-# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053
-set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+include(../ros2_controllers.cmake)
+set_compiler_options()
+export_windows_symbols()
set(THIS_PACKAGE_INCLUDE_DEPENDS
forward_command_controller
diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt
index be54c3bd58..05a1309715 100644
--- a/force_torque_sensor_broadcaster/CMakeLists.txt
+++ b/force_torque_sensor_broadcaster/CMakeLists.txt
@@ -1,15 +1,9 @@
cmake_minimum_required(VERSION 3.16)
project(force_torque_sensor_broadcaster)
-if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
- add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable
- -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct
- -Werror=missing-braces)
-endif()
-
-# using this instead of visibility macros
-# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053
-set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+include(../ros2_controllers.cmake)
+set_compiler_options()
+export_windows_symbols()
set(THIS_PACKAGE_INCLUDE_DEPENDS
controller_interface
diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt
index f7610c5f85..0c684e3dbf 100644
--- a/forward_command_controller/CMakeLists.txt
+++ b/forward_command_controller/CMakeLists.txt
@@ -1,15 +1,9 @@
cmake_minimum_required(VERSION 3.16)
project(forward_command_controller)
-if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
- add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable
- -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct
- -Werror=missing-braces)
-endif()
-
-# using this instead of visibility macros
-# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053
-set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+include(../ros2_controllers.cmake)
+set_compiler_options()
+export_windows_symbols()
set(THIS_PACKAGE_INCLUDE_DEPENDS
controller_interface
diff --git a/gpio_controllers/CMakeLists.txt b/gpio_controllers/CMakeLists.txt
index 62274b0cf9..da25a954f0 100644
--- a/gpio_controllers/CMakeLists.txt
+++ b/gpio_controllers/CMakeLists.txt
@@ -1,15 +1,9 @@
cmake_minimum_required(VERSION 3.8)
project(gpio_controllers)
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
- add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable
- -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct
- -Werror=missing-braces)
-endif()
-
-# using this instead of visibility macros
-# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053
-set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+include(../ros2_controllers.cmake)
+set_compiler_options()
+export_windows_symbols()
# find dependencies
find_package(ament_cmake REQUIRED)
diff --git a/gripper_controllers/CMakeLists.txt b/gripper_controllers/CMakeLists.txt
index 77c9d9183b..86df58a55a 100644
--- a/gripper_controllers/CMakeLists.txt
+++ b/gripper_controllers/CMakeLists.txt
@@ -1,15 +1,9 @@
cmake_minimum_required(VERSION 3.16)
project(gripper_controllers)
-# using this instead of visibility macros
-# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053
-set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
-
-if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
- add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable
- -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct
- -Werror=missing-braces)
-endif()
+include(../ros2_controllers.cmake)
+set_compiler_options()
+export_windows_symbols()
set(THIS_PACKAGE_INCLUDE_DEPENDS
control_msgs
diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt
index 7b12c9095e..892f6d1dd1 100644
--- a/imu_sensor_broadcaster/CMakeLists.txt
+++ b/imu_sensor_broadcaster/CMakeLists.txt
@@ -1,15 +1,9 @@
cmake_minimum_required(VERSION 3.16)
project(imu_sensor_broadcaster)
-if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
- add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable
- -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct
- -Werror=missing-braces)
-endif()
-
-# using this instead of visibility macros
-# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053
-set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+include(../ros2_controllers.cmake)
+set_compiler_options()
+export_windows_symbols()
set(THIS_PACKAGE_INCLUDE_DEPENDS
controller_interface
diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt
index adbd5254e1..07be3e425e 100644
--- a/joint_state_broadcaster/CMakeLists.txt
+++ b/joint_state_broadcaster/CMakeLists.txt
@@ -1,15 +1,9 @@
cmake_minimum_required(VERSION 3.16)
project(joint_state_broadcaster)
-if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
- add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable
- -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct
- -Werror=missing-braces)
-endif()
-
-# using this instead of visibility macros
-# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053
-set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+include(../ros2_controllers.cmake)
+set_compiler_options()
+export_windows_symbols()
set(THIS_PACKAGE_INCLUDE_DEPENDS
builtin_interfaces
diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt
index 8d4ae30695..0b5c0eee47 100644
--- a/joint_trajectory_controller/CMakeLists.txt
+++ b/joint_trajectory_controller/CMakeLists.txt
@@ -1,15 +1,9 @@
cmake_minimum_required(VERSION 3.16)
project(joint_trajectory_controller)
-if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
- add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable
- -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct
- -Werror=missing-braces)
-endif()
-
-# using this instead of visibility macros
-# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053
-set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+include(../ros2_controllers.cmake)
+set_compiler_options()
+export_windows_symbols()
set(THIS_PACKAGE_INCLUDE_DEPENDS
angles
diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt
index 095e910dee..386eb9aeec 100644
--- a/mecanum_drive_controller/CMakeLists.txt
+++ b/mecanum_drive_controller/CMakeLists.txt
@@ -1,15 +1,9 @@
cmake_minimum_required(VERSION 3.16)
project(mecanum_drive_controller)
-if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
- add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable
- -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct
- -Werror=missing-braces)
-endif()
-
-# using this instead of visibility macros
-# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053
-set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+include(../ros2_controllers.cmake)
+set_compiler_options()
+export_windows_symbols()
# find dependencies
set(THIS_PACKAGE_INCLUDE_DEPENDS
diff --git a/parallel_gripper_controller/CMakeLists.txt b/parallel_gripper_controller/CMakeLists.txt
index 8c1018895d..09ce9c6ac1 100644
--- a/parallel_gripper_controller/CMakeLists.txt
+++ b/parallel_gripper_controller/CMakeLists.txt
@@ -1,11 +1,9 @@
cmake_minimum_required(VERSION 3.16)
project(parallel_gripper_controller)
-if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
- add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable
- -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct
- -Werror=missing-braces)
-endif()
+include(../ros2_controllers.cmake)
+set_compiler_options()
+export_windows_symbols()
set(THIS_PACKAGE_INCLUDE_DEPENDS
control_msgs
diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt
index d1374d54cb..89e357d0c9 100644
--- a/pid_controller/CMakeLists.txt
+++ b/pid_controller/CMakeLists.txt
@@ -1,15 +1,9 @@
cmake_minimum_required(VERSION 3.16)
project(pid_controller)
-if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
- add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable
- -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct
- -Werror=missing-braces)
-endif()
-
-# using this instead of visibility macros
-# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053
-set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+include(../ros2_controllers.cmake)
+set_compiler_options()
+export_windows_symbols()
if(WIN32)
add_compile_definitions(
diff --git a/pose_broadcaster/CMakeLists.txt b/pose_broadcaster/CMakeLists.txt
index 2375779e33..08fc52d311 100644
--- a/pose_broadcaster/CMakeLists.txt
+++ b/pose_broadcaster/CMakeLists.txt
@@ -1,15 +1,9 @@
cmake_minimum_required(VERSION 3.16)
project(pose_broadcaster)
-if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
- add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable
- -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct
- -Werror=missing-braces)
-endif()
-
-# using this instead of visibility macros
-# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053
-set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+include(../ros2_controllers.cmake)
+set_compiler_options()
+export_windows_symbols()
set(THIS_PACKAGE_INCLUDE_DEPENDS
controller_interface
diff --git a/position_controllers/CMakeLists.txt b/position_controllers/CMakeLists.txt
index 71ffd0eeba..0103419ba4 100644
--- a/position_controllers/CMakeLists.txt
+++ b/position_controllers/CMakeLists.txt
@@ -1,15 +1,9 @@
cmake_minimum_required(VERSION 3.16)
project(position_controllers)
-if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
- add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable
- -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct
- -Werror=missing-braces)
-endif()
-
-# using this instead of visibility macros
-# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053
-set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+include(../ros2_controllers.cmake)
+set_compiler_options()
+export_windows_symbols()
set(THIS_PACKAGE_INCLUDE_DEPENDS
forward_command_controller
diff --git a/range_sensor_broadcaster/CMakeLists.txt b/range_sensor_broadcaster/CMakeLists.txt
index 744611bb65..f2d3aec13c 100644
--- a/range_sensor_broadcaster/CMakeLists.txt
+++ b/range_sensor_broadcaster/CMakeLists.txt
@@ -7,15 +7,9 @@ if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
endif()
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
- add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable
- -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct
- -Werror=missing-braces)
-endif()
-
-# using this instead of visibility macros
-# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053
-set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+include(../ros2_controllers.cmake)
+set_compiler_options()
+export_windows_symbols()
set(THIS_PACKAGE_INCLUDE_DEPENDS
controller_interface
diff --git a/ros2_controllers.cmake b/ros2_controllers.cmake
new file mode 100644
index 0000000000..eceacdf4d7
--- /dev/null
+++ b/ros2_controllers.cmake
@@ -0,0 +1,44 @@
+# Copyright 2025 AIT - Austrian Institute of Technology GmbH
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+# set compiler options depending on detected compiler
+macro(set_compiler_options)
+ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+ add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable
+ -Werror=return-type -Werror=shadow -Werror=format
+ -Werror=missing-braces)
+ message(STATUS "Compiler warnings enabled for ${CMAKE_CXX_COMPILER_ID}")
+
+ # Extract major version if g++ is used
+ if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+
+ string(REPLACE "." ";" VERSION_LIST ${CMAKE_CXX_COMPILER_VERSION})
+ list(GET VERSION_LIST 0 GCC_MAJOR_VERSION)
+ list(GET VERSION_LIST 1 GCC_MINOR_VERSION)
+
+ message(STATUS "Detected GCC Version: ${CMAKE_CXX_COMPILER_VERSION} (Major: ${GCC_MAJOR_VERSION}, Minor: ${GCC_MINOR_VERSION})")
+
+ if (GCC_MAJOR_VERSION GREATER 10)
+ # GCC 11 introduced -Werror=range-loop-construct
+ add_compile_options(-Werror=range-loop-construct)
+ endif()
+ endif()
+ endif()
+endmacro()
+
+# using this instead of visibility macros
+# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053
+macro(export_windows_symbols)
+ set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+endmacro()
diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt
index 2e80ed198f..700df94f5c 100644
--- a/steering_controllers_library/CMakeLists.txt
+++ b/steering_controllers_library/CMakeLists.txt
@@ -1,15 +1,9 @@
cmake_minimum_required(VERSION 3.16)
project(steering_controllers_library)
-if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
- add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable
- -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct
- -Werror=missing-braces)
-endif()
-
-# using this instead of visibility macros
-# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053
-set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+include(../ros2_controllers.cmake)
+set_compiler_options()
+export_windows_symbols()
# find dependencies
set(THIS_PACKAGE_INCLUDE_DEPENDS
diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt
index b5669530dc..f03cc628f0 100644
--- a/tricycle_controller/CMakeLists.txt
+++ b/tricycle_controller/CMakeLists.txt
@@ -1,15 +1,9 @@
cmake_minimum_required(VERSION 3.16)
project(tricycle_controller)
-if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
- add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable
- -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct
- -Werror=missing-braces)
-endif()
-
-# using this instead of visibility macros
-# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053
-set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+include(../ros2_controllers.cmake)
+set_compiler_options()
+export_windows_symbols()
set(THIS_PACKAGE_INCLUDE_DEPENDS
ackermann_msgs
diff --git a/tricycle_steering_controller/CMakeLists.txt b/tricycle_steering_controller/CMakeLists.txt
index 9dbf5e3543..0f55961474 100644
--- a/tricycle_steering_controller/CMakeLists.txt
+++ b/tricycle_steering_controller/CMakeLists.txt
@@ -1,15 +1,9 @@
cmake_minimum_required(VERSION 3.16)
project(tricycle_steering_controller)
-if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
- add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable
- -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct
- -Werror=missing-braces)
-endif()
-
-# using this instead of visibility macros
-# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053
-set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+include(../ros2_controllers.cmake)
+set_compiler_options()
+export_windows_symbols()
# find dependencies
set(THIS_PACKAGE_INCLUDE_DEPENDS
diff --git a/velocity_controllers/CMakeLists.txt b/velocity_controllers/CMakeLists.txt
index e7df93d2ab..b137070ddf 100644
--- a/velocity_controllers/CMakeLists.txt
+++ b/velocity_controllers/CMakeLists.txt
@@ -1,15 +1,9 @@
cmake_minimum_required(VERSION 3.16)
project(velocity_controllers)
-if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
- add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable
- -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct
- -Werror=missing-braces)
-endif()
-
-# using this instead of visibility macros
-# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053
-set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+include(../ros2_controllers.cmake)
+set_compiler_options()
+export_windows_symbols()
set(THIS_PACKAGE_INCLUDE_DEPENDS
forward_command_controller
From 78b1dc6f65968576ddfe69fac84fd1a91ba848cc Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Sat, 1 Mar 2025 13:12:06 +0000
Subject: [PATCH 25/36] Rename API to snake_case
---
.../src/joint_trajectory_controller.cpp | 8 +++----
.../test/test_trajectory_controller.cpp | 4 ++--
.../pid_trajectory_plugin.hpp | 8 +++----
.../trajectory_controller_base.hpp | 20 ++++++++---------
.../src/pid_trajectory_plugin.cpp | 4 ++--
.../test/test_pid_trajectory.cpp | 22 +++++++++----------
6 files changed, 33 insertions(+), 33 deletions(-)
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
index e81f4fb6d9..66680d06ec 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp
+++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -156,7 +156,7 @@ controller_interface::return_type JointTrajectoryController::update(
// update gains of controller
if (traj_contr_)
{
- if (traj_contr_->updateGainsRT() == false)
+ if (traj_contr_->update_gains_rt() == false)
{
RCLCPP_ERROR(get_node()->get_logger(), "Could not update gains of controller");
return controller_interface::return_type::ERROR;
@@ -299,7 +299,7 @@ controller_interface::return_type JointTrajectoryController::update(
{
if (traj_contr_)
{
- traj_contr_->computeCommands(
+ traj_contr_->compute_commands(
tmp_command_, state_current_, state_error_, command_next_,
time - traj_external_point_ptr_->time_from_start(), period);
}
@@ -1607,7 +1607,7 @@ void JointTrajectoryController::add_new_trajectory_msg_nonRT(
if (traj_contr_)
{
// this can take some time; trajectory won't start until control law is computed
- if (traj_contr_->computeControlLawNonRT(traj_msg) == false)
+ if (traj_contr_->compute_control_law_non_rt(traj_msg) == false)
{
RCLCPP_ERROR(get_node()->get_logger(), "Failed to compute gains for trajectory.");
}
@@ -1626,7 +1626,7 @@ void JointTrajectoryController::add_new_trajectory_msg_RT(
if (traj_contr_)
{
// this is used for set_hold_position() only -> this should (and must) not take a long time
- if (traj_contr_->computeControlLawRT(traj_msg) == false)
+ if (traj_contr_->compute_control_law_rt(traj_msg) == false)
{
RCLCPP_ERROR(get_node()->get_logger(), "Failed to compute gains for trajectory.");
}
diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp
index 956dcbfedb..bfdbb068d9 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller.cpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp
@@ -522,14 +522,14 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters)
rclcpp::Duration duration_since_start(std::chrono::milliseconds(250));
rclcpp::Duration period(std::chrono::milliseconds(100));
- pids->computeCommands(tmp_command, current, error, desired, duration_since_start, period);
+ pids->compute_commands(tmp_command, current, error, desired, duration_since_start, period);
EXPECT_EQ(tmp_command.at(0), 0.0);
double kp = 1.0;
SetPidParameters(kp);
updateControllerAsync();
- pids->computeCommands(tmp_command, current, error, desired, duration_since_start, period);
+ pids->compute_commands(tmp_command, current, error, desired, duration_since_start, period);
EXPECT_EQ(tmp_command.at(0), 1.0);
}
else
diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp
index 8d66837c71..2e6a9e475f 100644
--- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp
+++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp
@@ -41,23 +41,23 @@ class PidTrajectoryPlugin : public TrajectoryControllerBase
bool activate() override;
- bool computeControlLawNonRT_impl(
+ bool compute_control_law_non_rt_impl(
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /*trajectory*/) override
{
// nothing to do
return true;
}
- bool computeControlLawRT_impl(
+ bool compute_control_law_rt_impl(
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /*trajectory*/) override
{
// nothing to do
return true;
}
- bool updateGainsRT() override;
+ bool update_gains_rt() override;
- void computeCommands(
+ void compute_commands(
std::vector<double> & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current,
const trajectory_msgs::msg::JointTrajectoryPoint error,
const trajectory_msgs::msg::JointTrajectoryPoint desired,
diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp
index 612f7b6d9e..14f3346fdd 100644
--- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp
+++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp
@@ -71,16 +71,16 @@ class TrajectoryControllerBase
* of the trajectory until it finishes
*
* this method is not virtual, any overrides won't be called by JTC. Instead, override
- * computeControlLawNonRT_impl for your implementation
+ * compute_control_law_non_rt_impl for your implementation
*
* @return true if the gains were computed, false otherwise
*/
JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
- bool computeControlLawNonRT(
+ bool compute_control_law_non_rt(
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory)
{
rt_control_law_ready_.writeFromNonRT(false);
- auto ret = computeControlLawNonRT_impl(trajectory);
+ auto ret = compute_control_law_non_rt_impl(trajectory);
rt_control_law_ready_.writeFromNonRT(true);
return ret;
}
@@ -91,17 +91,17 @@ class TrajectoryControllerBase
* this method must finish quickly (within one controller-update rate)
*
* this method is not virtual, any overrides won't be called by JTC. Instead, override
- * computeControlLawRT_impl for your implementation
+ * compute_control_law_rt_impl for your implementation
*
* @return true if the gains were computed, false otherwise
*/
JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
- bool computeControlLawRT(
+ bool compute_control_law_rt(
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory)
{
// TODO(christophfroehlich): Need a lock-free write here
rt_control_law_ready_.writeFromNonRT(false);
- auto ret = computeControlLawRT_impl(trajectory);
+ auto ret = compute_control_law_rt_impl(trajectory);
rt_control_law_ready_.writeFromNonRT(true);
return ret;
}
@@ -121,7 +121,7 @@ class TrajectoryControllerBase
* @return true if the gains were updated, false otherwise
*/
JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
- virtual bool updateGainsRT(void) = 0;
+ virtual bool update_gains_rt(void) = 0;
/**
* @brief compute the commands with the precalculated control law
@@ -135,7 +135,7 @@ class TrajectoryControllerBase
* @param[in] period the period since the last update
*/
JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
- virtual void computeCommands(
+ virtual void compute_commands(
std::vector<double> & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current,
const trajectory_msgs::msg::JointTrajectoryPoint error,
const trajectory_msgs::msg::JointTrajectoryPoint desired,
@@ -166,7 +166,7 @@ class TrajectoryControllerBase
* @return true if the gains were computed, false otherwise
*/
JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
- virtual bool computeControlLawNonRT_impl(
+ virtual bool compute_control_law_non_rt_impl(
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory) = 0;
/**
@@ -174,7 +174,7 @@ class TrajectoryControllerBase
* @return true if the gains were computed, false otherwise
*/
JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
- virtual bool computeControlLawRT_impl(
+ virtual bool compute_control_law_rt_impl(
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory) = 0;
};
diff --git a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp
index 1a8982fd9c..eda1ca8403 100644
--- a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp
+++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp
@@ -81,7 +81,7 @@ bool PidTrajectoryPlugin::activate()
return true;
}
-bool PidTrajectoryPlugin::updateGainsRT()
+bool PidTrajectoryPlugin::update_gains_rt()
{
if (param_listener_->is_old(params_))
{
@@ -115,7 +115,7 @@ void PidTrajectoryPlugin::parseGains()
num_cmd_joints_);
}
-void PidTrajectoryPlugin::computeCommands(
+void PidTrajectoryPlugin::compute_commands(
std::vector<double> & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint /*current*/,
const trajectory_msgs::msg::JointTrajectoryPoint error,
const trajectory_msgs::msg::JointTrajectoryPoint desired,
diff --git a/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp b/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp
index bb10b62a06..fd82e03111 100644
--- a/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp
+++ b/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp
@@ -39,9 +39,9 @@ TEST_F(PidTrajectoryTest, TestSingleJoint)
node_->set_parameter(rclcpp::Parameter("gains.joint1.p", 1.0));
ASSERT_TRUE(traj_contr->configure());
ASSERT_TRUE(traj_contr->activate());
- ASSERT_TRUE(
- traj_contr->computeControlLawNonRT(std::make_shared<trajectory_msgs::msg::JointTrajectory>()));
- ASSERT_TRUE(traj_contr->updateGainsRT());
+ ASSERT_TRUE(traj_contr->compute_control_law_non_rt(
+ std::make_shared<trajectory_msgs::msg::JointTrajectory>()));
+ ASSERT_TRUE(traj_contr->update_gains_rt());
trajectory_msgs::msg::JointTrajectoryPoint traj_msg;
traj_msg.positions.push_back(0.0);
@@ -50,7 +50,7 @@ TEST_F(PidTrajectoryTest, TestSingleJoint)
const rclcpp::Duration time_since_start(1, 0);
const rclcpp::Duration period(1, 0);
- ASSERT_NO_THROW(traj_contr->computeCommands(
+ ASSERT_NO_THROW(traj_contr->compute_commands(
tmp_command, traj_msg, traj_msg, traj_msg, time_since_start, period));
}
@@ -73,13 +73,13 @@ TEST_F(PidTrajectoryTest, TestMultipleJoints)
node_->set_parameter(rclcpp::Parameter("gains.joint3.p", 3.0));
ASSERT_TRUE(traj_contr->configure());
ASSERT_TRUE(traj_contr->activate());
- ASSERT_TRUE(
- traj_contr->computeControlLawNonRT(std::make_shared<trajectory_msgs::msg::JointTrajectory>()));
- ASSERT_TRUE(traj_contr->updateGainsRT());
+ ASSERT_TRUE(traj_contr->compute_control_law_non_rt(
+ std::make_shared<trajectory_msgs::msg::JointTrajectory>()));
+ ASSERT_TRUE(traj_contr->update_gains_rt());
- ASSERT_TRUE(
- traj_contr->computeControlLawNonRT(std::make_shared<trajectory_msgs::msg::JointTrajectory>()));
- ASSERT_TRUE(traj_contr->updateGainsRT());
+ ASSERT_TRUE(traj_contr->compute_control_law_non_rt(
+ std::make_shared<trajectory_msgs::msg::JointTrajectory>()));
+ ASSERT_TRUE(traj_contr->update_gains_rt());
trajectory_msgs::msg::JointTrajectoryPoint traj_msg;
traj_msg.positions.push_back(1.0);
@@ -92,7 +92,7 @@ TEST_F(PidTrajectoryTest, TestMultipleJoints)
const rclcpp::Duration time_since_start(1, 0);
const rclcpp::Duration period(1, 0);
- ASSERT_NO_THROW(traj_contr->computeCommands(
+ ASSERT_NO_THROW(traj_contr->compute_commands(
tmp_command, traj_msg, traj_msg, traj_msg, time_since_start, period));
EXPECT_EQ(tmp_command[0], 1.0);
From e65cb311d8521589b806018b0fc4a1f0ce3032d5 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Sat, 1 Mar 2025 13:13:42 +0000
Subject: [PATCH 26/36] Remove visibility macros
---
.../CMakeLists.txt | 4 --
.../pid_trajectory_plugin.hpp | 1 -
.../trajectory_controller_base.hpp | 15 ------
.../visibility_control.h | 49 -------------------
4 files changed, 69 deletions(-)
delete mode 100644 joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/visibility_control.h
diff --git a/joint_trajectory_controller_plugins/CMakeLists.txt b/joint_trajectory_controller_plugins/CMakeLists.txt
index 5ef16e7b54..3a2a53bdab 100644
--- a/joint_trajectory_controller_plugins/CMakeLists.txt
+++ b/joint_trajectory_controller_plugins/CMakeLists.txt
@@ -38,10 +38,6 @@ target_link_libraries(pid_trajectory_plugin PUBLIC
)
pluginlib_export_plugin_description_file(joint_trajectory_controller_plugins plugins.xml)
-# Causes the visibility macros to use dllexport rather than dllimport,
-# which is appropriate when building the dll but not consuming it.
-target_compile_definitions(pid_trajectory_plugin PRIVATE "JOINT_TRAJECTORY_CONTROLLER_PLUGINS_BUILDING_LIBRARY")
-
install(
DIRECTORY include/
DESTINATION include/${PROJECT_NAME}
diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp
index 2e6a9e475f..3f56545484 100644
--- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp
+++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp
@@ -22,7 +22,6 @@
#include "control_toolbox/pid.hpp"
#include "joint_trajectory_controller_plugins/trajectory_controller_base.hpp"
-#include "joint_trajectory_controller_plugins/visibility_control.h"
#include "pid_trajectory_plugin_parameters.hpp" // auto-generated by generate_parameter_library
using PidPtr = std::shared_ptr<control_toolbox::Pid>;
diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp
index 14f3346fdd..378a8f1869 100644
--- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp
+++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp
@@ -26,17 +26,13 @@
#include "trajectory_msgs/msg/joint_trajectory.hpp"
#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
-#include "joint_trajectory_controller_plugins/visibility_control.h"
-
namespace joint_trajectory_controller_plugins
{
class TrajectoryControllerBase
{
public:
- JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
TrajectoryControllerBase() = default;
- JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
virtual ~TrajectoryControllerBase() = default;
/**
@@ -46,7 +42,6 @@ class TrajectoryControllerBase
* @param map_cmd_to_joints a mapping from the joint names in the trajectory messages to the
* command joints
*/
- JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
virtual bool initialize(
rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::vector<size_t> map_cmd_to_joints) = 0;
@@ -54,14 +49,12 @@ class TrajectoryControllerBase
* @brief configure the controller plugin.
* parse read-only parameters, pre-allocate memory for the controller
*/
- JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
virtual bool configure() = 0;
/**
* @brief activate the controller plugin.
* parse parameters
*/
- JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
virtual bool activate() = 0;
/**
@@ -75,7 +68,6 @@ class TrajectoryControllerBase
*
* @return true if the gains were computed, false otherwise
*/
- JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
bool compute_control_law_non_rt(
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory)
{
@@ -95,7 +87,6 @@ class TrajectoryControllerBase
*
* @return true if the gains were computed, false otherwise
*/
- JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
bool compute_control_law_rt(
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory)
{
@@ -120,7 +111,6 @@ class TrajectoryControllerBase
*
* @return true if the gains were updated, false otherwise
*/
- JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
virtual bool update_gains_rt(void) = 0;
/**
@@ -134,7 +124,6 @@ class TrajectoryControllerBase
* can be negative if the trajectory-start is in the future
* @param[in] period the period since the last update
*/
- JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
virtual void compute_commands(
std::vector<double> & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current,
const trajectory_msgs::msg::JointTrajectoryPoint error,
@@ -146,13 +135,11 @@ class TrajectoryControllerBase
*
* is called in on_deactivate() of JTC
*/
- JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
virtual void reset() = 0;
/**
* @return true if the control law is ready (updated with the trajectory)
*/
- JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
bool is_ready() { return rt_control_law_ready_.readFromRT(); }
protected:
@@ -165,7 +152,6 @@ class TrajectoryControllerBase
* @brief compute the control law from the given trajectory (in the non-RT loop)
* @return true if the gains were computed, false otherwise
*/
- JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
virtual bool compute_control_law_non_rt_impl(
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory) = 0;
@@ -173,7 +159,6 @@ class TrajectoryControllerBase
* @brief compute the control law for a single point (in the RT loop)
* @return true if the gains were computed, false otherwise
*/
- JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
virtual bool compute_control_law_rt_impl(
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory) = 0;
};
diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/visibility_control.h b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/visibility_control.h
deleted file mode 100644
index 71b56114c6..0000000000
--- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/visibility_control.h
+++ /dev/null
@@ -1,49 +0,0 @@
-// Copyright 2023 AIT Austrian Institute of Technology
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef JOINT_TRAJECTORY_CONTROLLER_PLUGINS__VISIBILITY_CONTROL_H_
-#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS__VISIBILITY_CONTROL_H_
-
-// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
-// https://gcc.gnu.org/wiki/Visibility
-
-#if defined _WIN32 || defined __CYGWIN__
-#ifdef __GNUC__
-#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_EXPORT __attribute__((dllexport))
-#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_IMPORT __attribute__((dllimport))
-#else
-#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_EXPORT __declspec(dllexport)
-#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_IMPORT __declspec(dllimport)
-#endif
-#ifdef JOINT_TRAJECTORY_CONTROLLER_PLUGINS_BUILDING_LIBRARY
-#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC JOINT_TRAJECTORY_CONTROLLER_PLUGINS_EXPORT
-#else
-#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC JOINT_TRAJECTORY_CONTROLLER_PLUGINS_IMPORT
-#endif
-#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC_TYPE JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
-#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_LOCAL
-#else
-#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_EXPORT __attribute__((visibility("default")))
-#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_IMPORT
-#if __GNUC__ >= 4
-#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC __attribute__((visibility("default")))
-#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_LOCAL __attribute__((visibility("hidden")))
-#else
-#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
-#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_LOCAL
-#endif
-#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC_TYPE
-#endif
-
-#endif // JOINT_TRAJECTORY_CONTROLLER_PLUGINS__VISIBILITY_CONTROL_H_
From 2ed7de2305d53b8805cae2e713adf470db9af96f Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Sat, 1 Mar 2025 13:35:19 +0000
Subject: [PATCH 27/36] Update API to snake_case
---
.../pid_trajectory_plugin.hpp | 2 +-
.../src/pid_trajectory_plugin.cpp | 13 ++++++-------
2 files changed, 7 insertions(+), 8 deletions(-)
diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp
index 3f56545484..57b639286a 100644
--- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp
+++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp
@@ -73,7 +73,7 @@ class PidTrajectoryPlugin : public TrajectoryControllerBase
/**
* @brief parse PID gains from parameter struct
*/
- void parseGains();
+ void parse_gains();
// number of command joints
size_t num_cmd_joints_;
diff --git a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp
index eda1ca8403..4f596c58fb 100644
--- a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp
+++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp
@@ -77,7 +77,7 @@ bool PidTrajectoryPlugin::configure()
bool PidTrajectoryPlugin::activate()
{
params_ = param_listener_->get_params();
- parseGains();
+ parse_gains();
return true;
}
@@ -86,13 +86,13 @@ bool PidTrajectoryPlugin::update_gains_rt()
if (param_listener_->is_old(params_))
{
params_ = param_listener_->get_params();
- parseGains();
+ parse_gains();
}
return true;
}
-void PidTrajectoryPlugin::parseGains()
+void PidTrajectoryPlugin::parse_gains()
{
for (size_t i = 0; i < num_cmd_joints_; ++i)
{
@@ -101,7 +101,7 @@ void PidTrajectoryPlugin::parseGains()
params_.command_joints[i].c_str());
const auto & gains = params_.gains.command_joints_map.at(params_.command_joints[i]);
- pids_[i]->setGains(gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp, true);
+ pids_[i]->set_gains(gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp, true);
ff_velocity_scale_[i] = gains.ff_velocity_scale;
RCLCPP_DEBUG(node_->get_logger(), "[PidTrajectoryPlugin] gains.p: %f", gains.p);
@@ -126,9 +126,8 @@ void PidTrajectoryPlugin::compute_commands(
{
tmp_command[map_cmd_to_joints_[i]] =
(desired.velocities[map_cmd_to_joints_[i]] * ff_velocity_scale_[i]) +
- pids_[i]->computeCommand(
- error.positions[map_cmd_to_joints_[i]], error.velocities[map_cmd_to_joints_[i]],
- (uint64_t)period.nanoseconds());
+ pids_[i]->compute_command(
+ error.positions[map_cmd_to_joints_[i]], error.velocities[map_cmd_to_joints_[i]], period);
}
}
From 7202f306d48fde47c6d11cb077c176280f6c6bf9 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Sat, 1 Mar 2025 13:35:44 +0000
Subject: [PATCH 28/36] Use new GPL include path
---
.../pid_trajectory_plugin.hpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp
index 57b639286a..1e9c3875c3 100644
--- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp
+++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp
@@ -21,8 +21,8 @@
#include "control_toolbox/pid.hpp"
+#include "joint_trajectory_controller_plugins/pid_trajectory_plugin_parameters.hpp" // auto-generated by generate_parameter_library
#include "joint_trajectory_controller_plugins/trajectory_controller_base.hpp"
-#include "pid_trajectory_plugin_parameters.hpp" // auto-generated by generate_parameter_library
using PidPtr = std::shared_ptr<control_toolbox::Pid>;
From 9d3caf06c8e8d13466c66b4ac8d38dc1a445b62c Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Tue, 4 Mar 2025 09:40:24 +0000
Subject: [PATCH 29/36] We don't have to link the parameters again
---
joint_trajectory_controller_plugins/CMakeLists.txt | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/joint_trajectory_controller_plugins/CMakeLists.txt b/joint_trajectory_controller_plugins/CMakeLists.txt
index 3a2a53bdab..11581906ad 100644
--- a/joint_trajectory_controller_plugins/CMakeLists.txt
+++ b/joint_trajectory_controller_plugins/CMakeLists.txt
@@ -52,11 +52,11 @@ install(
if(BUILD_TESTING)
ament_add_gmock(test_load_pid_trajectory test/test_load_pid_trajectory.cpp)
- target_link_libraries(test_load_pid_trajectory pid_trajectory_plugin pid_trajectory_plugin_parameters)
+ target_link_libraries(test_load_pid_trajectory pid_trajectory_plugin)
ament_target_dependencies(test_load_pid_trajectory ${THIS_PACKAGE_INCLUDE_DEPENDS})
ament_add_gmock(test_pid_trajectory test/test_pid_trajectory.cpp)
- target_link_libraries(test_pid_trajectory pid_trajectory_plugin pid_trajectory_plugin_parameters)
+ target_link_libraries(test_pid_trajectory pid_trajectory_plugin)
ament_target_dependencies(test_pid_trajectory ${THIS_PACKAGE_INCLUDE_DEPENDS})
endif()
From 23f55b4329e3af9e017d866edc8239317272549d Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Thu, 6 Mar 2025 21:22:57 +0000
Subject: [PATCH 30/36] Add missing gmock dependency
---
joint_trajectory_controller_plugins/package.xml | 3 ++-
1 file changed, 2 insertions(+), 1 deletion(-)
diff --git a/joint_trajectory_controller_plugins/package.xml b/joint_trajectory_controller_plugins/package.xml
index aaeb07abf9..2d09f4beb7 100644
--- a/joint_trajectory_controller_plugins/package.xml
+++ b/joint_trajectory_controller_plugins/package.xml
@@ -15,10 +15,11 @@
<depend>pluginlib</depend>
<depend>trajectory_msgs</depend>
+ <test_depend>ament_cmake_gmock</test_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
-</package>
\ No newline at end of file
+</package>
From fa83a4ea67fd0db65292946b1acd4354f02e6846 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Mon, 28 Jul 2025 18:43:00 +0000
Subject: [PATCH 31/36] Set precision in debug output
---
.../test/test_trajectory_controller_utils.hpp | 12 ++++++------
1 file changed, 6 insertions(+), 6 deletions(-)
diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
index 3535968556..726bea62e2 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
@@ -708,9 +708,9 @@ class TrajectoryControllerTest : public ::testing::Test
{
EXPECT_TRUE(is_same_sign_or_zero(
position.at(i) - pos_state_interfaces_[i].get_optional().value(), joint_vel_[i]))
- << "test position point " << position.at(i) << ", position state is "
- << pos_state_interfaces_[i].get_optional().value() << ", velocity command is "
- << joint_vel_[i];
+ << std::fixed << std::setprecision(2) << "test position point " << position.at(i)
+ << ", position state is " << pos_state_interfaces_[i].get_optional().value()
+ << ", velocity command is " << joint_vel_[i];
}
}
if (traj_controller_->has_effort_command_interface())
@@ -720,9 +720,9 @@ class TrajectoryControllerTest : public ::testing::Test
EXPECT_TRUE(is_same_sign_or_zero(
position.at(i) - pos_state_interfaces_[i].get_optional().value() + effort.at(i),
joint_eff_[i]))
- << "test position point " << position.at(i) << ", position state is "
- << pos_state_interfaces_[i].get_optional().value() << ", effort command is "
- << joint_eff_[i];
+ << std::fixed << std::setprecision(2) << "test position point " << position.at(i)
+ << ", position state is " << pos_state_interfaces_[i].get_optional().value()
+ << ", effort command is " << joint_eff_[i];
}
}
}
From a80e6f75654c75a7b4e76b707c6d50c7ff8ad7eb Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Mon, 28 Jul 2025 18:43:51 +0000
Subject: [PATCH 32/36] Fix command_joints override
---
.../src/joint_trajectory_controller_parameters.yaml | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml
index a5a45d86d1..7ee15fada3 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml
+++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml
@@ -17,7 +17,7 @@ joint_trajectory_controller:
* JTC is used in a controller chain where command and state interfaces don't have same names.
* If the number of command joints is smaller than the degrees-of-freedom. For example to track the state and error of passive joints. ``command_joints`` must then be a subset of ``joints``.",
- read_only: true,
+ read_only: false, # should be set to true after configuration of the controller
validation: {
unique<>: null,
}
From fefc38d3203deecf539deaa89f2c64652a14b934 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Mon, 28 Jul 2025 18:45:08 +0000
Subject: [PATCH 33/36] Implement effort feedforward
---
.../src/joint_trajectory_controller.cpp | 4 ++--
.../test/test_trajectory_controller_utils.hpp | 2 +-
.../src/pid_trajectory_plugin.cpp | 3 +++
3 files changed, 6 insertions(+), 3 deletions(-)
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
index 3f7b88943f..3221241b8e 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp
+++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -328,8 +328,6 @@ controller_interface::return_type JointTrajectoryController::update(
{
if (traj_contr_)
{
- // TODO(christophfroehlich) add has_effort_command_interface_ ?
- // command_next_.effort[index_cmd_joint] : 0.0)
traj_contr_->compute_commands(
tmp_command_, state_current_, state_error_, command_next_,
time - current_trajectory_->time_from_start(), period);
@@ -1837,6 +1835,7 @@ void JointTrajectoryController::add_new_trajectory_msg_nonRT(
RCLCPP_ERROR(get_node()->get_logger(), "Failed to compute gains for trajectory.");
}
}
+ RCLCPP_INFO(get_node()->get_logger(), "Called add_new_trajectory_msg_nonRT.");
}
void JointTrajectoryController::add_new_trajectory_msg_RT(
@@ -1856,6 +1855,7 @@ void JointTrajectoryController::add_new_trajectory_msg_RT(
RCLCPP_ERROR(get_node()->get_logger(), "Failed to compute gains for trajectory.");
}
}
+ RCLCPP_INFO(get_node()->get_logger(), "Called add_new_trajectory_msg_RT.");
}
void JointTrajectoryController::preempt_active_goal()
diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
index 726bea62e2..3c3594699b 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
@@ -698,7 +698,7 @@ class TrajectoryControllerTest : public ::testing::Test
EXPECT_EQ(effort.at(2), joint_eff_[2]);
}
}
- else // traj_controller_->use_closed_loop_pid_adapter() == true
+ else // traj_controller_->use_external_control_law() == true
{
// velocity or effort PID?
// --> set kp > 0.0 in test
diff --git a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp
index 260afad30e..23f3813c8a 100644
--- a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp
+++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp
@@ -128,11 +128,14 @@ void PidTrajectoryPlugin::compute_commands(
const trajectory_msgs::msg::JointTrajectoryPoint desired,
const rclcpp::Duration & /*duration_since_start*/, const rclcpp::Duration & period)
{
+ // if effort field is present, otherwise it would have been rejected
+ auto has_effort_command_interface = !desired.effort.empty();
// Update PIDs
for (auto i = 0ul; i < num_cmd_joints_; ++i)
{
tmp_command[map_cmd_to_joints_[i]] =
(desired.velocities[map_cmd_to_joints_[i]] * ff_velocity_scale_[i]) +
+ (has_effort_command_interface ? desired.effort[i] : 0.0) +
pids_[i]->compute_command(
error.positions[map_cmd_to_joints_[i]], error.velocities[map_cmd_to_joints_[i]], period);
}
From 48b6b5617b1651f78f3d374eecd2ec918c9fccb1 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Mon, 28 Jul 2025 18:59:24 +0000
Subject: [PATCH 34/36] Simplify code and remove debug output
---
.../src/joint_trajectory_controller.cpp | 2 --
.../src/pid_trajectory_plugin.cpp | 7 ++++---
2 files changed, 4 insertions(+), 5 deletions(-)
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
index 3221241b8e..60c2d2fe40 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp
+++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -1835,7 +1835,6 @@ void JointTrajectoryController::add_new_trajectory_msg_nonRT(
RCLCPP_ERROR(get_node()->get_logger(), "Failed to compute gains for trajectory.");
}
}
- RCLCPP_INFO(get_node()->get_logger(), "Called add_new_trajectory_msg_nonRT.");
}
void JointTrajectoryController::add_new_trajectory_msg_RT(
@@ -1855,7 +1854,6 @@ void JointTrajectoryController::add_new_trajectory_msg_RT(
RCLCPP_ERROR(get_node()->get_logger(), "Failed to compute gains for trajectory.");
}
}
- RCLCPP_INFO(get_node()->get_logger(), "Called add_new_trajectory_msg_RT.");
}
void JointTrajectoryController::preempt_active_goal()
diff --git a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp
index 23f3813c8a..2218a7c8f5 100644
--- a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp
+++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp
@@ -133,11 +133,12 @@ void PidTrajectoryPlugin::compute_commands(
// Update PIDs
for (auto i = 0ul; i < num_cmd_joints_; ++i)
{
- tmp_command[map_cmd_to_joints_[i]] =
- (desired.velocities[map_cmd_to_joints_[i]] * ff_velocity_scale_[i]) +
+ const auto map_cmd_to_joint = map_cmd_to_joints_[i];
+ tmp_command[map_cmd_to_joint] =
+ (desired.velocities[map_cmd_to_joint] * ff_velocity_scale_[i]) +
(has_effort_command_interface ? desired.effort[i] : 0.0) +
pids_[i]->compute_command(
- error.positions[map_cmd_to_joints_[i]], error.velocities[map_cmd_to_joints_[i]], period);
+ error.positions[map_cmd_to_joint], error.velocities[map_cmd_to_joint], period);
}
}
From 8cfbd91a8fe0db81540921ee8c1dd343c339b1db Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Mon, 28 Jul 2025 19:24:59 +0000
Subject: [PATCH 35/36] Rework plugin API
---
.../pid_trajectory_plugin.hpp | 52 ++++++++-------
.../trajectory_controller_base.hpp | 65 +++++++++++++++----
.../src/pid_trajectory_plugin.cpp | 28 +++-----
3 files changed, 89 insertions(+), 56 deletions(-)
diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp
index 1e9c3875c3..acd1232ff9 100644
--- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp
+++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp
@@ -32,44 +32,51 @@ namespace joint_trajectory_controller_plugins
class PidTrajectoryPlugin : public TrajectoryControllerBase
{
public:
- bool initialize(
- rclcpp_lifecycle::LifecycleNode::SharedPtr node,
- std::vector<size_t> map_cmd_to_joints) override;
+ bool update_gains_rt() override;
- bool configure() override;
+ void compute_commands(
+ std::vector<double> & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current,
+ const trajectory_msgs::msg::JointTrajectoryPoint error,
+ const trajectory_msgs::msg::JointTrajectoryPoint desired,
+ const rclcpp::Duration & duration_since_start, const rclcpp::Duration & period) override;
- bool activate() override;
+ void reset() override;
- bool compute_control_law_non_rt_impl(
- const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /*trajectory*/) override
+ void start() override
{
// nothing to do
- return true;
}
- bool compute_control_law_rt_impl(
- const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /*trajectory*/) override
+protected:
+ // Parameters from ROS for joint_trajectory_controller_plugins
+ std::shared_ptr<ParamListener> param_listener_;
+
+ bool on_initialize(void) override;
+
+ rclcpp::Logger set_logger() const override
{
- // nothing to do
- return true;
+ return node_->get_logger().get_child("PidTrajectoryPlugin");
}
- bool update_gains_rt() override;
+ bool on_configure() override;
- void compute_commands(
- std::vector<double> & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current,
- const trajectory_msgs::msg::JointTrajectoryPoint error,
- const trajectory_msgs::msg::JointTrajectoryPoint desired,
- const rclcpp::Duration & duration_since_start, const rclcpp::Duration & period) override;
+ bool on_activate() override;
- void reset() override;
+ bool on_compute_control_law_non_rt(
+ const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /*trajectory*/) override
+ {
+ // nothing to do
+ return true;
+ }
- void start() override
+ bool on_compute_control_law_rt(
+ const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /*trajectory*/) override
{
// nothing to do
+ return true;
}
-protected:
+private:
/**
* @brief parse PID gains from parameter struct
*/
@@ -77,15 +84,12 @@ class PidTrajectoryPlugin : public TrajectoryControllerBase
// number of command joints
size_t num_cmd_joints_;
- // map from joints in the message to command joints
- std::vector<size_t> map_cmd_to_joints_;
// PID controllers
std::vector<PidPtr> pids_;
// Feed-forward velocity weight factor when calculating closed loop pid adapter's command
std::vector<double> ff_velocity_scale_;
// Parameters from ROS for joint_trajectory_controller_plugins
- std::shared_ptr<ParamListener> param_listener_;
Params params_;
};
diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp
index 378a8f1869..5bd54b7989 100644
--- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp
+++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp
@@ -37,25 +37,28 @@ class TrajectoryControllerBase
/**
* @brief initialize the controller plugin.
- * declare parameters
* @param node the node handle to use for parameter handling
* @param map_cmd_to_joints a mapping from the joint names in the trajectory messages to the
* command joints
*/
- virtual bool initialize(
- rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::vector<size_t> map_cmd_to_joints) = 0;
+ bool initialize(
+ rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::vector<size_t> map_cmd_to_joints)
+ {
+ node_ = node;
+ map_cmd_to_joints_ = map_cmd_to_joints;
+ logger_ = this->set_logger();
+ return on_initialize();
+ };
/**
* @brief configure the controller plugin.
- * parse read-only parameters, pre-allocate memory for the controller
*/
- virtual bool configure() = 0;
+ bool configure() { return on_configure(); }
/**
* @brief activate the controller plugin.
- * parse parameters
*/
- virtual bool activate() = 0;
+ bool activate() { return on_activate(); }
/**
* @brief compute the control law for the given trajectory
@@ -64,7 +67,7 @@ class TrajectoryControllerBase
* of the trajectory until it finishes
*
* this method is not virtual, any overrides won't be called by JTC. Instead, override
- * compute_control_law_non_rt_impl for your implementation
+ * on_compute_control_law_non_rt for your implementation
*
* @return true if the gains were computed, false otherwise
*/
@@ -72,7 +75,7 @@ class TrajectoryControllerBase
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory)
{
rt_control_law_ready_.writeFromNonRT(false);
- auto ret = compute_control_law_non_rt_impl(trajectory);
+ auto ret = on_compute_control_law_non_rt(trajectory);
rt_control_law_ready_.writeFromNonRT(true);
return ret;
}
@@ -83,7 +86,7 @@ class TrajectoryControllerBase
* this method must finish quickly (within one controller-update rate)
*
* this method is not virtual, any overrides won't be called by JTC. Instead, override
- * compute_control_law_rt_impl for your implementation
+ * on_compute_control_law_rt for your implementation
*
* @return true if the gains were computed, false otherwise
*/
@@ -92,7 +95,7 @@ class TrajectoryControllerBase
{
// TODO(christophfroehlich): Need a lock-free write here
rt_control_law_ready_.writeFromNonRT(false);
- auto ret = compute_control_law_rt_impl(trajectory);
+ auto ret = on_compute_control_law_rt(trajectory);
rt_control_law_ready_.writeFromNonRT(true);
return ret;
}
@@ -145,22 +148,58 @@ class TrajectoryControllerBase
protected:
// the node handle for parameter handling
rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
+ // map from joints in the message to command joints
+ std::vector<size_t> map_cmd_to_joints_;
// Are we computing the control law or is it valid?
realtime_tools::RealtimeBuffer<bool> rt_control_law_ready_;
+ /**
+ * @brief Get the logger for this plugin
+ */
+ rclcpp::Logger get_logger() const { return logger_; }
+ /**
+ * @brief Get the logger for this plugin
+ */
+ virtual rclcpp::Logger set_logger() const = 0;
+
/**
* @brief compute the control law from the given trajectory (in the non-RT loop)
* @return true if the gains were computed, false otherwise
*/
- virtual bool compute_control_law_non_rt_impl(
+ virtual bool on_compute_control_law_non_rt(
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory) = 0;
/**
* @brief compute the control law for a single point (in the RT loop)
* @return true if the gains were computed, false otherwise
*/
- virtual bool compute_control_law_rt_impl(
+ virtual bool on_compute_control_law_rt(
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory) = 0;
+
+ /**
+ * @brief initialize the controller plugin.
+ *
+ * declare parameters
+ */
+ virtual bool on_initialize(void) = 0;
+
+ /**
+ * @brief configure the controller plugin.
+ *
+ * parse read-only parameters, pre-allocate memory for the controller
+ */
+ virtual bool on_configure() = 0;
+
+ /**
+ * @brief activate the controller plugin.
+ *
+ * parse parameters
+ */
+ virtual bool on_activate() = 0;
+
+private:
+ // child logger for this plugin
+ rclcpp::Logger logger_ = rclcpp::get_logger("joint_trajectory_controller_plugins");
};
} // namespace joint_trajectory_controller_plugins
diff --git a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp
index 2218a7c8f5..d8982cbc58 100644
--- a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp
+++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp
@@ -17,12 +17,8 @@
namespace joint_trajectory_controller_plugins
{
-bool PidTrajectoryPlugin::initialize(
- rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::vector<size_t> map_cmd_to_joints)
+bool PidTrajectoryPlugin::on_initialize()
{
- node_ = node;
- map_cmd_to_joints_ = map_cmd_to_joints;
-
try
{
// Create the parameter listener and get the parameters
@@ -37,7 +33,7 @@ bool PidTrajectoryPlugin::initialize(
return true;
}
-bool PidTrajectoryPlugin::configure()
+bool PidTrajectoryPlugin::on_configure()
{
try
{
@@ -53,14 +49,12 @@ bool PidTrajectoryPlugin::configure()
num_cmd_joints_ = params_.command_joints.size();
if (num_cmd_joints_ == 0)
{
- RCLCPP_ERROR(node_->get_logger(), "[PidTrajectoryPlugin] No command joints specified.");
+ RCLCPP_ERROR(get_logger(), "No command joints specified.");
return false;
}
if (num_cmd_joints_ != map_cmd_to_joints_.size())
{
- RCLCPP_ERROR(
- node_->get_logger(),
- "[PidTrajectoryPlugin] map_cmd_to_joints has to be of size num_cmd_joints.");
+ RCLCPP_ERROR(get_logger(), "map_cmd_to_joints has to be of size num_cmd_joints.");
return false;
}
pids_.resize(num_cmd_joints_); // memory for the shared pointers, will be nullptr
@@ -74,7 +68,7 @@ bool PidTrajectoryPlugin::configure()
return true;
}
-bool PidTrajectoryPlugin::activate()
+bool PidTrajectoryPlugin::on_activate()
{
params_ = param_listener_->get_params();
parse_gains();
@@ -97,8 +91,7 @@ void PidTrajectoryPlugin::parse_gains()
for (size_t i = 0; i < num_cmd_joints_; ++i)
{
RCLCPP_DEBUG(
- node_->get_logger(), "[PidTrajectoryPlugin] params_.command_joints %lu : %s", i,
- params_.command_joints[i].c_str());
+ get_logger(), "params_.command_joints %lu : %s", i, params_.command_joints[i].c_str());
const auto & gains = params_.gains.command_joints_map.at(params_.command_joints[i]);
control_toolbox::AntiWindupStrategy antiwindup_strat;
@@ -111,15 +104,12 @@ void PidTrajectoryPlugin::parse_gains()
gains.p, gains.i, gains.d, gains.u_clamp_max, gains.u_clamp_min, antiwindup_strat);
ff_velocity_scale_[i] = gains.ff_velocity_scale;
- RCLCPP_DEBUG(node_->get_logger(), "[PidTrajectoryPlugin] gains.p: %f", gains.p);
- RCLCPP_DEBUG(
- node_->get_logger(), "[PidTrajectoryPlugin] ff_velocity_scale_: %f", ff_velocity_scale_[i]);
+ RCLCPP_DEBUG(get_logger(), "gains.p: %f", gains.p);
+ RCLCPP_DEBUG(get_logger(), "ff_velocity_scale_: %f", ff_velocity_scale_[i]);
}
RCLCPP_INFO(
- node_->get_logger(),
- "[PidTrajectoryPlugin] Loaded PID gains from ROS parameters for %lu joint(s).",
- num_cmd_joints_);
+ get_logger(), "Loaded PID gains from ROS parameters for %lu joint(s).", num_cmd_joints_);
}
void PidTrajectoryPlugin::compute_commands(
From 771d5bd7159feffe2208c197ed04c3bf138886df Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Mon, 28 Jul 2025 19:34:45 +0000
Subject: [PATCH 36/36] Use a simple std::atomic instead
---
.../trajectory_controller_base.hpp | 16 ++++++++--------
1 file changed, 8 insertions(+), 8 deletions(-)
diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp
index 5bd54b7989..2436952190 100644
--- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp
+++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp
@@ -15,6 +15,7 @@
#ifndef JOINT_TRAJECTORY_CONTROLLER_PLUGINS__TRAJECTORY_CONTROLLER_BASE_HPP_
#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS__TRAJECTORY_CONTROLLER_BASE_HPP_
+#include <atomic>
#include <memory>
#include <string>
#include <vector>
@@ -74,9 +75,9 @@ class TrajectoryControllerBase
bool compute_control_law_non_rt(
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory)
{
- rt_control_law_ready_.writeFromNonRT(false);
+ rt_control_law_ready_ = false;
auto ret = on_compute_control_law_non_rt(trajectory);
- rt_control_law_ready_.writeFromNonRT(true);
+ rt_control_law_ready_ = true;
return ret;
}
@@ -93,10 +94,9 @@ class TrajectoryControllerBase
bool compute_control_law_rt(
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory)
{
- // TODO(christophfroehlich): Need a lock-free write here
- rt_control_law_ready_.writeFromNonRT(false);
+ rt_control_law_ready_ = false;
auto ret = on_compute_control_law_rt(trajectory);
- rt_control_law_ready_.writeFromNonRT(true);
+ rt_control_law_ready_ = true;
return ret;
}
@@ -143,15 +143,13 @@ class TrajectoryControllerBase
/**
* @return true if the control law is ready (updated with the trajectory)
*/
- bool is_ready() { return rt_control_law_ready_.readFromRT(); }
+ bool is_ready() { return rt_control_law_ready_; }
protected:
// the node handle for parameter handling
rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
// map from joints in the message to command joints
std::vector<size_t> map_cmd_to_joints_;
- // Are we computing the control law or is it valid?
- realtime_tools::RealtimeBuffer<bool> rt_control_law_ready_;
/**
* @brief Get the logger for this plugin
@@ -200,6 +198,8 @@ class TrajectoryControllerBase
private:
// child logger for this plugin
rclcpp::Logger logger_ = rclcpp::get_logger("joint_trajectory_controller_plugins");
+ // Are we computing the control law or is it valid?
+ std::atomic<bool> rt_control_law_ready_;
};
} // namespace joint_trajectory_controller_plugins