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

@@ -121,10 +121,14 @@ namespace armarx
         const IceUtil::Time& sensorValuesTimestamp,
         const IceUtil::Time& timeSinceLastIteration)
     {
-
-        if (sensorGlobalPositionCorrection == nullptr or sensorRelativePosition == nullptr)
+        if (sensorGlobalPositionCorrection == nullptr)
+        {
+            ARMARX_ERROR << "The global position correction sensor is not available.";
+            return;
+        }
+        if (sensorRelativePosition == nullptr)
         {
-            ARMARX_ERROR << "one of the sensors is not available";
+            ARMARX_ERROR << "The relative position sensor is not available.";
             return;
         }
 

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

@@ -33,6 +33,44 @@
 
 namespace armarx::RobotUnitModule
 {
+
+    RobotDataPropertyDefinitions::RobotDataPropertyDefinitions(std::string prefix) :
+        ModuleBasePropertyDefinitions(prefix)
+    {
+        defineRequiredProperty<std::string>("RobotFileName",
+                                            "Robot file name, e.g. robot_model.xml");
+        defineOptionalProperty<std::string>("RobotFileNameProject",
+                                            "",
+                                            "Project in which the robot filename is located "
+                                            "(if robot is loaded from an external project)");
+
+        defineOptionalProperty<std::string>(
+            "RobotName",
+            "",
+            "Override robot name if you want to load multiple robots of the same type");
+        defineOptionalProperty<std::string>(
+            "RobotNodeSetName",
+            "Robot",
+            "Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
+        defineOptionalProperty<std::string>(
+            "PlatformName",
+            "Platform",
+            "Name of the platform needs to correspond to a node in the virtual robot.");
+        defineOptionalProperty<bool>("PlatformAndLocalizationUnitsEnabled",
+                                     true,
+                                     "Enable or disable the platform and localization units.");
+        defineOptionalProperty<std::string>("PlatformInstanceName",
+                                            "Platform",
+                                            "Name of the platform instance (will publish "
+                                            "values on PlatformInstanceName + 'State')");
+    }
+
+    bool
+    RobotData::arePlatformAndLocalizationUnitsEnabled() const
+    {
+        return _arePlatformAndLocalizationUnitsEnabled;
+    }
+
     const std::string&
     RobotData::getRobotPlatformName() const
     {
@@ -112,6 +150,8 @@ namespace armarx::RobotUnitModule
         robotProjectName = getProperty<std::string>("RobotFileNameProject").getValue();
         robotFileName = getProperty<std::string>("RobotFileName").getValue();
         robotPlatformName = getProperty<std::string>("PlatformName").getValue();
+        _arePlatformAndLocalizationUnitsEnabled =
+            getProperty<bool>("PlatformAndLocalizationUnitsEnabled").getValue();
         robotPlatformInstanceName = getProperty<std::string>("PlatformInstanceName").getValue();
 
         //load robot
@@ -160,4 +200,5 @@ namespace armarx::RobotUnitModule
             robotPool.reset(new RobotPool(robot, 10));
         }
     }
+
 } // namespace armarx::RobotUnitModule

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

@@ -40,32 +40,7 @@ namespace armarx::RobotUnitModule
     class RobotDataPropertyDefinitions : public ModuleBasePropertyDefinitions
     {
     public:
-        RobotDataPropertyDefinitions(std::string prefix) : ModuleBasePropertyDefinitions(prefix)
-        {
-            defineRequiredProperty<std::string>("RobotFileName",
-                                                "Robot file name, e.g. robot_model.xml");
-            defineOptionalProperty<std::string>("RobotFileNameProject",
-                                                "",
-                                                "Project in which the robot filename is located "
-                                                "(if robot is loaded from an external project)");
-
-            defineOptionalProperty<std::string>(
-                "RobotName",
-                "",
-                "Override robot name if you want to load multiple robots of the same type");
-            defineOptionalProperty<std::string>(
-                "RobotNodeSetName",
-                "Robot",
-                "Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
-            defineOptionalProperty<std::string>(
-                "PlatformName",
-                "Platform",
-                "Name of the platform needs to correspond to a node in the virtual robot.");
-            defineOptionalProperty<std::string>("PlatformInstanceName",
-                                                "Platform",
-                                                "Name of the platform instance (will publish "
-                                                "values on PlatformInstanceName + 'State')");
-        }
+        RobotDataPropertyDefinitions(std::string prefix);
     };
 
     /**
@@ -99,21 +74,26 @@ namespace armarx::RobotUnitModule
         // /////////////////////////////////// Module interface /////////////////////////////////// //
         // //////////////////////////////////////////////////////////////////////////////////////// //
     public:
+        bool arePlatformAndLocalizationUnitsEnabled() const;
+
         /**
          * @brief Returns the name of the robot's platform
          * @return The name of the robot's platform
          */
         const std::string& getRobotPlatformName() const;
+
         /**
          * @brief Returns the name of the robot's RobotNodeSet
          * @return The name of the robot's RobotNodeSet
          */
         const std::string& getRobotNodetSeName() const;
+
         /**
          * @brief Returns the name of the project containing the robot's model
          * @return The name of the project containing the robot's model
          */
         const std::string& getRobotProjectName() const;
+
         /**
          * @brief Returns the file name of the robot's model
          * @return The file name of the robot's model
@@ -150,6 +130,8 @@ namespace armarx::RobotUnitModule
         std::string robotFileName;
         /// @brief The name of the robot's platform
         std::string robotPlatformName;
+        /// @brief Indicates whether the robot platform unit is enabled.
+        bool _arePlatformAndLocalizationUnitsEnabled = false;
         /// @brief The name of the robot's platform instance
         std::string robotPlatformInstanceName;
 

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

@@ -163,29 +163,54 @@ namespace armarx::RobotUnitModule
     void
     Units::initializeDefaultUnits()
     {
+        ARMARX_TRACE;
         throwIfInControlThread(BOOST_CURRENT_FUNCTION);
         auto beg = TimeUtil::GetTime(true);
         {
+            ARMARX_TRACE;
             auto guard = getGuard();
+
+            ARMARX_TRACE;
             throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
             ARMARX_INFO << "initializing default units";
+
+            ARMARX_TRACE;
             initializeKinematicUnit();
             ARMARX_DEBUG << "KinematicUnit initialized";
 
-            ARMARX_DEBUG << "initializing LocalizationUnit";
-            initializeLocalizationUnit();
-            ARMARX_DEBUG << "LocalizationUnit initialized";
+            ARMARX_TRACE;
+            if (_module<RobotData>().arePlatformAndLocalizationUnitsEnabled())
+            {
+                ARMARX_DEBUG << "initializing LocalizationUnit";
+                initializeLocalizationUnit();
+                ARMARX_DEBUG << "LocalizationUnit initialized";
+            }
+            ARMARX_TRACE;
+
+            if (_module<RobotData>().arePlatformAndLocalizationUnitsEnabled())
+            {
+                ARMARX_DEBUG << "initializing PlatformUnit";
+                initializePlatformUnit();
+                ARMARX_DEBUG << "PlatformUnit initialized";
+            }
 
-            initializePlatformUnit();
-            ARMARX_DEBUG << "PlatformUnit initialized";
+            ARMARX_TRACE;
             initializeForceTorqueUnit();
             ARMARX_DEBUG << "ForceTorqueUnit initialized";
+
+            ARMARX_TRACE;
             initializeInertialMeasurementUnit();
             ARMARX_DEBUG << "InertialMeasurementUnit initialized";
+
+            ARMARX_TRACE;
             initializeTrajectoryControllerUnit();
             ARMARX_DEBUG << "TrajectoryControllerUnit initialized";
+
+            ARMARX_TRACE;
             initializeTcpControllerUnit();
             ARMARX_DEBUG << "TcpControllerUnit initialized";
+
+            ARMARX_TRACE;
         }
         ARMARX_INFO << "initializing default units...done! "
                     << (TimeUtil::GetTime(true) - beg).toMicroSeconds() << " us";
@@ -371,6 +396,8 @@ namespace armarx::RobotUnitModule
         using UnitT = PlatformSubUnit;
         using IfaceT = PlatformUnitInterface;
 
+        ARMARX_TRACE;
+
         auto guard = getGuard();
         throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
         //check if unit is already added
@@ -378,12 +405,14 @@ namespace armarx::RobotUnitModule
         {
             return;
         }
+        ARMARX_TRACE;
         //is there a platform dev?
         if (_module<RobotData>().getRobotPlatformName().empty())
         {
             ARMARX_INFO << "no platform unit created since no platform name was given";
             return;
         }
+        ARMARX_TRACE;
         if (!_module<Devices>().getControlDevices().has(
                 _module<RobotData>().getRobotPlatformName()))
         {
@@ -392,16 +421,19 @@ namespace armarx::RobotUnitModule
                 << _module<RobotData>().getRobotPlatformName() << "' was not found";
             return;
         }
+        ARMARX_TRACE;
         const ControlDevicePtr& controlDevice =
             _module<Devices>().getControlDevices().at(_module<RobotData>().getRobotPlatformName());
         const SensorDevicePtr& sensorDevice =
             _module<Devices>().getSensorDevices().at(_module<RobotData>().getRobotPlatformName());
         JointController* jointVel =
             controlDevice->getJointController(ControlModes::HolonomicPlatformVelocity);
+        ARMARX_TRACE;
         ARMARX_CHECK_EXPRESSION(jointVel);
         ARMARX_CHECK_EXPRESSION(
             sensorDevice->getSensorValue()->asA<SensorValueHolonomicPlatform>());
         //add it
+        ARMARX_TRACE;
         const std::string configName = getProperty<std::string>("PlatformUnitName");
         const std::string confPre = getConfigDomain() + "." + configName + ".";
         Ice::PropertiesPtr properties = getIceProperties()->clone();
@@ -411,9 +443,11 @@ namespace armarx::RobotUnitModule
                                 _module<RobotData>().getRobotPlatformInstanceName());
         ARMARX_DEBUG << "creating unit " << configName
                      << " using these properties: " << properties->getPropertiesForPrefix("");
+        ARMARX_TRACE;
         IceInternal::Handle<UnitT> unit =
             Component::create<UnitT>(properties, configName, getConfigDomain());
         //config
+        ARMARX_TRACE;
         NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr config =
             new NJointHolonomicPlatformUnitVelocityPassThroughControllerConfig;
         config->initialVelocityX = 0;
@@ -427,8 +461,10 @@ namespace armarx::RobotUnitModule
                 config,
                 false,
                 true));
+        ARMARX_TRACE;
         ARMARX_CHECK_EXPRESSION(ctrl);
         unit->pt = ctrl;
+        ARMARX_TRACE;
 
         NJointHolonomicPlatformRelativePositionControllerConfigPtr configRelativePositionCtrlCfg =
             new NJointHolonomicPlatformRelativePositionControllerConfig;
@@ -457,9 +493,11 @@ namespace armarx::RobotUnitModule
                 configGlobalPositionCtrlCfg,
                 false,
                 true));
+        ARMARX_TRACE;
         ARMARX_CHECK_EXPRESSION(ctrlGlobalPosition);
         unit->pt = ctrl;
         unit->globalPosCtrl = ctrlGlobalPosition;
+        ARMARX_TRACE;
 
         unit->platformSensorIndex = _module<Devices>().getSensorDevices().index(
             _module<RobotData>().getRobotPlatformName());
@@ -470,12 +508,14 @@ namespace armarx::RobotUnitModule
     void
     Units::initializeLocalizationUnit()
     {
+        ARMARX_TRACE;
         ARMARX_DEBUG << "initializeLocalizationUnit";
 
         throwIfInControlThread(BOOST_CURRENT_FUNCTION);
         using UnitT = LocalizationSubUnit;
         using IfaceT = LocalizationUnitInterface;
 
+        ARMARX_TRACE;
         auto guard = getGuard();
         throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
         //check if unit is already added
@@ -483,13 +523,18 @@ namespace armarx::RobotUnitModule
         {
             return;
         }
+        ARMARX_TRACE;
 
         ARMARX_DEBUG << "Getting device SensorValueHolonomicPlatformRelativePosition";
+        ARMARX_CHECK(
+            _module<Devices>().getSensorDevices().has(_module<RobotData>().getRobotPlatformName()))
+            << _module<RobotData>().getRobotPlatformName();
         const SensorDevicePtr& sensorDeviceRelativePosition =
             _module<Devices>().getSensorDevices().at(_module<RobotData>().getRobotPlatformName());
         ARMARX_CHECK_EXPRESSION(sensorDeviceRelativePosition->getSensorValue()
                                     ->asA<SensorValueHolonomicPlatformRelativePosition>());
 
+        ARMARX_TRACE;
         // const SensorDevicePtr& sensorDevicePoseCorrection = _module<Devices>().getSensorDevices().at(GlobalRobotPoseCorrectionSensorDevice::DeviceName());
         // ARMARX_CHECK_EXPRESSION(sensorDeviceRelativePosition->getSensorValue()->asA<SensorValueGlobalPoseCorrection>());
 

source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp

@@ -82,8 +82,8 @@ namespace armarx
     ControlThreadOutputBuffer::forEachNewLoggingEntry(ConsumerFunctor consumer)
     {
         ARMARX_TRACE;
-        ARMARX_CHECK_EXPRESSION(isInitialized);
-        ARMARX_VERBOSE << VAROUT(entries.size());
+        ARMARX_CHECK(isInitialized);
+        ARMARX_DEBUG << VAROUT(entries.size());
         const size_t writePosition_local = writePosition.load(); // copy to prevent external changes
         if (writePosition_local - onePastLoggingReadPosition >= numEntries)
         {
@@ -107,7 +107,7 @@ namespace armarx
         }
         //consume all
         const std::size_t num = writePosition_local - onePastLoggingReadPosition;
-        ARMARX_VERBOSE << num << " new entries to be treated";
+        ARMARX_DEBUG << num << " new entries to be treated";
         for (std::size_t offset = 0; onePastLoggingReadPosition < writePosition_local;
              ++onePastLoggingReadPosition, ++offset)
         {
@@ -276,10 +276,8 @@ namespace armarx
         }
     }
 
-    const auto PotentiallyMinimizeMember = [](auto & member, bool minimize) {
-        return minimize ? 0 : member;
-    };
-
+    const auto PotentiallyMinimizeMember = [](auto& member, bool minimize)
+    { return minimize ? 0 : member; };
 
     detail::RtMessageLogBuffer::RtMessageLogBuffer(const detail::RtMessageLogBuffer& other,
                                                    bool minimize) :
@@ -301,13 +299,13 @@ namespace armarx
         maxAlign{1}
     {
         ARMARX_DEBUG << "copying RtMessageLogBuffer with minimize = " << minimize << " "
-                    << VAROUT(initialBufferSize) << " " << VAROUT(initialBufferEntryNumbers) << " "
-                    << VAROUT(bufferMaxSize) << " " << VAROUT(bufferMaxNumberEntries) << " "
-                    << VAROUT(buffer.size()) << " " << VAROUT(other.bufferSpace) << " "
-                    << VAROUT(bufferPlace) << " " << VAROUT(entries.size()) << " "
-                    << VAROUT(entriesWritten) << " " << VAROUT(requiredAdditionalBufferSpace) << " "
-                    << VAROUT(requiredAdditionalEntries) << " " << VAROUT(messagesLost) << " "
-                    << VAROUT(maxAlign);
+                     << VAROUT(initialBufferSize) << " " << VAROUT(initialBufferEntryNumbers) << " "
+                     << VAROUT(bufferMaxSize) << " " << VAROUT(bufferMaxNumberEntries) << " "
+                     << VAROUT(buffer.size()) << " " << VAROUT(other.bufferSpace) << " "
+                     << VAROUT(bufferPlace) << " " << VAROUT(entries.size()) << " "
+                     << VAROUT(entriesWritten) << " " << VAROUT(requiredAdditionalBufferSpace)
+                     << " " << VAROUT(requiredAdditionalEntries) << " " << VAROUT(messagesLost)
+                     << " " << VAROUT(maxAlign);
         for (std::size_t idx = 0; idx < other.entries.size() && other.entries.at(idx); ++idx)
         {
             const RtMessageLogEntryBase* entry = other.entries.at(idx);
@@ -361,10 +359,12 @@ namespace armarx
         deleteAll();
         if (requiredAdditionalEntries || entries.size() < numEntries)
         {
-            const auto numExcessEntries = std::max(requiredAdditionalEntries, numEntries - entries.size());
+            const auto numExcessEntries =
+                std::max(requiredAdditionalEntries, numEntries - entries.size());
             const auto requiredSize = entries.size() + numExcessEntries;
             ARMARX_WARNING << "Iteration " << iterationCount << " required "
-                           << requiredAdditionalEntries << " | " << numExcessEntries << " additional message entries. \n"
+                           << requiredAdditionalEntries << " | " << numExcessEntries
+                           << " additional message entries. \n"
                            << "The requested total number of entries is " << requiredSize << ". \n"
                            << "The current number of entries is " << entries.size() << ". \n"
                            << "The maximal number of entries is "
@@ -514,10 +514,10 @@ namespace armarx
     {
         ARMARX_TRACE;
         ARMARX_DEBUG << "Copy ControlThreadOutputBufferEntry with parameters: " << VAROUT(minimize)
-                    << " " << VAROUT(writeTimestamp) << " " << VAROUT(sensorValuesTimestamp) << " "
-                    << VAROUT(timeSinceLastIteration) << " " << VAROUT(iteration) << " "
-                    << VAROUT(buffer.size()) << " " << VAROUT(messages.buffer.size()) << " "
-                    << VAROUT(messages.entries.size());
+                     << " " << VAROUT(writeTimestamp) << " " << VAROUT(sensorValuesTimestamp) << " "
+                     << VAROUT(timeSinceLastIteration) << " " << VAROUT(iteration) << " "
+                     << VAROUT(buffer.size()) << " " << VAROUT(messages.buffer.size()) << " "
+                     << VAROUT(messages.entries.size());
         void* place = buffer.data();
         std::size_t space = buffer.size();
 

source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h

@@ -114,7 +114,7 @@ namespace armarx::introspection
                                                             const std::string& name)
     {
         ARMARX_TRACE;
-        ARMARX_CHECK_EQUAL(0, entries.count(name));
+        ARMARX_CHECK_EQUAL(entries.count(name), 0) << name;
         entries.add(name, Entry(name, ptr));
         return entries.at(name);
     }

source/RobotAPI/libraries/aron/common/aron_conversions/framed.cpp

@@ -67,6 +67,7 @@ namespace armarx::aron::framed
     {
         dto.header.frame = bo.frame;
         dto.header.agent = bo.agent;
+        dto.pose = Eigen::Matrix4f::Identity();
         Eigen::Vector3f vec;
         vec.x() = bo.position->x;
         vec.y() = bo.position->y;