diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.cpp b/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.cpp index 338e2e290e85814af337cfe067c1d87315b70666..644d96c4d6fbca14bd17c7e2e7dbe16fd67166b8 100644 --- a/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.cpp +++ b/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"; } } diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp index 3e7b96a7e533aec2de94d146a4db984f088384ea..e8fa25f0bcb2e8e8d0df0cf2ad8226f6996c6cc0 100644 --- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp +++ b/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() << ")";