Skip to content

Commit fcad0a3

Browse files
christophfroehlichchristophfroehlich
authored
Remove usage of get_ordered_interfaces but update parameter validation instead (#1816)
1 parent 228c586 commit fcad0a3

32 files changed

+192
-488
lines changed

‎admittance_controller/include/admittance_controller/admittance_controller.hpp

Lines changed: 0 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -82,15 +82,6 @@ class AdmittanceController : public controller_interface::ChainableControllerInt
8282
size_t num_joints_ = 0;
8383
std::vector<std::string> command_joint_names_;
8484

85-
// The interfaces are defined as the types in 'allowed_interface_types_' member.
86-
// For convenience, for each type the interfaces are ordered so that i-th position
87-
// matches i-th index in joint_names_
88-
template <typename T>
89-
using InterfaceReferences = std::vector<std::vector<std::reference_wrapper<T>>>;
90-
91-
InterfaceReferences<hardware_interface::LoanedCommandInterface> joint_command_interface_;
92-
InterfaceReferences<hardware_interface::LoanedStateInterface> joint_state_interface_;
93-
9485
bool has_position_state_interface_ = false;
9586
bool has_velocity_state_interface_ = false;
9687
bool has_acceleration_state_interface_ = false;

‎admittance_controller/src/admittance_controller.cpp

Lines changed: 2 additions & 42 deletions
Original file line numberDiff line numberDiff line change
@@ -226,16 +226,14 @@ controller_interface::CallbackReturn AdmittanceController::on_configure(
226226
}
227227
}
228228

229-
// Check if only allowed interface types are used and initialize storage to avoid memory
230-
// allocation during activation
229+
// Check if only allowed interface types are used
231230
auto contains_interface_type =
232231
[](const std::vector<std::string> & interface_type_list, const std::string & interface_type)
233232
{
234233
return std::find(interface_type_list.begin(), interface_type_list.end(), interface_type) !=
235234
interface_type_list.end();
236235
};
237236

238-
joint_command_interface_.resize(allowed_interface_types_.size());
239237
for (const auto & interface : admittance_->parameters_.command_interfaces)
240238
{
241239
auto it =
@@ -257,9 +255,7 @@ controller_interface::CallbackReturn AdmittanceController::on_configure(
257255
has_effort_command_interface_ = contains_interface_type(
258256
admittance_->parameters_.command_interfaces, hardware_interface::HW_IF_EFFORT);
259257

260-
// Check if only allowed interface types are used and initialize storage to avoid memory
261-
// allocation during activation
262-
joint_state_interface_.resize(allowed_interface_types_.size());
258+
// Check if only allowed interface types are used
263259
for (const auto & interface : admittance_->parameters_.state_interfaces)
264260
{
265261
auto it =
@@ -364,37 +360,6 @@ controller_interface::CallbackReturn AdmittanceController::on_activate(
364360
return controller_interface::CallbackReturn::ERROR;
365361
}
366362

367-
// order all joints in the storage
368-
for (const auto & interface : admittance_->parameters_.state_interfaces)
369-
{
370-
auto it =
371-
std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface);
372-
auto index = static_cast<size_t>(std::distance(allowed_interface_types_.begin(), it));
373-
if (!controller_interface::get_ordered_interfaces(
374-
state_interfaces_, admittance_->parameters_.joints, interface,
375-
joint_state_interface_[index]))
376-
{
377-
RCLCPP_ERROR(
378-
get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", num_joints_,
379-
interface.c_str(), joint_state_interface_[index].size());
380-
return CallbackReturn::ERROR;
381-
}
382-
}
383-
for (const auto & interface : admittance_->parameters_.command_interfaces)
384-
{
385-
auto it =
386-
std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface);
387-
auto index = static_cast<size_t>(std::distance(allowed_interface_types_.begin(), it));
388-
if (!controller_interface::get_ordered_interfaces(
389-
command_interfaces_, command_joint_names_, interface, joint_command_interface_[index]))
390-
{
391-
RCLCPP_ERROR(
392-
get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", num_joints_,
393-
interface.c_str(), joint_command_interface_[index].size());
394-
return CallbackReturn::ERROR;
395-
}
396-
}
397-
398363
// update parameters if any have changed
399364
admittance_->apply_parameters_update();
400365

@@ -522,11 +487,6 @@ controller_interface::CallbackReturn AdmittanceController::on_deactivate(
522487
}
523488
}
524489

525-
for (size_t index = 0; index < allowed_interface_types_.size(); ++index)
526-
{
527-
joint_command_interface_[index].clear();
528-
joint_state_interface_[index].clear();
529-
}
530490
release_interfaces();
531491
admittance_->reset(num_joints_);
532492

‎effort_controllers/CMakeLists.txt

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -38,6 +38,8 @@ if(BUILD_TESTING)
3838
find_package(controller_manager REQUIRED)
3939
find_package(ros2_control_test_assets REQUIRED)
4040

41+
add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
42+
4143
ament_add_gmock(test_load_joint_group_effort_controller
4244
test/test_load_joint_group_effort_controller.cpp
4345
)

‎effort_controllers/src/joint_group_effort_controller.cpp

Lines changed: 3 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -29,26 +29,19 @@ JointGroupEffortController::JointGroupEffortController()
2929

3030
controller_interface::CallbackReturn JointGroupEffortController::on_init()
3131
{
32-
auto ret = forward_command_controller::ForwardCommandController::on_init();
33-
if (ret != controller_interface::CallbackReturn::SUCCESS)
34-
{
35-
return ret;
36-
}
37-
3832
try
3933
{
4034
// Explicitly set the interface parameter declared by the forward_command_controller
4135
// to match the value set in the JointGroupEffortController constructor.
42-
get_node()->set_parameter(
43-
rclcpp::Parameter("interface_name", hardware_interface::HW_IF_EFFORT));
36+
auto_declare<std::string>("interface_name", interface_name_);
4437
}
4538
catch (const std::exception & e)
4639
{
4740
fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
48-
return controller_interface::CallbackReturn::ERROR;
41+
return CallbackReturn::ERROR;
4942
}
5043

51-
return controller_interface::CallbackReturn::SUCCESS;
44+
return forward_command_controller::ForwardCommandController::on_init();
5245
}
5346

5447
controller_interface::CallbackReturn JointGroupEffortController::on_deactivate(

‎effort_controllers/test/config/test_joint_group_effort_controller.yaml

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
test_joint_group_effort_controller:
2+
ros__parameters:
3+
joints: ["joint1"]

‎effort_controllers/test/test_joint_group_effort_controller.cpp

Lines changed: 19 additions & 49 deletions
Original file line numberDiff line numberDiff line change
@@ -36,10 +36,14 @@ void JointGroupEffortControllerTest::SetUp()
3636

3737
void JointGroupEffortControllerTest::TearDown() { controller_.reset(nullptr); }
3838

39-
void JointGroupEffortControllerTest::SetUpController()
39+
void JointGroupEffortControllerTest::SetUpController(
40+
const std::vector<rclcpp::Parameter> & parameters)
4041
{
41-
const auto result = controller_->init(
42-
"test_joint_group_effort_controller", "", 0, "", controller_->define_custom_node_options());
42+
auto node_options = controller_->define_custom_node_options();
43+
node_options.parameter_overrides(parameters);
44+
45+
const auto result =
46+
controller_->init("test_joint_group_effort_controller", "", 0, "", node_options);
4347
ASSERT_EQ(result, controller_interface::return_type::OK);
4448

4549
std::vector<LoanedCommandInterface> command_ifs;
@@ -50,54 +54,20 @@ void JointGroupEffortControllerTest::SetUpController()
5054
executor.add_node(controller_->get_node()->get_node_base_interface());
5155
}
5256

53-
TEST_F(JointGroupEffortControllerTest, JointsParameterNotSet)
54-
{
55-
SetUpController();
56-
57-
// configure failed, 'joints' parameter not set
58-
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
59-
}
60-
61-
TEST_F(JointGroupEffortControllerTest, JointsParameterIsEmpty)
62-
{
63-
SetUpController();
64-
controller_->get_node()->set_parameter({"joints", std::vector<std::string>()});
65-
66-
// configure failed, 'joints' is empty
67-
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
68-
}
69-
7057
TEST_F(JointGroupEffortControllerTest, ConfigureAndActivateParamsSuccess)
7158
{
72-
SetUpController();
73-
controller_->get_node()->set_parameter({"joints", joint_names_});
59+
SetUpController({rclcpp::Parameter("joints", joint_names_)});
7460

7561
// configure successful
7662
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
7763
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
7864
}
7965

80-
TEST_F(JointGroupEffortControllerTest, ActivateWithWrongJointsNamesFails)
81-
{
82-
SetUpController();
83-
controller_->get_node()->set_parameter({"joints", std::vector<std::string>{"joint1", "joint4"}});
84-
85-
// activate failed, 'joint4' is not a valid joint name for the hardware
86-
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
87-
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
88-
ASSERT_EQ(controller_->on_cleanup(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
89-
90-
controller_->get_node()->set_parameter({"joints", std::vector<std::string>{"joint1", "joint2"}});
91-
92-
// activate should succeed now
93-
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
94-
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
95-
}
96-
9766
TEST_F(JointGroupEffortControllerTest, CommandSuccessTest)
9867
{
99-
SetUpController();
100-
controller_->get_node()->set_parameter({"joints", joint_names_});
68+
SetUpController({rclcpp::Parameter("joints", joint_names_)});
69+
70+
// configure successful
10171
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
10272

10373
// update successful though no command has been send yet
@@ -128,8 +98,9 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest)
12898

12999
TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest)
130100
{
131-
SetUpController();
132-
controller_->get_node()->set_parameter({"joints", joint_names_});
101+
SetUpController({rclcpp::Parameter("joints", joint_names_)});
102+
103+
// configure successful
133104
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
134105

135106
// send command with wrong number of joints
@@ -150,8 +121,9 @@ TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest)
150121

151122
TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest)
152123
{
153-
SetUpController();
154-
controller_->get_node()->set_parameter({"joints", joint_names_});
124+
SetUpController({rclcpp::Parameter("joints", joint_names_)});
125+
126+
// configure successful
155127
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
156128

157129
// update successful, no command received yet
@@ -167,8 +139,7 @@ TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest)
167139

168140
TEST_F(JointGroupEffortControllerTest, CommandCallbackTest)
169141
{
170-
SetUpController();
171-
controller_->get_node()->set_parameter({"joints", joint_names_});
142+
SetUpController({rclcpp::Parameter("joints", joint_names_)});
172143

173144
// default values
174145
ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1);
@@ -211,8 +182,7 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest)
211182

212183
TEST_F(JointGroupEffortControllerTest, StopJointsOnDeactivateTest)
213184
{
214-
SetUpController();
215-
controller_->get_node()->set_parameter({"joints", joint_names_});
185+
SetUpController({rclcpp::Parameter("joints", joint_names_)});
216186

217187
// configure successful
218188
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);

‎effort_controllers/test/test_joint_group_effort_controller.hpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -46,7 +46,7 @@ class JointGroupEffortControllerTest : public ::testing::Test
4646
void SetUp();
4747
void TearDown();
4848

49-
void SetUpController();
49+
void SetUpController(const std::vector<rclcpp::Parameter> & parameters = {});
5050

5151
protected:
5252
std::unique_ptr<FriendJointGroupEffortController> controller_;

‎effort_controllers/test/test_load_joint_group_effort_controller.cpp

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -32,10 +32,14 @@ TEST(TestLoadJointGroupVelocityController, load_controller)
3232
controller_manager::ControllerManager cm(
3333
executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager");
3434

35-
ASSERT_NE(
36-
cm.load_controller(
37-
"test_joint_group_effort_controller", "effort_controllers/JointGroupEffortController"),
38-
nullptr);
35+
const std::string test_file_path =
36+
std::string(TEST_FILES_DIRECTORY) + "/config/test_joint_group_effort_controller.yaml";
37+
38+
cm.set_parameter({"test_joint_group_effort_controller.params_file", test_file_path});
39+
cm.set_parameter(
40+
{"test_joint_group_effort_controller.type", "effort_controllers/JointGroupEffortController"});
41+
42+
ASSERT_NE(cm.load_controller("test_joint_group_effort_controller"), nullptr);
3943

4044
rclcpp::shutdown();
4145
}

‎forward_command_controller/CMakeLists.txt

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -58,6 +58,8 @@ if(BUILD_TESTING)
5858
find_package(controller_manager REQUIRED)
5959
find_package(ros2_control_test_assets REQUIRED)
6060

61+
add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
62+
6163
ament_add_gmock(test_load_forward_command_controller
6264
test/test_load_forward_command_controller.cpp
6365
)

‎forward_command_controller/src/forward_command_controller_parameters.yaml

Lines changed: 8 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,17 @@
11
forward_command_controller:
22
joints: {
33
type: string_array,
4-
default_value: [],
54
description: "Name of the joints to control",
5+
validation: {
6+
not_empty<>: []
7+
},
8+
read_only: true
69
}
710
interface_name: {
811
type: string,
9-
default_value: "",
1012
description: "Name of the interface to command",
13+
validation: {
14+
not_empty<>: []
15+
},
16+
read_only: true
1117
}

0 commit comments

Comments
 (0)