Skip to content

Remove usage of get_ordered_interfaces but update parameter validation instead #1816

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 11 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -82,15 +82,6 @@ class AdmittanceController : public controller_interface::ChainableControllerInt
size_t num_joints_ = 0;
std::vector<std::string> command_joint_names_;

// The interfaces are defined as the types in 'allowed_interface_types_' member.
// For convenience, for each type the interfaces are ordered so that i-th position
// matches i-th index in joint_names_
template <typename T>
using InterfaceReferences = std::vector<std::vector<std::reference_wrapper<T>>>;

InterfaceReferences<hardware_interface::LoanedCommandInterface> joint_command_interface_;
InterfaceReferences<hardware_interface::LoanedStateInterface> joint_state_interface_;

bool has_position_state_interface_ = false;
bool has_velocity_state_interface_ = false;
bool has_acceleration_state_interface_ = false;
Expand Down
44 changes: 2 additions & 42 deletions admittance_controller/src/admittance_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -226,16 +226,14 @@ controller_interface::CallbackReturn AdmittanceController::on_configure(
}
}

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

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

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

// order all joints in the storage
for (const auto & interface : admittance_->parameters_.state_interfaces)
{
auto it =
std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface);
auto index = static_cast<size_t>(std::distance(allowed_interface_types_.begin(), it));
if (!controller_interface::get_ordered_interfaces(
state_interfaces_, admittance_->parameters_.joints, interface,
joint_state_interface_[index]))
{
RCLCPP_ERROR(
get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", num_joints_,
interface.c_str(), joint_state_interface_[index].size());
return CallbackReturn::ERROR;
}
}
for (const auto & interface : admittance_->parameters_.command_interfaces)
{
auto it =
std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface);
auto index = static_cast<size_t>(std::distance(allowed_interface_types_.begin(), it));
if (!controller_interface::get_ordered_interfaces(
command_interfaces_, command_joint_names_, interface, joint_command_interface_[index]))
{
RCLCPP_ERROR(
get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", num_joints_,
interface.c_str(), joint_command_interface_[index].size());
return CallbackReturn::ERROR;
}
}

// update parameters if any have changed
admittance_->apply_parameters_update();

Expand Down Expand Up @@ -522,11 +487,6 @@ controller_interface::CallbackReturn AdmittanceController::on_deactivate(
}
}

for (size_t index = 0; index < allowed_interface_types_.size(); ++index)
{
joint_command_interface_[index].clear();
joint_state_interface_[index].clear();
}
release_interfaces();
admittance_->reset(num_joints_);

Expand Down
2 changes: 2 additions & 0 deletions effort_controllers/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,8 @@ if(BUILD_TESTING)
find_package(controller_manager REQUIRED)
find_package(ros2_control_test_assets REQUIRED)

add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")

ament_add_gmock(test_load_joint_group_effort_controller
test/test_load_joint_group_effort_controller.cpp
)
Expand Down
13 changes: 3 additions & 10 deletions effort_controllers/src/joint_group_effort_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -29,26 +29,19 @@ JointGroupEffortController::JointGroupEffortController()

controller_interface::CallbackReturn JointGroupEffortController::on_init()
{
auto ret = forward_command_controller::ForwardCommandController::on_init();
if (ret != controller_interface::CallbackReturn::SUCCESS)
{
return ret;
}

try
{
// Explicitly set the interface parameter declared by the forward_command_controller
// to match the value set in the JointGroupEffortController constructor.
get_node()->set_parameter(
rclcpp::Parameter("interface_name", hardware_interface::HW_IF_EFFORT));
auto_declare<std::string>("interface_name", interface_name_);
}
catch (const std::exception & e)
{
fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
return controller_interface::CallbackReturn::ERROR;
return CallbackReturn::ERROR;
}

return controller_interface::CallbackReturn::SUCCESS;
return forward_command_controller::ForwardCommandController::on_init();
}

controller_interface::CallbackReturn JointGroupEffortController::on_deactivate(
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
test_joint_group_effort_controller:
ros__parameters:
joints: ["joint1"]
68 changes: 19 additions & 49 deletions effort_controllers/test/test_joint_group_effort_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -36,10 +36,14 @@ void JointGroupEffortControllerTest::SetUp()

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

void JointGroupEffortControllerTest::SetUpController()
void JointGroupEffortControllerTest::SetUpController(
const std::vector<rclcpp::Parameter> & parameters)
{
const auto result = controller_->init(
"test_joint_group_effort_controller", "", 0, "", controller_->define_custom_node_options());
auto node_options = controller_->define_custom_node_options();
node_options.parameter_overrides(parameters);

const auto result =
controller_->init("test_joint_group_effort_controller", "", 0, "", node_options);
ASSERT_EQ(result, controller_interface::return_type::OK);

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

TEST_F(JointGroupEffortControllerTest, JointsParameterNotSet)
{
SetUpController();

// configure failed, 'joints' parameter not set
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
}

TEST_F(JointGroupEffortControllerTest, JointsParameterIsEmpty)
{
SetUpController();
controller_->get_node()->set_parameter({"joints", std::vector<std::string>()});

// configure failed, 'joints' is empty
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
}

TEST_F(JointGroupEffortControllerTest, ConfigureAndActivateParamsSuccess)
{
SetUpController();
controller_->get_node()->set_parameter({"joints", joint_names_});
SetUpController({rclcpp::Parameter("joints", joint_names_)});

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

TEST_F(JointGroupEffortControllerTest, ActivateWithWrongJointsNamesFails)
{
SetUpController();
controller_->get_node()->set_parameter({"joints", std::vector<std::string>{"joint1", "joint4"}});

// activate failed, 'joint4' is not a valid joint name for the hardware
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
ASSERT_EQ(controller_->on_cleanup(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);

controller_->get_node()->set_parameter({"joints", std::vector<std::string>{"joint1", "joint2"}});

// activate should succeed now
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
}

TEST_F(JointGroupEffortControllerTest, CommandSuccessTest)
{
SetUpController();
controller_->get_node()->set_parameter({"joints", joint_names_});
SetUpController({rclcpp::Parameter("joints", joint_names_)});

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

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

TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest)
{
SetUpController();
controller_->get_node()->set_parameter({"joints", joint_names_});
SetUpController({rclcpp::Parameter("joints", joint_names_)});

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

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

TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest)
{
SetUpController();
controller_->get_node()->set_parameter({"joints", joint_names_});
SetUpController({rclcpp::Parameter("joints", joint_names_)});

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

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

TEST_F(JointGroupEffortControllerTest, CommandCallbackTest)
{
SetUpController();
controller_->get_node()->set_parameter({"joints", joint_names_});
SetUpController({rclcpp::Parameter("joints", joint_names_)});

// default values
ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1);
Expand Down Expand Up @@ -211,8 +182,7 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest)

TEST_F(JointGroupEffortControllerTest, StopJointsOnDeactivateTest)
{
SetUpController();
controller_->get_node()->set_parameter({"joints", joint_names_});
SetUpController({rclcpp::Parameter("joints", joint_names_)});

// configure successful
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@ class JointGroupEffortControllerTest : public ::testing::Test
void SetUp();
void TearDown();

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

protected:
std::unique_ptr<FriendJointGroupEffortController> controller_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -32,10 +32,14 @@ TEST(TestLoadJointGroupVelocityController, load_controller)
controller_manager::ControllerManager cm(
executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager");

ASSERT_NE(
cm.load_controller(
"test_joint_group_effort_controller", "effort_controllers/JointGroupEffortController"),
nullptr);
const std::string test_file_path =
std::string(TEST_FILES_DIRECTORY) + "/config/test_joint_group_effort_controller.yaml";

cm.set_parameter({"test_joint_group_effort_controller.params_file", test_file_path});
cm.set_parameter(
{"test_joint_group_effort_controller.type", "effort_controllers/JointGroupEffortController"});

ASSERT_NE(cm.load_controller("test_joint_group_effort_controller"), nullptr);

rclcpp::shutdown();
}
2 changes: 2 additions & 0 deletions forward_command_controller/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,8 @@ if(BUILD_TESTING)
find_package(controller_manager REQUIRED)
find_package(ros2_control_test_assets REQUIRED)

add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")

ament_add_gmock(test_load_forward_command_controller
test/test_load_forward_command_controller.cpp
)
Expand Down
Original file line number Diff line number Diff line change
@@ -1,11 +1,17 @@
forward_command_controller:
joints: {
type: string_array,
default_value: [],
description: "Name of the joints to control",
validation: {
not_empty<>: []
},
read_only: true
}
interface_name: {
type: string,
default_value: "",
description: "Name of the interface to command",
validation: {
not_empty<>: []
},
read_only: true
}
16 changes: 0 additions & 16 deletions forward_command_controller/src/forward_controllers_base.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -112,22 +112,6 @@ controller_interface::InterfaceConfiguration ForwardControllersBase::state_inter
controller_interface::CallbackReturn ForwardControllersBase::on_activate(
const rclcpp_lifecycle::State & /*previous_state*/)
{
// check if we have all resources defined in the "points" parameter
// also verify that we *only* have the resources defined in the "points" parameter
// ATTENTION(destogl): Shouldn't we use ordered interface all the time?
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
ordered_interfaces;
if (
!controller_interface::get_ordered_interfaces(
command_interfaces_, command_interface_types_, std::string(""), ordered_interfaces) ||
command_interface_types_.size() != ordered_interfaces.size())
{
RCLCPP_ERROR(
get_node()->get_logger(), "Expected %zu command interfaces, got %zu",
command_interface_types_.size(), ordered_interfaces.size());
return controller_interface::CallbackReturn::ERROR;
}

// reset command buffer if a command came through callback when controller was inactive
// Try to set default value in command.
// If this fails, then another command will be received soon anyways.
Expand Down
Original file line number Diff line number Diff line change
@@ -1,11 +1,17 @@
multi_interface_forward_command_controller:
joint: {
type: string,
default_value: "",
description: "Name of the joint to control",
validation: {
not_empty<>: []
},
read_only: true
}
interface_names: {
type: string_array,
default_value: [],
description: "Names of the interfaces to command",
validation: {
not_empty<>: []
},
read_only: true
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
test_forward_command_controller:
ros__parameters:
joints: ["joint1"]
interface_name: "position"
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
test_multi_interface_forward_command_controller:
ros__parameters:
joint: "joint1"
interface_names: ["position"]
Loading
Loading