drop support for foxy

fzi-forschungszentrum-informatik · May 27, 2024 · cd15e03 · cd15e03
1 parent dc80337
commit cd15e03
Show file tree

Hide file tree

Showing 27 changed files with 64 additions and 343 deletions.
diff --git a/...ance_controller/include/cartesian_compliance_controller/cartesian_compliance_controller.h b/...ance_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
@@ -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<std::string>("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
@@ -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
@@ -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<rclcpp_lifecycle::LifecycleNode> nh,
-#else
-  bool init(std::shared_ptr<rclcpp::Node> 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<KDL::ChainJntToJacSolver> 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
@@ -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<rclcpp_lifecycle::LifecycleNode> nh,
-#else
-  bool init(std::shared_ptr<rclcpp::Node> 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
@@ -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<rclcpp_lifecycle::LifecycleNode> nh,
-#else
-  virtual bool init(std::shared_ptr<rclcpp::Node> 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
@@ -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<rclcpp_lifecycle::LifecycleNode> /*nh*/,
-#else
-  bool init(std::shared_ptr<rclcpp::Node> /*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<KDL::ChainJntToJacSolver> 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
@@ -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<rclcpp_lifecycle::LifecycleNode> handle);
-#else
-  void init(const std::string & params, std::shared_ptr<rclcpp::Node> handle);
-#endif
 
   double operator()(const double & error, const rclcpp::Duration & period);
 
 private:
-#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
+
   std::shared_ptr<rclcpp_lifecycle::LifecycleNode> m_handle;
-#else
-  std::shared_ptr<rclcpp::Node> m_handle;  ///< handle for dynamic parameter interaction
-#endif
   std::string m_params;  ///< namespace for parameter access
 
   // Gain parameters

diff --git a/...n_controller_base/include/cartesian_controller_base/SelectivelyDampedLeastSquaresSolver.h b/...n_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<rclcpp_lifecycle::LifecycleNode> nh,
-#else
-  bool init(std::shared_ptr<rclcpp::Node> 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
@@ -58,11 +58,7 @@ class SpatialPDController
 public:
   SpatialPDController();
 
-#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
   bool init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> params);
-#else
-  bool init(std::shared_ptr<rclcpp::Node> 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
@@ -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
@@ -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<rclcpp_lifecycle::LifecycleNode> nh,
-#else
-bool DampedLeastSquaresSolver::init(std::shared_ptr<rclcpp::Node> 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
@@ -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<rclcpp_lifecycle::LifecycleNode> nh,
-#else
-bool ForwardDynamicsSolver::init(std::shared_ptr<rclcpp::Node> 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
@@ -102,13 +102,10 @@ void IKSolver::synchronizeJointPositions(
   }
 }
 
-#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
+
 bool IKSolver::init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> /*nh*/,
-#else
-bool IKSolver::init(std::shared_ptr<rclcpp::Node> /*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
@@ -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<rclcpp_lifecycle::LifecycleNode> nh,
-#else
-bool JacobianTransposeSolver::init(std::shared_ptr<rclcpp::Node> 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
@@ -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<rclcpp_lifecycle::LifecycleNode> handle)
-#else
-void PDController::init(const std::string & params, std::shared_ptr<rclcpp::Node> 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
@@ -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<rclcpp_lifecycle::LifecycleNode> nh,
-#else
-bool SelectivelyDampedLeastSquaresSolver::init(std::shared_ptr<rclcpp::Node> 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
@@ -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<rclcpp_lifecycle::LifecycleNode> handle)
-#else
-bool SpatialPDController::init(std::shared_ptr<rclcpp::Node> 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
@@ -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<std::string>("ik_solver", "forward_dynamics");
-    auto_declare<std::string>("robot_description", "");
-    auto_declare<std::string>("robot_base_link", "");
-    auto_declare<std::string>("end_effector_link", "");
-    auto_declare<std::vector<std::string>>("joints", std::vector<std::string>());
-    auto_declare<std::vector<std::string>>("command_interfaces", std::vector<std::string>());
-    auto_declare<double>("solver.error_scale", 1.0);
-    auto_declare<int>("solver.iterations", 1);
-    auto_declare<bool>("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;
   }