Skip to content

Commit 345a926

Browse files
christophfroehlichchristophfroehlich
committed
Add possibility to use optional state interfaces by the controller plugin
1 parent 2211a6e commit 345a926

File tree

7 files changed

+78
-19
lines changed

7 files changed

+78
-19
lines changed

‎joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -42,6 +42,7 @@
4242
#include "realtime_tools/realtime_buffer.hpp"
4343
#include "realtime_tools/realtime_publisher.hpp"
4444
#include "realtime_tools/realtime_server_goal_handle.hpp"
45+
#include "realtime_tools/realtime_thread_safe_box.hpp"
4546
#include "trajectory_msgs/msg/joint_trajectory.hpp"
4647
#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
4748

@@ -139,6 +140,11 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
139140
scaling_state_interface_;
140141
std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
141142
scaling_command_interface_;
143+
std::vector<std::string> traj_ctr_state_interface_names_;
144+
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
145+
traj_ctr_state_interfaces_;
146+
std::vector<double> traj_ctr_state_interfaces_values_;
147+
realtime_tools::RealtimeThreadSafeBox<std::vector<double>> traj_ctr_state_interfaces_box_;
142148

143149
bool has_position_state_interface_ = false;
144150
bool has_velocity_state_interface_ = false;

‎joint_trajectory_controller/src/joint_trajectory_controller.cpp‎

Lines changed: 43 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -133,7 +133,9 @@ JointTrajectoryController::state_interface_configuration() const
133133
{
134134
controller_interface::InterfaceConfiguration conf;
135135
conf.type = controller_interface::interface_configuration_type::INDIVIDUAL;
136-
conf.names.reserve(dof_ * params_.state_interfaces.size());
136+
137+
conf.names.reserve(
138+
dof_ * params_.state_interfaces.size() + traj_ctr_state_interface_names_.size());
137139
for (const auto & joint_name : params_.joints)
138140
{
139141
for (const auto & interface_type : params_.state_interfaces)
@@ -145,6 +147,9 @@ JointTrajectoryController::state_interface_configuration() const
145147
{
146148
conf.names.push_back(params_.speed_scaling.state_interface);
147149
}
150+
conf.names.insert(
151+
conf.names.end(), traj_ctr_state_interface_names_.begin(),
152+
traj_ctr_state_interface_names_.end());
148153
return conf;
149154
}
150155

@@ -332,7 +337,8 @@ controller_interface::return_type JointTrajectoryController::update(
332337
{
333338
traj_contr_->compute_commands(
334339
tmp_command_, state_current_, state_error_, command_next_,
335-
time - current_trajectory_->time_from_start(), period);
340+
traj_ctr_state_interfaces_values_, time - current_trajectory_->time_from_start(),
341+
period);
336342
}
337343

338344
// set values for next hardware write()
@@ -534,6 +540,22 @@ void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectory
534540
{
535541
assign_point_from_command_interface(state.effort, joint_command_interface_[3]);
536542
}
543+
544+
// Optional traj_ctrl_ state interfaces
545+
for (size_t index = 0; index < traj_ctr_state_interfaces_.size(); ++index)
546+
{
547+
const auto state_interface_value_op = traj_ctr_state_interfaces_[index].get().get_optional();
548+
if (!state_interface_value_op.has_value())
549+
{
550+
RCLCPP_DEBUG(
551+
logger, "Unable to retrieve state interface value for joint at index %zu", index);
552+
}
553+
else
554+
{
555+
traj_ctr_state_interfaces_values_[index] = state_interface_value_op.value();
556+
}
557+
}
558+
traj_ctr_state_interfaces_box_.try_set(traj_ctr_state_interfaces_values_);
537559
}
538560

539561
bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajectoryPoint & state)
@@ -929,6 +951,11 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
929951
}
930952
}
931953

954+
traj_ctr_state_interface_names_ =
955+
traj_contr_ ? traj_contr_->state_interface_configuration() : std::vector<std::string>{};
956+
traj_ctr_state_interfaces_.reserve(traj_ctr_state_interface_names_.size());
957+
traj_ctr_state_interfaces_values_.resize(traj_ctr_state_interface_names_.size(), 0.0);
958+
traj_ctr_state_interfaces_box_.set(traj_ctr_state_interfaces_values_);
932959
tmp_command_.resize(dof_, 0.0);
933960
}
934961

@@ -1218,6 +1245,14 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
12181245
return CallbackReturn::ERROR;
12191246
}
12201247
}
1248+
if (!controller_interface::get_ordered_interfaces(
1249+
state_interfaces_, traj_ctr_state_interface_names_, "", traj_ctr_state_interfaces_))
1250+
{
1251+
RCLCPP_ERROR(
1252+
logger, "Expected %zu state interfaces, got %zu.", traj_ctr_state_interface_names_.size(),
1253+
traj_ctr_state_interfaces_.size());
1254+
return CallbackReturn::ERROR;
1255+
}
12211256

12221257
current_trajectory_ = std::make_shared<Trajectory>();
12231258
new_trajectory_msg_.writeFromNonRT(std::shared_ptr<trajectory_msgs::msg::JointTrajectory>());
@@ -1343,6 +1378,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate(
13431378
joint_command_interface_[index].clear();
13441379
joint_state_interface_[index].clear();
13451380
}
1381+
traj_ctr_state_interfaces_.clear();
13461382
release_interfaces();
13471383

13481384
subscriber_is_active_ = false;
@@ -1841,7 +1877,9 @@ void JointTrajectoryController::add_new_trajectory_msg_nonRT(
18411877
if (traj_contr_)
18421878
{
18431879
// this can take some time; trajectory won't start until control law is computed
1844-
if (traj_contr_->compute_control_law_non_rt(traj_msg) == false)
1880+
if (
1881+
traj_contr_->compute_control_law_non_rt(traj_msg, traj_ctr_state_interfaces_box_.get()) ==
1882+
false)
18451883
{
18461884
RCLCPP_ERROR(get_node()->get_logger(), "Failed to compute gains for trajectory.");
18471885
}
@@ -1860,7 +1898,8 @@ void JointTrajectoryController::add_new_trajectory_msg_RT(
18601898
if (traj_contr_)
18611899
{
18621900
// this is used for set_hold_position() only -> this should (and must) not take a long time
1863-
if (traj_contr_->compute_control_law_rt(traj_msg) == false)
1901+
if (
1902+
traj_contr_->compute_control_law_rt(traj_msg, traj_ctr_state_interfaces_box_.get()) == false)
18641903
{
18651904
RCLCPP_ERROR(get_node()->get_logger(), "Failed to compute gains for trajectory.");
18661905
}

‎joint_trajectory_controller/test/test_trajectory_controller.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -487,14 +487,14 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters)
487487
rclcpp::Duration duration_since_start(std::chrono::milliseconds(250));
488488
rclcpp::Duration period(std::chrono::milliseconds(100));
489489

490-
pids->compute_commands(tmp_command, current, error, desired, duration_since_start, period);
490+
pids->compute_commands(tmp_command, current, error, desired, {}, duration_since_start, period);
491491
EXPECT_EQ(tmp_command.at(0), 0.0);
492492

493493
double kp = 1.0;
494494
SetPidParameters(kp);
495495
updateControllerAsync();
496496

497-
pids->compute_commands(tmp_command, current, error, desired, duration_since_start, period);
497+
pids->compute_commands(tmp_command, current, error, desired, {}, duration_since_start, period);
498498
EXPECT_EQ(tmp_command.at(0), 1.0);
499499
}
500500
else

‎joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp‎

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -38,6 +38,7 @@ class PidTrajectoryPlugin : public TrajectoryControllerBase
3838
std::vector<double> & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current,
3939
const trajectory_msgs::msg::JointTrajectoryPoint error,
4040
const trajectory_msgs::msg::JointTrajectoryPoint desired,
41+
const std::vector<double> & opt_state_interfaces_values_,
4142
const rclcpp::Duration & duration_since_start, const rclcpp::Duration & period) override;
4243

4344
void reset() override;
@@ -63,14 +64,16 @@ class PidTrajectoryPlugin : public TrajectoryControllerBase
6364
bool on_activate() override;
6465

6566
bool on_compute_control_law_non_rt(
66-
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /*trajectory*/) override
67+
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /*trajectory*/,
68+
const std::vector<double> & /*opt_state_interfaces_values_*/) override
6769
{
6870
// nothing to do
6971
return true;
7072
}
7173

7274
bool on_compute_control_law_rt(
73-
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /*trajectory*/) override
75+
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /*trajectory*/,
76+
const std::vector<double> & /*opt_state_interfaces_values_*/) override
7477
{
7578
// nothing to do
7679
return true;

‎joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp‎

Lines changed: 16 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,11 @@ class TrajectoryControllerBase
3636

3737
virtual ~TrajectoryControllerBase() = default;
3838

39+
/**
40+
* @brief get additional state interfaces required by this controller plugin
41+
*/
42+
virtual std::vector<std::string> state_interface_configuration() const { return {}; }
43+
3944
/**
4045
* @brief initialize the controller plugin.
4146
* @param node the node handle to use for parameter handling
@@ -73,10 +78,11 @@ class TrajectoryControllerBase
7378
* @return true if the gains were computed, false otherwise
7479
*/
7580
bool compute_control_law_non_rt(
76-
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory)
81+
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory,
82+
const std::vector<double> & opt_state_interfaces_values_)
7783
{
7884
rt_control_law_ready_ = false;
79-
auto ret = on_compute_control_law_non_rt(trajectory);
85+
auto ret = on_compute_control_law_non_rt(trajectory, opt_state_interfaces_values_);
8086
rt_control_law_ready_ = true;
8187
return ret;
8288
}
@@ -92,10 +98,11 @@ class TrajectoryControllerBase
9298
* @return true if the gains were computed, false otherwise
9399
*/
94100
bool compute_control_law_rt(
95-
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory)
101+
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory,
102+
const std::vector<double> & opt_state_interfaces_values_)
96103
{
97104
rt_control_law_ready_ = false;
98-
auto ret = on_compute_control_law_rt(trajectory);
105+
auto ret = on_compute_control_law_rt(trajectory, opt_state_interfaces_values_);
99106
rt_control_law_ready_ = true;
100107
return ret;
101108
}
@@ -131,6 +138,7 @@ class TrajectoryControllerBase
131138
std::vector<double> & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current,
132139
const trajectory_msgs::msg::JointTrajectoryPoint error,
133140
const trajectory_msgs::msg::JointTrajectoryPoint desired,
141+
const std::vector<double> & opt_state_interfaces_values_,
134142
const rclcpp::Duration & duration_since_start, const rclcpp::Duration & period) = 0;
135143

136144
/**
@@ -165,14 +173,16 @@ class TrajectoryControllerBase
165173
* @return true if the gains were computed, false otherwise
166174
*/
167175
virtual bool on_compute_control_law_non_rt(
168-
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory) = 0;
176+
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory,
177+
const std::vector<double> & opt_state_interfaces_values_) = 0;
169178

170179
/**
171180
* @brief compute the control law for a single point (in the RT loop)
172181
* @return true if the gains were computed, false otherwise
173182
*/
174183
virtual bool on_compute_control_law_rt(
175-
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory) = 0;
184+
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory,
185+
const std::vector<double> & opt_state_interfaces_values_) = 0;
176186

177187
/**
178188
* @brief initialize the controller plugin.

‎joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -116,6 +116,7 @@ void PidTrajectoryPlugin::compute_commands(
116116
std::vector<double> & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint /*current*/,
117117
const trajectory_msgs::msg::JointTrajectoryPoint error,
118118
const trajectory_msgs::msg::JointTrajectoryPoint desired,
119+
const std::vector<double> & /* opt_state_interfaces_values_*/,
119120
const rclcpp::Duration & /*duration_since_start*/, const rclcpp::Duration & period)
120121
{
121122
// if effort field is present, otherwise it would have been rejected

‎joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,7 @@ TEST_F(PidTrajectoryTest, TestSingleJoint)
4040
ASSERT_TRUE(traj_contr->configure());
4141
ASSERT_TRUE(traj_contr->activate());
4242
ASSERT_TRUE(traj_contr->compute_control_law_non_rt(
43-
std::make_shared<trajectory_msgs::msg::JointTrajectory>()));
43+
std::make_shared<trajectory_msgs::msg::JointTrajectory>(), {}));
4444
ASSERT_TRUE(traj_contr->update_gains_rt());
4545

4646
trajectory_msgs::msg::JointTrajectoryPoint traj_msg;
@@ -51,7 +51,7 @@ TEST_F(PidTrajectoryTest, TestSingleJoint)
5151
const rclcpp::Duration period(1, 0);
5252

5353
ASSERT_NO_THROW(traj_contr->compute_commands(
54-
tmp_command, traj_msg, traj_msg, traj_msg, time_since_start, period));
54+
tmp_command, traj_msg, traj_msg, traj_msg, {}, time_since_start, period));
5555
}
5656

5757
TEST_F(PidTrajectoryTest, TestMultipleJoints)
@@ -74,11 +74,11 @@ TEST_F(PidTrajectoryTest, TestMultipleJoints)
7474
ASSERT_TRUE(traj_contr->configure());
7575
ASSERT_TRUE(traj_contr->activate());
7676
ASSERT_TRUE(traj_contr->compute_control_law_non_rt(
77-
std::make_shared<trajectory_msgs::msg::JointTrajectory>()));
77+
std::make_shared<trajectory_msgs::msg::JointTrajectory>(), {}));
7878
ASSERT_TRUE(traj_contr->update_gains_rt());
7979

8080
ASSERT_TRUE(traj_contr->compute_control_law_non_rt(
81-
std::make_shared<trajectory_msgs::msg::JointTrajectory>()));
81+
std::make_shared<trajectory_msgs::msg::JointTrajectory>(), {}));
8282
ASSERT_TRUE(traj_contr->update_gains_rt());
8383

8484
trajectory_msgs::msg::JointTrajectoryPoint traj_msg;
@@ -93,7 +93,7 @@ TEST_F(PidTrajectoryTest, TestMultipleJoints)
9393
const rclcpp::Duration period(1, 0);
9494

9595
ASSERT_NO_THROW(traj_contr->compute_commands(
96-
tmp_command, traj_msg, traj_msg, traj_msg, time_since_start, period));
96+
tmp_command, traj_msg, traj_msg, traj_msg, {}, time_since_start, period));
9797

9898
EXPECT_EQ(tmp_command[0], 1.0);
9999
EXPECT_EQ(tmp_command[1], 2.0);

0 commit comments

Comments
 (0)