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 833a4332..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 @@ -85,7 +85,7 @@ class CartesianComplianceController : public cartesian_motion_controller::Cartes rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - + controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override; diff --git a/cartesian_compliance_controller/src/cartesian_compliance_controller.cpp b/cartesian_compliance_controller/src/cartesian_compliance_controller.cpp index 254d232c..22bd4650 100644 --- a/cartesian_compliance_controller/src/cartesian_compliance_controller.cpp +++ b/cartesian_compliance_controller/src/cartesian_compliance_controller.cpp @@ -76,7 +76,6 @@ CartesianComplianceController::on_init() return TYPE::SUCCESS; } - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianComplianceController::on_configure(const rclcpp_lifecycle::State & previous_state) { diff --git a/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h b/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h index 141a4164..79d8580e 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h +++ b/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h @@ -94,9 +94,9 @@ class DampedLeastSquaresSolver : public IKSolver * \return True, if everything went well */ - bool init(std::shared_ptr nh, - const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, - const KDL::JntArray & lower_pos_limits) override; + bool init(std::shared_ptr nh, 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 d353a8c3..85289772 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h +++ b/cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h @@ -104,9 +104,9 @@ class ForwardDynamicsSolver : public IKSolver * @return True, if everything went well */ - bool init(std::shared_ptr nh, - const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, - const KDL::JntArray & lower_pos_limits) override; + bool init(std::shared_ptr nh, 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 56958e35..5bfcb357 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h +++ b/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h @@ -148,9 +148,8 @@ class IKSolver * @return True, if everything went well */ - virtual bool init(std::shared_ptr nh, - 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 872b6196..27314a85 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/JacobianTransposeSolver.h +++ b/cartesian_controller_base/include/cartesian_controller_base/JacobianTransposeSolver.h @@ -88,9 +88,9 @@ class JacobianTransposeSolver : public IKSolver * \return True, if everything went well */ - bool init(std::shared_ptr /*nh*/, - const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, - const KDL::JntArray & lower_pos_limits) override; + bool init(std::shared_ptr /*nh*/, 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 d7824d76..34069846 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/PDController.h +++ b/cartesian_controller_base/include/cartesian_controller_base/PDController.h @@ -62,13 +62,11 @@ class PDController PDController(); ~PDController(); - void init(const std::string & params, std::shared_ptr handle); double operator()(const double & error, const rclcpp::Duration & period); private: - std::shared_ptr m_handle; std::string m_params; ///< namespace for parameter access diff --git a/cartesian_controller_base/include/cartesian_controller_base/SelectivelyDampedLeastSquaresSolver.h b/cartesian_controller_base/include/cartesian_controller_base/SelectivelyDampedLeastSquaresSolver.h index a94ac642..117cd496 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/SelectivelyDampedLeastSquaresSolver.h +++ b/cartesian_controller_base/include/cartesian_controller_base/SelectivelyDampedLeastSquaresSolver.h @@ -92,9 +92,9 @@ class SelectivelyDampedLeastSquaresSolver : public IKSolver * \return True, if everything went well */ - bool init(std::shared_ptr nh, - const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, - const KDL::JntArray & lower_pos_limits) override; + 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/src/IKSolver.cpp b/cartesian_controller_base/src/IKSolver.cpp index 8aaa5e53..20443fda 100644 --- a/cartesian_controller_base/src/IKSolver.cpp +++ b/cartesian_controller_base/src/IKSolver.cpp @@ -102,10 +102,9 @@ void IKSolver::synchronizeJointPositions( } } - bool IKSolver::init(std::shared_ptr /*nh*/, - 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 ddb96b17..a1278e76 100644 --- a/cartesian_controller_base/src/JacobianTransposeSolver.cpp +++ b/cartesian_controller_base/src/JacobianTransposeSolver.cpp @@ -103,8 +103,8 @@ trajectory_msgs::msg::JointTrajectoryPoint JacobianTransposeSolver::getJointCont } bool JacobianTransposeSolver::init(std::shared_ptr nh, - 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/SelectivelyDampedLeastSquaresSolver.cpp b/cartesian_controller_base/src/SelectivelyDampedLeastSquaresSolver.cpp index 0e56de86..a069e559 100644 --- a/cartesian_controller_base/src/SelectivelyDampedLeastSquaresSolver.cpp +++ b/cartesian_controller_base/src/SelectivelyDampedLeastSquaresSolver.cpp @@ -136,11 +136,10 @@ trajectory_msgs::msg::JointTrajectoryPoint SelectivelyDampedLeastSquaresSolver:: return control_cmd; } - bool SelectivelyDampedLeastSquaresSolver::init(std::shared_ptr nh, - 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 e0a837a5..327738d1 100644 --- a/cartesian_controller_base/src/SpatialPDController.cpp +++ b/cartesian_controller_base/src/SpatialPDController.cpp @@ -56,7 +56,6 @@ ctrl::Vector6D SpatialPDController::operator()(const ctrl::Vector6D & error, return m_cmd; } - bool SpatialPDController::init(std::shared_ptr handle) { // Create pd controllers for each Cartesian dimension diff --git a/cartesian_controller_base/src/cartesian_controller_base.cpp b/cartesian_controller_base/src/cartesian_controller_base.cpp index 1b13f980..0948584b 100644 --- a/cartesian_controller_base/src/cartesian_controller_base.cpp +++ b/cartesian_controller_base/src/cartesian_controller_base.cpp @@ -86,7 +86,6 @@ CartesianControllerBase::state_interface_configuration() const return conf; } - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianControllerBase::on_init() { diff --git a/cartesian_controller_handles/src/motion_control_handle.cpp b/cartesian_controller_handles/src/motion_control_handle.cpp index 85d71cc0..5239b9fa 100644 --- a/cartesian_controller_handles/src/motion_control_handle.cpp +++ b/cartesian_controller_handles/src/motion_control_handle.cpp @@ -123,20 +123,15 @@ controller_interface::InterfaceConfiguration MotionControlHandle::state_interfac return conf; } - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn MotionControlHandle::on_init() { - - auto_declare("robot_description", ""); auto_declare("robot_base_link", ""); auto_declare("end_effector_link", ""); auto_declare >("joints", std::vector()); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } 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 2e81ce07..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,8 +52,6 @@ #include "rclcpp/macros.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" - - namespace cartesian_controller_simulation { // Two custom hardware interfaces for torque-actuated robots: @@ -87,12 +85,9 @@ class Simulator : public hardware_interface::SystemInterface const std::vector & start_interfaces, const std::vector & stop_interfaces) override; - return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; - - private: // Command buffers for the controllers std::vector m_position_commands; diff --git a/cartesian_controller_simulation/src/system_interface.cpp b/cartesian_controller_simulation/src/system_interface.cpp index 3557352a..e0c4ff69 100644 --- a/cartesian_controller_simulation/src/system_interface.cpp +++ b/cartesian_controller_simulation/src/system_interface.cpp @@ -97,7 +97,6 @@ Simulator::CallbackReturn Simulator::on_init(const hardware_interface::HardwareI "Joint '%s' needs two possible command interfaces.", joint.name.c_str()); return Simulator::CallbackReturn::ERROR; - } if (!(joint.command_interfaces[0].name == hardware_interface::HW_IF_POSITION || @@ -109,7 +108,6 @@ Simulator::CallbackReturn Simulator::on_init(const hardware_interface::HardwareI hardware_interface::HW_IF_VELOCITY); return Simulator::CallbackReturn::ERROR; - } if (joint.state_interfaces.size() != 3) @@ -118,7 +116,6 @@ Simulator::CallbackReturn Simulator::on_init(const hardware_interface::HardwareI joint.name.c_str()); return Simulator::CallbackReturn::ERROR; - } if (!(joint.state_interfaces[0].name == hardware_interface::HW_IF_POSITION || @@ -131,12 +128,10 @@ Simulator::CallbackReturn Simulator::on_init(const hardware_interface::HardwareI hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_EFFORT); return Simulator::CallbackReturn::ERROR; - } } return Simulator::CallbackReturn::SUCCESS; - } std::vector Simulator::export_state_interfaces() @@ -182,9 +177,6 @@ Simulator::return_type Simulator::prepare_command_mode_switch( return return_type::OK; } - - - Simulator::return_type Simulator::read([[maybe_unused]] const rclcpp::Time & time, [[maybe_unused]] const rclcpp::Duration & period) @@ -208,7 +200,6 @@ Simulator::return_type Simulator::read([[maybe_unused]] const rclcpp::Time & tim return return_type::OK; } - Simulator::return_type Simulator::write([[maybe_unused]] const rclcpp::Time & time, [[maybe_unused]] const rclcpp::Duration & period) 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 d02524c8..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,7 +76,6 @@ class CartesianForceController : public virtual cartesian_controller_base::Carte public: CartesianForceController(); - virtual LifecycleNodeInterface::CallbackReturn on_init() override; rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( @@ -91,7 +90,6 @@ class CartesianForceController : public virtual cartesian_controller_base::Carte controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override; - using Base = cartesian_controller_base::CartesianControllerBase; protected: diff --git a/cartesian_force_controller/src/cartesian_force_controller.cpp b/cartesian_force_controller/src/cartesian_force_controller.cpp index 63a894eb..5230a525 100644 --- a/cartesian_force_controller/src/cartesian_force_controller.cpp +++ b/cartesian_force_controller/src/cartesian_force_controller.cpp @@ -51,7 +51,6 @@ CartesianForceController::CartesianForceController() { } - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianForceController::on_init() { @@ -67,7 +66,6 @@ CartesianForceController::on_init() return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianForceController::on_configure(const rclcpp_lifecycle::State & previous_state) { @@ -120,7 +118,6 @@ CartesianForceController::on_deactivate(const rclcpp_lifecycle::State & previous return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - controller_interface::return_type CartesianForceController::update(const rclcpp::Time & time, const rclcpp::Duration & period) { 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 c85b4fd0..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 @@ -91,7 +91,6 @@ class CartesianMotionController : public virtual cartesian_controller_base::Cart controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override; - using Base = cartesian_controller_base::CartesianControllerBase; protected: diff --git a/cartesian_motion_controller/src/cartesian_motion_controller.cpp b/cartesian_motion_controller/src/cartesian_motion_controller.cpp index 712b8b36..33b0756f 100644 --- a/cartesian_motion_controller/src/cartesian_motion_controller.cpp +++ b/cartesian_motion_controller/src/cartesian_motion_controller.cpp @@ -63,7 +63,6 @@ CartesianMotionController::on_init() return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianMotionController::on_configure(const rclcpp_lifecycle::State & previous_state) { @@ -100,7 +99,6 @@ CartesianMotionController::on_deactivate(const rclcpp_lifecycle::State & previou return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - controller_interface::return_type CartesianMotionController::update(const rclcpp::Time & time, const rclcpp::Duration & period) {