-devel/
+install/
logs/
build/
bin/
# Catkin custom files
CATKIN_IGNORE
+
+# Colcon custom files
+COLCON_IGNORE
-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
+ $<BUILD_INTERFACE:${EIGEN3_INCLUDE_DIR}>)
+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()
+++ /dev/null
-/******************************************************************************
- ROS linearizing_controllers Package
- Dynamics Linearizing Controller
- Copyright (C) 2014..2018 Walter Fetter Lages <w.fetter@ieee.org>
-
- 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
- <http://www.gnu.org/licenses/>.
-
-*******************************************************************************/
-
-#ifndef LINEARIZING_CONTROLLERS_DYNAMICS_LINEARIZING_CONTROLLER_H
-#define LINEARIZING_CONTROLLERS_DYNAMICS_LINEARIZING_CONTROLLER_H
-
-#include <cstddef>
-#include <vector>
-#include <string>
-
-#include <Eigen/Dense>
-#include <boost/scoped_ptr.hpp>
-
-#include <ros/node_handle.h>
-
-#include <std_msgs/Float64MultiArray.h>
-#include <geometry_msgs/Accel.h>
-#include <nav_msgs/Odometry.h>
-#include <tf/tfMessage.h>
-
-#include <hardware_interface/joint_command_interface.h>
-#include <controller_interface/controller.h>
-#include <realtime_tools/realtime_publisher.h>
-
-#include <arc_odometry/diff_odometry.h>
-#include <linearizing_controllers_msgs/DynamicsLinearizingControllerStatus.h>
-
-namespace effort_controllers
-{
- class DynamicsLinearizingController: public controller_interface::
- Controller<hardware_interface::EffortJointInterface>
- {
- 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<hardware_interface::JointHandle> joints_;
-
- boost::scoped_ptr<realtime_tools::RealtimePublisher
- <linearizing_controllers_msgs::DynamicsLinearizingControllerStatus>
- > status_publisher_ ;
-
- boost::shared_ptr<realtime_tools::RealtimePublisher
- <nav_msgs::Odometry> > odom_publisher_;
- boost::shared_ptr<realtime_tools::RealtimePublisher
- <tf::tfMessage> > 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
--- /dev/null
+/******************************************************************************
+ ROS 2 linearizing_controllers Package
+ Dynamics Linearizing Controller
+ Copyright (C) 2014..2022 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************/
+
+#ifndef LINEARIZING_CONTROLLERS__DYNAMICS_LINEARIZING_CONTROLLER_HPP_
+#define LINEARIZING_CONTROLLERS__DYNAMICS_LINEARIZING_CONTROLLER_HPP_
+
+#include <std_msgs/msg/float64_multi_array.hpp>
+#include <geometry_msgs/msg/accel.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <tf2_msgs/msg/tf_message.hpp>
+
+#include <controller_interface/controller_interface.hpp>
+#include <realtime_tools/realtime_buffer.h>
+#include <realtime_tools/realtime_publisher.h>
+
+#include <arc_odometry/diff_odometry.hpp>
+#include <linearizing_controllers_msgs/msg/dynamics_linearizing_controller_status.hpp>
+
+#include <linearizing_controllers/visibility_control.h>
+
+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<std::string> joints_;
+
+ std::shared_ptr<rclcpp::Publisher<linearizing_controllers_msgs::msg::DynamicsLinearizingControllerStatus>> status_publisher_;
+ std::shared_ptr<realtime_tools::RealtimePublisher<linearizing_controllers_msgs::msg::DynamicsLinearizingControllerStatus>> rt_status_publisher_;
+
+ std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> odom_publisher_;
+ std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>> rt_odom_publisher_;
+
+ std::shared_ptr<rclcpp::Publisher<tf2_msgs::msg::TFMessage>> tf_odom_publisher_;
+ std::shared_ptr<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>> rt_tf_odom_publisher_;
+
+ rclcpp::Subscription<geometry_msgs::msg::Accel>::SharedPtr sub_command_;
+ rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::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<std::shared_ptr<geometry_msgs::msg::Accel>> rt_command_;
+ realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Float64MultiArray>> rt_param_;
+
+ void commandCB(const geometry_msgs::msg::Accel::SharedPtr command);
+ void parametersCB(const std_msgs::msg::Float64MultiArray::SharedPtr param);
+ };
+}
+#endif
+++ /dev/null
-/******************************************************************************
- ROS linearizing_controllers Package
- Twist MRAC Linearizing Controller
- Copyright (C) 2018 Walter Fetter Lages <w.fetter@ieee.org>
-
- 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
- <http://www.gnu.org/licenses/>.
-
-*******************************************************************************/
-
-#ifndef LINEARIZING_CONTROLLERS_TWIST_MRAC_LINEARIZING_CONTROLLER_H
-#define LINEARIZING_CONTROLLERS_TWIST_MRAC_LINEARIZING_CONTROLLER_H
-
-#include <cstddef>
-#include <vector>
-#include <string>
-
-#include <Eigen/Dense>
-#include <boost/scoped_ptr.hpp>
-
-#include <ros/node_handle.h>
-
-#include <std_msgs/Float64MultiArray.h>
-#include <geometry_msgs/Twist.h>
-#include <nav_msgs/Odometry.h>
-#include <tf/tfMessage.h>
-
-#include <hardware_interface/joint_command_interface.h>
-#include <controller_interface/controller.h>
-#include <realtime_tools/realtime_publisher.h>
-
-#include <arc_odometry/diff_odometry.h>
-#include <linearizing_controllers_msgs/TwistMracLinearizingControllerStatus.h>
-
-namespace effort_controllers
-{
- class TwistMracLinearizingController: public controller_interface::
- Controller<hardware_interface::EffortJointInterface>
- {
- 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<hardware_interface::JointHandle> joints_;
-
- boost::scoped_ptr<realtime_tools::RealtimePublisher
- <linearizing_controllers_msgs::TwistMracLinearizingControllerStatus>
- > status_publisher_ ;
-
- boost::shared_ptr<realtime_tools::RealtimePublisher
- <nav_msgs::Odometry> > odom_publisher_;
- boost::shared_ptr<realtime_tools::RealtimePublisher
- <tf::tfMessage> > 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<double,2> Alpha_;
-
- void commandCB(const geometry_msgs::Twist &command);
- void parametersCB(const std_msgs::Float64MultiArray &command);
- };
-}
-#endif
--- /dev/null
+/******************************************************************************
+ ROS 2 linearizing_controllers Package
+ Twist MRAC Linearizing Controller
+ Copyright (C) 2018..2022 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************/
+
+#ifndef LINEARIZING_CONTROLLERS__TWIST_MRAC_LINEARIZING_CONTROLLER_HPP_
+#define LINEARIZING_CONTROLLERS__TWIST_MRAC_LINEARIZING_CONTROLLER_HPP_
+
+#include <std_msgs/msg/float64_multi_array.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <tf2_msgs/msg//tf_message.hpp>
+
+#include <controller_interface/controller_interface.hpp>
+#include <realtime_tools/realtime_buffer.h>
+#include <realtime_tools/realtime_publisher.h>
+
+#include <arc_odometry/diff_odometry.hpp>
+#include <linearizing_controllers_msgs/msg/twist_mrac_linearizing_controller_status.hpp>
+
+#include <linearizing_controllers/visibility_control.h>
+
+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<std::string> joints_;
+
+ std::shared_ptr<rclcpp::Publisher<linearizing_controllers_msgs::msg::TwistMracLinearizingControllerStatus>> status_publisher_;
+ std::shared_ptr<realtime_tools::RealtimePublisher<linearizing_controllers_msgs::msg::TwistMracLinearizingControllerStatus>> rt_status_publisher_;
+
+ std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> odom_publisher_;
+ std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>> rt_odom_publisher_;
+
+ std::shared_ptr<rclcpp::Publisher<tf2_msgs::msg::TFMessage>> tf_odom_publisher_;
+ std::shared_ptr<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>> rt_tf_odom_publisher_;
+
+ rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr sub_command_;
+ rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::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<double,2> Alpha_;
+
+ int priority_;
+
+ realtime_tools::RealtimeBuffer<std::shared_ptr<geometry_msgs::msg::Twist>> rt_command_;
+ realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Float64MultiArray>> rt_param_;
+
+ void commandCB(const geometry_msgs::msg::Twist::SharedPtr command);
+ void parametersCB(const std_msgs::msg::Float64MultiArray::SharedPtr param);
+ };
+}
+#endif
--- /dev/null
+#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_
-<library path="lib/liblinearizing_controllers">
-
+<library path="linearizing_controllers">
<class name="effort_controllers/DynamicsLinearizingController"
type="effort_controllers::DynamicsLinearizingController"
- base_class_type="controller_interface::ControllerBase">
+ base_class_type="controller_interface::ControllerInterface">
<description>
The DynamicsLinearizingController linearizes the dynamics of
a differential-drive mobile robot. The linearized inputs are linear
<class name="effort_controllers/TwistMracLinearizingController"
type="effort_controllers::TwistMracLinearizingController"
- base_class_type="controller_interface::ControllerBase">
+ base_class_type="controller_interface::ControllerInterface">
<description>
The TwistMracLinearizingController implements a Model Reference
(Adaptive) Linearizing Controller for the twist of a
<class name="effort_controllers/PositionMracLinearizingController"
type="effort_controllers::PositionMracLinearizingController"
- base_class_type="controller_interface::ControllerBase">
+ base_class_type="controller_interface::ControllerInterface">
<description>
The PositionMracLinearizingController implements a Model Reference
(Adaptive) Linearizing Controller for the position of a
<?xml version="1.0"?>
-<package format="2">
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
<name>linearizing_controllers</name>
- <version>0.1.0</version>
- <description>The linearizing_controllers package</description>
-
- <!-- One maintainer tag required, multiple allowed, one person per tag -->
- <!-- Example: -->
- <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <version>2.0.0</version>
+ <description>Linearizing controllers for mobile robots</description>
<maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
-
-
- <!-- One license tag required, multiple allowed, one license per tag -->
- <!-- Commonly used license strings: -->
- <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>GPLv3</license>
+ <buildtool_depend>ament_cmake_ros</buildtool_depend>
- <!-- Url tags are optional, but multiple are allowed, one per tag -->
- <!-- Optional attribute type can be: website, bugtracker, or repository -->
- <!-- Example: -->
- <!-- <url type="website">http://wiki.ros.org/linearizing_controllers</url> -->
-
-
- <!-- Author tags are optional, multiple are allowed, one per tag -->
- <!-- Authors do not have to be maintainers, but could be -->
- <!-- Example: -->
- <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
- <author email="fetter@ece.ufrgs.br">Walter Fetter Lages</author>
+ <depend>rclcpp</depend>
+ <depend>arc_odometry</depend>
+ <depend>controller_interface</depend>
+ <depend>controller_manager</depend>
+ <depend>geometry_msgs</depend>
+ <depend>linearizing_controllers_msgs</depend>
+ <depend>nav_msgs</depend>
+ <depend>realtime_tools</depend>
+ <depend>tf2</depend>
+ <depend>tf2_msgs</depend>
+ <depend>tf2_ros</depend>
+ <depend>Eigen3</depend>
+ <test_depend>ament_lint_auto</test_depend>
+ <test_depend>ament_lint_common</test_depend>
- <!-- The *depend tags are used to specify dependencies -->
- <!-- Dependencies can be catkin packages or system dependencies -->
- <!-- Examples: -->
- <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
- <!-- <depend>roscpp</depend> -->
- <!-- Note that this is equivalent to the following: -->
- <!-- <build_depend>roscpp</build_depend> -->
- <!-- <exec_depend>roscpp</exec_depend> -->
- <!-- Use build_depend for packages you need at compile time: -->
- <!-- <build_depend>message_generation</build_depend> -->
- <!-- Use build_export_depend for packages you need in order to build against this package: -->
- <!-- <build_export_depend>message_generation</build_export_depend> -->
- <!-- Use buildtool_depend for build tool packages: -->
- <!-- <buildtool_depend>catkin</buildtool_depend> -->
- <!-- Use exec_depend for packages you need at runtime: -->
- <!-- <exec_depend>message_runtime</exec_depend> -->
- <!-- Use test_depend for packages you need only for testing: -->
- <!-- <test_depend>gtest</test_depend> -->
- <!-- Use doc_depend for packages you need only for building documentation: -->
- <!-- <doc_depend>doxygen</doc_depend> -->
- <buildtool_depend>catkin</buildtool_depend>
- <build_depend>Eigen3</build_depend>
- <build_depend>arc_odometry</build_depend>
- <build_depend>controller_interface</build_depend>
- <build_depend>effort_controllers</build_depend>
- <build_depend>geometry_msgs</build_depend>
- <build_depend>linearizing_controllers_msgs</build_depend>
- <build_depend>roscpp</build_depend>
- <build_depend>tf</build_depend>
- <build_export_depend>Eigen3</build_export_depend>
- <build_export_depend>arc_odometry</build_export_depend>
- <build_export_depend>controller_interface</build_export_depend>
- <build_export_depend>effort_controllers</build_export_depend>
- <build_export_depend>geometry_msgs</build_export_depend>
- <build_export_depend>linearizing_controllers_msgs</build_export_depend>
- <build_export_depend>roscpp</build_export_depend>
- <build_export_depend>tf</build_export_depend>
- <exec_depend>Eigen3</exec_depend>
- <exec_depend>arc_odometry</exec_depend>
- <exec_depend>controller_interface</exec_depend>
- <exec_depend>effort_controllers</exec_depend>
- <exec_depend>geometry_msgs</exec_depend>
- <exec_depend>linearizing_controllers_msgs</exec_depend>
- <exec_depend>roscpp</exec_depend>
- <exec_depend>tf</exec_depend>
-
-
- <!-- The export tag contains other, unspecified, tags -->
<export>
- <!-- Other tools can request additional information be placed here -->
-
- <controller_interface plugin="${prefix}/linearizing_controllers_plugins.xml" />
-
+ <build_type>ament_cmake</build_type>
</export>
</package>
-#!/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()
#!/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}}"
-#!/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()
#!/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}}"
/******************************************************************************
- ROS linearing_controllers Package
+ ROS 2 linearing_controllers Package
Dynamics Linearizing Controller
- Copyright (C) 2014..2018 Walter Fetter Lages <w.fetter@ieee.org>
+ Copyright (C) 2014..2022 Walter Fetter Lages <w.fetter@ieee.org>
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
#include <sys/mman.h>
-#include <boost/assign.hpp>
+#include <rclcpp/logging.hpp>
+#include <hardware_interface/types/hardware_interface_type_values.hpp>
+#include <pluginlib/class_list_macros.hpp>
-#include <pluginlib/class_list_macros.h>
-
-#include <linearizing_controllers/dynamics_linearizing_controller.h>
+#include <linearizing_controllers/dynamics_linearizing_controller.hpp>
#define sqr(x) ((x)*(x))
odom_(0.0,std::vector<double>(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<linearizing_controllers_msgs::DynamicsLinearizingControllerStatus>(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<nav_msgs::Odometry>(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<tf::tfMessage>(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<std::vector<std::string>>("joints",joints_);
+
+ auto_declare<std::string>("odom_frame_id","odom");
+ auto_declare<std::string>("base_frame_id","base_link");
+ auto_declare<double>("wheel_separation",0.0);
+ auto_declare<std::vector<double>>("wheel_radius",std::vector<double>());
+
+ auto_declare<std::vector<double>>("F",std::vector<double>());
+ auto_declare<std::vector<double>>("G",std::vector<double>());
+
+ auto_declare<double>("time_step",0.01);
+
+ auto_declare<int>("priority",sched_get_priority_max(SCHED_FIFO));
}
-
- std::vector<double> 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<double> 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<linearizing_controllers_msgs::msg::DynamicsLinearizingControllerStatus>("~/status",rclcpp::SystemDefaultsQoS());
+ rt_status_publisher_=std::make_shared<realtime_tools::RealtimePublisher<linearizing_controllers_msgs::msg::DynamicsLinearizingControllerStatus>>(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<nav_msgs::msg::Odometry>("~/odom",rclcpp::SystemDefaultsQoS());
+ rt_odom_publisher_=std::make_shared<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>>(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<tf2_msgs::msg::TFMessage>("/tf",rclcpp::SystemDefaultsQoS());
+ rt_tf_odom_publisher_=std::make_shared<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>>(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<geometry_msgs::msg::Accel>("~/command",1000,
+ std::bind(&DynamicsLinearizingController::commandCB,this,_1));
+
+ sub_parameters_=node_->create_subscription<std_msgs::msg::Float64MultiArray>("~/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<double> wheelRadius(2);
+ if(!node_->get_parameter("wheel_radius",wheelRadius))
+ throw std::runtime_error("No 'wheel_radius' parameter.");
+
+ odom_.setParams(wheelBase,wheelRadius);
+
+ std::vector<double> FVec;
+ if(!node_->get_parameter("F",FVec))
+ throw std::runtime_error("No 'F' parameter.");
+ F_=Eigen::Map<Eigen::Matrix2d>(FVec.data(),2,2).transpose();
+
+ std::vector<double> GVec;
+ if(!node_->get_parameter("G",GVec))
+ throw std::runtime_error("No 'G' parameter.");
+ Ginv_=Eigen::Map<Eigen::Matrix2d>(GVec.data(),2,2).transpose().inverse();
+
+ time_step_=node_->get_parameter("time_step").as_double();
}
- F_=Eigen::Map<Eigen::Matrix2d>(FVec.data(),2,2).transpose();
-
- std::vector<double> 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<Eigen::Matrix2d>(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)
#include <sys/mman.h>
-#include <boost/assign.hpp>
+#include <rclcpp/logging.hpp>
+#include <hardware_interface/types/hardware_interface_type_values.hpp>
+#include <pluginlib/class_list_macros.hpp>
-#include <pluginlib/class_list_macros.h>
-
-#include <linearizing_controllers/twist_mrac_linearizing_controller.h>
+#include <linearizing_controllers/twist_mrac_linearizing_controller.hpp>
#define sqr(x) ((x)*(x))
odom_(0.0,std::vector<double>(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<linearizing_controllers_msgs::TwistMracLinearizingControllerStatus>(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<nav_msgs::Odometry>(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<tf::tfMessage>(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<double> 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<std::vector<std::string>>("joints",joints_);
+
+ auto_declare<std::string>("odom_frame_id","odom");
+ auto_declare<std::string>("base_frame_id","base_link");
+ auto_declare<double>("wheel_separation",0.0);
+ auto_declare<std::vector<double>>("wheel_radius",std::vector<double>());
+
+ auto_declare<std::vector<double>>("F",std::vector<double>());
+ auto_declare<std::vector<double>>("G",std::vector<double>());
+
+ auto_declare<std::vector<double>>("Alpha",std::vector<double>());
+
+ auto_declare<double>("time_step",0.01);
+
+ auto_declare<int>("priority",sched_get_priority_max(SCHED_FIFO));
}
-
- odom_.setParams(wheelBase,wheelRadius);
-
- std::vector<double> 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<Eigen::Matrix2d>(FVec.data(),2,2).transpose();
-
- std::vector<double> 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<linearizing_controllers_msgs::msg::TwistMracLinearizingControllerStatus>("~/status",rclcpp::SystemDefaultsQoS());
+ rt_status_publisher_=std::make_shared<realtime_tools::RealtimePublisher<linearizing_controllers_msgs::msg::TwistMracLinearizingControllerStatus>>(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<nav_msgs::msg::Odometry>("~/odom",rclcpp::SystemDefaultsQoS());
+ rt_odom_publisher_=std::make_shared<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>>(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<tf2_msgs::msg::TFMessage>("/tf",rclcpp::SystemDefaultsQoS());
+ rt_tf_odom_publisher_=std::make_shared<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>>(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<geometry_msgs::msg::Twist>("~/command",1000,
+ std::bind(&TwistMracLinearizingController::commandCB,this,_1));
+
+ sub_parameters_=node_->create_subscription<std_msgs::msg::Float64MultiArray>("~/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<double> wheelRadius(2);
+ if(!node_->get_parameter("wheel_radius",wheelRadius))
+ throw std::runtime_error("No 'wheel_radius' parameter.");
+
+ odom_.setParams(wheelBase,wheelRadius);
+
+ std::vector<double> FVec;
+ if(!node_->get_parameter("F",FVec))
+ throw std::runtime_error("No 'F' parameter.");
+ F_=Eigen::Map<Eigen::Matrix2d>(FVec.data(),2,2).transpose();
+
+ std::vector<double> GVec;
+ if(!node_->get_parameter("G",GVec))
+ throw std::runtime_error("No 'G' parameter.");
+ Ginv_=Eigen::Map<Eigen::Matrix2d>(GVec.data(),2,2).transpose().inverse();
+
+ std::vector<double> AlphaVec;
+ if(!node_->get_parameter("Alpha",AlphaVec))
+ throw std::runtime_error("No 'Alpha' parameter.");
+ Alpha_.diagonal()=Eigen::Map<Eigen::Vector2d>(AlphaVec.data(),2);
+
+ time_step_=node_->get_parameter("time_step").as_double();
}
- Ginv_=Eigen::Map<Eigen::Matrix2d>(GVec.data(),2,2).transpose().inverse();
-
- std::vector<double> 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<Eigen::Vector2d>(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)
projects / linearizing_controllers.git / commitdiff