Clean up code for adding a KinematicUnit to the RobotUnit

source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp

@@ -216,31 +216,68 @@ namespace armarx::RobotUnitModule
                     _module<Devices>().getSensorDevices().has(controlDeviceName) ?
                     _module<Devices>().getSensorDevices().index(controlDeviceName) :
                     std::numeric_limits<std::size_t>::max();
-                /// TODO use better kinematic unit controllers (posThroughVel + ramps)
-#define initializeKinematicUnit_tmp_helper(mode, CtargT, ctrl)                                                          \
-    {                                                                                                                   \
-        const auto& controlMode = mode;                                                                                 \
-        NJointKinematicUnitPassThroughControllerPtr& pt = ctrl;                                                           \
-        JointController* jointCtrl = controlDevice->getJointController(controlMode);                                           \
-        if (jointCtrl && jointCtrl->getControlTarget()->isA<CtargT>())                                                            \
-        {                                                                                                               \
-            NJointKinematicUnitPassThroughControllerConfigPtr config = new NJointKinematicUnitPassThroughControllerConfig;  \
-            config->controlMode=controlMode;                                                                            \
-            config->deviceName=controlDeviceName;                                                                       \
-            const NJointControllerBasePtr& nJointCtrl = _module<ControllerManagement>().createNJointController(                                                       \
-                    "NJointKinematicUnitPassThroughController",                                   \
-                    getName() + "_" + "NJointKU_PTCtrl_"+controlDeviceName+"_"+controlMode,                         \
-                    config,                                                                     \
-                    false, true);                                                               \
-            pt = NJointKinematicUnitPassThroughControllerPtr::dynamicCast(nJointCtrl);                                          \
-            ARMARX_CHECK_EXPRESSION(pt);                                                                                \
-        }                                                                                                               \
-    }
-                initializeKinematicUnit_tmp_helper(positionControlMode, ControlTarget1DoFActuatorPosition, ad.ctrlPos)
-                initializeKinematicUnit_tmp_helper(velocityControlMode, ControlTarget1DoFActuatorVelocity, ad.ctrlVel)
-                initializeKinematicUnit_tmp_helper(torqueControlMode, ControlTarget1DoFActuatorTorque, ad.ctrlTor)
-#undef initializeKinematicUnit_tmp_helper
 
+                const auto init_controller = [&](const std::string & requestedControlMode, auto & ctrl, const auto * tpptr)
+                {
+                    auto controlMode = requestedControlMode;
+                    using CtargT = std::remove_const_t<std::remove_reference_t<decltype(*tpptr)>>;
+                    NJointKinematicUnitPassThroughControllerPtr& pt = ctrl;
+
+                    auto testMode = [&](const auto & m)
+                    {
+                        JointController* jointCtrl = controlDevice->getJointController(m);
+                        return jointCtrl && jointCtrl->getControlTarget()->isA<CtargT>();
+                    };
+
+                    if (!testMode(controlMode))
+                    {
+                        controlMode = "";
+                        //try to find the correct mode (maybe the specified target was derived!
+                        //this only works, if exactly one controller provides this mode
+                        const JointController* selected_ctrl{nullptr};
+                        for (const auto ctrl : controlDevice->getJointControllers())
+                        {
+                            if (ctrl->getControlTarget()->isA<CtargT>())
+                            {
+                                if (selected_ctrl)
+                                {
+                                    selected_ctrl = nullptr;
+                                    ARMARX_INFO << "Autodetected two controllers supporting "
+                                                << requestedControlMode
+                                                << "! autoselection failed";
+                                    break;
+                                }
+                                selected_ctrl = ctrl;
+                            }
+                        }
+                        if (selected_ctrl)
+                        {
+                            controlMode = selected_ctrl->getControlMode();
+                            ARMARX_INFO << "Autodetected controller with mode "
+                                        << controlMode << "(the requested mode was "
+                                        << requestedControlMode << ")";
+                        }
+                    }
+
+                    if (!controlMode.empty())
+                    {
+                        NJointKinematicUnitPassThroughControllerConfigPtr config = new NJointKinematicUnitPassThroughControllerConfig;
+                        config->controlMode = controlMode;
+                        config->deviceName = controlDeviceName;
+                        auto ctrl_name = getName() + "_" + "NJointKU_PTCtrl_" + controlDeviceName + "_" + controlMode;
+                        std::replace(ctrl_name.begin(), ctrl_name.end(), '.', '_');
+                        std::replace(ctrl_name.begin(), ctrl_name.end(), '-', '_');
+                        std::replace(ctrl_name.begin(), ctrl_name.end(), ':', '_');
+                        const NJointControllerBasePtr& nJointCtrl = _module<ControllerManagement>().createNJointController(
+                                    "NJointKinematicUnitPassThroughController",
+                                    ctrl_name, config, false, true);
+                        pt = NJointKinematicUnitPassThroughControllerPtr::dynamicCast(nJointCtrl);
+                        ARMARX_CHECK_EXPRESSION(pt);
+                    }
+                };
+                init_controller(positionControlMode, ad.ctrlPos, (ControlTarget1DoFActuatorPosition*)0);
+                init_controller(velocityControlMode, ad.ctrlVel, (ControlTarget1DoFActuatorVelocity*)0);
+                init_controller(torqueControlMode,   ad.ctrlTor, (ControlTarget1DoFActuatorTorque*)0);
 
                 if (ad.ctrlPos || ad.ctrlTor || ad.ctrlVel)
                 {