From 27cf571ef5fde6952304ec81faef4327c2d0ed37 Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Thu, 6 Jul 2023 05:08:19 -0300 Subject: [PATCH] Port to Galactic. --- .gitignore | 5 +- CMakeLists.txt | 274 +++------- .../nonsmooth_backstep_controller.h | 98 ---- .../nonsmooth_backstep_controller.hpp | 114 +++++ .../visibility_control.h | 35 ++ ...xml => nonsmooth_backstep_controller_plugin.xml | 4 +- package.xml | 98 +--- scripts/pose_step.py | 53 +- scripts/pose_step.sh | 2 +- src/nonsmooth_backstep_controller.cpp | 557 +++++++++++---------- 10 files changed, 575 insertions(+), 665 deletions(-) delete mode 100644 include/nonsmooth_backstep_controller/nonsmooth_backstep_controller.h create mode 100644 include/nonsmooth_backstep_controller/nonsmooth_backstep_controller.hpp create mode 100644 include/nonsmooth_backstep_controller/visibility_control.h rename nonsmooth_backstep_controller_plugins.xml => nonsmooth_backstep_controller_plugin.xml (82%) diff --git a/.gitignore b/.gitignore index 35d74bb..a6b6ae8 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 13cf13c..59fbad5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,215 +1,89 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.8) project(nonsmooth_backstep_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 - arc_odometry - controller_interface - effort_controllers - geometry_msgs - nav_msgs - nonsmooth_backstep_controller_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(nav_msgs REQUIRED) +find_package(realtime_tools REQUIRED) +find_package(nonsmooth_backstep_controller_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_msgs REQUIRED) +find_package(tf2_ros REQUIRED) find_package(Eigen3 REQUIRED) +add_library(nonsmooth_backstep_controller SHARED + src/nonsmooth_backstep_controller.cpp) +target_compile_features(nonsmooth_backstep_controller PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_include_directories(nonsmooth_backstep_controller PRIVATE + $ + $ + $) +ament_target_dependencies( + nonsmooth_backstep_controller + "rclcpp" + "arc_odometry" + "controller_interface" + "controller_manager" + "geometry_msgs" + "nav_msgs" + "nonsmooth_backstep_controller_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# nav_msgs# nonsmooth_backstep_controller_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(nonsmooth_backstep_controller PRIVATE "NONSMOOTH_BACKSTEP_CONTROLLER_BUILDING_LIBRARY") -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) +pluginlib_export_plugin_description_file(controller_interface nonsmooth_backstep_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 nonsmooth_backstep_controller - CATKIN_DEPENDS arc_odometry controller_interface effort_controllers geometry_msgs nav_msgs nonsmooth_backstep_controller_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 nonsmooth_backstep_controller + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) - -## Declare a C++ library -add_library(${PROJECT_NAME} - src/nonsmooth_backstep_controller.cpp +install(PROGRAMS scripts/pose_step.py scripts/pose_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/nonsmooth_backstep_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} -# ${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( + nonsmooth_backstep_controller ) - -## 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_nonsmooth_backstep_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/nonsmooth_backstep_controller/nonsmooth_backstep_controller.h b/include/nonsmooth_backstep_controller/nonsmooth_backstep_controller.h deleted file mode 100644 index 022c0ee..0000000 --- a/include/nonsmooth_backstep_controller/nonsmooth_backstep_controller.h +++ /dev/null @@ -1,98 +0,0 @@ -/****************************************************************************** - ROS nonsmooth_backstep_controller Package - Non-Smooth Backstepping 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 NONSMOOTH_BACKSTEP_CONTROLLER_NONSMOOTH_BACKSTEP_CONTROLLER_H -#define NONSMOOTH_BACKSTEP_CONTROLLER_NONSMOOTH_BACKSTEP_CONTROLLER_H - -#include -#include -#include - -#include -#include - -#include - -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -namespace effort_controllers -{ - class NonSmoothBackstepController: public controller_interface:: - Controller - { - public: - NonSmoothBackstepController(void); - ~NonSmoothBackstepController(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::Vector3d xRef_; - - Eigen::Vector2d eta_; - - double time_step_; - ros::Time lastSamplingTime_; - - Eigen::Vector2d phi_; - - std::vector lambda_; - std::vector gamma_; - - void commandCB(const geometry_msgs::Pose2D::ConstPtr &command); - void parametersCB(const std_msgs::Float64MultiArray::ConstPtr &command); - }; -} -#endif diff --git a/include/nonsmooth_backstep_controller/nonsmooth_backstep_controller.hpp b/include/nonsmooth_backstep_controller/nonsmooth_backstep_controller.hpp new file mode 100644 index 0000000..a7750bf --- /dev/null +++ b/include/nonsmooth_backstep_controller/nonsmooth_backstep_controller.hpp @@ -0,0 +1,114 @@ +/****************************************************************************** + ROS 2 nonsmooth_backstep_controller Package + Non-Smooth Backstepping 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 NONSMOOTH_BACKSTEP_CONTROLLER__NONSMOOTH_BACKSTEP_CONTROLLER_HPP_ +#define NONSMOOTH_BACKSTEP_CONTROLLER__NONSMOOTH_BACKSTEP_CONTROLLER_HPP_ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include + +namespace effort_controllers +{ + + using CallbackReturn=rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + class NonSmoothBackstepController: public controller_interface::ControllerInterface + { + public: + NONSMOOTH_BACKSTEP_CONTROLLER_PUBLIC + NonSmoothBackstepController(void); + + NONSMOOTH_BACKSTEP_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration(void) const override; + + NONSMOOTH_BACKSTEP_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration(void) const override; + + NONSMOOTH_BACKSTEP_CONTROLLER_PUBLIC + CallbackReturn on_init(void) override; + + NONSMOOTH_BACKSTEP_CONTROLLER_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override; + + NONSMOOTH_BACKSTEP_CONTROLLER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override; + + NONSMOOTH_BACKSTEP_CONTROLLER_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + + NONSMOOTH_BACKSTEP_CONTROLLER_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_; + + Eigen::Vector3d xRef_; + + Eigen::Vector2d eta_; + + double time_step_; + rclcpp::Time lastSamplingTime_; + + Eigen::Vector2d phi_; + + std::vector lambda_; + std::vector gamma_; + + int priority_; + + realtime_tools::RealtimeBuffer> rt_command_; + realtime_tools::RealtimeBuffer> rt_param_; + + void commandCB(const geometry_msgs::msg::Pose2D::SharedPtr command); + void parametersCB(const std_msgs::msg::Float64MultiArray::SharedPtr param); + }; +} +#endif diff --git a/include/nonsmooth_backstep_controller/visibility_control.h b/include/nonsmooth_backstep_controller/visibility_control.h new file mode 100644 index 0000000..6d8f7af --- /dev/null +++ b/include/nonsmooth_backstep_controller/visibility_control.h @@ -0,0 +1,35 @@ +#ifndef NONSMOOTH_BACKSTEP_CONTROLLER__VISIBILITY_CONTROL_H_ +#define NONSMOOTH_BACKSTEP_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 NONSMOOTH_BACKSTEP_CONTROLLER_EXPORT __attribute__ ((dllexport)) + #define NONSMOOTH_BACKSTEP_CONTROLLER_IMPORT __attribute__ ((dllimport)) + #else + #define NONSMOOTH_BACKSTEP_CONTROLLER_EXPORT __declspec(dllexport) + #define NONSMOOTH_BACKSTEP_CONTROLLER_IMPORT __declspec(dllimport) + #endif + #ifdef NONSMOOTH_BACKSTEP_CONTROLLER_BUILDING_LIBRARY + #define NONSMOOTH_BACKSTEP_CONTROLLER_PUBLIC NONSMOOTH_BACKSTEP_CONTROLLER_EXPORT + #else + #define NONSMOOTH_BACKSTEP_CONTROLLER_PUBLIC NONSMOOTH_BACKSTEP_CONTROLLER_IMPORT + #endif + #define NONSMOOTH_BACKSTEP_CONTROLLER_PUBLIC_TYPE NONSMOOTH_BACKSTEP_CONTROLLER_PUBLIC + #define NONSMOOTH_BACKSTEP_CONTROLLER_LOCAL +#else + #define NONSMOOTH_BACKSTEP_CONTROLLER_EXPORT __attribute__ ((visibility("default"))) + #define NONSMOOTH_BACKSTEP_CONTROLLER_IMPORT + #if __GNUC__ >= 4 + #define NONSMOOTH_BACKSTEP_CONTROLLER_PUBLIC __attribute__ ((visibility("default"))) + #define NONSMOOTH_BACKSTEP_CONTROLLER_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define NONSMOOTH_BACKSTEP_CONTROLLER_PUBLIC + #define NONSMOOTH_BACKSTEP_CONTROLLER_LOCAL + #endif + #define NONSMOOTH_BACKSTEP_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // NONSMOOTH_BACKSTEP_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/nonsmooth_backstep_controller_plugins.xml b/nonsmooth_backstep_controller_plugin.xml similarity index 82% rename from nonsmooth_backstep_controller_plugins.xml rename to nonsmooth_backstep_controller_plugin.xml index dd9fb48..299153d 100644 --- a/nonsmooth_backstep_controller_plugins.xml +++ b/nonsmooth_backstep_controller_plugin.xml @@ -1,8 +1,8 @@ - + + base_class_type="controller_interface::ControllerInterface"> The NonSmoothBackstepController implements a non-smooth controller based on a discontinuous transformation and backstepping. A feedback diff --git a/package.xml b/package.xml index e65bedf..954fa18 100644 --- a/package.xml +++ b/package.xml @@ -1,89 +1,31 @@ - + + nonsmooth_backstep_controller - 3.2.0 - The nonsmooth_backstep_controller package - - - - + 4.0.0 + Nonsmooth backstepping controller for mobile robots Walter Fetter Lages - - - - - GPLv3 + ament_cmake_ros - - - - - - - - - - - Walter Fetter Lages + rclcpp + arc_odometry + controller_interface + controller_manager + geometry_msgs + nav_msgs + nonsmooth_backstep_controller_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 - nav_msgs - nonsmooth_backstep_controller_msgs - roscpp - tf - Eigen3 - arc_odometry - controller_interface - effort_controllers - geometry_msgs - nav_msgs - nonsmooth_backstep_controller_msgs - roscpp - tf - Eigen3 - arc_odometry - controller_interface - effort_controllers - geometry_msgs - nav_msgs - nonsmooth_backstep_controller_msgs - roscpp - tf - - - - - - - + ament_cmake diff --git a/scripts/pose_step.py b/scripts/pose_step.py index 0605032..61c01f6 100755 --- a/scripts/pose_step.py +++ b/scripts/pose_step.py @@ -1,32 +1,35 @@ -#!/usr/bin/python +#!/usr/bin/python3 import sys import time -import rospy +import rclpy +from rclpy.node import Node from geometry_msgs.msg import Pose2D -def step(): - if len(sys.argv) < 4: - print 'pose_step.py x y theta' - exit() - pose=Pose2D() - pose.x=float(sys.argv[1]) - pose.y=float(sys.argv[2]) - pose.theta=float(sys.argv[3]) - - pub=rospy.Publisher('/nonsmooth_backstep_controller/command', Pose2D, queue_size=1) - - rospy.init_node('pose_step',anonymous=True) - - pub.publish(pose) - time.sleep(1) - - pub.publish(pose) - time.sleep(1) - +class Step(Node): + def __init__(self): + super().__init__('step') + pose=Pose2D() + pose.x=float(sys.argv[1]) + pose.y=float(sys.argv[2]) + pose.theta=float(sys.argv[3]) + self.pub=self.create_publisher(Pose2D,'/nonsmooth_backstep_controller/command',1) + self.pub.publish(pose) + exit() + +def main(args=None): + if len(sys.argv) < 4: + print('pose_step.py x y theta') + 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/pose_step.sh b/scripts/pose_step.sh index d8179f3..daa3d24 100755 --- a/scripts/pose_step.sh +++ b/scripts/pose_step.sh @@ -1,3 +1,3 @@ #!/bin/bash -rostopic pub -1 /nonsmooth_backstep_controller/command geometry_msgs/Pose2D "{x: $1, y: $2, theta: $3}" +ros2 topic pub -1 /nonsmooth_backstep_controller/command geometry_msgs/msg/Pose2D "{x: $1, y: $2, theta: $3}" diff --git a/src/nonsmooth_backstep_controller.cpp b/src/nonsmooth_backstep_controller.cpp index d2ab303..d55f144 100644 --- a/src/nonsmooth_backstep_controller.cpp +++ b/src/nonsmooth_backstep_controller.cpp @@ -1,7 +1,7 @@ /****************************************************************************** - ROS nonsmooth_backstep_controller Package + ROS 2 nonsmooth_backstep_controller Package Non-Smooth Backstepping 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)) #define cub(x) ((x)*(x)*(x)) @@ -36,208 +36,241 @@ namespace effort_controllers odom_(0.0,std::vector(2)),lambda_(5),gamma_(4) { } - - NonSmoothBackstepController::~NonSmoothBackstepController(void) - { - sub_command_.shutdown(); - sub_parameters_.shutdown(); - } - - bool NonSmoothBackstepController::init(hardware_interface::EffortJointInterface *robot,ros::NodeHandle &n) + + CallbackReturn NonSmoothBackstepController::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=odom_frame_id; - - 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,&NonSmoothBackstepController::commandCB,this); - - sub_parameters_=node_.subscribe("dynamic_parameters",1000,&NonSmoothBackstepController::parametersCB,this); - - double wheelBase; - if(!node_.getParam("wheel_separation",wheelBase)) + try { - 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()); + + gamma_[0]=10.0; + gamma_[1]=1.0; + gamma_[2]=10.0; + gamma_[3]=50.0; + auto_declare>("gamma",gamma_); + + lambda_[0]=200.0; + lambda_[1]=6.0; + lambda_[2]=6.0; + lambda_[3]=500.0; + lambda_[4]=1000.0; + auto_declare>("lambda",lambda_); + + 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); - - gamma_[0]=10.0; - gamma_[1]=1.0; - gamma_[2]=10.0; - gamma_[3]=50.0; - node_.param("gamma",gamma_,gamma_); - - lambda_[0]=200.0; - lambda_[1]=6.0; - lambda_[2]=6.0; - lambda_[3]=500.0; - lambda_[4]=1000.0; - node_.param("lambda",lambda_,lambda_); - - std::vector FVec; - if(!node_.getParam("F",FVec)) + + return CallbackReturn::SUCCESS; + } + + CallbackReturn NonSmoothBackstepController::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=odom_frame_id; + + 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(&NonSmoothBackstepController::commandCB,this,_1)); + + sub_parameters_=node_->create_subscription("~/dynamics_parameters",1000, + std::bind(&NonSmoothBackstepController::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); + + if(!node_->get_parameter("gamma",gamma_)) + throw std::runtime_error("No 'gamma' parameter."); + + if(!node_->get_parameter("lambda",lambda_)) + throw std::runtime_error("No 'lambda' parameter."); + + 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 true; + return CallbackReturn::SUCCESS; } - void NonSmoothBackstepController::starting(const ros::Time& time) + controller_interface::InterfaceConfiguration NonSmoothBackstepController::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 NonSmoothBackstepController::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 config; + } + + CallbackReturn NonSmoothBackstepController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { Eigen::Vector3d x; x.setZero(); odom_.setPose(x); xRef_.setZero(); eta_.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; } - - void NonSmoothBackstepController::update(const ros::Time& time, - const ros::Duration& duration) + + CallbackReturn NonSmoothBackstepController::on_deactivate(const rclcpp_lifecycle::State &/*previous_state*/) { - ros::Duration dt=time-lastSamplingTime_; - - if(fabs(dt.toSec()-time_step_) > time_step_/20) return; + for(unsigned int i=0;i < joints_.size();i++) + command_interfaces_[i].set_value(0); + + return CallbackReturn::SUCCESS; + } + + + controller_interface::return_type NonSmoothBackstepController::update(void) + { + 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::Vector3d x; odom_.getPose(x); - + Eigen::Vector2d u; odom_.getVelocity(u); + auto command=rt_command_.readFromRT(); + if(command && *command) + { + xRef_[0]=(*command)->x; + xRef_[1]=(*command)->y; + xRef_[2]=(*command)->theta; + } + // Change of coordinates Eigen::Matrix3d R; R << cos(xRef_[2]), sin(xRef_[2]), 0.0, -sin(xRef_[2]), cos(xRef_[2]), 0.0, 0.0, 0.0, 1.0; Eigen::Vector3d xBar=R*(x-xRef_); - + // Discontinuous transformation double e=sqrt(sqr(xBar[0])+sqr(xBar[1])); double psi=atan2(xBar[1],xBar[0]); @@ -245,21 +278,21 @@ namespace effort_controllers // deta=(eta(k)-eta(k-1)/dt Eigen::Vector2d deta=-eta_; - + // Backstepping eta_[0]=-gamma_[0]*e*cos(alpha); - if(fabs(alpha ) > DBL_EPSILON) eta_[1]=-gamma_[1]*alpha + if(fabs(alpha ) > std::numeric_limits::epsilon()) eta_[1]=-gamma_[1]*alpha -gamma_[0]*sin(alpha)*cos(alpha)+gamma_[0]*lambda_[2]*psi*sin(alpha)/ lambda_[1]/alpha*cos(alpha); else eta_[1]=gamma_[0]*lambda_[2]*psi/lambda_[1]; deta+=eta_; - deta/=dt.toSec(); - + deta/=dt.seconds(); + Eigen::Vector2d eb=u-eta_; - + Eigen::Vector2d vBar; - if(fabs(e) > DBL_EPSILON) vBar[0]=-gamma_[2]*eb[0] + if(fabs(e) > std::numeric_limits::epsilon()) vBar[0]=-gamma_[2]*eb[0] -lambda_[0]/lambda_[3]*e*cos(alpha) +lambda_[1]/lambda_[3]*alpha*sin(alpha)/e -lambda_[2]/lambda_[3]*psi*sin(alpha)/e; @@ -268,132 +301,136 @@ namespace effort_controllers Eigen::Vector2d v=vBar+deta; + 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); - + // 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.x=xRef_[0]; - status_publisher_->msg_.set_point.y=xRef_[1]; - status_publisher_->msg_.set_point.theta=xRef_[2]; - - status_publisher_->msg_.process_value.x=x[0]; - status_publisher_->msg_.process_value.y=x[1]; - status_publisher_->msg_.process_value.theta=x[2]; - - status_publisher_->msg_.process_value_dot.x=u[0]*cos(x[2]); - status_publisher_->msg_.process_value_dot.y=u[0]*sin(x[2]); - status_publisher_->msg_.process_value_dot.theta=u[1]; - - status_publisher_->msg_.error.x=xRef_[0]-x[0]; - status_publisher_->msg_.error.y=xRef_[1]-x[1]; - status_publisher_->msg_.error.theta=xRef_[2]-x[2]; - - status_publisher_->msg_.time_step=dt.toSec(); - + rt_status_publisher_->msg_.header.stamp=time; + + rt_status_publisher_->msg_.set_point.x=xRef_[0]; + rt_status_publisher_->msg_.set_point.y=xRef_[1]; + rt_status_publisher_->msg_.set_point.theta=xRef_[2]; + + rt_status_publisher_->msg_.process_value.x=x[0]; + rt_status_publisher_->msg_.process_value.y=x[1]; + rt_status_publisher_->msg_.process_value.theta=x[2]; + + rt_status_publisher_->msg_.process_value_dot.x=u[0]*cos(x[2]); + rt_status_publisher_->msg_.process_value_dot.y=u[0]*sin(x[2]); + rt_status_publisher_->msg_.process_value_dot.theta=u[1]; + + rt_status_publisher_->msg_.error.x=xRef_[0]-x[0]; + rt_status_publisher_->msg_.error.y=xRef_[1]-x[1]; + rt_status_publisher_->msg_.error.theta=xRef_[2]-x[2]; + + rt_status_publisher_->msg_.time_step=dt.seconds(); + for(int i=0;i < torque.size();i++) - status_publisher_->msg_.command[i]=torque[i]; - - for(int i=0;i < lambda_.size();i++) - status_publisher_->msg_.lambda[i]=lambda_[i]; - - for(int i=0;i < gamma_.size();i++) - status_publisher_->msg_.gamma[i]=gamma_[i]; - - status_publisher_->msg_.polar_error.range=e; - status_publisher_->msg_.polar_error.angle=psi; - status_publisher_->msg_.polar_error.orientation=alpha; - + rt_status_publisher_->msg_.command[i]=torque[i]; + + for(unsigned int i=0;i < lambda_.size();i++) + rt_status_publisher_->msg_.v_lambda[i]=lambda_[i]; + + for(unsigned int i=0;i < gamma_.size();i++) + rt_status_publisher_->msg_.gamma[i]=gamma_[i]; + + rt_status_publisher_->msg_.polar_error.range=e; + rt_status_publisher_->msg_.polar_error.angle=psi; + rt_status_publisher_->msg_.polar_error.orientation=alpha; + for(int i=0;i < eta_.size();i++) - status_publisher_->msg_.backstep_set_point[i]=eta_[i]; + rt_status_publisher_->msg_.backstep_set_point[i]=eta_[i]; for(int i=0;i < deta.size();i++) - status_publisher_->msg_.backstep_set_point_dot[i]=deta[i]; - + rt_status_publisher_->msg_.backstep_set_point_dot[i]=deta[i]; + for(int i=0;i < u.size();i++) - status_publisher_->msg_.backstep_process_value[i]=u[i]; - + rt_status_publisher_->msg_.backstep_process_value[i]=u[i]; + for(int i=0;i < eb.size();i++) - status_publisher_->msg_.backstep_error[i]=eb[i]; - + rt_status_publisher_->msg_.backstep_error[i]=eb[i]; + for(int i=0;i < vBar.size();i++) - status_publisher_->msg_.backstep_command[i]=vBar[i]; - + rt_status_publisher_->msg_.backstep_command[i]=vBar[i]; + for(int i=0;i < v.size();i++) - status_publisher_->msg_.linear_dynamics_command[i]=v[i]; - - status_publisher_->unlockAndPublish(); + rt_status_publisher_->msg_.linear_dynamics_command[i]=v[i]; + + rt_status_publisher_->unlockAndPublish(); } - - 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]; - 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_.pose.pose.orientation.z=sin(x[2]/2); + rt_odom_publisher_->msg_.pose.pose.orientation.w=cos(x[2]/2); - odom_publisher_->unlockAndPublish(); + 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]; + + 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 NonSmoothBackstepController::commandCB(const geometry_msgs::Pose2D::ConstPtr &command) + + void NonSmoothBackstepController::commandCB(const geometry_msgs::msg::Pose2D::SharedPtr command) { - xRef_[0]=command->x; - xRef_[1]=command->y; - xRef_[2]=command->theta; + rt_command_.writeFromNonRT(command); } - - void NonSmoothBackstepController::parametersCB(const std_msgs::Float64MultiArray::ConstPtr ¶m) + + void NonSmoothBackstepController::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::NonSmoothBackstepController, - controller_interface::ControllerBase) + controller_interface::ControllerInterface) -- 2.12.0