From ad803a13625a5c79ce2524866e4a53a618b20fe1 Mon Sep 17 00:00:00 2001
From: armarx-user <armarx-user@kit.edu>
Date: Sat, 15 Dec 2018 15:03:57 +0100
Subject: [PATCH] some ethercat teaks
---
.../ArmarXEtherCAT/DeviceContainer.cpp | 42 ++++++++++++-------
.../libraries/ArmarXEtherCAT/EtherCAT.cpp | 15 ++++---
.../ArmarXEtherCAT/EtherCATRTUnit.cpp | 41 +++++++++---------
3 files changed, 57 insertions(+), 41 deletions(-)
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.cpp
index 9295ae9a4..792583d9d 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.cpp
+++ b/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;
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
index efd378173..c215f9e38 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
@@ -228,10 +228,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();
}
@@ -418,6 +419,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) + "] ";
@@ -425,15 +427,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)
{
@@ -559,9 +562,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();
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
index c91a62161..ce734f3e3 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
@@ -332,25 +332,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();
@@ -379,6 +360,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
@@ -487,8 +487,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();
--
GitLab