From eef0a59a23b96c4dae12315ca6a1399a7b8d9c54 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 24 Mar 2022 17:16:28 +0100 Subject: [PATCH] Fix all bugs in chained-controllers mode. --- .../forward_controllers_base.hpp | 4 ++- .../src/chainable_forward_controller.cpp | 22 ++++++++++----- .../src/forward_controller.cpp | 2 +- .../src/forward_controllers_base.cpp | 8 ++++-- .../joint_trajectory_controller.hpp | 1 + .../src/joint_trajectory_controller.cpp | 28 +++++++++++++++---- 6 files changed, 49 insertions(+), 16 deletions(-) diff --git a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp index 45e0691ee7..3d9ed5d625 100644 --- a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp @@ -59,7 +59,9 @@ class ForwardControllersBase CallbackReturn execute_init(const std::shared_ptr & node); FORWARD_COMMAND_CONTROLLER_PUBLIC - CallbackReturn execute_configure(const rclcpp_lifecycle::State & previous_state); + CallbackReturn execute_configure( + const rclcpp_lifecycle::State & previous_state, + std::vector & command_interfaces); FORWARD_COMMAND_CONTROLLER_PUBLIC CallbackReturn execute_activate( diff --git a/forward_command_controller/src/chainable_forward_controller.cpp b/forward_command_controller/src/chainable_forward_controller.cpp index 21452d236c..3f08bae922 100644 --- a/forward_command_controller/src/chainable_forward_controller.cpp +++ b/forward_command_controller/src/chainable_forward_controller.cpp @@ -37,7 +37,8 @@ CallbackReturn ChainableForwardController::on_init() { return execute_init(get_n CallbackReturn ChainableForwardController::on_configure( const rclcpp_lifecycle::State & previous_state) { - auto ret = execute_configure(previous_state); + auto ret = execute_configure(previous_state, command_interfaces_); + RCLCPP_ERROR(get_node()->get_logger(), "Configure is successful, now tryting configure ref_itfs"); if (ret != CallbackReturn::SUCCESS) { return ret; @@ -47,7 +48,12 @@ CallbackReturn ChainableForwardController::on_configure( reference_interface_names_ = command_interface_names_; // for any case make reference interfaces size of command interfaces reference_interfaces_.resize( - command_interfaces_.size(), std::numeric_limits::quiet_NaN()); + reference_interface_names_.size(), std::numeric_limits::quiet_NaN()); + + for (const auto & value : reference_interfaces_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Reference interface value is %f", value); + } return CallbackReturn::SUCCESS; } @@ -93,8 +99,9 @@ CallbackReturn ChainableForwardController::on_activate( return ret; } - reference_interfaces_.resize( - command_interfaces_.size(), std::numeric_limits::quiet_NaN()); + std::fill( + reference_interfaces_.begin(), reference_interfaces_.end(), + std::numeric_limits::quiet_NaN()); return CallbackReturn::SUCCESS; } @@ -117,14 +124,15 @@ controller_interface::return_type ChainableForwardController::update( reference_interfaces_ = (*joint_commands)->data; } } - RCLCPP_ERROR_THROTTLE( - get_node()->get_logger(), *(get_node()->get_clock()), 2000, "Reference interface value is %f", - reference_interfaces_[0]); for (size_t i = 0; i < command_interfaces_.size(); ++i) { if (!std::isnan(reference_interfaces_[i])) { + RCLCPP_ERROR( + get_node()->get_logger(), "Reference interface value is %f; Command interface name is '%s'", + reference_interfaces_[i], command_interfaces_[i].get_full_name().c_str()); + command_interfaces_[i].set_value(reference_interfaces_[i]); } } diff --git a/forward_command_controller/src/forward_controller.cpp b/forward_command_controller/src/forward_controller.cpp index 1667ccb9c8..0574ce7b5c 100644 --- a/forward_command_controller/src/forward_controller.cpp +++ b/forward_command_controller/src/forward_controller.cpp @@ -39,7 +39,7 @@ CallbackReturn ForwardController::on_init() { return execute_init(get_node()); } CallbackReturn ForwardController::on_configure(const rclcpp_lifecycle::State & previous_state) { - return execute_configure(previous_state); + return execute_configure(previous_state, command_interfaces_); } CallbackReturn ForwardController::on_activate(const rclcpp_lifecycle::State & previous_state) diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp index 658a77d1ae..7314086e0f 100644 --- a/forward_command_controller/src/forward_controllers_base.cpp +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -51,12 +51,13 @@ CallbackReturn ForwardControllersBase::execute_init( } CallbackReturn ForwardControllersBase::execute_configure( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::State & /*previous_state*/, + std::vector & command_interfaces) { auto ret = this->read_parameters(); if (ret != CallbackReturn::SUCCESS) { - RCLCPP_ERROR(get_node()->get_logger(), "Error when reading parameters."); + RCLCPP_ERROR(node_->get_logger(), "Error when reading parameters."); return ret; } @@ -69,6 +70,9 @@ CallbackReturn ForwardControllersBase::execute_configure( } }); + // pre-reserve command interfaces + command_interfaces.reserve(command_interface_names_.size()); + RCLCPP_INFO(node_->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; } 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 f0e74da0c6..349d75da40 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 @@ -108,6 +108,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa protected: std::vector joint_names_; + std::vector command_joint_names_; std::vector command_interface_types_; std::vector state_interface_types_; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 9a43d9195e..e7e433f105 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -56,6 +56,7 @@ CallbackReturn JointTrajectoryController::on_init() { // with the lifecycle node being initialized, we can declare parameters auto_declare>("joints", joint_names_); + auto_declare>("command_joints", command_joint_names_); auto_declare>("command_interfaces", command_interface_types_); auto_declare>("state_interfaces", state_interface_types_); auto_declare("state_publish_rate", 50.0); @@ -79,8 +80,8 @@ JointTrajectoryController::command_interface_configuration() const { controller_interface::InterfaceConfiguration conf; conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; - conf.names.reserve(joint_names_.size() * command_interface_types_.size()); - for (const auto & joint_name : joint_names_) + conf.names.reserve(command_joint_names_.size() * command_interface_types_.size()); + for (const auto & joint_name : command_joint_names_) { for (const auto & interface_type : command_interface_types_) { @@ -436,6 +437,7 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S // update parameters joint_names_ = node_->get_parameter("joints").as_string_array(); + // TODO(destogl): why is this here? Add comment or move if (!reset()) { return CallbackReturn::FAILURE; @@ -443,9 +445,25 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S if (joint_names_.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."); } + command_joint_names_ = get_node()->get_parameter("command_joints").as_string_array(); + + if (command_joint_names_.empty()) + { + command_joint_names_ = joint_names_; + RCLCPP_INFO( + logger, "No specific joint names are used for command interfaces. Using 'joints' parameter."); + } + else if (command_joint_names_.size() != joint_names_.size()) + { + RCLCPP_ERROR( + logger, "'command_joints' parameter has to have the same size as 'joints' parameter."); + return CallbackReturn::FAILURE; + } + // Specialized, child controllers set interfaces before calling configure function. if (command_interface_types_.empty()) { @@ -731,11 +749,11 @@ CallbackReturn JointTrajectoryController::on_activate(const rclcpp_lifecycle::St 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_, joint_names_, interface, joint_command_interface_[index])) + command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) { RCLCPP_ERROR( - node_->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", joint_names_.size(), - interface.c_str(), joint_command_interface_[index].size()); + node_->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", + command_joint_names_.size(), interface.c_str(), joint_command_interface_[index].size()); return CallbackReturn::ERROR; } }