some ethercat teaks

source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.cpp

@@ -33,6 +33,7 @@ namespace armarx
 
     size_t DeviceContainer::load(const MultiNodeRapidXMLReader& rootNodeConfigs, const VirtualRobot::RobotPtr& robot)
     {
+        //        sleep(5);
         size_t addedDevices = 0;
         //        auto children = robot->getRobotNodes();
         //        auto getSceneObject = [&](const std::string & name)
@@ -58,25 +59,34 @@ namespace armarx
         auto defaultNode = DefaultRapidXmlReaderNode(rootNodeConfigs.nodes("DefaultConfiguration"));
         for (auto& node : rootNodeConfigs.nodes(nullptr))
         {
-            if (node.name() == "DefaultConfiguration" || node.name() == "include" || node.name().empty())
+            try
             {
-                continue;
-            }
-            auto name = node.attribute_value("name");
-            ARMARX_DEBUG << "Handling: " << node.name() << " name: " << name;
-            //            auto obj = getSceneObject(name);
-            //            ARMARX_CHECK_EXPRESSION_W_HINT(obj, name);
-            auto tuple = std::make_tuple(node, defaultNode, robot);
-            auto instance = VirtualDeviceFactory::fromName(node.name(), tuple);
-            if (!instance)
-            {
-                ARMARX_WARNING << "No factory found for virtual device " << node.name();
+
+
+                if (node.name() == "DefaultConfiguration" || node.name() == "include" || node.name().empty())
+                {
+                    continue;
+                }
+                auto name = node.attribute_value("name");
+                ARMARX_DEBUG << "Handling: " << node.name() << " name: " << name;
+                //            auto obj = getSceneObject(name);
+                //            ARMARX_CHECK_EXPRESSION_W_HINT(obj, name);
+                auto tuple = std::make_tuple(node, defaultNode, robot);
+                auto instance = VirtualDeviceFactory::fromName(node.name(), tuple);
+                if (!instance)
+                {
+                    ARMARX_WARNING << "No factory found for virtual device " << node.name();
+                }
+                else
+                {
+                    ARMARX_VERBOSE << "Created instance of type " << node.name();
+                    devices.push_back(instance);
+                    addedDevices++;
+                }
             }
-            else
+            catch (...)
             {
-                ARMARX_VERBOSE << "Created instance of type " << node.name();
-                devices.push_back(instance);
-                addedDevices++;
+                handleExceptions();
             }
         }
         return addedDevices;

source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp

@@ -227,10 +227,11 @@ bool EtherCAT::startBus(bool createDevices)
 
     /// init functional devices
     functionalDevices = deviceContainerPtr->getAllInitializedFunctionalDevices();
-
+    ARMARX_INFO << "Found " << functionalDevices.size() << " meta devices";
     /// setting the data pointer in the functional devices
     for (AbstractFunctionalDevicePtr& device : functionalDevices)
     {
+        ARMARX_INFO << "init for " << device->getClassName();
         device->initData();
     }
 
@@ -417,6 +418,7 @@ std::pair<std::vector<ControlDevicePtr>, std::vector<SensorDevicePtr>> EtherCAT:
         throw LocalException("no robot Container set! set a robotContainer before you start the bus!");
     }
     auto etherCATFactoryNames = EtherCATDeviceFactory::getAvailableClasses();
+    ARMARX_INFO << "PPP: " << etherCATFactoryNames;
     for (uint16_t currentSlaveIndex = 1; currentSlaveIndex <= ec_slavecount; currentSlaveIndex++)
     {
         const std::string messageSlaveIdentifier = "[Slave index: " + std::to_string(currentSlaveIndex) + "] ";
@@ -424,15 +426,16 @@ std::pair<std::vector<ControlDevicePtr>, std::vector<SensorDevicePtr>> EtherCAT:
         //check the device type to identify the EtherCAT Hub
         //        uint32 deviceType = ETHTERCAT_HUB_DEVICE_TYPE;
         //uint32 vendorID = 0;
-        ARMARX_DEBUG << messageSlaveIdentifier << " device type: " <<  ec_slave[currentSlaveIndex].Dtype << "\n itype: " << std::hex << ec_slave[currentSlaveIndex].Itype <<
-                     "\neep_id: " << ec_slave[currentSlaveIndex].eep_id << "\neep_man: " << ec_slave[currentSlaveIndex].eep_man << "\neep_rev: " << ec_slave[currentSlaveIndex].eep_rev;
-        ARMARX_DEBUG << messageSlaveIdentifier << "CoE details " << (int)(ec_slave[currentSlaveIndex].mbx_proto & ECT_MBXPROT_COE);
+        ARMARX_INFO << messageSlaveIdentifier << " device type: " <<  ec_slave[currentSlaveIndex].Dtype << "\n itype: " << std::hex << ec_slave[currentSlaveIndex].Itype <<
+                    "\neep_id: " << ec_slave[currentSlaveIndex].eep_id << "\neep_man: " << ec_slave[currentSlaveIndex].eep_man << "\neep_rev: " << ec_slave[currentSlaveIndex].eep_rev;
+        ARMARX_INFO << messageSlaveIdentifier << "CoE details " << (int)(ec_slave[currentSlaveIndex].mbx_proto & ECT_MBXPROT_COE);
 
         bool foundDevice = false;
         try
         {
             for (auto& facName : etherCATFactoryNames)
             {
+                ARMARX_INFO << "Trying factory " << facName;
                 auto device = EtherCATDeviceFactory::fromName(facName, std::make_tuple(this, currentSlaveIndex, deviceContainerPtr));
                 if (device)
                 {
@@ -558,9 +561,9 @@ bool EtherCAT::updateBus(bool doErrorHandling)
         }
 
         //send an receive process data
-        RT_TIMING_START(UpdatePDO);
+        //        RT_TIMING_START(UpdatePDO);
         updatePDO();
-        RT_TIMING_CEND(UpdatePDO, 0.7);
+        //        RT_TIMING_CEND(UpdatePDO, 0.7);
         busUpdateLastUpdateTime = IceUtil::Time::now();
 
 

source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp

@@ -328,25 +328,6 @@ void EtherCATRTUnit::rtRun()
     }
     if (busStartSucceeded)
     {
-        ARMARX_DEBUG << "Setting up gravity calculation";
-        // init data structs for gravity calculation
-        for (size_t i = 0; i < rtRobotJointSet->getSize(); i++)
-        {
-            auto selectedJoint = getSensorDevice(rtRobotJointSet->getNode(i)->getName());
-            if (selectedJoint)
-            {
-                auto sensorValue = const_cast<SensorValue1DoFGravityTorque*>(selectedJoint->getSensorValue()->asA<SensorValue1DoFGravityTorque>());
-                ARMARX_DEBUG << "will calculate gravity for robot node " << rtRobotJointSet->getNode(i)->getName();
-                nodeJointDataVec.push_back(std::make_pair(rtRobotJointSet->getNode(i), sensorValue));
-            }
-            else
-            {
-                ARMARX_INFO << "Joint " << rtRobotJointSet->getNode(i)->getName() << " not found";
-                nodeJointDataVec.push_back(std::make_pair(rtRobotJointSet->getNode(i), nullptr));
-            }
-        }
-
-
         elevateThreadPriority(RT_THREAD_PRIORITY);
         //        set_latency_target();
 
@@ -375,6 +356,25 @@ void EtherCATRTUnit::rtRun()
             ARMARX_WARNING << "Bus was not started!";
         }
     }
+
+    ARMARX_DEBUG << "Setting up gravity calculation";
+    // init data structs for gravity calculation
+    for (size_t i = 0; i < rtRobotJointSet->getSize(); i++)
+    {
+        auto selectedJoint = getSensorDevice(rtRobotJointSet->getNode(i)->getName());
+        if (selectedJoint)
+        {
+            auto sensorValue = const_cast<SensorValue1DoFGravityTorque*>(selectedJoint->getSensorValue()->asA<SensorValue1DoFGravityTorque>());
+            ARMARX_DEBUG << "will calculate gravity for robot node " << rtRobotJointSet->getNode(i)->getName();
+            nodeJointDataVec.push_back(std::make_pair(rtRobotJointSet->getNode(i), sensorValue));
+        }
+        else
+        {
+            ARMARX_INFO << "Joint " << rtRobotJointSet->getNode(i)->getName() << " not found";
+            nodeJointDataVec.push_back(std::make_pair(rtRobotJointSet->getNode(i), nullptr));
+        }
+    }
+
     //    unitInitTask.join();
     controlLoopRTThread();
     //switching off the bus and be sure that the bus will receive
@@ -453,8 +453,11 @@ void EtherCATRTUnit::controlLoopRTThread()
         ARMARX_CHECK_EXPRESSION(rtRobotJointSet->getSize() > 0);
         VirtualRobot::Gravity gravity(rtGetRobot(), rtRobotJointSet, rtRobotBodySet);
 
+        //#if 0
         std::vector<float> gravityValues(rtRobotJointSet->getSize());
         ARMARX_CHECK_EQUAL(rtRobotJointSet->getSize(), nodeJointDataVec.size());
+        //#endif
+
         ARMARX_INFO << "RT control loop started";
         EmergencyStopState lastSoftwareEmergencyStopState = rtGetEmergencyStopState();