Add debug output

source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.cpp

@@ -88,6 +88,7 @@ namespace armarx
 
     void ControlDevice::addJointController(JointController* jointCtrl)
     {
+        ARMARX_DEBUG << "adding Controller " << jointCtrl;
         if (!jointCtrl)
         {
             throw std::invalid_argument
@@ -103,6 +104,7 @@ namespace armarx
                 "(Don't try to add a JointController multiple to a ControlDevice)"
             };
         }
+        ARMARX_DEBUG << "getControlMode of " << jointCtrl;
         const std::string& mode = jointCtrl->getControlMode();
         ARMARX_IMPORTANT << "adding joint controller mode '" << mode << "' for device " << getDeviceName();
         if (jointControllers.has(mode))
@@ -114,7 +116,9 @@ namespace armarx
             };
         }
         jointCtrl->parent = this;
+        ARMARX_DEBUG << "resetting target";
         jointCtrl->rtResetTarget();
+        ARMARX_DEBUG << "resetting target...done!";
         if (mode == ControlModes::EmergencyStop)
         {
             jointEmergencyStopController = jointCtrl;
@@ -135,6 +139,7 @@ namespace armarx
         jointCtrl->controlModeHash = hash(jointCtrl->getControlMode());
         jointCtrl->hardwareControlModeHash = hash(jointCtrl->getHardwareControlMode());
         jointControllers.add(mode, std::move(jointCtrl));
+        ARMARX_DEBUG << "adding Controller " << jointCtrl << "...done";
     }
 
 }

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

@@ -273,16 +273,20 @@ namespace armarx
             throwIfInControlThread(__FUNCTION__);
             //this guards prevents the RobotUnitState to change
             auto guard = getGuard();
+            ARMARX_DEBUG << "Check RobotUnit State";
             throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
+            ARMARX_DEBUG << "Adding the ControlDevice "  << cd->getDeviceName() << " " << &cd ;
             controlDevices.add(cd->getDeviceName(), cd);
             ARMARX_INFO << "added ControlDevice " << cd->getDeviceName();
         }
 
         void Devices::addSensorDevice(const SensorDevicePtr &sd)
         {
+            ARMARX_DEBUG << "SensorDevice "  << &sd;
             throwIfInControlThread(__FUNCTION__);
             //this guards prevents the RobotUnitState to change
             auto guard = getGuard();
+            ARMARX_DEBUG << "Check RobotUnit State";
             throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
             if (!sd)
             {
@@ -301,6 +305,7 @@ namespace armarx
             }
             if (sd->getDeviceName() == rtThreadTimingsSensorDeviceName)
             {
+                ARMARX_DEBUG << "Device is the "  << rtThreadTimingsSensorDeviceName;
                 if (!std::dynamic_pointer_cast<RTThreadTimingsSensorDevice>(sd))
                 {
                     throw InvalidArgumentException
@@ -310,11 +315,13 @@ namespace armarx
                     };
                 }
                 //this checks if we already added such a device (do this before setting timingSensorDevice)
+                ARMARX_DEBUG << "Adding the SensorDevice "  << sd->getDeviceName() << " " << &sd ;
                 sensorDevices.add(sd->getDeviceName(), sd);
                 rtThreadTimingsSensorDevice = std::dynamic_pointer_cast<RTThreadTimingsSensorDevice>(sd);
             }
             else
             {
+                ARMARX_DEBUG << "Adding the SensorDevice "  << sd->getDeviceName() << " " << &sd ;
                 sensorDevices.add(sd->getDeviceName(), sd);
             }
             ARMARX_INFO << "added SensorDevice " << sd->getDeviceName() << " (valuetype = " << sd->getSensorValueType() << ")";