feat(controllers): allow control type change after construction

CHANGELOG.md

@@ -26,6 +26,7 @@ Release Versions:
 ## Upcoming changes
 
 - fix(controllers): check for finite wrench values (#216)
+- feat(controllers): allow control type change after construction (#217)
 
 ## 5.2.3
 

source/modulo_controllers/include/modulo_controllers/RobotControllerInterface.hpp

@@ -14,7 +14,7 @@ namespace modulo_controllers {
 /**
  * @class RobotControllerInterface
  * @brief Base controller class that automatically associates joints with a JointState object
- * @details The robot controller interface extends the functionality of the modulo controller interface byautomatically
+ * @details The robot controller interface extends the functionality of the modulo controller interface by automatically
  * claiming all state interfaces from joints and command interfaces of a given type (position, velocity, effort or
  * acceleration) for those same joints. Joint state and command are associated with these interfaces and abstracted as
  * JointState pointers for derived classes to access. A robot model, Cartesian state and force-torque sensor state are
@@ -100,6 +100,18 @@ class RobotControllerInterface : public ControllerInterface {
   bool
   on_validate_parameter_callback(const std::shared_ptr<state_representation::ParameterInterface>& parameter) override;
 
+  /**
+   * @brief Get the control type of the controller, empty or one of [position, velocity, effort or acceleration]
+   */
+  std::string get_control_type() const;
+
+  /**
+   * @brief Set the control type of the controller after construction
+   * @param control_type One of [position, velocity, effort or acceleration]
+   * @throws std::runtime_error if the control type is already fixed (after configuring) or invalid
+   */
+  void set_control_type(const std::string& control_type);
+
   std::shared_ptr<robot_model::Model> robot_;///< Robot model object generated from URDF
   std::string task_space_frame_;///< The frame in task space for forward kinematics calculations, if applicable
 
@@ -121,6 +133,7 @@ class RobotControllerInterface : public ControllerInterface {
 
   std::vector<std::string> joints_;///< The joint names provided by a parameter
   std::string control_type_;       ///< The high-level interface type (position, velocity, acceleration or effort)
+  bool control_type_fixed_;        ///< If true, the control type cannot be changed after
 
   bool robot_model_required_;///< If true, check that a robot model is available on configure
   bool load_geometries_;     ///< If true, load geometries from the URDF into the robot model

source/modulo_controllers/src/RobotControllerInterface.cpp

@@ -1,6 +1,6 @@
 #include "modulo_controllers/RobotControllerInterface.hpp"
 
-#include "ament_index_cpp/get_package_share_directory.hpp"
+#include <ament_index_cpp/get_package_share_directory.hpp>
 #include <hardware_interface/types/hardware_interface_type_values.hpp>
 
 #include <state_representation/exceptions/JointNotFoundException.hpp>
@@ -21,14 +21,15 @@ RobotControllerInterface::RobotControllerInterface(
     bool robot_model_required, const std::string& control_type, bool load_geometries)
     : ControllerInterface(true),
       control_type_(control_type),
+      control_type_fixed_(false),
       robot_model_required_(robot_model_required),
       load_geometries_(load_geometries),
       new_joint_command_ready_(false),
       command_decay_factor_(0.0),
       command_rate_limit_(std::numeric_limits<double>::infinity()) {
   if (!control_type.empty() && interface_map.find(control_type) == interface_map.cend()) {
     RCLCPP_ERROR(get_node()->get_logger(), "Invalid control type: %s", control_type.c_str());
-    throw std::invalid_argument("Invalid control type");
+    throw std::invalid_argument("Invalid control type: " + control_type);
   }
 }
 
@@ -144,6 +145,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotC
     for (const auto& joint : joints_) {
       add_command_interface(joint, control_type_);
     }
+    control_type_fixed_ = true;
   }
 
   auto ft_sensor_name = get_parameter("ft_sensor_name");
@@ -377,4 +379,18 @@ bool RobotControllerInterface::on_validate_parameter_callback(const std::shared_
   return true;
 }
 
+std::string RobotControllerInterface::get_control_type() const {
+  return control_type_;
+}
+
+void RobotControllerInterface::set_control_type(const std::string& control_type) {
+  if (control_type_fixed_) {
+    throw std::runtime_error("Control type is fixed and cannot be changed anymore");
+  }
+  if (!control_type.empty() && interface_map.find(control_type) == interface_map.cend()) {
+    throw std::runtime_error("Invalid control type: " + control_type);
+  }
+  control_type_ = control_type;
+}
+
 }// namespace modulo_controllers