Apply changes from 0e87721a ("made velocity...

Apply changes from 0e87721a ("made velocity mode for kinematicunit configurable")

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

@@ -151,10 +151,9 @@ namespace armarx
             ARMARX_INFO << "initializing default units...done! " << (MicroNow() - beg).count() << " us";
         }
 
+
         void Units::initializeKinematicUnit()
         {
-            /// TODO move init code to Kinematic sub unit
-            using UnitT = KinematicSubUnit;
             using IfaceT = KinematicUnitInterface;
 
             auto guard = getGuard();
@@ -164,6 +163,24 @@ namespace armarx
             {
                 return;
             }
+            auto unit = createKinematicSubUnit(getIceProperties()->clone());
+            //add
+            if (unit)
+            {
+                addUnit(std::move(unit));
+            }
+        }
+
+        ManagedIceObjectPtr Units::createKinematicSubUnit(
+            const Ice::PropertiesPtr& properties,
+            const std::string& positionControlMode,
+            const std::string& velocityControlMode,
+            const std::string& torqueControlMode
+        )
+        {
+            /// TODO move init code to Kinematic sub unit
+            using UnitT = KinematicSubUnit;
+
             //add ctrl et al
             std::map<std::string, UnitT::ActuatorData> devs;
             for (const ControlDevicePtr& controlDevice : _module<Devices>().getControlDevices().values())
@@ -198,9 +215,9 @@ namespace armarx
             ARMARX_CHECK_EXPRESSION(pt);                                                                                \
         }                                                                                                               \
     }
-                    initializeKinematicUnit_tmp_helper(ControlModes::Position1DoF, ControlTarget1DoFActuatorPosition, ad.ctrlPos)
-                    initializeKinematicUnit_tmp_helper(ControlModes::Velocity1DoF, ControlTarget1DoFActuatorVelocity, ad.ctrlVel)
-                    initializeKinematicUnit_tmp_helper(ControlModes::Torque1DoF  , ControlTarget1DoFActuatorTorque  , ad.ctrlTor)
+                    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
 
 
@@ -213,13 +230,12 @@ namespace armarx
             if (devs.empty())
             {
                 ARMARX_IMPORTANT << "No joint devices found - skipping adding of KinematicUnit";
-                return;
+                return nullptr;
             }
             ARMARX_CHECK_EXPRESSION_W_HINT(!devs.empty(), "no 1DoF ControlDevice found with matching SensorDevice");
             //add it
             const std::string configName = getProperty<std::string>("KinematicUnitName");
             const std::string confPre = getConfigDomain() + "." + configName + ".";
-            Ice::PropertiesPtr properties = getIceProperties()->clone();
             //properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
             //fill properties
             properties->setProperty(confPre + "RobotNodeSetName", _module<RobotData>().getRobotNodetSeName());
@@ -237,8 +253,7 @@ namespace armarx
                 _module<Devices>().getControlModeGroups().groupsMerged,
                 dynamic_cast<RobotUnit*>(this)
             );
-            //add
-            addUnit(std::move(unit));
+            return unit;
         }
 
         void Units::initializePlatformUnit()
@@ -437,6 +452,7 @@ namespace armarx
 
         void Units::addUnit(ManagedIceObjectPtr unit)
         {
+            ARMARX_CHECK_NOT_NULL(unit);
             auto guard = getGuard();
             throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
             getArmarXManager()->addObjectAsync(unit, "", true, false);

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

@@ -248,6 +248,13 @@ namespace armarx
             virtual void initializeTrajectoryControllerUnit();
             /// @brief Initializes the TcpControllerUnit
             virtual void initializeTcpControllerUnit();
+            /// @brief Create the \ref KinematicUnit (this function should be called in \ref initializeKinematicUnit)
+            ManagedIceObjectPtr createKinematicSubUnit(
+                const Ice::PropertiesPtr& properties,
+                const std::string& positionControlMode = ControlModes::Position1DoF,
+                const std::string& velocityControlMode = ControlModes::Velocity1DoF,
+                const std::string& torqueControlMode = ControlModes::Torque1DoF
+            );
             /**
              * @brief Registers a unit for management (the Unit is added and removed to the \ref ArmarXManager
              * and updated with new values.