From 05b9ce23f48d25cef2298c0c99603ff4b56d9544 Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Sun, 26 Sep 2021 03:59:23 -0300 Subject: [PATCH] Port to Galactic. --- CMakeLists.txt | 269 +++++-------------- .../pid_plus_gravity_controller.h | 77 ------ .../pid_plus_gravity_controller.hpp | 96 +++++++ .../visibility_control.h | 56 ++++ package.xml | 94 ++----- ...s.xml => pid_plus_gravity_controller_plugin.xml | 8 +- src/pid_plus_gravity_controller.cpp | 291 +++++++++++++-------- 7 files changed, 420 insertions(+), 471 deletions(-) delete mode 100644 include/pid_plus_gravity_controller/pid_plus_gravity_controller.h create mode 100644 include/pid_plus_gravity_controller/pid_plus_gravity_controller.hpp create mode 100644 include/pid_plus_gravity_controller/visibility_control.h rename pid_plus_gravity_controller_plugins.xml => pid_plus_gravity_controller_plugin.xml (72%) diff --git a/CMakeLists.txt b/CMakeLists.txt index ce08ca7..1742ad6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,215 +1,80 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.8) project(pid_plus_gravity_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 - control_msgs - 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(control_toolbox REQUIRED) +find_package(trajectory_msgs REQUIRED) +find_package(control_msgs REQUIRED) +find_package(urdf REQUIRED) +find_package(kdl_parser REQUIRED) find_package(orocos_kdl REQUIRED) +find_package(realtime_tools REQUIRED) + +add_library(pid_plus_gravity_controller SHARED src/pid_plus_gravity_controller.cpp) +target_compile_features(pid_plus_gravity_controller PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_include_directories(pid_plus_gravity_controller PRIVATE + $ + $) +ament_target_dependencies( + pid_plus_gravity_controller + "rclcpp" + "controller_interface" + "controller_manager" + "control_toolbox" + "trajectory_msgs" + "control_msgs" + "urdf" + "kdl_parser" + "orocos_kdl" + "realtime_tools" +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(pid_plus_gravity_controller PRIVATE "PID_PLUS_GRAVITY_CONTROLLER_BUILDING_LIBRARY") -## 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 -# control_msgs# trajectory_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## 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 - -## 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 pid_plus_gravity_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 pid_plus_gravity_controller - CATKIN_DEPENDS cmake_modules control_msgs controller_interface controller_manager kdl_parser roscpp trajectory_msgs urdf - DEPENDS orocos_kdl +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) - ${orocos_kdl_INCLUDE_DIRS} +install( + TARGETS pid_plus_gravity_controller + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -## Declare a C++ library -add_library(${PROJECT_NAME} - src/pid_plus_gravity_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 -## einther 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/pid_plus_gravity_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} +ament_export_libraries( + pid_plus_gravity_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_pid_plus_gravity_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/include/pid_plus_gravity_controller/pid_plus_gravity_controller.h b/include/pid_plus_gravity_controller/pid_plus_gravity_controller.h deleted file mode 100644 index 1732cea..0000000 --- a/include/pid_plus_gravity_controller/pid_plus_gravity_controller.h +++ /dev/null @@ -1,77 +0,0 @@ -/****************************************************************************** - ROS pid_plus_gravity_controller Package - PID+Gravity Compensation Controller - Copyright (C) 2017,2018 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 PID_PLUS_GRAVITY_CONTROLLER_PID_PLUS_GRAVITY_CONTROLLER_H -#define PID_PLUS_GRAVITY_CONTROLLER_PID_PLUS_GRAVITY_CONTROLLER_H - -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include - -namespace effort_controllers -{ - class PidPlusGravityController: public controller_interface:: - Controller - { - ros::NodeHandle node_; - - hardware_interface::EffortJointInterface *hw_; - std::vector joints_; - - ros::Subscriber sub_command_; - - KDL::Tree tree_; - KDL::Chain chain_; - KDL::ChainDynParam *chainParam_; - - KDL::JntArray q_; - KDL::JntArray qr_; - KDL::JntArray gravity_; - - std::vector pid_; - - void commandCB(const trajectory_msgs::JointTrajectoryPoint::ConstPtr - &referencePoint); - - public: - PidPlusGravityController(void); - ~PidPlusGravityController(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/pid_plus_gravity_controller/pid_plus_gravity_controller.hpp b/include/pid_plus_gravity_controller/pid_plus_gravity_controller.hpp new file mode 100644 index 0000000..1f1c4bc --- /dev/null +++ b/include/pid_plus_gravity_controller/pid_plus_gravity_controller.hpp @@ -0,0 +1,96 @@ +/****************************************************************************** + ROS 2 pid_plus_gravity_controller Package + PID+Gravity Compensation Controller + Copyright (C) 2017..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 PID_PLUS_GRAVITY_CONTROLLER__PID_PLUS_GRAVITY_CONTROLLER_HPP_ +#define PID_PLUS_GRAVITY_CONTROLLER__PID_PLUS_GRAVITY_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include + +namespace effort_controllers +{ + + using CallbackReturn=rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + class PidPlusGravityController: public controller_interface::ControllerInterface + { + std::vector jointNames_; + unsigned int nJoints_; + + rclcpp::Subscription::SharedPtr sub_command_; + + std::string robotDescription_; + KDL::Chain chain_; + std::unique_ptr chainParam_; + + KDL::JntArray q_; + KDL::JntArray qr_; + KDL::JntArray gravity_; + + std::vector> pid_; + + int priority_; + + rclcpp::Time lastTime_; + + realtime_tools::RealtimeBuffer> referencePoint_; + + void commandCB(const trajectory_msgs::msg::JointTrajectoryPoint::SharedPtr referencePoint); + void robotDescriptionCB(const std_msgs::msg::String::SharedPtr robotDescription); + + public: + PID_PLUS_GRAVITY_CONTROLLER_PUBLIC + PidPlusGravityController(void); + + PID_PLUS_GRAVITY_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration(void) const override; + + PID_PLUS_GRAVITY_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration(void) const override; + + PID_PLUS_GRAVITY_CONTROLLER_PUBLIC + CallbackReturn on_init(void) override; + + PID_PLUS_GRAVITY_CONTROLLER_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override; + + PID_PLUS_GRAVITY_CONTROLLER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override; + + PID_PLUS_GRAVITY_CONTROLLER_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + + PID_PLUS_GRAVITY_CONTROLLER_PUBLIC + controller_interface::return_type update(void) override; + }; +} +#endif diff --git a/include/pid_plus_gravity_controller/visibility_control.h b/include/pid_plus_gravity_controller/visibility_control.h new file mode 100644 index 0000000..89f9133 --- /dev/null +++ b/include/pid_plus_gravity_controller/visibility_control.h @@ -0,0 +1,56 @@ +/****************************************************************************** + ROS 2 pid_plus_gravity_controller Package + PID+Gravity Compensation Controller Visibility Control + Copyright (C) 2017..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 PID_PLUS_GRAVITY_CONTROLLER__VISIBILITY_CONTROL_H_ +#define PID_PLUS_GRAVITY_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 PID_PLUS_GRAVITY_CONTROLLER_EXPORT __attribute__ ((dllexport)) + #define PID_PLUS_GRAVITY_CONTROLLER_IMPORT __attribute__ ((dllimport)) + #else + #define PID_PLUS_GRAVITY_CONTROLLER_EXPORT __declspec(dllexport) + #define PID_PLUS_GRAVITY_CONTROLLER_IMPORT __declspec(dllimport) + #endif + #ifdef PID_PLUS_GRAVITY_CONTROLLER_BUILDING_LIBRARY + #define PID_PLUS_GRAVITY_CONTROLLER_PUBLIC PID_PLUS_GRAVITY_CONTROLLER_EXPORT + #else + #define PID_PLUS_GRAVITY_CONTROLLER_PUBLIC PID_PLUS_GRAVITY_CONTROLLER_IMPORT + #endif + #define PID_PLUS_GRAVITY_CONTROLLER_PUBLIC_TYPE PID_PLUS_GRAVITY_CONTROLLER_PUBLIC + #define PID_PLUS_GRAVITY_CONTROLLER_LOCAL +#else + #define PID_PLUS_GRAVITY_CONTROLLER_EXPORT __attribute__ ((visibility("default"))) + #define PID_PLUS_GRAVITY_CONTROLLER_IMPORT + #if __GNUC__ >= 4 + #define PID_PLUS_GRAVITY_CONTROLLER_PUBLIC __attribute__ ((visibility("default"))) + #define PID_PLUS_GRAVITY_CONTROLLER_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define PID_PLUS_GRAVITY_CONTROLLER_PUBLIC + #define PID_PLUS_GRAVITY_CONTROLLER_LOCAL + #endif + #define PID_PLUS_GRAVITY_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // PID_PLUS_GRAVITY_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/package.xml b/package.xml index 62aa0a4..0d5e663 100644 --- a/package.xml +++ b/package.xml @@ -1,87 +1,29 @@ - + + pid_plus_gravity_controller - 1.1.0 - The pid_plus_gravity_controller package - - - - + 2.0.0 + A PID controller with gravity compensation Walter Fetter Lages - - - - - GPLv3 + ament_cmake_ros - - - - - - - - - - - Walter Fetter Lages - - - - - - - - - - - - - - - - - - - - - - - catkin - cmake_modules - control_msgs - controller_interface - controller_manager - kdl_parser - orocos_kdl - roscpp - trajectory_msgs - urdf - cmake_modules - control_msgs - controller_interface - controller_manager - kdl_parser - orocos_kdl - roscpp - trajectory_msgs - urdf - cmake_modules - control_msgs - controller_interface - controller_manager - kdl_parser - orocos_kdl - roscpp - trajectory_msgs - urdf + rclcpp + controller_interface + controller_manager + control_toolbox + trajectory_msgs + control_msgs + urdf + kdl_parser + orocos_kdl + realtime_tools + ament_lint_auto + ament_lint_common - - - + ament_cmake diff --git a/pid_plus_gravity_controller_plugins.xml b/pid_plus_gravity_controller_plugin.xml similarity index 72% rename from pid_plus_gravity_controller_plugins.xml rename to pid_plus_gravity_controller_plugin.xml index 5654593..dcedd9d 100644 --- a/pid_plus_gravity_controller_plugins.xml +++ b/pid_plus_gravity_controller_plugin.xml @@ -1,8 +1,7 @@ - - + + type="effort_controllers::PidPlusGravityController" + base_class_type="controller_interface::ControllerInterface"> The PidPlusGravityController implements a PID controller with gravity compensation in joint space for a robot with open chain dynamic model. @@ -11,5 +10,4 @@ type of hardware interface. - diff --git a/src/pid_plus_gravity_controller.cpp b/src/pid_plus_gravity_controller.cpp index 1748753..1834e8e 100644 --- a/src/pid_plus_gravity_controller.cpp +++ b/src/pid_plus_gravity_controller.cpp @@ -1,7 +1,7 @@ /****************************************************************************** - ROS pid_plus_gravity_controller Package + ROS 2 pid_plus_gravity_controller Package PID+Gravity Compensation Controller - Copyright (C) 2017 Walter Fetter Lages + Copyright (C) 2017..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,155 +21,224 @@ #include -#include -#include +#include +#include +#include + +#include namespace effort_controllers { PidPlusGravityController::PidPlusGravityController(void): - q_(0),qr_(0),gravity_(0) + q_(0),qr_(0),gravity_(0),referencePoint_(nullptr) { } - - PidPlusGravityController::~PidPlusGravityController(void) + + CallbackReturn PidPlusGravityController::on_init(void) { - sub_command_.shutdown(); + try + { + 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("priority",sched_get_priority_max(SCHED_FIFO)); + } + catch(const std::exception &e) + { + RCLCPP_ERROR_STREAM(node_->get_logger(),"Exception thrown in on_init() with message: " << e.what()); + return CallbackReturn::ERROR; + } + + using std::placeholders::_1; + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local(); + auto robotDescriptionSubscriber=node_->create_subscription("robot_description",qos,std::bind(&PidPlusGravityController::robotDescriptionCB,this,_1)); + while(robotDescription_.empty()) + { + 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()); + } + + return CallbackReturn::SUCCESS; } - - bool PidPlusGravityController:: - init(hardware_interface::EffortJointInterface *hw,ros::NodeHandle &n) + + CallbackReturn PidPlusGravityController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { - node_=n; - hw_=hw; - - std::vector joint_names; - if(!node_.getParam("joints",joint_names)) + jointNames_=node_->get_parameter("joints").as_string_array(); + if(jointNames_.empty()) { - ROS_ERROR("No 'joints' in controller. (namespace: %s)", - node_.getNamespace().c_str()); - return false; + RCLCPP_ERROR(node_->get_logger(),"'joints' parameter was empty,"); + return CallbackReturn::ERROR; } - pid_.resize(joint_names.size()); + nJoints_=jointNames_.size(); - for(int i=0; i < joint_names.size();i++) + try { - try - { - joints_.push_back(hw->getHandle(joint_names[i])); - } - catch(const hardware_interface::HardwareInterfaceException& e) - { - ROS_ERROR_STREAM("Exception thrown: " << e.what()); - return false; - } - if(!pid_[i].init(ros::NodeHandle(node_,joint_names[i]))) + for(const auto &joint : jointNames_) { - ROS_ERROR_STREAM("Failed to load PID parameters from " << joint_names[i] + "/pid"); - return false; + auto pid=std::make_shared(node_,joint); + pid->initPid(); + pid_.push_back(pid); } } - - sub_command_=node_.subscribe("command",1, - &PidPlusGravityController::commandCB, this); - - std::string robot_desc_string; - if(!node_.getParam("/robot_description",robot_desc_string)) - { - ROS_ERROR("Could not find '/robot_description'."); - return false; + catch(const std::exception &e) + { + RCLCPP_ERROR_STREAM(node_->get_logger(),"Exception thrown while creating PidROS: " << e.what()); + return CallbackReturn::ERROR; } - - if (!kdl_parser::treeFromString(robot_desc_string,tree_)) + + using std::placeholders::_1; + sub_command_=node_->create_subscription("command",1, + std::bind(&PidPlusGravityController::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("Failed to construct KDL tree."); - return false; + RCLCPP_ERROR(node_->get_logger(),"Failed to construct KDL tree."); + return CallbackReturn::ERROR; } - - std::string chainRoot; - if(!node_.getParam("chain/root",chainRoot)) - { - ROS_ERROR("Could not find 'chain_root' parameter."); - return false; + + std::string chainRoot=node_->get_parameter("chain.root").as_string(); + if(chainRoot.empty()) + { + RCLCPP_ERROR(node_->get_logger(),"Could not find 'chain.root' parameter."); + return CallbackReturn::ERROR; } - - std::string chainTip; - if(!node_.getParam("chain/tip",chainTip)) - { - ROS_ERROR("Could not find 'chain/tip' parameter."); - return false; + + 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_)) + + if (!tree.getChain(chainRoot,chainTip,chain_)) { - ROS_ERROR("Failed to get chain from KDL tree."); - return false; + 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); - - if((chainParam_=new KDL::ChainDynParam(chain_,g)) == NULL) + 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(); + + try { - ROS_ERROR("Failed to create ChainDynParam."); - return false; + chainParam_=std::make_unique(chain_,g); + } + catch(const std::exception &e) + { + RCLCPP_ERROR_STREAM(node_->get_logger(),"Exception thrown while creating ChainDynParam: " << e.what()); + return CallbackReturn::ERROR; } q_.resize(chain_.getNrOfJoints()); qr_.resize(chain_.getNrOfJoints()); gravity_.resize(chain_.getNrOfJoints()); - - return true; + + if(!node_->get_parameter("priority",priority_)) + RCLCPP_WARN(node_->get_logger(),"No 'priority' configured for controller. Using highest possible priority."); + + return CallbackReturn::SUCCESS; } - - void PidPlusGravityController::starting(const ros::Time& time) + + controller_interface::InterfaceConfiguration PidPlusGravityController::command_interface_configuration(void) const { - for(unsigned int i=0;i < joints_.size();i++) - q_(i)=joints_[i].getPosition(); + 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 PidPlusGravityController::state_interface_configuration(void) const + { + 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_POSITION); + + return config; + } + + CallbackReturn PidPlusGravityController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) + { + for(unsigned int i=0;i < nJoints_;i++) + q_(i)=state_interfaces_[i].get_value(); qr_=q_; - + 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); - } - if(sched_setscheduler(0,SCHED_FIFO,¶m) == -1) - { - ROS_WARN("Failed to set real-time scheduler."); - return; - } - if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) - ROS_WARN("Failed to lock memory."); + param.sched_priority=priority_; + if(sched_setscheduler(0,SCHED_FIFO,¶m) == -1) + RCLCPP_WARN(node_->get_logger(),"Failed to set real-time scheduler."); + else if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) + RCLCPP_WARN(node_->get_logger(),"Failed to lock memory."); + + lastTime_=node_->get_clock()->now(); + + return CallbackReturn::SUCCESS; } - - void PidPlusGravityController::update(const ros::Time& time, - const ros::Duration& duration) + + CallbackReturn PidPlusGravityController::on_deactivate(const rclcpp_lifecycle::State &/*previous_state*/) { - for(unsigned int i=0;i < joints_.size();i++) - q_(i)=joints_[i].getPosition(); - + for(unsigned int i=0;i < nJoints_;i++) + q_(i)=state_interfaces_[i].get_value(); + if(chainParam_->JntToGravity(q_,gravity_) < 0) - ROS_ERROR("KDL dynamics solver failed."); - - for(unsigned int i=0;i < joints_.size();i++) - joints_[i].setCommand(gravity_(i)+pid_[i].computeCommand(qr_(i)-q_(i),duration)); + RCLCPP_ERROR(node_->get_logger(),"KDL dynamics solver failed."); + + auto time=node_->get_clock()->now(); + for(unsigned int i=0;i < nJoints_;i++) + command_interfaces_[i].set_value(gravity_(i)+pid_[i]->computeCommand(0,time-lastTime_)); + lastTime_=time; + + return CallbackReturn::SUCCESS; } - - void PidPlusGravityController::commandCB(const trajectory_msgs:: - JointTrajectoryPoint::ConstPtr &referencePoint) + + controller_interface::return_type PidPlusGravityController::update(void) { - if(referencePoint->positions.size()!=qr_.rows()) - { - ROS_ERROR_STREAM("Dimension of command (" << referencePoint->positions.size() << ") does not match number of joints (" << qr_.rows() << ")!"); - return; - } - for(unsigned int i=0;i < qr_.rows();i++) - qr_(i)=referencePoint->positions[i]; + auto referencePoint=referencePoint_.readFromRT(); + if(referencePoint && *referencePoint) + if((*referencePoint)->positions.size() == nJoints_) + qr_.data=Eigen::VectorXd::Map((*referencePoint)->positions.data(),nJoints_); + + for(unsigned int i=0;i < nJoints_;i++) + q_(i)=state_interfaces_[i].get_value(); + + if(chainParam_->JntToGravity(q_,gravity_) < 0) + RCLCPP_ERROR(node_->get_logger(),"KDL dynamics solver failed."); + + auto time=node_->get_clock()->now(); + for(unsigned int i=0;i < nJoints_;i++) + command_interfaces_[i].set_value(gravity_(i)+pid_[i]->computeCommand(qr_(i)-q_(i),time-lastTime_)); + lastTime_=time; + + return controller_interface::return_type::OK; + } + + void PidPlusGravityController::commandCB(const trajectory_msgs::msg::JointTrajectoryPoint::SharedPtr referencePoint) + { + referencePoint_.writeFromNonRT(referencePoint); + } + + void PidPlusGravityController::robotDescriptionCB(const std_msgs::msg::String::SharedPtr robotDescription) + { + robotDescription_=robotDescription->data; } } -PLUGINLIB_EXPORT_CLASS(effort_controllers::PidPlusGravityController, - controller_interface::ControllerBase) +PLUGINLIB_EXPORT_CLASS(effort_controllers::PidPlusGravityController,controller_interface::ControllerInterface) -- 2.12.0