diff --git a/cartesian_compliance_controller/include/cartesian_compliance_controller/cartesian_compliance_controller.h b/cartesian_compliance_controller/include/cartesian_compliance_controller/cartesian_compliance_controller.h index dab0986f..833a4332 100644 --- a/cartesian_compliance_controller/include/cartesian_compliance_controller/cartesian_compliance_controller.h +++ b/cartesian_compliance_controller/include/cartesian_compliance_controller/cartesian_compliance_controller.h @@ -75,12 +75,7 @@ class CartesianComplianceController : public cartesian_motion_controller::Cartes public: CartesianComplianceController(); -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON virtual LifecycleNodeInterface::CallbackReturn on_init() override; -#elif defined CARTESIAN_CONTROLLERS_FOXY - virtual controller_interface::return_type init(const std::string & controller_name) override; -#endif rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; @@ -90,14 +85,9 @@ class CartesianComplianceController : public cartesian_motion_controller::Cartes rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON + controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override; -#elif defined CARTESIAN_CONTROLLERS_FOXY - controller_interface::return_type update() override; -#endif using Base = cartesian_controller_base::CartesianControllerBase; using MotionBase = cartesian_motion_controller::CartesianMotionController; diff --git a/cartesian_compliance_controller/src/cartesian_compliance_controller.cpp b/cartesian_compliance_controller/src/cartesian_compliance_controller.cpp index 6865e3fb..254d232c 100644 --- a/cartesian_compliance_controller/src/cartesian_compliance_controller.cpp +++ b/cartesian_compliance_controller/src/cartesian_compliance_controller.cpp @@ -53,8 +53,6 @@ CartesianComplianceController::CartesianComplianceController() { } -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianComplianceController::on_init() { @@ -77,21 +75,7 @@ CartesianComplianceController::on_init() return TYPE::SUCCESS; } -#elif defined CARTESIAN_CONTROLLERS_FOXY -controller_interface::return_type CartesianComplianceController::init( - const std::string & controller_name) -{ - using TYPE = controller_interface::return_type; - if (MotionBase::init(controller_name) != TYPE::OK || ForceBase::init(controller_name) != TYPE::OK) - { - return TYPE::ERROR; - } - auto_declare("compliance_ref_link", ""); - - return TYPE::OK; -} -#endif rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianComplianceController::on_configure(const rclcpp_lifecycle::State & previous_state) @@ -146,13 +130,8 @@ CartesianComplianceController::on_deactivate(const rclcpp_lifecycle::State & pre return TYPE::SUCCESS; } -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON controller_interface::return_type CartesianComplianceController::update( const rclcpp::Time & time, const rclcpp::Duration & period) -#elif defined CARTESIAN_CONTROLLERS_FOXY -controller_interface::return_type CartesianComplianceController::update() -#endif { // Synchronize the internal model and the real robot Base::m_ik_solver->synchronizeJointPositions(Base::m_joint_state_pos_handles); diff --git a/cartesian_controller_base/CMakeLists.txt b/cartesian_controller_base/CMakeLists.txt index bf6b0186..dc1beddd 100644 --- a/cartesian_controller_base/CMakeLists.txt +++ b/cartesian_controller_base/CMakeLists.txt @@ -8,16 +8,18 @@ add_compile_options(${ADDITIONAL_COMPILE_OPTIONS}) # Use CMake to pass the current ROS_DISTRO via variables into a preprocessor template. # We then include this file and switch between the different APIs. -if($ENV{ROS_DISTRO} STREQUAL "iron") - set(CARTESIAN_CONTROLLERS_IRON TRUE) +if($ENV{ROS_DISTRO} STREQUAL "jazzy") + set(CARTESIAN_CONTROLLERS_JAZZY TRUE) +elseif($ENV{ROS_DISTRO} STREQUAL "iron") + set(CARTESIAN_CONTROLLERS_IRON TRUE) elseif($ENV{ROS_DISTRO} STREQUAL "humble") - set(CARTESIAN_CONTROLLERS_HUMBLE TRUE) + set(CARTESIAN_CONTROLLERS_HUMBLE TRUE) elseif($ENV{ROS_DISTRO} STREQUAL "galactic") - set(CARTESIAN_CONTROLLERS_GALACTIC TRUE) + set(CARTESIAN_CONTROLLERS_GALACTIC TRUE) elseif($ENV{ROS_DISTRO} STREQUAL "foxy") - set(CARTESIAN_CONTROLLERS_FOXY TRUE) + set(CARTESIAN_CONTROLLERS_FOXY TRUE) else() - message(WARNING "ROS2 version must be {iron|humble|galactic|foxy}") + message(WARNING "ROS2 version must be {jazzy|iron|humble|galactic|foxy}") endif() configure_file(include/cartesian_controller_base/ROS2VersionConfig.h.in ROS2VersionConfig.h) diff --git a/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h b/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h index 9bc9f161..141a4164 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h +++ b/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h @@ -93,13 +93,10 @@ class DampedLeastSquaresSolver : public IKSolver * * \return True, if everything went well */ -#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON + bool init(std::shared_ptr nh, -#else - bool init(std::shared_ptr nh, -#endif - const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, - const KDL::JntArray & lower_pos_limits) override; + const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, + const KDL::JntArray & lower_pos_limits) override; private: std::shared_ptr m_jnt_jacobian_solver; diff --git a/cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h b/cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h index db6cc999..d353a8c3 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h +++ b/cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h @@ -103,13 +103,10 @@ class ForwardDynamicsSolver : public IKSolver * * @return True, if everything went well */ -#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON + bool init(std::shared_ptr nh, -#else - bool init(std::shared_ptr nh, -#endif - const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, - const KDL::JntArray & lower_pos_limits) override; + const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, + const KDL::JntArray & lower_pos_limits) override; private: //! Build a generic robot model for control diff --git a/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h b/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h index 79f0bc04..56958e35 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h +++ b/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h @@ -147,13 +147,10 @@ class IKSolver * * @return True, if everything went well */ -#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON + virtual bool init(std::shared_ptr nh, -#else - virtual bool init(std::shared_ptr nh, -#endif - const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, - const KDL::JntArray & lower_pos_limits); + const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, + const KDL::JntArray & lower_pos_limits); /** * @brief Update the robot kinematics of the solver diff --git a/cartesian_controller_base/include/cartesian_controller_base/JacobianTransposeSolver.h b/cartesian_controller_base/include/cartesian_controller_base/JacobianTransposeSolver.h index 28529e19..872b6196 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/JacobianTransposeSolver.h +++ b/cartesian_controller_base/include/cartesian_controller_base/JacobianTransposeSolver.h @@ -87,13 +87,10 @@ class JacobianTransposeSolver : public IKSolver * * \return True, if everything went well */ -#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON + bool init(std::shared_ptr /*nh*/, -#else - bool init(std::shared_ptr /*nh*/, -#endif - const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, - const KDL::JntArray & lower_pos_limits) override; + const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, + const KDL::JntArray & lower_pos_limits) override; private: std::shared_ptr m_jnt_jacobian_solver; diff --git a/cartesian_controller_base/include/cartesian_controller_base/PDController.h b/cartesian_controller_base/include/cartesian_controller_base/PDController.h index 75d237ef..d7824d76 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/PDController.h +++ b/cartesian_controller_base/include/cartesian_controller_base/PDController.h @@ -62,20 +62,14 @@ class PDController PDController(); ~PDController(); -#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON + void init(const std::string & params, std::shared_ptr handle); -#else - void init(const std::string & params, std::shared_ptr handle); -#endif double operator()(const double & error, const rclcpp::Duration & period); private: -#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON + std::shared_ptr m_handle; -#else - std::shared_ptr m_handle; ///< handle for dynamic parameter interaction -#endif std::string m_params; ///< namespace for parameter access // Gain parameters diff --git a/cartesian_controller_base/include/cartesian_controller_base/SelectivelyDampedLeastSquaresSolver.h b/cartesian_controller_base/include/cartesian_controller_base/SelectivelyDampedLeastSquaresSolver.h index a5b44be2..a94ac642 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/SelectivelyDampedLeastSquaresSolver.h +++ b/cartesian_controller_base/include/cartesian_controller_base/SelectivelyDampedLeastSquaresSolver.h @@ -91,13 +91,10 @@ class SelectivelyDampedLeastSquaresSolver : public IKSolver * * \return True, if everything went well */ -#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON + bool init(std::shared_ptr nh, -#else - bool init(std::shared_ptr nh, -#endif - const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, - const KDL::JntArray & lower_pos_limits) override; + const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, + const KDL::JntArray & lower_pos_limits) override; private: /** diff --git a/cartesian_controller_base/include/cartesian_controller_base/SpatialPDController.h b/cartesian_controller_base/include/cartesian_controller_base/SpatialPDController.h index 36dc8d69..fd79ceb9 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/SpatialPDController.h +++ b/cartesian_controller_base/include/cartesian_controller_base/SpatialPDController.h @@ -58,11 +58,7 @@ class SpatialPDController public: SpatialPDController(); -#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON bool init(std::shared_ptr params); -#else - bool init(std::shared_ptr params); -#endif /** * @brief Call operator for one control cycle diff --git a/cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.h b/cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.h index b526f817..7d02dfdb 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.h +++ b/cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.h @@ -87,12 +87,7 @@ class CartesianControllerBase : public controller_interface::ControllerInterface virtual controller_interface::InterfaceConfiguration state_interface_configuration() const override; -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON virtual LifecycleNodeInterface::CallbackReturn on_init() override; -#elif defined CARTESIAN_CONTROLLERS_FOXY - virtual controller_interface::return_type init(const std::string & controller_name) override; -#endif rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; diff --git a/cartesian_controller_base/src/DampedLeastSquaresSolver.cpp b/cartesian_controller_base/src/DampedLeastSquaresSolver.cpp index c03eba2f..21e13c0a 100644 --- a/cartesian_controller_base/src/DampedLeastSquaresSolver.cpp +++ b/cartesian_controller_base/src/DampedLeastSquaresSolver.cpp @@ -112,11 +112,7 @@ trajectory_msgs::msg::JointTrajectoryPoint DampedLeastSquaresSolver::getJointCon return control_cmd; } -#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON bool DampedLeastSquaresSolver::init(std::shared_ptr nh, -#else -bool DampedLeastSquaresSolver::init(std::shared_ptr nh, -#endif const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, const KDL::JntArray & lower_pos_limits) diff --git a/cartesian_controller_base/src/ForwardDynamicsSolver.cpp b/cartesian_controller_base/src/ForwardDynamicsSolver.cpp index d0d10513..7a11cd30 100644 --- a/cartesian_controller_base/src/ForwardDynamicsSolver.cpp +++ b/cartesian_controller_base/src/ForwardDynamicsSolver.cpp @@ -117,11 +117,7 @@ trajectory_msgs::msg::JointTrajectoryPoint ForwardDynamicsSolver::getJointContro return control_cmd; } -#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON bool ForwardDynamicsSolver::init(std::shared_ptr nh, -#else -bool ForwardDynamicsSolver::init(std::shared_ptr nh, -#endif const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, const KDL::JntArray & lower_pos_limits) { diff --git a/cartesian_controller_base/src/IKSolver.cpp b/cartesian_controller_base/src/IKSolver.cpp index 4d8e00d2..8aaa5e53 100644 --- a/cartesian_controller_base/src/IKSolver.cpp +++ b/cartesian_controller_base/src/IKSolver.cpp @@ -102,13 +102,10 @@ void IKSolver::synchronizeJointPositions( } } -#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON + bool IKSolver::init(std::shared_ptr /*nh*/, -#else -bool IKSolver::init(std::shared_ptr /*nh*/, -#endif - const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, - const KDL::JntArray & lower_pos_limits) + const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, + const KDL::JntArray & lower_pos_limits) { // Initialize m_chain = chain; diff --git a/cartesian_controller_base/src/JacobianTransposeSolver.cpp b/cartesian_controller_base/src/JacobianTransposeSolver.cpp index 5d5c7426..ddb96b17 100644 --- a/cartesian_controller_base/src/JacobianTransposeSolver.cpp +++ b/cartesian_controller_base/src/JacobianTransposeSolver.cpp @@ -102,13 +102,9 @@ trajectory_msgs::msg::JointTrajectoryPoint JacobianTransposeSolver::getJointCont return control_cmd; } -#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON bool JacobianTransposeSolver::init(std::shared_ptr nh, -#else -bool JacobianTransposeSolver::init(std::shared_ptr nh, -#endif - const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, - const KDL::JntArray & lower_pos_limits) + const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, + const KDL::JntArray & lower_pos_limits) { IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits); diff --git a/cartesian_controller_base/src/PDController.cpp b/cartesian_controller_base/src/PDController.cpp index efb41ae6..cb2735fc 100644 --- a/cartesian_controller_base/src/PDController.cpp +++ b/cartesian_controller_base/src/PDController.cpp @@ -47,12 +47,8 @@ PDController::PDController() : m_last_p_error(0.0) {} PDController::~PDController() {} -#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON void PDController::init(const std::string & params, std::shared_ptr handle) -#else -void PDController::init(const std::string & params, std::shared_ptr handle) -#endif { m_params = params; m_handle = std::move(handle); diff --git a/cartesian_controller_base/src/SelectivelyDampedLeastSquaresSolver.cpp b/cartesian_controller_base/src/SelectivelyDampedLeastSquaresSolver.cpp index d9eb0b61..0e56de86 100644 --- a/cartesian_controller_base/src/SelectivelyDampedLeastSquaresSolver.cpp +++ b/cartesian_controller_base/src/SelectivelyDampedLeastSquaresSolver.cpp @@ -136,14 +136,11 @@ trajectory_msgs::msg::JointTrajectoryPoint SelectivelyDampedLeastSquaresSolver:: return control_cmd; } -#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON + bool SelectivelyDampedLeastSquaresSolver::init(std::shared_ptr nh, -#else -bool SelectivelyDampedLeastSquaresSolver::init(std::shared_ptr nh, -#endif - const KDL::Chain & chain, - const KDL::JntArray & upper_pos_limits, - const KDL::JntArray & lower_pos_limits) + const KDL::Chain & chain, + const KDL::JntArray & upper_pos_limits, + const KDL::JntArray & lower_pos_limits) { IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits); diff --git a/cartesian_controller_base/src/SpatialPDController.cpp b/cartesian_controller_base/src/SpatialPDController.cpp index 8a6c7c49..e0a837a5 100644 --- a/cartesian_controller_base/src/SpatialPDController.cpp +++ b/cartesian_controller_base/src/SpatialPDController.cpp @@ -56,11 +56,8 @@ ctrl::Vector6D SpatialPDController::operator()(const ctrl::Vector6D & error, return m_cmd; } -#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON + bool SpatialPDController::init(std::shared_ptr handle) -#else -bool SpatialPDController::init(std::shared_ptr handle) -#endif { // Create pd controllers for each Cartesian dimension for (int i = 0; i < 6; ++i) // 3 transition, 3 rotation diff --git a/cartesian_controller_base/src/cartesian_controller_base.cpp b/cartesian_controller_base/src/cartesian_controller_base.cpp index b51441c4..1b13f980 100644 --- a/cartesian_controller_base/src/cartesian_controller_base.cpp +++ b/cartesian_controller_base/src/cartesian_controller_base.cpp @@ -86,8 +86,7 @@ CartesianControllerBase::state_interface_configuration() const return conf; } -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianControllerBase::on_init() { @@ -107,34 +106,6 @@ CartesianControllerBase::on_init() return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -#elif defined CARTESIAN_CONTROLLERS_FOXY -controller_interface::return_type CartesianControllerBase::init(const std::string & controller_name) -{ - if (!m_initialized) - { - // Initialize lifecycle node - const auto ret = ControllerInterface::init(controller_name); - if (ret != controller_interface::return_type::OK) - { - return ret; - } - - auto_declare("ik_solver", "forward_dynamics"); - auto_declare("robot_description", ""); - auto_declare("robot_base_link", ""); - auto_declare("end_effector_link", ""); - auto_declare>("joints", std::vector()); - auto_declare>("command_interfaces", std::vector()); - auto_declare("solver.error_scale", 1.0); - auto_declare("solver.iterations", 1); - auto_declare("solver.publish_state_feedback", false); - - m_initialized = true; - } - return controller_interface::return_type::OK; -} -#endif - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianControllerBase::on_configure(const rclcpp_lifecycle::State & previous_state) { @@ -385,12 +356,7 @@ void CartesianControllerBase::writeJointControlCmds() RCLCPP_ERROR( get_node()->get_logger(), "NaN detected in internal model. It's unlikely to recover from this. Shutting down."); - -#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON get_node()->shutdown(); -#elif defined CARTESIAN_CONTROLLERS_FOXY || defined CARTESIAN_CONTROLLERS_GALACTIC - this->shutdown(); -#endif return; } diff --git a/cartesian_controller_handles/include/cartesian_controller_handles/motion_control_handle.h b/cartesian_controller_handles/include/cartesian_controller_handles/motion_control_handle.h index 18dbe85f..08b77fab 100644 --- a/cartesian_controller_handles/include/cartesian_controller_handles/motion_control_handle.h +++ b/cartesian_controller_handles/include/cartesian_controller_handles/motion_control_handle.h @@ -75,12 +75,7 @@ class MotionControlHandle : public controller_interface::ControllerInterface MotionControlHandle(); ~MotionControlHandle(); -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON virtual LifecycleNodeInterface::CallbackReturn on_init() override; -#elif defined CARTESIAN_CONTROLLERS_FOXY - virtual controller_interface::return_type init(const std::string & controller_name) override; -#endif rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; @@ -94,13 +89,8 @@ class MotionControlHandle : public controller_interface::ControllerInterface controller_interface::InterfaceConfiguration command_interface_configuration() const override; controller_interface::InterfaceConfiguration state_interface_configuration() const override; -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override; -#elif defined CARTESIAN_CONTROLLERS_FOXY - controller_interface::return_type update() override; -#endif private: /** diff --git a/cartesian_controller_handles/src/motion_control_handle.cpp b/cartesian_controller_handles/src/motion_control_handle.cpp index 18cc210f..931e0d15 100644 --- a/cartesian_controller_handles/src/motion_control_handle.cpp +++ b/cartesian_controller_handles/src/motion_control_handle.cpp @@ -81,13 +81,8 @@ MotionControlHandle::on_deactivate(const rclcpp_lifecycle::State & previous_stat return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON controller_interface::return_type MotionControlHandle::update(const rclcpp::Time & time, const rclcpp::Duration & period) -#elif defined CARTESIAN_CONTROLLERS_FOXY -controller_interface::return_type MotionControlHandle::update() -#endif { // Publish marker pose m_current_pose.header.stamp = get_node()->now(); @@ -119,33 +114,20 @@ controller_interface::InterfaceConfiguration MotionControlHandle::state_interfac return conf; } -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn MotionControlHandle::on_init() { -#elif defined CARTESIAN_CONTROLLERS_FOXY -controller_interface::return_type MotionControlHandle::init(const std::string & controller_name) -{ - // Initialize lifecycle node - const auto ret = ControllerInterface::init(controller_name); - if (ret != controller_interface::return_type::OK) - { - return ret; - } -#endif + auto_declare("robot_description", ""); auto_declare("robot_base_link", ""); auto_declare("end_effector_link", ""); auto_declare >("joints", std::vector()); -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -#elif defined CARTESIAN_CONTROLLERS_FOXY - return controller_interface::return_type::OK; -#endif + } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn diff --git a/cartesian_controller_simulation/include/cartesian_controller_simulation/system_interface.h b/cartesian_controller_simulation/include/cartesian_controller_simulation/system_interface.h index b97c8d38..2e81ce07 100644 --- a/cartesian_controller_simulation/include/cartesian_controller_simulation/system_interface.h +++ b/cartesian_controller_simulation/include/cartesian_controller_simulation/system_interface.h @@ -52,9 +52,7 @@ #include "rclcpp/macros.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#if defined CARTESIAN_CONTROLLERS_FOXY -#include "hardware_interface/base_interface.hpp" -#endif + namespace cartesian_controller_simulation { @@ -71,12 +69,7 @@ constexpr char HW_IF_DAMPING[] = "damping"; * controller_manager coordinated library. * */ -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON class Simulator : public hardware_interface::SystemInterface -#elif defined CARTESIAN_CONTROLLERS_FOXY -class Simulator : public hardware_interface::BaseInterface -#endif { public: using return_type = hardware_interface::return_type; @@ -84,12 +77,7 @@ class Simulator : public hardware_interface::BaseInterface export_state_interfaces() override; @@ -99,19 +87,11 @@ class Simulator : public hardware_interface::BaseInterface & start_interfaces, const std::vector & stop_interfaces) override; -#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; -#elif defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_FOXY - return_type read() override; - return_type write() override; -#endif -#if defined CARTESIAN_CONTROLLERS_FOXY - return_type start() override; - return_type stop() override; -#endif private: // Command buffers for the controllers diff --git a/cartesian_controller_simulation/src/system_interface.cpp b/cartesian_controller_simulation/src/system_interface.cpp index 2d3ae9bd..3557352a 100644 --- a/cartesian_controller_simulation/src/system_interface.cpp +++ b/cartesian_controller_simulation/src/system_interface.cpp @@ -56,8 +56,7 @@ namespace cartesian_controller_simulation { -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON + Simulator::CallbackReturn Simulator::on_init(const hardware_interface::HardwareInfo & info) { // Keep an internal copy of the given configuration @@ -65,15 +64,7 @@ Simulator::CallbackReturn Simulator::on_init(const hardware_interface::HardwareI { return Simulator::CallbackReturn::ERROR; } -#elif defined CARTESIAN_CONTROLLERS_FOXY -Simulator::return_type Simulator::configure(const hardware_interface::HardwareInfo & info) -{ - // Keep an internal copy of the given configuration - if (configure_default(info) != return_type::OK) - { - return return_type::ERROR; - } -#endif + // Start the simulator in parallel. // Let the thread's destructor clean-up all resources // once users close the simulation window. @@ -104,12 +95,9 @@ Simulator::return_type Simulator::configure(const hardware_interface::HardwareIn { RCLCPP_ERROR(rclcpp::get_logger("Simulator"), "Joint '%s' needs two possible command interfaces.", joint.name.c_str()); -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON + return Simulator::CallbackReturn::ERROR; -#elif defined CARTESIAN_CONTROLLERS_FOXY - return Simulator::return_type::ERROR; -#endif + } if (!(joint.command_interfaces[0].name == hardware_interface::HW_IF_POSITION || @@ -119,24 +107,18 @@ Simulator::return_type Simulator::configure(const hardware_interface::HardwareIn "Joint '%s' needs the following command interfaces in that order: %s, %s.", joint.name.c_str(), hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY); -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON + return Simulator::CallbackReturn::ERROR; -#elif defined CARTESIAN_CONTROLLERS_FOXY - return Simulator::return_type::ERROR; -#endif + } if (joint.state_interfaces.size() != 3) { RCLCPP_ERROR(rclcpp::get_logger("Simulator"), "Joint '%s' needs 3 state interfaces.", joint.name.c_str()); -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON + return Simulator::CallbackReturn::ERROR; -#elif defined CARTESIAN_CONTROLLERS_FOXY - return Simulator::return_type::ERROR; -#endif + } if (!(joint.state_interfaces[0].name == hardware_interface::HW_IF_POSITION || @@ -147,21 +129,14 @@ Simulator::return_type Simulator::configure(const hardware_interface::HardwareIn "Joint '%s' needs the following state interfaces in that order: %s, %s, and %s.", joint.name.c_str(), hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_EFFORT); -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON + return Simulator::CallbackReturn::ERROR; -#elif defined CARTESIAN_CONTROLLERS_FOXY - return Simulator::return_type::ERROR; -#endif + } } -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON return Simulator::CallbackReturn::SUCCESS; -#elif defined CARTESIAN_CONTROLLERS_FOXY - return Simulator::return_type::OK; -#endif + } std::vector Simulator::export_state_interfaces() @@ -207,26 +182,12 @@ Simulator::return_type Simulator::prepare_command_mode_switch( return return_type::OK; } -#if defined CARTESIAN_CONTROLLERS_FOXY -Simulator::return_type Simulator::start() -{ - this->status_ = hardware_interface::status::STARTED; - return return_type::OK; -} -Simulator::return_type Simulator::stop() -{ - this->status_ = hardware_interface::status::STOPPED; - return return_type::OK; -} -#endif -#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON + Simulator::return_type Simulator::read([[maybe_unused]] const rclcpp::Time & time, [[maybe_unused]] const rclcpp::Duration & period) -#elif defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_FOXY -Simulator::return_type Simulator::read() -#endif + { MuJoCoSimulator::getInstance().read(m_positions, m_velocities, m_efforts); @@ -247,12 +208,10 @@ Simulator::return_type Simulator::read() return return_type::OK; } -#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON + Simulator::return_type Simulator::write([[maybe_unused]] const rclcpp::Time & time, [[maybe_unused]] const rclcpp::Duration & period) -#elif defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_FOXY -Simulator::return_type Simulator::write() -#endif + { MuJoCoSimulator::getInstance().write(m_position_commands, m_velocity_commands, m_stiffness, m_damping); diff --git a/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.h b/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.h index 6afa9c88..d02524c8 100644 --- a/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.h +++ b/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.h @@ -76,12 +76,8 @@ class CartesianForceController : public virtual cartesian_controller_base::Carte public: CartesianForceController(); -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON + virtual LifecycleNodeInterface::CallbackReturn on_init() override; -#elif defined CARTESIAN_CONTROLLERS_FOXY - virtual controller_interface::return_type init(const std::string & controller_name) override; -#endif rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; @@ -92,13 +88,9 @@ class CartesianForceController : public virtual cartesian_controller_base::Carte rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override; -#elif defined CARTESIAN_CONTROLLERS_FOXY - controller_interface::return_type update() override; -#endif + using Base = cartesian_controller_base::CartesianControllerBase; diff --git a/cartesian_force_controller/src/cartesian_force_controller.cpp b/cartesian_force_controller/src/cartesian_force_controller.cpp index ae3fd2c5..63a894eb 100644 --- a/cartesian_force_controller/src/cartesian_force_controller.cpp +++ b/cartesian_force_controller/src/cartesian_force_controller.cpp @@ -51,8 +51,7 @@ CartesianForceController::CartesianForceController() { } -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianForceController::on_init() { @@ -67,22 +66,7 @@ CartesianForceController::on_init() return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -#elif defined CARTESIAN_CONTROLLERS_FOXY -controller_interface::return_type CartesianForceController::init( - const std::string & controller_name) -{ - const auto ret = Base::init(controller_name); - if (ret != controller_interface::return_type::OK) - { - return ret; - } - auto_declare("ft_sensor_ref_link", ""); - auto_declare("hand_frame_control", true); - - return controller_interface::return_type::OK; -} -#endif rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianForceController::on_configure(const rclcpp_lifecycle::State & previous_state) @@ -136,13 +120,9 @@ CartesianForceController::on_deactivate(const rclcpp_lifecycle::State & previous return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON + controller_interface::return_type CartesianForceController::update(const rclcpp::Time & time, const rclcpp::Duration & period) -#elif defined CARTESIAN_CONTROLLERS_FOXY -controller_interface::return_type CartesianForceController::update() -#endif { // Synchronize the internal model and the real robot Base::m_ik_solver->synchronizeJointPositions(Base::m_joint_state_pos_handles); @@ -179,12 +159,7 @@ ctrl::Vector6D CartesianForceController::computeForceError() } // Superimpose target wrench and sensor wrench in base frame -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON return Base::displayInBaseLink(m_ft_sensor_wrench, m_new_ft_sensor_ref) + target_wrench; -#elif defined CARTESIAN_CONTROLLERS_FOXY - return m_ft_sensor_wrench + target_wrench; -#endif } void CartesianForceController::setFtSensorReferenceFrame(const std::string & new_ref) @@ -250,8 +225,6 @@ void CartesianForceController::ftSensorWrenchCallback( return; } -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON KDL::Wrench tmp; tmp[0] = wrench->wrench.force.x; tmp[1] = wrench->wrench.force.y; @@ -269,16 +242,6 @@ void CartesianForceController::ftSensorWrenchCallback( m_ft_sensor_wrench[3] = tmp[3]; m_ft_sensor_wrench[4] = tmp[4]; m_ft_sensor_wrench[5] = tmp[5]; -#elif defined CARTESIAN_CONTROLLERS_FOXY - // We assume base frame for the measurements - // This is currently URe-ROS2 driver-specific (branch foxy). - m_ft_sensor_wrench[0] = wrench->wrench.force.x; - m_ft_sensor_wrench[1] = wrench->wrench.force.y; - m_ft_sensor_wrench[2] = wrench->wrench.force.z; - m_ft_sensor_wrench[3] = wrench->wrench.torque.x; - m_ft_sensor_wrench[4] = wrench->wrench.torque.y; - m_ft_sensor_wrench[5] = wrench->wrench.torque.z; -#endif } } // namespace cartesian_force_controller diff --git a/cartesian_motion_controller/include/cartesian_motion_controller/cartesian_motion_controller.h b/cartesian_motion_controller/include/cartesian_motion_controller/cartesian_motion_controller.h index 92458b2c..c85b4fd0 100644 --- a/cartesian_motion_controller/include/cartesian_motion_controller/cartesian_motion_controller.h +++ b/cartesian_motion_controller/include/cartesian_motion_controller/cartesian_motion_controller.h @@ -77,12 +77,7 @@ class CartesianMotionController : public virtual cartesian_controller_base::Cart CartesianMotionController(); virtual ~CartesianMotionController() = default; -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON virtual LifecycleNodeInterface::CallbackReturn on_init() override; -#elif defined CARTESIAN_CONTROLLERS_FOXY - virtual controller_interface::return_type init(const std::string & controller_name) override; -#endif rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; @@ -93,13 +88,9 @@ class CartesianMotionController : public virtual cartesian_controller_base::Cart rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override; -#elif defined CARTESIAN_CONTROLLERS_FOXY - controller_interface::return_type update() override; -#endif + using Base = cartesian_controller_base::CartesianControllerBase; diff --git a/cartesian_motion_controller/src/cartesian_motion_controller.cpp b/cartesian_motion_controller/src/cartesian_motion_controller.cpp index 45037759..712b8b36 100644 --- a/cartesian_motion_controller/src/cartesian_motion_controller.cpp +++ b/cartesian_motion_controller/src/cartesian_motion_controller.cpp @@ -51,8 +51,6 @@ namespace cartesian_motion_controller { CartesianMotionController::CartesianMotionController() : Base::CartesianControllerBase() {} -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianMotionController::on_init() { @@ -64,19 +62,7 @@ CartesianMotionController::on_init() return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -#elif defined CARTESIAN_CONTROLLERS_FOXY -controller_interface::return_type CartesianMotionController::init( - const std::string & controller_name) -{ - const auto ret = Base::init(controller_name); - if (ret != controller_interface::return_type::OK) - { - return ret; - } - return controller_interface::return_type::OK; -} -#endif rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianMotionController::on_configure(const rclcpp_lifecycle::State & previous_state) @@ -114,13 +100,9 @@ CartesianMotionController::on_deactivate(const rclcpp_lifecycle::State & previou return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ - defined CARTESIAN_CONTROLLERS_IRON + controller_interface::return_type CartesianMotionController::update(const rclcpp::Time & time, const rclcpp::Duration & period) -#elif defined CARTESIAN_CONTROLLERS_FOXY -controller_interface::return_type CartesianMotionController::update() -#endif { // Synchronize the internal model and the real robot Base::m_ik_solver->synchronizeJointPositions(Base::m_joint_state_pos_handles);