diff --git a/.github/workflows/industrial_ci_galactic_action.yml b/.github/workflows/industrial_ci_galactic_action.yml deleted file mode 100644 index 294821c4..00000000 --- a/.github/workflows/industrial_ci_galactic_action.yml +++ /dev/null @@ -1,19 +0,0 @@ -name: Galactic - -on: [push, pull_request] - - -jobs: - industrial_ci: - env: - BEFORE_BUILD_TARGET_WORKSPACE: '.github/script/install_mujoco.sh' - strategy: - fail-fast: false - matrix: - env: - - {ROS_DISTRO: galactic, ROS_REPO: main} - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v1 - - uses: 'ros-industrial/industrial_ci@master' - env: ${{matrix.env}} diff --git a/.github/workflows/industrial_ci_humble_action.yml b/.github/workflows/industrial_ci_humble_action.yml index 2135ee3b..b5e80b36 100644 --- a/.github/workflows/industrial_ci_humble_action.yml +++ b/.github/workflows/industrial_ci_humble_action.yml @@ -20,6 +20,6 @@ jobs: - {ROS_DISTRO: humble, ROS_REPO: main} runs-on: ubuntu-latest steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 - uses: 'ros-industrial/industrial_ci@master' env: ${{matrix.env}} diff --git a/.github/workflows/industrial_ci_iron_action.yml b/.github/workflows/industrial_ci_iron_action.yml index 3abdb455..1baba3de 100644 --- a/.github/workflows/industrial_ci_iron_action.yml +++ b/.github/workflows/industrial_ci_iron_action.yml @@ -20,6 +20,6 @@ jobs: - {ROS_DISTRO: iron, ROS_REPO: main} runs-on: ubuntu-latest steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 - uses: 'ros-industrial/industrial_ci@master' env: ${{matrix.env}} diff --git a/.github/workflows/industrial_ci_foxy_action.yml b/.github/workflows/industrial_ci_jazzy_action.yml similarity index 51% rename from .github/workflows/industrial_ci_foxy_action.yml rename to .github/workflows/industrial_ci_jazzy_action.yml index 0e5d0337..15853ce0 100644 --- a/.github/workflows/industrial_ci_foxy_action.yml +++ b/.github/workflows/industrial_ci_jazzy_action.yml @@ -1,6 +1,11 @@ -name: Foxy +name: Jazzy -on: [push, pull_request] +on: + push: + pull_request: + schedule: + # Run every Friday at 6:30 am to detect breaking APIs + - cron: '30 6 * * 5' jobs: @@ -11,9 +16,10 @@ jobs: fail-fast: false matrix: env: - - {ROS_DISTRO: foxy, ROS_REPO: main} + - {ROS_DISTRO: jazzy, ROS_REPO: testing} + - {ROS_DISTRO: jazzy, ROS_REPO: main} runs-on: ubuntu-latest steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 - uses: 'ros-industrial/industrial_ci@master' env: ${{matrix.env}} diff --git a/README.md b/README.md index 0f14b346..8ed3dcf8 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,6 @@ -![build badge](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers/actions/workflows/industrial_ci_foxy_action.yml/badge.svg) -![build badge](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers/actions/workflows/industrial_ci_galactic_action.yml/badge.svg) ![build badge](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers/actions/workflows/industrial_ci_humble_action.yml/badge.svg) ![build badge](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers/actions/workflows/industrial_ci_iron_action.yml/badge.svg) +![build badge](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers/actions/workflows/industrial_ci_jazzy_action.yml/badge.svg) [![License](https://img.shields.io/badge/License-BSD_3--Clause-blue.svg)](https://opensource.org/licenses/BSD-3-Clause) # Cartesian Controllers 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..5eae3a3e 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; @@ -91,13 +86,8 @@ 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..22bd4650 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,6 @@ 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 +129,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 fc14c308..1f6ff9b7 100644 --- a/cartesian_controller_base/CMakeLists.txt +++ b/cartesian_controller_base/CMakeLists.txt @@ -13,16 +13,14 @@ endif(NOT CMAKE_BUILD_TYPE) # 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) -elseif($ENV{ROS_DISTRO} STREQUAL "galactic") - set(CARTESIAN_CONTROLLERS_GALACTIC TRUE) -elseif($ENV{ROS_DISTRO} STREQUAL "foxy") - set(CARTESIAN_CONTROLLERS_FOXY TRUE) + set(CARTESIAN_CONTROLLERS_HUMBLE TRUE) else() - message(WARNING "ROS2 version must be {iron|humble|galactic|foxy}") + message(WARNING "ROS2 version must be {jazzy|iron|humble}") 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..79d8580e 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h +++ b/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h @@ -93,12 +93,9 @@ 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, + + bool init(std::shared_ptr nh, 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/ForwardDynamicsSolver.h b/cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h index db6cc999..85289772 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h +++ b/cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h @@ -103,12 +103,9 @@ 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, + + bool init(std::shared_ptr nh, 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/IKSolver.h b/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h index 79f0bc04..5bfcb357 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h +++ b/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h @@ -147,13 +147,9 @@ 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); + + virtual bool init(std::shared_ptr nh, 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..27314a85 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/JacobianTransposeSolver.h +++ b/cartesian_controller_base/include/cartesian_controller_base/JacobianTransposeSolver.h @@ -87,12 +87,9 @@ 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, + + bool init(std::shared_ptr /*nh*/, 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/PDController.h b/cartesian_controller_base/include/cartesian_controller_base/PDController.h index 75d237ef..34069846 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/PDController.h +++ b/cartesian_controller_base/include/cartesian_controller_base/PDController.h @@ -62,20 +62,12 @@ 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/ROS2VersionConfig.h.in b/cartesian_controller_base/include/cartesian_controller_base/ROS2VersionConfig.h.in index eae574b8..cf2a8220 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/ROS2VersionConfig.h.in +++ b/cartesian_controller_base/include/cartesian_controller_base/ROS2VersionConfig.h.in @@ -1,7 +1,7 @@ // Cmake will update these defines depending on our current ROS_DISTRO. // We use this mechanism to react to API changes in ROS2-control. // -#cmakedefine CARTESIAN_CONTROLLERS_FOXY -#cmakedefine CARTESIAN_CONTROLLERS_GALACTIC + #cmakedefine CARTESIAN_CONTROLLERS_HUMBLE #cmakedefine CARTESIAN_CONTROLLERS_IRON +#cmakedefine CARTESIAN_CONTROLLERS_JAZZY diff --git a/cartesian_controller_base/include/cartesian_controller_base/SelectivelyDampedLeastSquaresSolver.h b/cartesian_controller_base/include/cartesian_controller_base/SelectivelyDampedLeastSquaresSolver.h index a5b44be2..117cd496 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/SelectivelyDampedLeastSquaresSolver.h +++ b/cartesian_controller_base/include/cartesian_controller_base/SelectivelyDampedLeastSquaresSolver.h @@ -91,12 +91,9 @@ 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, + + bool init(std::shared_ptr nh, 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..20443fda 100644 --- a/cartesian_controller_base/src/IKSolver.cpp +++ b/cartesian_controller_base/src/IKSolver.cpp @@ -102,11 +102,7 @@ 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) { diff --git a/cartesian_controller_base/src/JacobianTransposeSolver.cpp b/cartesian_controller_base/src/JacobianTransposeSolver.cpp index 5d5c7426..a1278e76 100644 --- a/cartesian_controller_base/src/JacobianTransposeSolver.cpp +++ b/cartesian_controller_base/src/JacobianTransposeSolver.cpp @@ -102,11 +102,7 @@ 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) { 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..a069e559 100644 --- a/cartesian_controller_base/src/SelectivelyDampedLeastSquaresSolver.cpp +++ b/cartesian_controller_base/src/SelectivelyDampedLeastSquaresSolver.cpp @@ -136,11 +136,7 @@ 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) diff --git a/cartesian_controller_base/src/SpatialPDController.cpp b/cartesian_controller_base/src/SpatialPDController.cpp index 8a6c7c49..327738d1 100644 --- a/cartesian_controller_base/src/SpatialPDController.cpp +++ b/cartesian_controller_base/src/SpatialPDController.cpp @@ -56,11 +56,7 @@ 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..528bf3a4 100644 --- a/cartesian_controller_base/src/cartesian_controller_base.cpp +++ b/cartesian_controller_base/src/cartesian_controller_base.cpp @@ -86,8 +86,6 @@ 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 +105,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) { @@ -161,7 +131,12 @@ CartesianControllerBase::on_configure(const rclcpp_lifecycle::State & previous_s urdf::Model robot_model; KDL::Tree robot_tree; +#if defined CARTESIAN_CONTROLLERS_JAZZY + m_robot_description = this->get_robot_description(); +#else m_robot_description = get_node()->get_parameter("robot_description").as_string(); +#endif + if (m_robot_description.empty()) { RCLCPP_ERROR(get_node()->get_logger(), "robot_description is empty"); @@ -385,12 +360,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 f29e404a..5239b9fa 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 { if (m_current_pose.header.frame_id != m_robot_base_link && m_current_pose.header.frame_id != "") { @@ -128,33 +123,15 @@ 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..fb4889be 100644 --- a/cartesian_controller_simulation/include/cartesian_controller_simulation/system_interface.h +++ b/cartesian_controller_simulation/include/cartesian_controller_simulation/system_interface.h @@ -52,10 +52,6 @@ #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 { // Two custom hardware interfaces for torque-actuated robots: @@ -71,12 +67,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 +75,7 @@ class Simulator : public hardware_interface::BaseInterface export_state_interfaces() override; @@ -99,20 +85,9 @@ 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 std::vector m_position_commands; diff --git a/cartesian_controller_simulation/launch/simulation.launch.py b/cartesian_controller_simulation/launch/simulation.launch.py index 538a46e5..f23e6e0d 100644 --- a/cartesian_controller_simulation/launch/simulation.launch.py +++ b/cartesian_controller_simulation/launch/simulation.launch.py @@ -46,14 +46,6 @@ from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare -import os - -distro = os.environ["ROS_DISTRO"] -if distro in ["galactic", "humble", "iron"]: - spawner = "spawner" -else: # foxy - spawner = "spawner.py" - def generate_launch_description(): # Declare arguments @@ -99,9 +91,10 @@ def generate_launch_description(): package="controller_manager", executable="ros2_control_node", parameters=[robot_description, robot_controllers], - # prefix="screen -d -m gdb -command=/home/scherzin/.ros/my_debug_log --ex run --args", # noqa: E501 + # prefix="screen -d -m gdb -command=/home/stefan/.gdb_debug_config --ex run --args", # noqa E501 output="both", remappings=[ + ("~/robot_description", "/robot_description"), ("motion_control_handle/target_frame", "target_frame"), ("cartesian_motion_controller/target_frame", "target_frame"), ("cartesian_compliance_controller/target_frame", "target_frame"), @@ -116,7 +109,7 @@ def generate_launch_description(): def controller_spawner(name, *args): return Node( package="controller_manager", - executable=spawner, + executable="spawner", output="screen", arguments=[name] + [a for a in args], ) @@ -137,7 +130,7 @@ def controller_spawner(name, *args): "invalid_cartesian_compliance_controller", "invalid_cartesian_force_controller", ] - state = "--inactive" if distro in ["humble", "iron"] else "--stopped" + state = "--inactive" inactive_spawners = [ controller_spawner(controller, state) for controller in inactive_list ] diff --git a/cartesian_controller_simulation/src/system_interface.cpp b/cartesian_controller_simulation/src/system_interface.cpp index 2d3ae9bd..e0c4ff69 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,8 @@ 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 +106,16 @@ 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 +126,12 @@ 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 +177,9 @@ 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 +200,9 @@ 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_controller_tests/integration_tests/integration_tests.py b/cartesian_controller_tests/integration_tests/integration_tests.py index 0266e99d..ad87b6ba 100755 --- a/cartesian_controller_tests/integration_tests/integration_tests.py +++ b/cartesian_controller_tests/integration_tests/integration_tests.py @@ -7,7 +7,6 @@ from launch.substitutions import PathJoinSubstitution from launch_ros.substitutions import FindPackageShare -import os import time import rclpy from rclpy.node import Node @@ -16,8 +15,6 @@ from geometry_msgs.msg import PoseStamped from geometry_msgs.msg import WrenchStamped -distro = os.environ["ROS_DISTRO"] - def generate_test_description(): setup = IncludeLaunchDescription( @@ -118,10 +115,7 @@ def test_invalid_controller_initialization(self): controller manager contains our controllers and if they have the expected state. """ - if os.environ["ROS_DISTRO"] == "humble" or os.environ["ROS_DISTRO"] == "iron": - expected_state = "unconfigured" - else: # galactic, foxy - expected_state = "finalized" + expected_state = "unconfigured" for name in self.invalid_controllers: self.assertTrue( self.check_state(name, expected_state), @@ -215,19 +209,13 @@ def check_state(self, controller, state): def start_controller(self, controller): """Start the given controller""" req = SwitchController.Request() - if distro in ["humble", "iron"]: - req.activate_controllers = [controller] - else: - req.start_controllers = [controller] + req.activate_controllers = [controller] self.perform_switch(req) def stop_controller(self, controller): """Stop the given controller""" req = SwitchController.Request() - if distro in ["humble", "iron"]: - req.deactivate_controllers = [controller] - else: - req.stop_controllers = [controller] + req.deactivate_controllers = [controller] self.perform_switch(req) def perform_switch(self, req): 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..9b6a4f55 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,7 @@ 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 +87,8 @@ 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..5230a525 100644 --- a/cartesian_force_controller/src/cartesian_force_controller.cpp +++ b/cartesian_force_controller/src/cartesian_force_controller.cpp @@ -51,8 +51,6 @@ 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 +65,6 @@ 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 +118,8 @@ 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 +156,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 +222,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 +239,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..d4784b4a 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,8 @@ 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..33b0756f 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,6 @@ 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 +99,8 @@ 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);