From 7bfffc91e13b2d7fc834f93886285f30b6eabfc0 Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Fri, 18 Mar 2022 15:59:53 -0300 Subject: [PATCH] Port to Galactic. --- .gitignore | 5 +- CMakeLists.txt | 273 +++-------- .../dynamics_linearizing_controller.h | 93 ---- .../dynamics_linearizing_controller.hpp | 106 +++++ .../twist_mrac_linearizing_controller.h | 97 ---- .../twist_mrac_linearizing_controller.hpp | 110 +++++ .../linearizing_controllers/visibility_control.h | 35 ++ linearizing_controllers_plugins.xml | 9 +- package.xml | 95 +--- scripts/accel_step.py | 59 +-- scripts/accel_step.sh | 2 +- scripts/twist_step.py | 59 +-- scripts/twist_step.sh | 2 +- src/dynamics_linearizing_controller.cpp | 458 +++++++++--------- src/twist_mrac_linearizing_controller.cpp | 513 +++++++++++---------- 15 files changed, 927 insertions(+), 989 deletions(-) delete mode 100644 include/linearizing_controllers/dynamics_linearizing_controller.h create mode 100644 include/linearizing_controllers/dynamics_linearizing_controller.hpp delete mode 100644 include/linearizing_controllers/twist_mrac_linearizing_controller.h create mode 100644 include/linearizing_controllers/twist_mrac_linearizing_controller.hpp create mode 100644 include/linearizing_controllers/visibility_control.h diff --git a/.gitignore b/.gitignore index c1edc29..c2a0365 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,4 @@ -devel/ +install/ logs/ build/ bin/ @@ -49,3 +49,6 @@ qtcreator-* # Catkin custom files CATKIN_IGNORE + +# Colcon custom files +COLCON_IGNORE diff --git a/CMakeLists.txt b/CMakeLists.txt index cc408f6..8460f0c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,215 +1,88 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.8) project(linearizing_controllers) -## 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 - arc_odometry - controller_interface - effort_controllers - geometry_msgs - linearizing_controllers_msgs - roscpp - tf -) - -## 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(arc_odometry REQUIRED) +find_package(controller_interface REQUIRED) +find_package(controller_manager REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(linearizing_controllers_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(realtime_tools REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_msgs REQUIRED) find_package(Eigen3 REQUIRED) +add_library(linearizing_controllers SHARED + src/dynamics_linearizing_controller.cpp + src/twist_mrac_linearizing_controller.cpp) +target_compile_features(linearizing_controllers PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_include_directories(linearizing_controllers PRIVATE include + $) +ament_target_dependencies( + linearizing_controllers + "rclcpp" + "arc_odometry" + "controller_interface" + "controller_manager" + "geometry_msgs" + "linearizing_controllers_msgs" + "nav_msgs" + "realtime_tools" + "tf2" + "tf2_msgs" + "tf2_ros" + "Eigen3" +) -## 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 -# geometry_msgs# linearizing_controllers_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 +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(linearizing_controllers PRIVATE "LINEARIZING_CONTROLLERS_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 linearizing_controllers_plugins.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 linearizing_controllers - CATKIN_DEPENDS arc_odometry controller_interface effort_controllers geometry_msgs linearizing_controllers_msgs roscpp tf - DEPENDS 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 (Eigen3) - ${EIGEN3_INCLUDE_DIRS} +install( + TARGETS linearizing_controllers + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) - -## Declare a C++ library -add_library(${PROJECT_NAME} - src/dynamics_linearizing_controller.cpp - src/twist_mrac_linearizing_controller.cpp +install(PROGRAMS scripts/accel_step.py scripts/accel_step.sh scripts/twist_step.py scripts/twist_step.sh + DESTINATION lib/${PROJECT_NAME} ) -## 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/linearizing_controllers_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} -# ${Eigen3_LIBRARIES} +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 ) - -############# -## 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_libraries( + linearizing_controllers ) - -## Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE +ament_export_targets( + export_${PROJECT_NAME} ) -## 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_linearizing_controllers.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/linearizing_controllers/dynamics_linearizing_controller.h b/include/linearizing_controllers/dynamics_linearizing_controller.h deleted file mode 100644 index b738ab9..0000000 --- a/include/linearizing_controllers/dynamics_linearizing_controller.h +++ /dev/null @@ -1,93 +0,0 @@ -/****************************************************************************** - ROS linearizing_controllers Package - Dynamics Linearizing Controller - Copyright (C) 2014..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 LINEARIZING_CONTROLLERS_DYNAMICS_LINEARIZING_CONTROLLER_H -#define LINEARIZING_CONTROLLERS_DYNAMICS_LINEARIZING_CONTROLLER_H - -#include -#include -#include - -#include -#include - -#include - -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -namespace effort_controllers -{ - class DynamicsLinearizingController: public controller_interface:: - Controller - { - public: - DynamicsLinearizingController(void); - ~DynamicsLinearizingController(void); - - bool init(hardware_interface::EffortJointInterface *robot, - ros::NodeHandle &n); - void starting(const ros::Time& time); - void update(const ros::Time& time,const ros::Duration& duration); - - private: - ros::NodeHandle node_; - hardware_interface::EffortJointInterface *robot_; - std::vector joints_; - - boost::scoped_ptr - > status_publisher_ ; - - boost::shared_ptr > odom_publisher_; - boost::shared_ptr > tf_odom_publisher_; - - ros::Subscriber sub_command_; - ros::Subscriber sub_parameters_; - - Eigen::Matrix2d Ginv_; - Eigen::Matrix2d F_; - - arc_odometry::DiffOdometry odom_; - - Eigen::Vector2d v_; - - double time_step_; - ros::Time lastSamplingTime_; - - Eigen::Vector2d phi_; - - void commandCB(const geometry_msgs::Accel &command); - void parametersCB(const std_msgs::Float64MultiArray &command); - }; -} -#endif diff --git a/include/linearizing_controllers/dynamics_linearizing_controller.hpp b/include/linearizing_controllers/dynamics_linearizing_controller.hpp new file mode 100644 index 0000000..4b64829 --- /dev/null +++ b/include/linearizing_controllers/dynamics_linearizing_controller.hpp @@ -0,0 +1,106 @@ +/****************************************************************************** + ROS 2 linearizing_controllers Package + Dynamics Linearizing Controller + Copyright (C) 2014..2022 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 LINEARIZING_CONTROLLERS__DYNAMICS_LINEARIZING_CONTROLLER_HPP_ +#define LINEARIZING_CONTROLLERS__DYNAMICS_LINEARIZING_CONTROLLER_HPP_ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include + +namespace effort_controllers +{ + using CallbackReturn=rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + class DynamicsLinearizingController: public controller_interface::ControllerInterface + { + public: + LINEARIZING_CONTROLLERS_PUBLIC + DynamicsLinearizingController(void); + + LINEARIZING_CONTROLLERS_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration(void) const override; + + LINEARIZING_CONTROLLERS_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration(void) const override; + + LINEARIZING_CONTROLLERS_PUBLIC + CallbackReturn on_init(void) override; + + LINEARIZING_CONTROLLERS_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override; + + LINEARIZING_CONTROLLERS_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override; + + LINEARIZING_CONTROLLERS_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + + LINEARIZING_CONTROLLERS_PUBLIC + controller_interface::return_type update(void) override; + + private: + std::vector joints_; + + std::shared_ptr> status_publisher_; + std::shared_ptr> rt_status_publisher_; + + std::shared_ptr> odom_publisher_; + std::shared_ptr> rt_odom_publisher_; + + std::shared_ptr> tf_odom_publisher_; + std::shared_ptr> rt_tf_odom_publisher_; + + rclcpp::Subscription::SharedPtr sub_command_; + rclcpp::Subscription::SharedPtr sub_parameters_; + + Eigen::Matrix2d Ginv_; + Eigen::Matrix2d F_; + + arc_odometry::DiffOdometry odom_; + + Eigen::Vector2d v_; + + double time_step_; + rclcpp::Time lastSamplingTime_; + + Eigen::Vector2d phi_; + + int priority_; + + realtime_tools::RealtimeBuffer> rt_command_; + realtime_tools::RealtimeBuffer> rt_param_; + + void commandCB(const geometry_msgs::msg::Accel::SharedPtr command); + void parametersCB(const std_msgs::msg::Float64MultiArray::SharedPtr param); + }; +} +#endif diff --git a/include/linearizing_controllers/twist_mrac_linearizing_controller.h b/include/linearizing_controllers/twist_mrac_linearizing_controller.h deleted file mode 100644 index 1d5a92d..0000000 --- a/include/linearizing_controllers/twist_mrac_linearizing_controller.h +++ /dev/null @@ -1,97 +0,0 @@ -/****************************************************************************** - ROS linearizing_controllers Package - Twist MRAC Linearizing Controller - Copyright (C) 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 LINEARIZING_CONTROLLERS_TWIST_MRAC_LINEARIZING_CONTROLLER_H -#define LINEARIZING_CONTROLLERS_TWIST_MRAC_LINEARIZING_CONTROLLER_H - -#include -#include -#include - -#include -#include - -#include - -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -namespace effort_controllers -{ - class TwistMracLinearizingController: public controller_interface:: - Controller - { - public: - TwistMracLinearizingController(void); - ~TwistMracLinearizingController(void); - - bool init(hardware_interface::EffortJointInterface *robot, - ros::NodeHandle &n); - void starting(const ros::Time& time); - void update(const ros::Time& time,const ros::Duration& duration); - - private: - ros::NodeHandle node_; - hardware_interface::EffortJointInterface *robot_; - std::vector joints_; - - boost::scoped_ptr - > status_publisher_ ; - - boost::shared_ptr > odom_publisher_; - boost::shared_ptr > tf_odom_publisher_; - - ros::Subscriber sub_command_; - ros::Subscriber sub_parameters_; - - Eigen::Matrix2d Ginv_; - Eigen::Matrix2d F_; - - arc_odometry::DiffOdometry odom_; - - Eigen::Vector2d uRef_; - - double time_step_; - ros::Time lastSamplingTime_; - - Eigen::Vector2d phi_; - - Eigen::Vector2d uModel_; - - Eigen::DiagonalMatrix Alpha_; - - void commandCB(const geometry_msgs::Twist &command); - void parametersCB(const std_msgs::Float64MultiArray &command); - }; -} -#endif diff --git a/include/linearizing_controllers/twist_mrac_linearizing_controller.hpp b/include/linearizing_controllers/twist_mrac_linearizing_controller.hpp new file mode 100644 index 0000000..6ea19e3 --- /dev/null +++ b/include/linearizing_controllers/twist_mrac_linearizing_controller.hpp @@ -0,0 +1,110 @@ +/****************************************************************************** + ROS 2 linearizing_controllers Package + Twist MRAC Linearizing Controller + Copyright (C) 2018..2022 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 LINEARIZING_CONTROLLERS__TWIST_MRAC_LINEARIZING_CONTROLLER_HPP_ +#define LINEARIZING_CONTROLLERS__TWIST_MRAC_LINEARIZING_CONTROLLER_HPP_ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include + +namespace effort_controllers +{ + using CallbackReturn=rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + class TwistMracLinearizingController: public controller_interface::ControllerInterface + { + public: + LINEARIZING_CONTROLLERS_PUBLIC + TwistMracLinearizingController(void); + + LINEARIZING_CONTROLLERS_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration(void) const override; + + LINEARIZING_CONTROLLERS_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration(void) const override; + + LINEARIZING_CONTROLLERS_PUBLIC + CallbackReturn on_init(void) override; + + LINEARIZING_CONTROLLERS_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override; + + LINEARIZING_CONTROLLERS_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override; + + LINEARIZING_CONTROLLERS_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + + LINEARIZING_CONTROLLERS_PUBLIC + controller_interface::return_type update(void) override; + + private: + std::vector joints_; + + std::shared_ptr> status_publisher_; + std::shared_ptr> rt_status_publisher_; + + std::shared_ptr> odom_publisher_; + std::shared_ptr> rt_odom_publisher_; + + std::shared_ptr> tf_odom_publisher_; + std::shared_ptr> rt_tf_odom_publisher_; + + rclcpp::Subscription::SharedPtr sub_command_; + rclcpp::Subscription::SharedPtr sub_parameters_; + + Eigen::Matrix2d Ginv_; + Eigen::Matrix2d F_; + + arc_odometry::DiffOdometry odom_; + + Eigen::Vector2d uRef_; + + double time_step_; + rclcpp::Time lastSamplingTime_; + + Eigen::Vector2d phi_; + + Eigen::Vector2d uModel_; + + Eigen::DiagonalMatrix Alpha_; + + int priority_; + + realtime_tools::RealtimeBuffer> rt_command_; + realtime_tools::RealtimeBuffer> rt_param_; + + void commandCB(const geometry_msgs::msg::Twist::SharedPtr command); + void parametersCB(const std_msgs::msg::Float64MultiArray::SharedPtr param); + }; +} +#endif diff --git a/include/linearizing_controllers/visibility_control.h b/include/linearizing_controllers/visibility_control.h new file mode 100644 index 0000000..2052554 --- /dev/null +++ b/include/linearizing_controllers/visibility_control.h @@ -0,0 +1,35 @@ +#ifndef LINEARIZING_CONTROLLERS__VISIBILITY_CONTROL_H_ +#define LINEARIZING_CONTROLLERS__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 LINEARIZING_CONTROLLERS_EXPORT __attribute__ ((dllexport)) + #define LINEARIZING_CONTROLLERS_IMPORT __attribute__ ((dllimport)) + #else + #define LINEARIZING_CONTROLLERS_EXPORT __declspec(dllexport) + #define LINEARIZING_CONTROLLERS_IMPORT __declspec(dllimport) + #endif + #ifdef LINEARIZING_CONTROLLERS_BUILDING_LIBRARY + #define LINEARIZING_CONTROLLERS_PUBLIC LINEARIZING_CONTROLLERS_EXPORT + #else + #define LINEARIZING_CONTROLLERS_PUBLIC LINEARIZING_CONTROLLERS_IMPORT + #endif + #define LINEARIZING_CONTROLLERS_PUBLIC_TYPE LINEARIZING_CONTROLLERS_PUBLIC + #define LINEARIZING_CONTROLLERS_LOCAL +#else + #define LINEARIZING_CONTROLLERS_EXPORT __attribute__ ((visibility("default"))) + #define LINEARIZING_CONTROLLERS_IMPORT + #if __GNUC__ >= 4 + #define LINEARIZING_CONTROLLERS_PUBLIC __attribute__ ((visibility("default"))) + #define LINEARIZING_CONTROLLERS_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define LINEARIZING_CONTROLLERS_PUBLIC + #define LINEARIZING_CONTROLLERS_LOCAL + #endif + #define LINEARIZING_CONTROLLERS_PUBLIC_TYPE +#endif + +#endif // LINEARIZING_CONTROLLERS__VISIBILITY_CONTROL_H_ diff --git a/linearizing_controllers_plugins.xml b/linearizing_controllers_plugins.xml index c7d95c6..44efe8b 100644 --- a/linearizing_controllers_plugins.xml +++ b/linearizing_controllers_plugins.xml @@ -1,8 +1,7 @@ - - + + base_class_type="controller_interface::ControllerInterface"> The DynamicsLinearizingController linearizes the dynamics of a differential-drive mobile robot. The linearized inputs are linear @@ -13,7 +12,7 @@ + base_class_type="controller_interface::ControllerInterface"> The TwistMracLinearizingController implements a Model Reference (Adaptive) Linearizing Controller for the twist of a @@ -27,7 +26,7 @@ + base_class_type="controller_interface::ControllerInterface"> The PositionMracLinearizingController implements a Model Reference (Adaptive) Linearizing Controller for the position of a diff --git a/package.xml b/package.xml index 2ebc319..43345ac 100644 --- a/package.xml +++ b/package.xml @@ -1,86 +1,31 @@ - + + linearizing_controllers - 0.1.0 - The linearizing_controllers package - - - - + 2.0.0 + Linearizing controllers for mobile robots Walter Fetter Lages - - - - - GPLv3 + ament_cmake_ros - - - - - - - - - - - Walter Fetter Lages + rclcpp + arc_odometry + controller_interface + controller_manager + geometry_msgs + linearizing_controllers_msgs + nav_msgs + realtime_tools + tf2 + tf2_msgs + tf2_ros + Eigen3 + ament_lint_auto + ament_lint_common - - - - - - - - - - - - - - - - - - - - - catkin - Eigen3 - arc_odometry - controller_interface - effort_controllers - geometry_msgs - linearizing_controllers_msgs - roscpp - tf - Eigen3 - arc_odometry - controller_interface - effort_controllers - geometry_msgs - linearizing_controllers_msgs - roscpp - tf - Eigen3 - arc_odometry - controller_interface - effort_controllers - geometry_msgs - linearizing_controllers_msgs - roscpp - tf - - - - - - - + ament_cmake diff --git a/scripts/accel_step.py b/scripts/accel_step.py index 0ad1bb0..5128fb8 100755 --- a/scripts/accel_step.py +++ b/scripts/accel_step.py @@ -1,35 +1,38 @@ -#!/usr/bin/python +#!/usr/bin/python3 import sys import time -import rospy +import rclpy +from rclpy.node import Node from geometry_msgs.msg import Accel -def step(): - if len(sys.argv) < 3: - print 'accel_step.py v w' - exit() - accel=Accel() - accel.linear.x=float(sys.argv[1]) - accel.linear.y=0.0 - accel.linear.z=0.0 - accel.angular.x=0.0 - accel.angular.y=0.0 - accel.angular.z=float(sys.argv[2]) - - pub=rospy.Publisher('/dynamics_linearizing_controller/command', Accel, queue_size=1) - - rospy.init_node('accel_step',anonymous=True) - - pub.publish(accel) - time.sleep(1) - - pub.publish(accel) - time.sleep(1) - +class Step(Node): + def __init__(self): + super().__init__('step') + accel=Accel() + accel.linear.x=float(sys.argv[1]) + accel.linear.y=0.0 + accel.linear.z=0.0 + accel.angular.x=0.0 + accel.angular.y=0.0 + accel.angular.z=float(sys.argv[2]) + self.pub=self.create_publisher(Accel,'/dynamics_linearizing_controller/command',1) + self.pub.publish(accel) + exit() + +def main(args=None): + if len(sys.argv) < 3: + print('accel_step.py v w') + exit() + + rclpy.init(args=args) + + step=Step() + rclpy.spin(step) + + step.destroy_node() + rclpy.shutdown() + if __name__ == '__main__': - try: - step() - except rospy.ROSInterruptException: - pass + main() diff --git a/scripts/accel_step.sh b/scripts/accel_step.sh index 9ed33b5..670cce9 100755 --- a/scripts/accel_step.sh +++ b/scripts/accel_step.sh @@ -1,3 +1,3 @@ #!/bin/bash -rostopic pub -1 /dynamics_linearizing_controller/command geometry_msgs/Accel "{linear: {x: $1, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: $2}}" +ros2 topic pub -1 /dynamics_linearizing_controller/command geometry_msgs/Accel "{linear: {x: $1, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: $2}}" diff --git a/scripts/twist_step.py b/scripts/twist_step.py index 0a7d0a9..702ee46 100755 --- a/scripts/twist_step.py +++ b/scripts/twist_step.py @@ -1,35 +1,38 @@ -#!/usr/bin/python +#!/usr/bin/python3 import sys import time -import rospy +import rclpy +from rclpy.node import Node from geometry_msgs.msg import Twist -def step(): - if len(sys.argv) < 3: - print 'twist_step.py v w' - exit() - twist=Twist() - twist.linear.x=float(sys.argv[1]) - twist.linear.y=0.0 - twist.linear.z=0.0 - twist.angular.x=0.0 - twist.angular.y=0.0 - twist.angular.z=float(sys.argv[2]) - - pub=rospy.Publisher('/twist_mrac_linearizing_controller/command', Twist, queue_size=1) - - rospy.init_node('twist_step',anonymous=True) - - pub.publish(twist) - time.sleep(1) - - pub.publish(twist) - time.sleep(1) - +class Step(Node): + def __init__(self): + super().__init__('step') + twist=Twist() + twist.linear.x=float(sys.argv[1]) + twist.linear.y=0.0 + twist.linear.z=0.0 + twist.angular.x=0.0 + twist.angular.y=0.0 + twist.angular.z=float(sys.argv[2]) + self.pub=self.create_publisher(Twist,'/twist_mrac_linearizing_controller/command', 1) + self.pub.publish(twist) + exit() + +def main(args=None): + if len(sys.argv) < 3: + print('twist_step.py v w') + exit() + + rclpy.init(args=args) + + step=Step() + rclpy.spin(step) + + step.destroy_node() + rclpy.shutdown() + if __name__ == '__main__': - try: - step() - except rospy.ROSInterruptException: - pass + main() diff --git a/scripts/twist_step.sh b/scripts/twist_step.sh index 4ab343a..506da73 100755 --- a/scripts/twist_step.sh +++ b/scripts/twist_step.sh @@ -1,3 +1,3 @@ #!/bin/bash -rostopic pub -1 /twist_mrac_linearizing_controller/command geometry_msgs/Twist "{linear: {x: $1, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: $2}}" +ros2 topic pub -1 /twist_mrac_linearizing_controller/command geometry_msgs/msg/Twist "{linear: {x: $1, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: $2}}" diff --git a/src/dynamics_linearizing_controller.cpp b/src/dynamics_linearizing_controller.cpp index d0d55d6..1e5a18a 100644 --- a/src/dynamics_linearizing_controller.cpp +++ b/src/dynamics_linearizing_controller.cpp @@ -1,7 +1,7 @@ /****************************************************************************** - ROS linearing_controllers Package + ROS 2 linearing_controllers Package Dynamics Linearizing Controller - Copyright (C) 2014..2018 Walter Fetter Lages + Copyright (C) 2014..2022 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,11 +21,11 @@ #include -#include +#include +#include +#include -#include - -#include +#include #define sqr(x) ((x)*(x)) @@ -35,283 +35,311 @@ namespace effort_controllers odom_(0.0,std::vector(2)) { } - - DynamicsLinearizingController::~DynamicsLinearizingController(void) - { - sub_command_.shutdown(); - sub_parameters_.shutdown(); - } - - bool DynamicsLinearizingController::init(hardware_interface::EffortJointInterface *robot,ros::NodeHandle &n) + + CallbackReturn DynamicsLinearizingController::on_init(void) { - node_=n; - robot_=robot; - - XmlRpc::XmlRpcValue joint_names; - if(!node_.getParam("joints",joint_names)) - { - ROS_ERROR("No 'joints' parameter in controller. (namespace: %s)",node_.getNamespace().c_str()); - return false; - } - - if(joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray) + try { - ROS_ERROR("The 'joints' parameter is not a struct. (namespace: %s)",node_.getNamespace().c_str()); - return false; - } - - for(int i=0; i < joint_names.size();i++) - { - XmlRpc::XmlRpcValue &name_value=joint_names[i]; - if(name_value.getType() != XmlRpc::XmlRpcValue::TypeString) - { - ROS_ERROR("Array of joint names should contain only strings. (namespace: %s)",node_.getNamespace().c_str()); - return false; - } - - hardware_interface::JointHandle j=robot->getHandle((std::string)name_value); - joints_.push_back(j); - } - - std::string odom_frame_id="odom"; - node_.param("odom_frame_id",odom_frame_id,odom_frame_id); - - std::string base_frame_id="base_link"; - node_.param("base_frame_id",base_frame_id,base_frame_id); - - status_publisher_.reset(new realtime_tools::RealtimePublisher(node_,"status",1)); - status_publisher_->msg_.header.frame_id=base_frame_id; - status_publisher_->msg_.set_point.linear.y=0.0; - status_publisher_->msg_.set_point.linear.z=0.0; - status_publisher_->msg_.set_point.angular.x=0.0; - status_publisher_->msg_.set_point.angular.y=0.0; - status_publisher_->msg_.process_value.linear.y=0.0; - status_publisher_->msg_.process_value.linear.z=0.0; - status_publisher_->msg_.process_value.angular.x=0.0; - status_publisher_->msg_.process_value.angular.y=0.0; - - odom_publisher_.reset(new realtime_tools::RealtimePublisher(node_,"odom",100)); - odom_publisher_->msg_.header.frame_id=odom_frame_id; - odom_publisher_->msg_.child_frame_id=base_frame_id; - odom_publisher_->msg_.pose.pose.position.z=0; - odom_publisher_->msg_.pose.pose.orientation.x=0; - odom_publisher_->msg_.pose.pose.orientation.y=0; - - // Fake covariance - double pose_cov[]={1e-6, 1e-6, 1e-16,1e-16,1e-16,1e-9}; - odom_publisher_->msg_.pose.covariance=boost::assign::list_of - (pose_cov[0]) (0) (0) (0) (0) (0) - (0) (pose_cov[1]) (0) (0) (0) (0) - (0) (0) (pose_cov[2]) (0) (0) (0) - (0) (0) (0) (pose_cov[3]) (0) (0) - (0) (0) (0) (0) (pose_cov[4]) (0) - (0) (0) (0) (0) (0) (pose_cov[5]); - - odom_publisher_->msg_.twist.twist.linear.z=0; - odom_publisher_->msg_.twist.twist.angular.x=0; - odom_publisher_->msg_.twist.twist.angular.y=0; - - //Fake covariance - double twist_cov[]={1e-6,1e-6,1e-16,1e-16,1e-16,1e-9}; - odom_publisher_->msg_.twist.covariance=boost::assign::list_of - (twist_cov[0]) (0) (0) (0) (0) (0) - (0) (twist_cov[1]) (0) (0) (0) (0) - (0) (0) (twist_cov[2]) (0) (0) (0) - (0) (0) (0) (twist_cov[3]) (0) (0) - (0) (0) (0) (0) (twist_cov[4]) (0) - (0) (0) (0) (0) (0) (twist_cov[5]); - - tf_odom_publisher_.reset(new realtime_tools::RealtimePublisher(node_,"/tf",100)); - tf_odom_publisher_->msg_.transforms.resize(1); - tf_odom_publisher_->msg_.transforms[0].transform.translation.z=0.0; - tf_odom_publisher_->msg_.transforms[0].transform.rotation.x=0.0; - tf_odom_publisher_->msg_.transforms[0].transform.rotation.y=0.0; - tf_odom_publisher_->msg_.transforms[0].child_frame_id=base_frame_id; - tf_odom_publisher_->msg_.transforms[0].header.frame_id=odom_frame_id; - - sub_command_=node_.subscribe("command",1000,&DynamicsLinearizingController::commandCB,this); - - sub_parameters_=node_.subscribe("dynamic_parameters",1000,&DynamicsLinearizingController::parametersCB,this); - - double wheelBase; - if(!node_.getParam("wheel_separation",wheelBase)) - { - ROS_ERROR("No 'wheel_separation' in controller %s.",node_.getNamespace().c_str()); - return false; + auto_declare>("joints",joints_); + + auto_declare("odom_frame_id","odom"); + auto_declare("base_frame_id","base_link"); + auto_declare("wheel_separation",0.0); + auto_declare>("wheel_radius",std::vector()); + + auto_declare>("F",std::vector()); + auto_declare>("G",std::vector()); + + auto_declare("time_step",0.01); + + auto_declare("priority",sched_get_priority_max(SCHED_FIFO)); } - - std::vector wheelRadius(2); - if(!node_.getParam("wheel_radius",wheelRadius)) + catch(const std::exception &e) { - ROS_ERROR("No 'wheel_radius' in controller %s.",node_.getNamespace().c_str()); - return false; + RCLCPP_ERROR_STREAM(node_->get_logger(),"Exception thrown in on_init() with message: " << e.what()); + return CallbackReturn::ERROR; } - - odom_.setParams(wheelBase,wheelRadius); - - std::vector FVec; - if(!node_.getParam("F",FVec)) + + return CallbackReturn::SUCCESS; + } + + CallbackReturn DynamicsLinearizingController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) + { + try { - ROS_ERROR("No 'F' in controller %s.",node_.getNamespace().c_str()); - return false; + joints_=node_->get_parameter("joints").as_string_array(); + if(joints_.empty()) + throw std::runtime_error("'joints' parameter was empty,"); + + auto odom_frame_id=node_->get_parameter("odom_frame_id").as_string(); + auto base_frame_id=node_->get_parameter("base_frame_id").as_string(); + + status_publisher_=node_->create_publisher("~/status",rclcpp::SystemDefaultsQoS()); + rt_status_publisher_=std::make_shared>(status_publisher_); + rt_status_publisher_->msg_.header.frame_id=base_frame_id; + rt_status_publisher_->msg_.set_point.linear.y=0.0; + rt_status_publisher_->msg_.set_point.linear.z=0.0; + rt_status_publisher_->msg_.set_point.angular.x=0.0; + rt_status_publisher_->msg_.set_point.angular.y=0.0; + rt_status_publisher_->msg_.process_value.linear.y=0.0; + rt_status_publisher_->msg_.process_value.linear.z=0.0; + rt_status_publisher_->msg_.process_value.angular.x=0.0; + rt_status_publisher_->msg_.process_value.angular.y=0.0; + + odom_publisher_=node_->create_publisher("~/odom",rclcpp::SystemDefaultsQoS()); + rt_odom_publisher_=std::make_shared>(odom_publisher_); + rt_odom_publisher_->msg_.header.frame_id=odom_frame_id; + rt_odom_publisher_->msg_.child_frame_id=base_frame_id; + rt_odom_publisher_->msg_.pose.pose.position.z=0; + rt_odom_publisher_->msg_.pose.pose.orientation.x=0; + rt_odom_publisher_->msg_.pose.pose.orientation.y=0; + + // Fake covariance + double pose_cov[]={1e-6, 1e-6, 1e-16,1e-16,1e-16,1e-9}; + rt_odom_publisher_->msg_.pose.covariance={ + pose_cov[0], 0, 0, 0, 0, 0, + 0, pose_cov[1], 0, 0, 0, 0, + 0, 0, pose_cov[2], 0, 0, 0, + 0, 0, 0, pose_cov[3], 0, 0, + 0, 0, 0, 0, pose_cov[4], 0, + 0, 0, 0, 0, 0, pose_cov[5]}; + + rt_odom_publisher_->msg_.twist.twist.linear.z=0; + rt_odom_publisher_->msg_.twist.twist.angular.x=0; + rt_odom_publisher_->msg_.twist.twist.angular.y=0; + + //Fake covariance + double twist_cov[]={1e-6,1e-6,1e-16,1e-16,1e-16,1e-9}; + rt_odom_publisher_->msg_.twist.covariance={ + twist_cov[0], 0, 0, 0, 0, 0, + 0, twist_cov[1], 0, 0, 0, 0, + 0, 0, twist_cov[2], 0, 0, 0, + 0, 0, 0, twist_cov[3], 0, 0, + 0, 0, 0, 0, twist_cov[4], 0, + 0, 0, 0, 0, 0, twist_cov[5]}; + + tf_odom_publisher_=node_->create_publisher("/tf",rclcpp::SystemDefaultsQoS()); + rt_tf_odom_publisher_=std::make_shared>(tf_odom_publisher_); + rt_tf_odom_publisher_->msg_.transforms.resize(1); + rt_tf_odom_publisher_->msg_.transforms[0].transform.translation.z=0.0; + rt_tf_odom_publisher_->msg_.transforms[0].transform.rotation.x=0.0; + rt_tf_odom_publisher_->msg_.transforms[0].transform.rotation.y=0.0; + rt_tf_odom_publisher_->msg_.transforms[0].child_frame_id=base_frame_id; + rt_tf_odom_publisher_->msg_.transforms[0].header.frame_id=odom_frame_id; + + using std::placeholders::_1; + sub_command_=node_->create_subscription("~/command",1000, + std::bind(&DynamicsLinearizingController::commandCB,this,_1)); + + sub_parameters_=node_->create_subscription("~/dynamics_paremeters",1000, + std::bind(&DynamicsLinearizingController::parametersCB,this,_1)); + + double wheelBase; + if(!node_->get_parameter("wheel_separation",wheelBase)) + throw std::runtime_error("No 'wheel_separation' parameter."); + + std::vector wheelRadius(2); + if(!node_->get_parameter("wheel_radius",wheelRadius)) + throw std::runtime_error("No 'wheel_radius' parameter."); + + odom_.setParams(wheelBase,wheelRadius); + + std::vector FVec; + if(!node_->get_parameter("F",FVec)) + throw std::runtime_error("No 'F' parameter."); + F_=Eigen::Map(FVec.data(),2,2).transpose(); + + std::vector GVec; + if(!node_->get_parameter("G",GVec)) + throw std::runtime_error("No 'G' parameter."); + Ginv_=Eigen::Map(GVec.data(),2,2).transpose().inverse(); + + time_step_=node_->get_parameter("time_step").as_double(); } - F_=Eigen::Map(FVec.data(),2,2).transpose(); - - std::vector GVec; - if(!node_.getParam("G",GVec)) + catch(const std::exception &e) { - ROS_ERROR("No 'G' in controller %s.",node_.getNamespace().c_str()); - return false; + RCLCPP_ERROR_STREAM(node_->get_logger(),"Exception thrown in on_confiture(): " << e.what()); + return CallbackReturn::ERROR; } - Ginv_=Eigen::Map(GVec.data(),2,2).transpose().inverse(); - node_.param("time_step",time_step_,0.01); + if(!node_->get_parameter("priority",priority_)) + RCLCPP_WARN(node_->get_logger(),"No 'priority' configured for controller. Using highest possible priority."); + + return CallbackReturn::SUCCESS; + } + + controller_interface::InterfaceConfiguration DynamicsLinearizingController::command_interface_configuration(void) const + { + controller_interface::InterfaceConfiguration config; + config.type=controller_interface::interface_configuration_type::INDIVIDUAL; + + for(const auto &joint : joints_) + config.names.push_back(joint + "/" + hardware_interface::HW_IF_EFFORT); + + return config; + } + + controller_interface::InterfaceConfiguration DynamicsLinearizingController::state_interface_configuration(void) const + { + controller_interface::InterfaceConfiguration config; + config.type=controller_interface::interface_configuration_type::INDIVIDUAL; + + for(const auto &joint : joints_) + config.names.push_back(joint + "/" + hardware_interface::HW_IF_POSITION); - return true; + return config; } - void DynamicsLinearizingController::starting(const ros::Time& time) + CallbackReturn DynamicsLinearizingController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { Eigen::Vector3d x; x.setZero(); odom_.setPose(x); v_.setZero(); - lastSamplingTime_=time; + lastSamplingTime_=node_->get_clock()->now(); for(unsigned int i=0;i < joints_.size();i++) - { - phi_[i]=joints_[i].getPosition(); - } - + phi_[i]=state_interfaces_[i].get_value(); + 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."); 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; + } + + CallbackReturn DynamicsLinearizingController::on_deactivate(const rclcpp_lifecycle::State &/*previous_state*/) + { + for(unsigned int i=0;i < joints_.size();i++) + command_interfaces_[i].set_value(0); + + return CallbackReturn::SUCCESS; } - - void DynamicsLinearizingController::update(const ros::Time& time, - const ros::Duration& duration) + + controller_interface::return_type DynamicsLinearizingController::update(void) { - ros::Duration dt=time-lastSamplingTime_; - - if(fabs(dt.toSec()-time_step_) > time_step_/20) return; + auto time=node_->get_clock()->now(); + auto dt=time-lastSamplingTime_; + + if(dt.seconds() < time_step_) return controller_interface::return_type::OK; lastSamplingTime_=time; - + // Incremental encoders sense angular displacement and // not velocity // phi[0] is the left wheel angular displacement // phi[1] is the right wheel angular displacement Eigen::Vector2d deltaPhi=-phi_; for(unsigned int i=0;i < joints_.size();i++) - { - phi_[i]=joints_[i].getPosition(); - } + phi_[i]=state_interfaces_[i].get_value(); deltaPhi+=phi_; odom_.update(deltaPhi,dt); - + Eigen::Vector2d u; odom_.getVelocity(u); + auto command=rt_command_.readFromRT(); + if(command && *command) + { + v_[0]=(*command)->linear.x; + v_[1]=(*command)->angular.z; + } + + auto param=rt_param_.readFromRT(); + if(param && *param) + { + // data[0]=f12, data[1]=f21, data[2]=g11, data[3]=g21 + // data[4]=Pf12, data[5]=Pkf21, data[6]=Pg11, data[7]=Pg21 + // F= [0 f12 + // f21 0] + // Ginv=[0.5/g11 0.5/g21 + // 0.5/g11 -0.5/g21] + if((*param)->data[4] < 1e-3) F_(0,1)=(*param)->data[0]; + if((*param)->data[5] < 1e-3) F_(1,0)=(*param)->data[1]; + if((*param)->data[6] < 1e-3) + { + Ginv_(0,0)=0.5/(*param)->data[2]; + Ginv_(1,0)=0.5/(*param)->data[2]; + } + if((*param)->data[7] < 1e-3) + { + Ginv_(0,1)=0.5/(*param)->data[3]; + Ginv_(1,1)=-0.5/(*param)->data[3]; + } + } + // Linearization Eigen::Vector2d uf(u[0]*u[1],sqr(u[1])); - Eigen::Vector2d torque=Ginv_*(v_-F_*uf); - + auto torque=Ginv_*(v_-F_*uf); + // Apply torques for(unsigned int i=0;i < joints_.size();i++) - { - joints_[i].setCommand(torque[i]); - } + command_interfaces_[i].set_value(torque[i]); - - if(status_publisher_ && status_publisher_->trylock()) + if(rt_status_publisher_ && rt_status_publisher_->trylock()) { - status_publisher_->msg_.header.stamp=time; + rt_status_publisher_->msg_.header.stamp=time; - status_publisher_->msg_.set_point.linear.x=v_[0]; - status_publisher_->msg_.set_point.angular.z=v_[1]; + rt_status_publisher_->msg_.set_point.linear.x=v_[0]; + rt_status_publisher_->msg_.set_point.angular.z=v_[1]; - status_publisher_->msg_.process_value.linear.x=u[0]; - status_publisher_->msg_.process_value.angular.z=u[1]; + rt_status_publisher_->msg_.process_value.linear.x=u[0]; + rt_status_publisher_->msg_.process_value.angular.z=u[1]; + + rt_status_publisher_->msg_.time_step=dt.seconds(); - status_publisher_->msg_.time_step=dt.toSec(); - - for(int i=0;i < torque.size();i++) - status_publisher_->msg_.command[i]=torque[i]; + for(unsigned int i=0;i < torque.size();i++) + rt_status_publisher_->msg_.command[i]=torque[i]; - status_publisher_->unlockAndPublish(); + rt_status_publisher_->unlockAndPublish(); } - + Eigen::Vector3d x; odom_.getPose(x); - - if(odom_publisher_ && odom_publisher_->trylock()) + + if(rt_odom_publisher_ && rt_odom_publisher_->trylock()) { - odom_publisher_->msg_.header.stamp=time; + rt_odom_publisher_->msg_.header.stamp=time; - odom_publisher_->msg_.pose.pose.position.x=x[0]; - odom_publisher_->msg_.pose.pose.position.y=x[1]; - odom_publisher_->msg_.pose.pose.orientation.z=sin(x[2]/2); - odom_publisher_->msg_.pose.pose.orientation.w=cos(x[2]/2); + rt_odom_publisher_->msg_.pose.pose.position.x=x[0]; + rt_odom_publisher_->msg_.pose.pose.position.y=x[1]; + rt_odom_publisher_->msg_.pose.pose.orientation.z=sin(x[2]/2); + rt_odom_publisher_->msg_.pose.pose.orientation.w=cos(x[2]/2); - odom_publisher_->msg_.twist.twist.linear.x=u[0]*cos(x[2]); - odom_publisher_->msg_.twist.twist.linear.y=u[0]*sin(x[2]); - odom_publisher_->msg_.twist.twist.angular.z=u[1]; + rt_odom_publisher_->msg_.twist.twist.linear.x=u[0]*cos(x[2]); + rt_odom_publisher_->msg_.twist.twist.linear.y=u[0]*sin(x[2]); + rt_odom_publisher_->msg_.twist.twist.angular.z=u[1]; - odom_publisher_->unlockAndPublish(); + rt_odom_publisher_->unlockAndPublish(); } - - if(tf_odom_publisher_ && tf_odom_publisher_->trylock()) + + if(rt_tf_odom_publisher_ && rt_tf_odom_publisher_->trylock()) { - geometry_msgs::TransformStamped &odom_frame= - tf_odom_publisher_->msg_.transforms[0]; + geometry_msgs::msg::TransformStamped &odom_frame= + rt_tf_odom_publisher_->msg_.transforms[0]; odom_frame.header.stamp=time; odom_frame.transform.translation.x=x[0]; odom_frame.transform.translation.y=x[1]; odom_frame.transform.rotation.z=sin(x[2]/2); odom_frame.transform.rotation.w=cos(x[2]/2); - tf_odom_publisher_->unlockAndPublish(); + rt_tf_odom_publisher_->unlockAndPublish(); } + return controller_interface::return_type::OK; } - - void DynamicsLinearizingController::commandCB(const geometry_msgs::Accel &command) + + void DynamicsLinearizingController::commandCB(const geometry_msgs::msg::Accel::SharedPtr command) { - v_[0]=command.linear.x; - v_[1]=command.angular.z; + rt_command_.writeFromNonRT(command); } - - void DynamicsLinearizingController::parametersCB(const std_msgs::Float64MultiArray ¶m) + + void DynamicsLinearizingController::parametersCB(const std_msgs::msg::Float64MultiArray::SharedPtr param) { - // data[0]=f12, data[1]=f21, data[2]=g11, data[3]=g21 - // data[4]=Pf12, data[5]=Pkf21, data[6]=Pg11, data[7]=Pg21 - // F= [0 f12 - // f21 0] - // Ginv=[0.5/g11 0.5/g21 - // 0.5/g11 -0.5/g21] - if(param.data[4] < 1e-3) F_(0,1)=param.data[0]; - if(param.data[5] < 1e-3) F_(1,0)=param.data[1]; - if(param.data[6] < 1e-3) - { - Ginv_(0,0)=0.5/param.data[2]; - Ginv_(1,0)=0.5/param.data[2]; - } - if(param.data[7] < 1e-3) - { - Ginv_(0,1)=0.5/param.data[3]; - Ginv_(1,1)=-0.5/param.data[3]; - } + rt_param_.writeFromNonRT(param); } - } PLUGINLIB_EXPORT_CLASS(effort_controllers::DynamicsLinearizingController, - controller_interface::ControllerBase) + controller_interface::ControllerInterface) diff --git a/src/twist_mrac_linearizing_controller.cpp b/src/twist_mrac_linearizing_controller.cpp index bf70d93..bf9d15f 100644 --- a/src/twist_mrac_linearizing_controller.cpp +++ b/src/twist_mrac_linearizing_controller.cpp @@ -21,11 +21,11 @@ #include -#include +#include +#include +#include -#include - -#include +#include #define sqr(x) ((x)*(x)) @@ -35,314 +35,337 @@ namespace effort_controllers odom_(0.0,std::vector(2)) { } - - TwistMracLinearizingController::~TwistMracLinearizingController(void) - { - sub_command_.shutdown(); - sub_parameters_.shutdown(); - } - - bool TwistMracLinearizingController::init(hardware_interface::EffortJointInterface *robot,ros::NodeHandle &n) + + CallbackReturn TwistMracLinearizingController::on_init(void) { - node_=n; - robot_=robot; - - XmlRpc::XmlRpcValue joint_names; - if(!node_.getParam("joints",joint_names)) - { - ROS_ERROR("No 'joints' parameter in controller. (namespace: %s)",node_.getNamespace().c_str()); - return false; - } - - if(joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray) - { - ROS_ERROR("The 'joints' parameter is not a struct. (namespace: %s)",node_.getNamespace().c_str()); - return false; - } - - for(int i=0; i < joint_names.size();i++) - { - XmlRpc::XmlRpcValue &name_value=joint_names[i]; - if(name_value.getType() != XmlRpc::XmlRpcValue::TypeString) - { - ROS_ERROR("Array of joint names should contain only strings. (namespace: %s)",node_.getNamespace().c_str()); - return false; - } - - hardware_interface::JointHandle j=robot->getHandle((std::string)name_value); - joints_.push_back(j); - } - - std::string odom_frame_id="odom"; - node_.param("odom_frame_id",odom_frame_id,odom_frame_id); - - std::string base_frame_id="base_link"; - node_.param("base_frame_id",base_frame_id,base_frame_id); - - status_publisher_.reset(new realtime_tools::RealtimePublisher(node_,"status",1)); - status_publisher_->msg_.header.frame_id=base_frame_id; - status_publisher_->msg_.set_point.linear.y=0.0; - status_publisher_->msg_.set_point.linear.z=0.0; - status_publisher_->msg_.set_point.angular.x=0.0; - status_publisher_->msg_.set_point.angular.y=0.0; - status_publisher_->msg_.process_value.linear.y=0.0; - status_publisher_->msg_.process_value.linear.z=0.0; - status_publisher_->msg_.process_value.angular.x=0.0; - status_publisher_->msg_.process_value.angular.y=0.0; - status_publisher_->msg_.error.linear.y=0.0; - status_publisher_->msg_.error.linear.z=0.0; - status_publisher_->msg_.error.angular.x=0.0; - status_publisher_->msg_.error.angular.y=0.0; - - odom_publisher_.reset(new realtime_tools::RealtimePublisher(node_,"odom",100)); - odom_publisher_->msg_.header.frame_id=odom_frame_id; - odom_publisher_->msg_.child_frame_id=base_frame_id; - odom_publisher_->msg_.pose.pose.position.z=0; - odom_publisher_->msg_.pose.pose.orientation.x=0; - odom_publisher_->msg_.pose.pose.orientation.y=0; - - // Fake covariance - double pose_cov[]={1e-6, 1e-6, 1e-16,1e-16,1e-16,1e-9}; - odom_publisher_->msg_.pose.covariance=boost::assign::list_of - (pose_cov[0]) (0) (0) (0) (0) (0) - (0) (pose_cov[1]) (0) (0) (0) (0) - (0) (0) (pose_cov[2]) (0) (0) (0) - (0) (0) (0) (pose_cov[3]) (0) (0) - (0) (0) (0) (0) (pose_cov[4]) (0) - (0) (0) (0) (0) (0) (pose_cov[5]); - - odom_publisher_->msg_.twist.twist.linear.z=0; - odom_publisher_->msg_.twist.twist.angular.x=0; - odom_publisher_->msg_.twist.twist.angular.y=0; - - //Fake covariance - double twist_cov[]={1e-6,1e-6,1e-16,1e-16,1e-16,1e-9}; - odom_publisher_->msg_.twist.covariance=boost::assign::list_of - (twist_cov[0]) (0) (0) (0) (0) (0) - (0) (twist_cov[1]) (0) (0) (0) (0) - (0) (0) (twist_cov[2]) (0) (0) (0) - (0) (0) (0) (twist_cov[3]) (0) (0) - (0) (0) (0) (0) (twist_cov[4]) (0) - (0) (0) (0) (0) (0) (twist_cov[5]); - - tf_odom_publisher_.reset(new realtime_tools::RealtimePublisher(node_,"/tf",100)); - tf_odom_publisher_->msg_.transforms.resize(1); - tf_odom_publisher_->msg_.transforms[0].transform.translation.z=0.0; - tf_odom_publisher_->msg_.transforms[0].transform.rotation.x=0.0; - tf_odom_publisher_->msg_.transforms[0].transform.rotation.y=0.0; - tf_odom_publisher_->msg_.transforms[0].child_frame_id=base_frame_id; - tf_odom_publisher_->msg_.transforms[0].header.frame_id=odom_frame_id; - - sub_command_=node_.subscribe("command",1000,&TwistMracLinearizingController::commandCB,this); - - sub_parameters_=node_.subscribe("dynamic_parameters",1000,&TwistMracLinearizingController::parametersCB,this); - - double wheelBase; - if(!node_.getParam("wheel_separation",wheelBase)) - { - ROS_ERROR("No 'wheel_separation' in controller %s.",node_.getNamespace().c_str()); - return false; - } - - std::vector wheelRadius(2); - if(!node_.getParam("wheel_radius",wheelRadius)) + try { - ROS_ERROR("No 'wheel_radius' in controller %s.",node_.getNamespace().c_str()); - return false; + auto_declare>("joints",joints_); + + auto_declare("odom_frame_id","odom"); + auto_declare("base_frame_id","base_link"); + auto_declare("wheel_separation",0.0); + auto_declare>("wheel_radius",std::vector()); + + auto_declare>("F",std::vector()); + auto_declare>("G",std::vector()); + + auto_declare>("Alpha",std::vector()); + + auto_declare("time_step",0.01); + + auto_declare("priority",sched_get_priority_max(SCHED_FIFO)); } - - odom_.setParams(wheelBase,wheelRadius); - - std::vector FVec; - if(!node_.getParam("F",FVec)) + catch(const std::exception &e) { - ROS_ERROR("No 'F' in controller %s.",node_.getNamespace().c_str()); - return false; + RCLCPP_ERROR_STREAM(node_->get_logger(),"Exception thrown in on_init() with message: " << e.what()); + return CallbackReturn::ERROR; } - F_=Eigen::Map(FVec.data(),2,2).transpose(); - - std::vector GVec; - if(!node_.getParam("G",GVec)) + + return CallbackReturn::SUCCESS; + } + + CallbackReturn TwistMracLinearizingController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) + { + try { - ROS_ERROR("No 'G' in controller %s.",node_.getNamespace().c_str()); - return false; + joints_=node_->get_parameter("joints").as_string_array(); + if(joints_.empty()) + throw std::runtime_error("'joints' parameter was empty,"); + + auto odom_frame_id=node_->get_parameter("odom_frame_id").as_string(); + auto base_frame_id=node_->get_parameter("base_frame_id").as_string(); + + status_publisher_=node_->create_publisher("~/status",rclcpp::SystemDefaultsQoS()); + rt_status_publisher_=std::make_shared>(status_publisher_); + rt_status_publisher_->msg_.header.frame_id=base_frame_id; + rt_status_publisher_->msg_.set_point.linear.y=0.0; + rt_status_publisher_->msg_.set_point.linear.z=0.0; + rt_status_publisher_->msg_.set_point.angular.x=0.0; + rt_status_publisher_->msg_.set_point.angular.y=0.0; + rt_status_publisher_->msg_.process_value.linear.y=0.0; + rt_status_publisher_->msg_.process_value.linear.z=0.0; + rt_status_publisher_->msg_.process_value.angular.x=0.0; + rt_status_publisher_->msg_.process_value.angular.y=0.0; + + odom_publisher_=node_->create_publisher("~/odom",rclcpp::SystemDefaultsQoS()); + rt_odom_publisher_=std::make_shared>(odom_publisher_); + rt_odom_publisher_->msg_.header.frame_id=odom_frame_id; + rt_odom_publisher_->msg_.child_frame_id=base_frame_id; + rt_odom_publisher_->msg_.pose.pose.position.z=0; + rt_odom_publisher_->msg_.pose.pose.orientation.x=0; + rt_odom_publisher_->msg_.pose.pose.orientation.y=0; + + // Fake covariance + double pose_cov[]={1e-6, 1e-6, 1e-16,1e-16,1e-16,1e-9}; + rt_odom_publisher_->msg_.pose.covariance={ + pose_cov[0], 0, 0, 0, 0, 0, + 0, pose_cov[1], 0, 0, 0, 0, + 0, 0, pose_cov[2], 0, 0, 0, + 0, 0, 0, pose_cov[3], 0, 0, + 0, 0, 0, 0, pose_cov[4], 0, + 0, 0, 0, 0, 0, pose_cov[5]}; + + rt_odom_publisher_->msg_.twist.twist.linear.z=0; + rt_odom_publisher_->msg_.twist.twist.angular.x=0; + rt_odom_publisher_->msg_.twist.twist.angular.y=0; + + //Fake covariance + double twist_cov[]={1e-6,1e-6,1e-16,1e-16,1e-16,1e-9}; + rt_odom_publisher_->msg_.twist.covariance={ + twist_cov[0], 0, 0, 0, 0, 0, + 0, twist_cov[1], 0, 0, 0, 0, + 0, 0, twist_cov[2], 0, 0, 0, + 0, 0, 0, twist_cov[3], 0, 0, + 0, 0, 0, 0, twist_cov[4], 0, + 0, 0, 0, 0, 0, twist_cov[5]}; + + tf_odom_publisher_=node_->create_publisher("/tf",rclcpp::SystemDefaultsQoS()); + rt_tf_odom_publisher_=std::make_shared>(tf_odom_publisher_); + rt_tf_odom_publisher_->msg_.transforms.resize(1); + rt_tf_odom_publisher_->msg_.transforms[0].transform.translation.z=0.0; + rt_tf_odom_publisher_->msg_.transforms[0].transform.rotation.x=0.0; + rt_tf_odom_publisher_->msg_.transforms[0].transform.rotation.y=0.0; + rt_tf_odom_publisher_->msg_.transforms[0].child_frame_id=base_frame_id; + rt_tf_odom_publisher_->msg_.transforms[0].header.frame_id=odom_frame_id; + + using std::placeholders::_1; + sub_command_=node_->create_subscription("~/command",1000, + std::bind(&TwistMracLinearizingController::commandCB,this,_1)); + + sub_parameters_=node_->create_subscription("~/dynamics_paremeters",1000, + std::bind(&TwistMracLinearizingController::parametersCB,this,_1)); + + double wheelBase; + if(!node_->get_parameter("wheel_separation",wheelBase)) + throw std::runtime_error("No 'wheel_separation' parameter."); + + std::vector wheelRadius(2); + if(!node_->get_parameter("wheel_radius",wheelRadius)) + throw std::runtime_error("No 'wheel_radius' parameter."); + + odom_.setParams(wheelBase,wheelRadius); + + std::vector FVec; + if(!node_->get_parameter("F",FVec)) + throw std::runtime_error("No 'F' parameter."); + F_=Eigen::Map(FVec.data(),2,2).transpose(); + + std::vector GVec; + if(!node_->get_parameter("G",GVec)) + throw std::runtime_error("No 'G' parameter."); + Ginv_=Eigen::Map(GVec.data(),2,2).transpose().inverse(); + + std::vector AlphaVec; + if(!node_->get_parameter("Alpha",AlphaVec)) + throw std::runtime_error("No 'Alpha' parameter."); + Alpha_.diagonal()=Eigen::Map(AlphaVec.data(),2); + + time_step_=node_->get_parameter("time_step").as_double(); } - Ginv_=Eigen::Map(GVec.data(),2,2).transpose().inverse(); - - std::vector AlphaVec; - if(!node_.getParam("Alpha",AlphaVec)) + catch(const std::exception &e) { - ROS_ERROR("No 'Alpha' in controller %s.",node_.getNamespace().c_str()); - return false; + RCLCPP_ERROR_STREAM(node_->get_logger(),"Exception thrown in on_confiture(): " << e.what()); + return CallbackReturn::ERROR; } - Alpha_.diagonal()=Eigen::Map(AlphaVec.data(),2); - node_.param("time_step",time_step_,0.01); + if(!node_->get_parameter("priority",priority_)) + RCLCPP_WARN(node_->get_logger(),"No 'priority' configured for controller. Using highest possible priority."); + + return CallbackReturn::SUCCESS; + } + + controller_interface::InterfaceConfiguration TwistMracLinearizingController::command_interface_configuration(void) const + { + controller_interface::InterfaceConfiguration config; + config.type=controller_interface::interface_configuration_type::INDIVIDUAL; + + for(const auto &joint : joints_) + config.names.push_back(joint + "/" + hardware_interface::HW_IF_EFFORT); + + return config; + } + + controller_interface::InterfaceConfiguration TwistMracLinearizingController::state_interface_configuration(void) const + { + controller_interface::InterfaceConfiguration config; + config.type=controller_interface::interface_configuration_type::INDIVIDUAL; + + for(const auto &joint : joints_) + config.names.push_back(joint + "/" + hardware_interface::HW_IF_POSITION); - return true; + return config; } - void TwistMracLinearizingController::starting(const ros::Time& time) + CallbackReturn TwistMracLinearizingController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { Eigen::Vector3d x; x.setZero(); - odom_.setPose(x); uRef_.setZero(); uModel_.setZero(); - - lastSamplingTime_=time; + lastSamplingTime_=node_->get_clock()->now(); for(unsigned int i=0;i < joints_.size();i++) - { - phi_[i]=joints_[i].getPosition(); - } - + phi_[i]=state_interfaces_[i].get_value(); + 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."); 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; + } + + CallbackReturn TwistMracLinearizingController::on_deactivate(const rclcpp_lifecycle::State &/*previous_state*/) + { + for(unsigned int i=0;i < joints_.size();i++) + command_interfaces_[i].set_value(0); + + return CallbackReturn::SUCCESS; } - - void TwistMracLinearizingController::update(const ros::Time& time, - const ros::Duration& duration) + + controller_interface::return_type TwistMracLinearizingController::update(void) { - ros::Duration dt=time-lastSamplingTime_; - - if(fabs(dt.toSec()-time_step_) > time_step_/20) return; + auto time=node_->get_clock()->now(); + auto dt=time-lastSamplingTime_; + + if(dt.seconds() < time_step_) return controller_interface::return_type::OK; lastSamplingTime_=time; - + // Incremental encoders sense angular displacement and // not velocity // phi[0] is the left wheel angular displacement // phi[1] is the right wheel angular displacement Eigen::Vector2d deltaPhi=-phi_; for(unsigned int i=0;i < joints_.size();i++) - { - phi_[i]=joints_[i].getPosition(); - } + phi_[i]=state_interfaces_[i].get_value(); deltaPhi+=phi_; odom_.update(deltaPhi,dt); - + Eigen::Vector2d u; odom_.getVelocity(u); - + + auto command=rt_command_.readFromRT(); + if(command && *command) + { + uRef_[0]=(*command)->linear.x; + uRef_[1]=(*command)->angular.z; + } + + auto param=rt_param_.readFromRT(); + if(param && *param) + { + // data[0]=f12, data[1]=f21, data[2]=g11, data[3]=g21 + // data[4]=Pf12, data[5]=Pkf21, data[6]=Pg11, data[7]=Pg21 + // F= [0 f12 + // f21 0] + // Ginv=[0.5/g11 0.5/g21 + // 0.5/g11 -0.5/g21] + if((*param)->data[4] < 1e-3) F_(0,1)=(*param)->data[0]; + if((*param)->data[5] < 1e-3) F_(1,0)=(*param)->data[1]; + if((*param)->data[6] < 1e-3) + { + Ginv_(0,0)=0.5/(*param)->data[2]; + Ginv_(1,0)=0.5/(*param)->data[2]; + } + if((*param)->data[7] < 1e-3) + { + Ginv_(0,1)=0.5/(*param)->data[3]; + Ginv_(1,1)=-0.5/(*param)->data[3]; + } + } + // Compute reference model - Eigen::Vector2d deltaUModel=Alpha_*(uRef_-uModel_); - - Eigen::Vector2d v=Alpha_*(uModel_-u)+deltaUModel; - + auto deltaUModel=Alpha_*(uRef_-uModel_); + + auto v=Alpha_*(uModel_-u)+deltaUModel; + //update reference model - uModel_+=deltaUModel*dt.toSec(); + uModel_+=deltaUModel*dt.seconds(); // Linearization Eigen::Vector2d uf(u[0]*u[1],sqr(u[1])); - Eigen::Vector2d torque=Ginv_*(v-F_*uf); - + auto torque=Ginv_*(v-F_*uf); + // Apply torques for(unsigned int i=0;i < joints_.size();i++) - { - joints_[i].setCommand(torque[i]); - } - - if(status_publisher_ && status_publisher_->trylock()) + command_interfaces_[i].set_value(torque[i]); + + if(rt_status_publisher_ && rt_status_publisher_->trylock()) { - status_publisher_->msg_.header.stamp=time; - - status_publisher_->msg_.set_point.linear.x=uRef_[0]; - status_publisher_->msg_.set_point.angular.z=uRef_[1]; - - status_publisher_->msg_.process_value.linear.x=u[0]; - status_publisher_->msg_.process_value.angular.z=u[1]; - - status_publisher_->msg_.error.linear.x=uRef_[0]-u[0]; - status_publisher_->msg_.error.angular.z=uRef_[1]-u[1]; - - status_publisher_->msg_.time_step=dt.toSec(); - - for(int i=0;i < torque.size();i++) + rt_status_publisher_->msg_.header.stamp=time; + + rt_status_publisher_->msg_.set_point.linear.x=uRef_[0]; + rt_status_publisher_->msg_.set_point.angular.z=uRef_[1]; + + rt_status_publisher_->msg_.process_value.linear.x=u[0]; + rt_status_publisher_->msg_.process_value.angular.z=u[1]; + + rt_status_publisher_->msg_.error.linear.x=uRef_[0]-u[0]; + rt_status_publisher_->msg_.error.angular.z=uRef_[1]-u[1]; + + rt_status_publisher_->msg_.time_step=dt.seconds(); + + for(unsigned int i=0;i < torque.size();i++) { - status_publisher_->msg_.command[i]=torque[i]; - status_publisher_->msg_.alpha[i]=Alpha_.diagonal()(i); - status_publisher_->msg_.reference_model[i]=uModel_[i]; - status_publisher_->msg_.reference_model_dot[i]=deltaUModel[i]; - status_publisher_->msg_.reference_model_error[i]=uRef_[i]-uModel_[i]; - status_publisher_->msg_.linear_dynamics_command[i]=v[i]; - } - status_publisher_->unlockAndPublish(); + rt_status_publisher_->msg_.command[i]=torque[i]; + rt_status_publisher_->msg_.alpha[i]=Alpha_.diagonal()(i); + rt_status_publisher_->msg_.reference_model[i]=uModel_[i]; + rt_status_publisher_->msg_.reference_model_dot[i]=deltaUModel[i]; + rt_status_publisher_->msg_.reference_model_error[i]=uRef_[i]-uModel_[i]; + rt_status_publisher_->msg_.linear_dynamics_command[i]=v[i]; + } + + rt_status_publisher_->unlockAndPublish(); } - + Eigen::Vector3d x; odom_.getPose(x); - - if(odom_publisher_ && odom_publisher_->trylock()) + + if(rt_odom_publisher_ && rt_odom_publisher_->trylock()) { - odom_publisher_->msg_.header.stamp=time; + rt_odom_publisher_->msg_.header.stamp=time; - odom_publisher_->msg_.pose.pose.position.x=x[0]; - odom_publisher_->msg_.pose.pose.position.y=x[1]; - odom_publisher_->msg_.pose.pose.orientation.z=sin(x[2]/2); - odom_publisher_->msg_.pose.pose.orientation.w=cos(x[2]/2); + rt_odom_publisher_->msg_.pose.pose.position.x=x[0]; + rt_odom_publisher_->msg_.pose.pose.position.y=x[1]; + rt_odom_publisher_->msg_.pose.pose.orientation.z=sin(x[2]/2); + rt_odom_publisher_->msg_.pose.pose.orientation.w=cos(x[2]/2); - odom_publisher_->msg_.twist.twist.linear.x=u[0]*cos(x[2]); - odom_publisher_->msg_.twist.twist.linear.y=u[0]*sin(x[2]); - odom_publisher_->msg_.twist.twist.angular.z=u[1]; + rt_odom_publisher_->msg_.twist.twist.linear.x=u[0]*cos(x[2]); + rt_odom_publisher_->msg_.twist.twist.linear.y=u[0]*sin(x[2]); + rt_odom_publisher_->msg_.twist.twist.angular.z=u[1]; - odom_publisher_->unlockAndPublish(); + rt_odom_publisher_->unlockAndPublish(); } - - if(tf_odom_publisher_ && tf_odom_publisher_->trylock()) + + if(rt_tf_odom_publisher_ && rt_tf_odom_publisher_->trylock()) { - geometry_msgs::TransformStamped &odom_frame= - tf_odom_publisher_->msg_.transforms[0]; + geometry_msgs::msg::TransformStamped &odom_frame= + rt_tf_odom_publisher_->msg_.transforms[0]; odom_frame.header.stamp=time; odom_frame.transform.translation.x=x[0]; odom_frame.transform.translation.y=x[1]; odom_frame.transform.rotation.z=sin(x[2]/2); odom_frame.transform.rotation.w=cos(x[2]/2); - tf_odom_publisher_->unlockAndPublish(); + rt_tf_odom_publisher_->unlockAndPublish(); } + return controller_interface::return_type::OK; } - - void TwistMracLinearizingController::commandCB(const geometry_msgs::Twist &command) + + void TwistMracLinearizingController::commandCB(const geometry_msgs::msg::Twist::SharedPtr command) { - uRef_[0]=command.linear.x; - uRef_[1]=command.angular.z; + rt_command_.writeFromNonRT(command); } - - void TwistMracLinearizingController::parametersCB(const std_msgs::Float64MultiArray ¶m) + + void TwistMracLinearizingController::parametersCB(const std_msgs::msg::Float64MultiArray::SharedPtr param) { - // data[0]=f12, data[1]=f21, data[2]=g11, data[3]=g21 - // data[4]=Pf12, data[5]=Pkf21, data[6]=Pg11, data[7]=Pg21 - // F= [0 f12 - // f21 0] - // Ginv=[0.5/g11 0.5/g21 - // 0.5/g11 -0.5/g21] - if(param.data[4] < 1e-3) F_(0,1)=param.data[0]; - if(param.data[5] < 1e-3) F_(1,0)=param.data[1]; - if(param.data[6] < 1e-3) - { - Ginv_(0,0)=0.5/param.data[2]; - Ginv_(1,0)=0.5/param.data[2]; - } - if(param.data[7] < 1e-3) - { - Ginv_(0,1)=0.5/param.data[3]; - Ginv_(1,1)=-0.5/param.data[3]; - } + rt_param_.writeFromNonRT(param); } - } PLUGINLIB_EXPORT_CLASS(effort_controllers::TwistMracLinearizingController, - controller_interface::ControllerBase) + controller_interface::ControllerInterface) -- 2.12.0