From 22f8a74ccdb1d1d50c6ccb8648542b3f055f12b2 Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Tue, 21 Sep 2021 02:01:05 -0300 Subject: [PATCH] Port to Galactic. --- .gitignore | 4 + CMakeLists.txt | 261 +++++-------------- ...ns.xml => computed_torque_controller_plugin.xml | 6 +- .../computed_torque_controller.h | 85 ------ .../computed_torque_controller.hpp | 96 +++++++ .../visibility_control.h | 56 ++++ package.xml | 91 ++----- src/computed_torque_controller.cpp | 285 +++++++++++++-------- 8 files changed, 407 insertions(+), 477 deletions(-) rename computed_torque_controller_plugins.xml => computed_torque_controller_plugin.xml (72%) delete mode 100644 include/computed_torque_controller/computed_torque_controller.h create mode 100644 include/computed_torque_controller/computed_torque_controller.hpp create mode 100644 include/computed_torque_controller/visibility_control.h diff --git a/.gitignore b/.gitignore index c1edc29..8a30844 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ devel/ +install/ logs/ build/ bin/ @@ -49,3 +50,6 @@ qtcreator-* # Catkin custom files CATKIN_IGNORE + +# Colcon custom files +COLCON_IGNORE diff --git a/CMakeLists.txt b/CMakeLists.txt index 6652c83..660815e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,217 +1,72 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.8) project(computed_torque_controller) -## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - cmake_modules - controller_interface - controller_manager - kdl_parser - roscpp - trajectory_msgs - urdf -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(rclcpp REQUIRED) +find_package(controller_interface REQUIRED) +find_package(controller_manager REQUIRED) +find_package(trajectory_msgs REQUIRED) +find_package(urdf REQUIRED) +find_package(kdl_parser REQUIRED) find_package(orocos_kdl REQUIRED) -find_package(Eigen3 REQUIRED) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# trajectory_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ +add_library(computed_torque_controller SHARED src/computed_torque_controller.cpp) +target_compile_features(computed_torque_controller PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_include_directories(computed_torque_controller PRIVATE include) +ament_target_dependencies( + computed_torque_controller + "rclcpp" + "controller_interface" + "controller_manager" + "trajectory_msgs" + "urdf" + "kdl_parser" + "orocos_kdl" +) -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(computed_torque_controller PRIVATE "COMPUTED_TORQUE_CONTROLLER_BUILDING_LIBRARY") -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) +pluginlib_export_plugin_description_file(controller_interface computed_torque_controller_plugin.xml) -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include - LIBRARIES computed_torque_controller -# CATKIN_DEPENDS cmake_modules controller_interface controller_manager kdl_parser roscpp trajectory_msgs urdf -# DEPENDS orocos_kdl Eigen3 +install( + DIRECTORY include/ + DESTINATION include ) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include - ${catkin_INCLUDE_DIRS} -# TODO: Check names of system library include directories (orocos_kdl, Eigen3) - ${orocos_kdl_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} +install( + TARGETS computed_torque_controller + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -## Declare a C++ library -add_library(${PROJECT_NAME} - src/computed_torque_controller.cpp +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories( + include ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/computed_torque_controller_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${orocos_kdl_LIBRARIES} -# ${EIGEN3_LIBRARIES} +ament_export_libraries( + computed_torque_controller ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +ament_export_targets( + export_${PROJECT_NAME} ) -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_computed_torque_controller.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +ament_package() diff --git a/computed_torque_controller_plugins.xml b/computed_torque_controller_plugin.xml similarity index 72% rename from computed_torque_controller_plugins.xml rename to computed_torque_controller_plugin.xml index 8c137af..2d77942 100644 --- a/computed_torque_controller_plugins.xml +++ b/computed_torque_controller_plugin.xml @@ -1,13 +1,13 @@ - + + base_class_type="controller_interface::ControllerInterface"> The ComputedTorqueController implements a computed torque controller in joint space for a robot with open chain dynamic model. The reference inputs (command in the ROS nomenclature) are joint positions, velocities and accelerations. This type of controller - expects an EffortJointInterface type of hardware interface. + expects an effort interface. diff --git a/include/computed_torque_controller/computed_torque_controller.h b/include/computed_torque_controller/computed_torque_controller.h deleted file mode 100644 index 5a8d61a..0000000 --- a/include/computed_torque_controller/computed_torque_controller.h +++ /dev/null @@ -1,85 +0,0 @@ -/****************************************************************************** - ROS computed_torque_controller Package - Computed Torque Controller - Copyright (C) 2013..2017 Walter Fetter Lages - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see - . - -*******************************************************************************/ - -#ifndef COMPUTED_TORQUE_CONTROLLER_COMPUTED_TORQUE_CONTROLLER -#define COMPUTED_TORQUE_CONTROLLER_COMPUTED_TORQUE_CONTROLLER - -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include -#include -#include - -namespace effort_controllers -{ - class ComputedTorqueController: public controller_interface:: - Controller - { - ros::NodeHandle node_; - - hardware_interface::EffortJointInterface *hw_; - std::vector joints_; - int nJoints_; - - ros::Subscriber sub_command_; - - KDL::Tree tree_; - KDL::Chain chain_; - KDL::ChainIdSolver_RNE *idsolver_; - - KDL::JntArray q_; - KDL::JntArray dq_; - KDL::JntArray v_; - - KDL::JntArray qr_; - KDL::JntArray dqr_; - KDL::JntArray ddqr_; - - KDL::JntArray torque_; - - KDL::Wrenches fext_; - - Eigen::MatrixXd Kp_; - Eigen::MatrixXd Kd_; - - void commandCB(const trajectory_msgs::JointTrajectoryPoint:: - ConstPtr &referencePoint); - - public: - ComputedTorqueController(void); - ~ComputedTorqueController(void); - - bool init(hardware_interface::EffortJointInterface *hw, - ros::NodeHandle &n); - void starting(const ros::Time& time); - void update(const ros::Time& time,const ros::Duration& duration); - }; -} -#endif diff --git a/include/computed_torque_controller/computed_torque_controller.hpp b/include/computed_torque_controller/computed_torque_controller.hpp new file mode 100644 index 0000000..669f53b --- /dev/null +++ b/include/computed_torque_controller/computed_torque_controller.hpp @@ -0,0 +1,96 @@ +/****************************************************************************** + ROS 2 computed_torque_controller Package + Computed Torque Controller + Copyright (C) 2013..2021 Walter Fetter Lages + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see + . + +*******************************************************************************/ + +#ifndef COMPUTED_TORQUE_CONTROLLER__COMPUTED_TORQUE_CONTROLLER_HPP_ +#define COMPUTED_TORQUE_CONTROLLER__COMPUTED_TORQUE_CONTROLLER_HPP_ + +#include +#include +#include +#include + +#include +#include + +#include + +namespace effort_controllers +{ + using CallbackReturn=rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + class ComputedTorqueController: public controller_interface::ControllerInterface + { + std::vector jointNames_; + int nJoints_; + + rclcpp::Subscription::SharedPtr sub_command_; + + std::string robotDescription_; + KDL::Chain chain_; + KDL::ChainIdSolver_RNE *idsolver_; + + KDL::JntArray q_; + KDL::JntArray dq_; + KDL::JntArray v_; + + KDL::JntArray qr_; + KDL::JntArray dqr_; + KDL::JntArray ddqr_; + + KDL::JntArray torque_; + + KDL::Wrenches fext_; + + Eigen::MatrixXd Kp_; + Eigen::MatrixXd Kd_; + + int priority_; + + void commandCB(const trajectory_msgs::msg::JointTrajectoryPoint::SharedPtr referencePoint); + void robotDescriptionCB(const std_msgs::msg::String::SharedPtr robotDescription); + + public: + COMPUTED_TORQUE_CONTROLLER_PUBLIC + ComputedTorqueController(void); + + COMPUTED_TORQUE_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration(void) const override; + + COMPUTED_TORQUE_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration(void) const override; + + COMPUTED_TORQUE_CONTROLLER_PUBLIC + CallbackReturn on_init(void) override; + + COMPUTED_TORQUE_CONTROLLER_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override; + + COMPUTED_TORQUE_CONTROLLER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override; + + COMPUTED_TORQUE_CONTROLLER_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + + COMPUTED_TORQUE_CONTROLLER_PUBLIC + controller_interface::return_type update(void) override; + }; +} +#endif diff --git a/include/computed_torque_controller/visibility_control.h b/include/computed_torque_controller/visibility_control.h new file mode 100644 index 0000000..6bac870 --- /dev/null +++ b/include/computed_torque_controller/visibility_control.h @@ -0,0 +1,56 @@ +/****************************************************************************** + ROS 2 computed_torque_controller Package + Computed Torque Controller Visibility Control + Copyright (C) 2013..2021 Walter Fetter Lages + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see + . + +*******************************************************************************/ + +#ifndef COMPUTED_TORQUE_CONTROLLER__VISIBILITY_CONTROL_H_ +#define COMPUTED_TORQUE_CONTROLLER__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 COMPUTED_TORQUE_CONTROLLER_EXPORT __attribute__ ((dllexport)) + #define COMPUTED_TORQUE_CONTROLLER_IMPORT __attribute__ ((dllimport)) + #else + #define COMPUTED_TORQUE_CONTROLLER_EXPORT __declspec(dllexport) + #define COMPUTED_TORQUE_CONTROLLER_IMPORT __declspec(dllimport) + #endif + #ifdef COMPUTED_TORQUE_CONTROLLER_BUILDING_LIBRARY + #define COMPUTED_TORQUE_CONTROLLER_PUBLIC COMPUTED_TORQUE_CONTROLLER_EXPORT + #else + #define COMPUTED_TORQUE_CONTROLLER_PUBLIC COMPUTED_TORQUE_CONTROLLER_IMPORT + #endif + #define COMPUTED_TORQUE_CONTROLLER_PUBLIC_TYPE COMPUTED_TORQUE_CONTROLLER_PUBLIC + #define COMPUTED_TORQUE_CONTROLLER_LOCAL +#else + #define COMPUTED_TORQUE_CONTROLLER_EXPORT __attribute__ ((visibility("default"))) + #define COMPUTED_TORQUE_CONTROLLER_IMPORT + #if __GNUC__ >= 4 + #define COMPUTED_TORQUE_CONTROLLER_PUBLIC __attribute__ ((visibility("default"))) + #define COMPUTED_TORQUE_CONTROLLER_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define COMPUTED_TORQUE_CONTROLLER_PUBLIC + #define COMPUTED_TORQUE_CONTROLLER_LOCAL + #endif + #define COMPUTED_TORQUE_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // COMPUTED_TORQUE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/package.xml b/package.xml index 004c57a..0fa433d 100644 --- a/package.xml +++ b/package.xml @@ -1,87 +1,26 @@ - + + computed_torque_controller - 3.1.0 - The computed_torque_controller package - - - - + 0.0.0 + The computed_torque_controller implements a computed torque controller in joint space Walter Fetter Lages - - - - - GPLv3 + ament_cmake_ros - - - - - - - - - - - Walter Fetter Lages - - - - - - - - - - - - - - - - - - - - - - - catkin - Eigen3 - cmake_modules - controller_interface - controller_manager - kdl_parser - orocos_kdl - roscpp - trajectory_msgs - urdf - Eigen3 - cmake_modules - controller_interface - controller_manager - kdl_parser - orocos_kdl - roscpp - trajectory_msgs - urdf - Eigen3 - cmake_modules - controller_interface - controller_manager - kdl_parser - orocos_kdl - roscpp - trajectory_msgs - urdf + rclcpp + controller_interface + controller_manager + trajectory_msgs + urdf + kdl_parser + orocos_kdl + ament_lint_auto + ament_lint_common - - - + ament_cmake diff --git a/src/computed_torque_controller.cpp b/src/computed_torque_controller.cpp index 0dc4574..a36154a 100644 --- a/src/computed_torque_controller.cpp +++ b/src/computed_torque_controller.cpp @@ -1,7 +1,7 @@ /****************************************************************************** - ROS computed_torque_controller Package + ROS 2 computed_torque_controller Package Computed Torque Controller - Copyright (C) 2013..2017 Walter Fetter Lages + Copyright (C) 2013..2021 Walter Fetter Lages This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -21,8 +21,11 @@ #include -#include -#include +#include +#include +#include + +#include namespace effort_controllers { @@ -30,85 +33,98 @@ namespace effort_controllers q_(0),dq_(0),v_(0),qr_(0),dqr_(0),ddqr_(0),torque_(0),fext_(0) { } - - ComputedTorqueController::~ComputedTorqueController(void) - { - sub_command_.shutdown(); - } - - bool ComputedTorqueController:: - init(hardware_interface::EffortJointInterface *hw,ros::NodeHandle &n) + + CallbackReturn ComputedTorqueController::on_init(void) { - node_=n; - hw_=hw; - - std::vector joint_names; - if(!node_.getParam("joints",joint_names)) + try { - ROS_ERROR("No 'joints' in controller. (namespace: %s)", - node_.getNamespace().c_str()); - return false; + auto_declare>("joints",jointNames_); + + auto_declare("chain.root","origin_link"); + auto_declare("chain.tip", "tool_link"); + + auto_declare("gravity.x",0.0); + auto_declare("gravity.y",0.0); + auto_declare("gravity.z",-9.8); + + auto_declare>("Kp",std::vector()); + auto_declare>("Kd",std::vector()); + + auto_declare("priority",sched_get_priority_max(SCHED_FIFO)); } - - nJoints_=joint_names.size(); - - for(int i=0; i < nJoints_;i++) - { - try - { - joints_.push_back(hw->getHandle(joint_names[i])); - } - catch(const hardware_interface::HardwareInterfaceException& e) - { - ROS_ERROR_STREAM("Exception thrown: " << e.what()); - return false; - } - } - sub_command_=node_.subscribe("command",1, - &ComputedTorqueController::commandCB, this); - - std::string robot_desc_string; - if(!node_.getParam("/robot_description",robot_desc_string)) + catch(const std::exception &e) { - ROS_ERROR("Could not find '/robot_description'."); - return false; + RCLCPP_ERROR_STREAM(node_->get_logger(),"Exception thrown in on_init() with message: " << e.what() << "\n"); + return CallbackReturn::ERROR; } - - if (!kdl_parser::treeFromString(robot_desc_string,tree_)) + + using std::placeholders::_1; + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local(); + auto robotDescriptionSubscriber=node_->create_subscription("robot_description",qos,std::bind(&ComputedTorqueController::robotDescriptionCB,this,_1)); + while(robotDescription_.empty()) { - ROS_ERROR("Failed to construct KDL tree."); - return false; + RCLCPP_WARN_SKIPFIRST_THROTTLE(node_->get_logger(),*node_->get_clock(),1000,"Waiting for robot model on /robot_description."); + rclcpp::spin_some(node_->get_node_base_interface()); } - - std::string chainRoot; - if(!node_.getParam("chain/root",chainRoot)) + + return CallbackReturn::SUCCESS; + } + + CallbackReturn ComputedTorqueController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) + { + jointNames_=node_->get_parameter("joints").as_string_array(); + if(jointNames_.empty()) { - ROS_ERROR("Could not find 'chain_root' parameter."); - return false; + RCLCPP_ERROR(node_->get_logger(),"'joints' parameter was empty,"); + return CallbackReturn::ERROR; } - - std::string chainTip; - if(!node_.getParam("chain/tip",chainTip)) + + nJoints_=jointNames_.size(); + + using std::placeholders::_1; + sub_command_=node_->create_subscription("command",1, + std::bind(&ComputedTorqueController::commandCB,this,_1)); + + while(robotDescription_.empty()) + RCLCPP_WARN_SKIPFIRST_THROTTLE(node_->get_logger(),*node_->get_clock(),1000,"Waiting for robot model on /robot_description."); + + KDL::Tree tree; + if (!kdl_parser::treeFromString(robotDescription_,tree)) { - ROS_ERROR("Could not find 'chain/tip' parameter."); - return false; + RCLCPP_ERROR(node_->get_logger(),"Failed to construct KDL tree."); + return CallbackReturn::ERROR; } - - if (!tree_.getChain(chainRoot,chainTip,chain_)) + + std::string chainRoot=node_->get_parameter("chain.root").as_string(); + if(chainRoot.empty()) { - ROS_ERROR("Failed to get chain from KDL tree."); - return false; + RCLCPP_ERROR(node_->get_logger(),"Could not find 'chain.root' parameter."); + return CallbackReturn::ERROR; } - + + std::string chainTip=node_->get_parameter("chain.tip").as_string(); + if(chainTip.empty()) + { + RCLCPP_ERROR(node_->get_logger(),"Could not find 'chain,tip' parameter."); + return CallbackReturn::ERROR; + } + + if (!tree.getChain(chainRoot,chainTip,chain_)) + { + RCLCPP_ERROR(node_->get_logger(),"Failed to get chain from KDL tree."); + return CallbackReturn::ERROR; + } + KDL::Vector g; - node_.param("gravity/x",g[0],0.0); - node_.param("gravity/y",g[1],0.0); - node_.param("gravity/z",g[2],-9.8); + g[0]=node_->get_parameter("gravity.x").get_value(); + g[1]=node_->get_parameter("gravity.y").get_value(); + g[2]=node_->get_parameter("gravity.x").get_value(); if((idsolver_=new KDL::ChainIdSolver_RNE(chain_,g)) == NULL) { - ROS_ERROR("Failed to create ChainIDSolver_RNE."); - return false; + RCLCPP_ERROR(node_->get_logger(),"Failed to create ChainIDSolver_RNE."); + return CallbackReturn::ERROR; } q_.resize(nJoints_); @@ -120,84 +136,133 @@ namespace effort_controllers torque_.resize(nJoints_); fext_.resize(chain_.getNrOfSegments()); - + Kp_.resize(nJoints_,nJoints_); Kd_.resize(nJoints_,nJoints_); - - std::vector KpVec; - if(!node_.getParam("Kp",KpVec)) + + std::vector KpVec=node_->get_parameter("Kp").as_double_array(); + if(KpVec.empty()) { - ROS_ERROR("No 'Kp' in controller %s.",node_.getNamespace().c_str()); - return false; + RCLCPP_ERROR(node_->get_logger(),"No 'Kp' in controller %s.",node_->get_namespace()); + return CallbackReturn::ERROR; } Kp_=Eigen::Map(KpVec.data(),nJoints_,nJoints_).transpose(); - - std::vector KdVec; - if(!node_.getParam("Kd",KdVec)) + + std::vector KdVec=node_->get_parameter("Kd").as_double_array(); + if(KdVec.empty()) { - ROS_ERROR("No 'Kd' in controller %s.",node_.getNamespace().c_str()); - return false; + RCLCPP_ERROR(node_->get_logger(),"No 'Kd' in controller %s.",node_->get_namespace()); + return CallbackReturn::ERROR; } Kd_=Eigen::Map(KdVec.data(),nJoints_,nJoints_).transpose(); - - return true; + + if(!node_->get_parameter("priority",priority_)) + RCLCPP_WARN(node_->get_logger(),"No 'priority' configured for controller %s. Using highest possible priority.",node_->get_namespace()); + + return CallbackReturn::SUCCESS; } - - void ComputedTorqueController::starting(const ros::Time& time) + + controller_interface::InterfaceConfiguration ComputedTorqueController::command_interface_configuration(void) const { - for(unsigned int i=0;i < nJoints_;i++) + controller_interface::InterfaceConfiguration config; + config.type=controller_interface::interface_configuration_type::INDIVIDUAL; + + for(const auto &joint : jointNames_) + config.names.push_back(joint + "/" + hardware_interface::HW_IF_EFFORT); + + return config; + } + + controller_interface::InterfaceConfiguration ComputedTorqueController::state_interface_configuration(void) const + { + controller_interface::InterfaceConfiguration config; + config.type=controller_interface::interface_configuration_type::INDIVIDUAL; + + for(const auto &joint : jointNames_) { - q_(i)=joints_[i].getPosition(); - dq_(i)=joints_[i].getVelocity(); + config.names.push_back(joint + "/" + hardware_interface::HW_IF_POSITION); + config.names.push_back(joint + "/" + hardware_interface::HW_IF_VELOCITY); + } + + return config; + } + + CallbackReturn ComputedTorqueController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) + { + for(int i=0;i < nJoints_;i++) + { + q_(i)=state_interfaces_[2*i].get_value(); + dq_(i)=state_interfaces_[2*i+1].get_value(); } qr_=q_; dqr_=dq_; SetToZero(ddqr_); - + struct sched_param param; - if(!node_.getParam("priority",param.sched_priority)) - { - ROS_WARN("No 'priority' configured for controller %s. Using highest possible priority.",node_.getNamespace().c_str()); - param.sched_priority=sched_get_priority_max(SCHED_FIFO); - } + param.sched_priority=priority_; if(sched_setscheduler(0,SCHED_FIFO,¶m) == -1) { - ROS_WARN("Failed to set real-time scheduler."); - return; + RCLCPP_WARN(node_->get_logger(),"Failed to set real-time scheduler."); + return CallbackReturn::SUCCESS; } if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) - ROS_WARN("Failed to lock memory."); + RCLCPP_WARN(node_->get_logger(),"Failed to lock memory."); + + return CallbackReturn::SUCCESS; } - - void ComputedTorqueController::update(const ros::Time& time, - const ros::Duration& duration) + + CallbackReturn ComputedTorqueController::on_deactivate(const rclcpp_lifecycle::State &/*previous_state*/) + { + for(int i=0;i < nJoints_;i++) + q_(i)=state_interfaces_[2*i].get_value(); + SetToZero(dqr_); + SetToZero(ddqr_); + + for(unsigned int i=0;i < fext_.size();i++) fext_[i].Zero(); + + if(idsolver_->CartToJnt(q_,dqr_,ddqr_,fext_,torque_) < 0) + RCLCPP_ERROR(node_->get_logger(),"KDL inverse dynamics solver failed."); + + for(int i=0;i < nJoints_;i++) + command_interfaces_[i].set_value(torque_(i)); + + return CallbackReturn::SUCCESS; + } + + controller_interface::return_type ComputedTorqueController::update(void) { - for(unsigned int i=0;i < nJoints_;i++) + for(int i=0;i < nJoints_;i++) { - q_(i)=joints_[i].getPosition(); - dq_(i)=joints_[i].getVelocity(); + q_(i)=state_interfaces_[2*i].get_value(); + dq_(i)=state_interfaces_[2*i+1].get_value(); } for(unsigned int i=0;i < fext_.size();i++) fext_[i].Zero(); - + v_.data=ddqr_.data+Kp_*(qr_.data-q_.data)+Kd_*(dqr_.data-dq_.data); - if(idsolver_->CartToJnt(q_,dq_,v_,fext_,torque_) < 0) - ROS_ERROR("KDL inverse dynamics solver failed."); - for(unsigned int i=0;i < nJoints_;i++) - joints_[i].setCommand(torque_(i)); + if(idsolver_->CartToJnt(q_,dq_,v_,fext_,torque_) < 0) + RCLCPP_ERROR(node_->get_logger(),"KDL inverse dynamics solver failed."); + + for(int i=0;i < nJoints_;i++) + command_interfaces_[i].set_value(torque_(i)); + + return controller_interface::return_type::OK; } - - void ComputedTorqueController::commandCB(const trajectory_msgs:: - JointTrajectoryPoint::ConstPtr &referencePoint) + + void ComputedTorqueController::commandCB(const trajectory_msgs::msg::JointTrajectoryPoint::SharedPtr referencePoint) { - for(unsigned int i=0;i < nJoints_;i++) + for(int i=0;i < nJoints_;i++) { qr_(i)=referencePoint->positions[i]; dqr_(i)=referencePoint->velocities[i]; ddqr_(i)=referencePoint->accelerations[i]; } } + + void ComputedTorqueController::robotDescriptionCB(const std_msgs::msg::String::SharedPtr robotDescription) + { + robotDescription_=robotDescription->data; + } } -PLUGINLIB_EXPORT_CLASS(effort_controllers::ComputedTorqueController, - controller_interface::ControllerBase) +PLUGINLIB_EXPORT_CLASS(effort_controllers::ComputedTorqueController,controller_interface::ControllerInterface) -- 2.12.0