diff --git a/source/RobotAPI/applications/ForceTorqueObserver/ForceTorqueObserverApp.h b/source/RobotAPI/applications/ForceTorqueObserver/ForceTorqueObserverApp.h
index d1fd84d25423ef685bfb9b845a181206016f84d9..c415f85c18c650dc3b277d802cb72f8524f8bd58 100644
--- a/source/RobotAPI/applications/ForceTorqueObserver/ForceTorqueObserverApp.h
+++ b/source/RobotAPI/applications/ForceTorqueObserver/ForceTorqueObserverApp.h
@@ -27,11 +27,11 @@
namespace armarx
{
class ForceTorqueObserverApp :
- virtual public armarx::Application
- {
+ virtual public armarx::Application
+ {
void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
{
- registry->addObject( Component::create<ForceTorqueObserver>(properties) );
+ registry->addObject(Component::create<ForceTorqueObserver>(properties));
}
};
}
diff --git a/source/RobotAPI/applications/ForceTorqueUnitSimulation/ForceTorqueUnitSimulationApp.h b/source/RobotAPI/applications/ForceTorqueUnitSimulation/ForceTorqueUnitSimulationApp.h
index 3d3d2b8f07e3bdd12bdd8d5df513b2998cd0e982..83c5468821c29a916335db9e2f94094d159f9fcb 100644
--- a/source/RobotAPI/applications/ForceTorqueUnitSimulation/ForceTorqueUnitSimulationApp.h
+++ b/source/RobotAPI/applications/ForceTorqueUnitSimulation/ForceTorqueUnitSimulationApp.h
@@ -35,8 +35,8 @@ namespace armarx
/**
* @see armarx::Application::setup()
*/
- void setup(const ManagedIceObjectRegistryInterfacePtr ®istry, Ice::PropertiesPtr properties)
- {
+ void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
+ {
registry->addObject(Component::create<ForceTorqueUnitSimulation>(properties));
}
};
diff --git a/source/RobotAPI/applications/HandUnitSimulation/HandUnitSimulationApp.h b/source/RobotAPI/applications/HandUnitSimulation/HandUnitSimulationApp.h
index 85d5f779a7cf0475556166da3973a4548b255bc8..ba67452edd496005d9e3d0b5d8778de2d79f3101 100644
--- a/source/RobotAPI/applications/HandUnitSimulation/HandUnitSimulationApp.h
+++ b/source/RobotAPI/applications/HandUnitSimulation/HandUnitSimulationApp.h
@@ -45,7 +45,7 @@ namespace armarx
void setup(const ManagedIceObjectRegistryInterfacePtr& registry,
Ice::PropertiesPtr properties)
{
- registry->addObject( Component::create<HandUnitSimulation>(properties) );
+ registry->addObject(Component::create<HandUnitSimulation>(properties));
}
};
}
diff --git a/source/RobotAPI/applications/HapticObserver/HapticObserverApp.h b/source/RobotAPI/applications/HapticObserver/HapticObserverApp.h
index 27661c8c5bd795b10dd8aa72e99ccb21496b81d4..8bae1433630337201d8bb4d3324d069332cb1fbe 100644
--- a/source/RobotAPI/applications/HapticObserver/HapticObserverApp.h
+++ b/source/RobotAPI/applications/HapticObserver/HapticObserverApp.h
@@ -43,7 +43,7 @@ namespace armarx
void setup(const ManagedIceObjectRegistryInterfacePtr& registry,
Ice::PropertiesPtr properties)
{
- registry->addObject( Component::create<HapticObserver>(properties) );
+ registry->addObject(Component::create<HapticObserver>(properties));
}
};
}
diff --git a/source/RobotAPI/applications/HeadIKUnit/HeadIKUnitApp.h b/source/RobotAPI/applications/HeadIKUnit/HeadIKUnitApp.h
index 4fad3f0d0cbfe10b956344baf4bf2486c56d6dc5..1e4d804b41948b6f339bbb210c4684ee25e15a7b 100644
--- a/source/RobotAPI/applications/HeadIKUnit/HeadIKUnitApp.h
+++ b/source/RobotAPI/applications/HeadIKUnit/HeadIKUnitApp.h
@@ -31,7 +31,7 @@ namespace armarx
{
void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
{
- registry->addObject( Component::create<HeadIKUnit>(properties) );
+ registry->addObject(Component::create<HeadIKUnit>(properties));
}
};
}
diff --git a/source/RobotAPI/applications/InertialMeasurementUnitObserver/InertialMeasurementUnitObserverApp.h b/source/RobotAPI/applications/InertialMeasurementUnitObserver/InertialMeasurementUnitObserverApp.h
index b11a61759294dec3ccfa26ccfdf13a8fcb799f46..39898375eef5a33542b0d692b4fbd5be7bbc038f 100644
--- a/source/RobotAPI/applications/InertialMeasurementUnitObserver/InertialMeasurementUnitObserverApp.h
+++ b/source/RobotAPI/applications/InertialMeasurementUnitObserver/InertialMeasurementUnitObserverApp.h
@@ -47,7 +47,7 @@ namespace armarx
void setup(const ManagedIceObjectRegistryInterfacePtr& registry,
Ice::PropertiesPtr properties)
{
- registry->addObject( Component::create<InertialMeasurementUnitObserver>(properties) );
+ registry->addObject(Component::create<InertialMeasurementUnitObserver>(properties));
}
};
}
diff --git a/source/RobotAPI/applications/KinematicUnitObserver/KinematicUnitObserverApp.h b/source/RobotAPI/applications/KinematicUnitObserver/KinematicUnitObserverApp.h
index feb30b46a610959dd46e74d61526ede04448f6d3..5dbc9624a5e8481582d1e3a67fd87fcf72d943b3 100644
--- a/source/RobotAPI/applications/KinematicUnitObserver/KinematicUnitObserverApp.h
+++ b/source/RobotAPI/applications/KinematicUnitObserver/KinematicUnitObserverApp.h
@@ -29,15 +29,15 @@ namespace armarx
/**
* Application for testing the armarx::KinematicUnitObserver
*/
- class KinematicUnitObserverApp :
- virtual public armarx::Application
- {
+ class KinematicUnitObserverApp :
+ virtual public armarx::Application
+ {
/**
* @see armarx::Application::setup()
*/
- void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
- {
- registry->addObject( Component::create<KinematicUnitObserver>(properties) );
+ void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
+ {
+ registry->addObject(Component::create<KinematicUnitObserver>(properties));
}
- };
+ };
}
diff --git a/source/RobotAPI/applications/KinematicUnitSimulation/KinematicUnitSimulationApp.h b/source/RobotAPI/applications/KinematicUnitSimulation/KinematicUnitSimulationApp.h
index d3279b41b84e09153c9f3fa0a9ae4f5f69b8677a..46e2fa71bb276fd00f971ca8892bb87438cc700e 100644
--- a/source/RobotAPI/applications/KinematicUnitSimulation/KinematicUnitSimulationApp.h
+++ b/source/RobotAPI/applications/KinematicUnitSimulation/KinematicUnitSimulationApp.h
@@ -35,9 +35,9 @@ namespace armarx
/**
* @see armarx::Application::setup()
*/
- void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
- {
- registry->addObject( Component::create<KinematicUnitSimulation>(properties) );
+ void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
+ {
+ registry->addObject(Component::create<KinematicUnitSimulation>(properties));
}
};
}
diff --git a/source/RobotAPI/applications/MMMPlayer/MMMPlayerApp.h b/source/RobotAPI/applications/MMMPlayer/MMMPlayerApp.h
index ec6ccbc0191ce21e4cb3a53422f7cd6db593df50..e1026ef8377fa2ab00d2424676c584fdec17794e 100644
--- a/source/RobotAPI/applications/MMMPlayer/MMMPlayerApp.h
+++ b/source/RobotAPI/applications/MMMPlayer/MMMPlayerApp.h
@@ -47,7 +47,7 @@ namespace armarx
void setup(const ManagedIceObjectRegistryInterfacePtr& registry,
Ice::PropertiesPtr properties)
{
- registry->addObject( Component::create<MMMPlayer>(properties) );
+ registry->addObject(Component::create<MMMPlayer>(properties));
}
};
}
diff --git a/source/RobotAPI/applications/PlatformUnitObserver/PlatformUnitObserverApp.h b/source/RobotAPI/applications/PlatformUnitObserver/PlatformUnitObserverApp.h
index 846d3c57df3e4ee086559996f5cdefdb2e794322..15eb07fc527af47aef13782b4718483ddae2fb35 100644
--- a/source/RobotAPI/applications/PlatformUnitObserver/PlatformUnitObserverApp.h
+++ b/source/RobotAPI/applications/PlatformUnitObserver/PlatformUnitObserverApp.h
@@ -37,7 +37,7 @@ namespace armarx
*/
void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
{
- registry->addObject( Component::create<PlatformUnitObserver>(properties) );
+ registry->addObject(Component::create<PlatformUnitObserver>(properties));
}
- };
+ };
}
diff --git a/source/RobotAPI/applications/PlatformUnitSimulation/PlatformUnitSimulationApp.h b/source/RobotAPI/applications/PlatformUnitSimulation/PlatformUnitSimulationApp.h
index 3d58e699ed101e60c3cd33ed746fba07122933a1..172ca667045152e9005c73198adef48535e7f208 100644
--- a/source/RobotAPI/applications/PlatformUnitSimulation/PlatformUnitSimulationApp.h
+++ b/source/RobotAPI/applications/PlatformUnitSimulation/PlatformUnitSimulationApp.h
@@ -35,9 +35,9 @@ namespace armarx
/**
* @see armarx::Application::setup()
*/
- void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
- {
- registry->addObject( Component::create<PlatformUnitSimulation>(properties) );
+ void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
+ {
+ registry->addObject(Component::create<PlatformUnitSimulation>(properties));
}
};
}
diff --git a/source/RobotAPI/applications/RobotControl/RobotControlApp.h b/source/RobotAPI/applications/RobotControl/RobotControlApp.h
index 884072e4a3b8d98837daf0b2fee14acd0b8a3275..c290545210136c8014c21e29b50f53f859ffd788 100644
--- a/source/RobotAPI/applications/RobotControl/RobotControlApp.h
+++ b/source/RobotAPI/applications/RobotControl/RobotControlApp.h
@@ -30,14 +30,14 @@ namespace armarx
* Application for testing armarx::RobotControl
*/
class RobotControlApp :
- virtual public armarx::Application
- {
+ virtual public armarx::Application
+ {
/**
* @see armarx::Application::setup()
*/
- void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
- {
- registry->addObject( Component::create<RobotControl>(properties) );
+ void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
+ {
+ registry->addObject(Component::create<RobotControl>(properties));
}
- };
+ };
}
diff --git a/source/RobotAPI/applications/RobotControlUI/RobotControlUI.cpp b/source/RobotAPI/applications/RobotControlUI/RobotControlUI.cpp
index 75d5d8cd8d8c4f4ad3f346e9d1f9d3c52ad34730..1200654b333ff92b2767293d04be750a81770c71 100644
--- a/source/RobotAPI/applications/RobotControlUI/RobotControlUI.cpp
+++ b/source/RobotAPI/applications/RobotControlUI/RobotControlUI.cpp
@@ -51,22 +51,25 @@ void RobotControlUI::run()
{
string eventstring;
cout << "Please insert the event string: " << std::flush;
-// cin >> eventstring;
+ // cin >> eventstring;
eventstring = "EvLoadScenario";
- if(eventstring == "q")
+
+ if (eventstring == "q")
{
-// shutdown();
+ // shutdown();
}
- else{
+ else
+ {
cout << "Please insert the state id of the state that should process the event: " << std::flush;
int id;
-// cin >> id;
+ // cin >> id;
id = 11;
cout << "sending to id:" << id << endl;
- EventPtr evt = new Event("EVENTTOALL",eventstring);
+ EventPtr evt = new Event("EVENTTOALL", eventstring);
StateUtilFunctions::addToDictionary(evt, "proxyName", "RemoteStateOfferer");
StateUtilFunctions::addToDictionary(evt, "stateName", "MoveArm");
//robotProxy->issueEvent(id, evt);
}
-// cin >> eventstring;
+
+ // cin >> eventstring;
}
diff --git a/source/RobotAPI/applications/RobotControlUI/RobotControlUI.h b/source/RobotAPI/applications/RobotControlUI/RobotControlUI.h
index 8f6d223becf456e4c59557a67338e46c78f9310f..0fa2a7b12cd6929cedaf3aa886f4e7ab2d4929e8 100644
--- a/source/RobotAPI/applications/RobotControlUI/RobotControlUI.h
+++ b/source/RobotAPI/applications/RobotControlUI/RobotControlUI.h
@@ -38,7 +38,10 @@ namespace armarx
public:
int stateId;
RobotControlInterfacePrx robotProxy;
- virtual std::string getDefaultName() const { return "RobotControlUI"; }
+ virtual std::string getDefaultName() const
+ {
+ return "RobotControlUI";
+ }
virtual void onInitComponent();
virtual void onConnectComponent();
virtual void onExitComponent();
diff --git a/source/RobotAPI/applications/RobotControlUI/RobotControlUIApp.h b/source/RobotAPI/applications/RobotControlUI/RobotControlUIApp.h
index c596c983d5463d63b897f1e145d4b78382e68865..0764e757abf269af90a40ec661bbbd0e958256a2 100644
--- a/source/RobotAPI/applications/RobotControlUI/RobotControlUIApp.h
+++ b/source/RobotAPI/applications/RobotControlUI/RobotControlUIApp.h
@@ -37,7 +37,7 @@ namespace armarx
*/
void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
{
- registry->addObject( Component::create<RobotControlUI>(properties) );
+ registry->addObject(Component::create<RobotControlUI>(properties));
}
};
}
diff --git a/source/RobotAPI/applications/RobotStateComponent/RobotStateComponentApp.h b/source/RobotAPI/applications/RobotStateComponent/RobotStateComponentApp.h
index afab04d494e52d6d95bcb935f33c93bc26fd43f8..065d5f07715699b1c499b567338f80271207dede 100644
--- a/source/RobotAPI/applications/RobotStateComponent/RobotStateComponentApp.h
+++ b/source/RobotAPI/applications/RobotStateComponent/RobotStateComponentApp.h
@@ -36,13 +36,13 @@ namespace armarx
/**
* @see armarx::Application::setup()
*/
- void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
- {
+ void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
+ {
auto state = Component::create<RobotStateComponent>(properties);
auto observer = Component::create<RobotStateObserver>(properties);
state->setRobotStateObserver(observer);
- registry->addObject( state );
- registry->addObject( observer );
+ registry->addObject(state);
+ registry->addObject(observer);
}
};
}
diff --git a/source/RobotAPI/applications/RobotStateObserver/RobotStateObserverApp.h b/source/RobotAPI/applications/RobotStateObserver/RobotStateObserverApp.h
index f641c2f3b6ac1ba7ea6022684cf6c96f8424ee45..f0b6ca4cba6e32a7c03903827e5b3bbafd24cdcc 100644
--- a/source/RobotAPI/applications/RobotStateObserver/RobotStateObserverApp.h
+++ b/source/RobotAPI/applications/RobotStateObserver/RobotStateObserverApp.h
@@ -29,15 +29,15 @@ namespace armarx
/**
* Application for testing armarx::RobotStateObserver
*/
- class RobotStateObserverApp :
- virtual public armarx::Application
- {
+ class RobotStateObserverApp :
+ virtual public armarx::Application
+ {
/**
* @see armarx::Application::setup()
*/
- void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
- {
- registry->addObject( Component::create<RobotStateObserver>(properties) );
+ void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
+ {
+ registry->addObject(Component::create<RobotStateObserver>(properties));
}
- };
+ };
}
diff --git a/source/RobotAPI/applications/TCPControlUnit/TCPControlUnitApp.h b/source/RobotAPI/applications/TCPControlUnit/TCPControlUnitApp.h
index c1560f9c356e9c17574934320a7f569edbe7680c..5a395d6fd4aae92dd4ec535d91e2539f4d119571 100644
--- a/source/RobotAPI/applications/TCPControlUnit/TCPControlUnitApp.h
+++ b/source/RobotAPI/applications/TCPControlUnit/TCPControlUnitApp.h
@@ -28,12 +28,12 @@
namespace armarx
{
class TCPControlUnitApp :
- virtual public armarx::Application
- {
+ virtual public armarx::Application
+ {
void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
{
- registry->addObject( Component::create<TCPControlUnit>(properties) );
- registry->addObject( Component::create<TCPControlUnitObserver>(properties) );
+ registry->addObject(Component::create<TCPControlUnit>(properties));
+ registry->addObject(Component::create<TCPControlUnitObserver>(properties));
}
};
}
diff --git a/source/RobotAPI/applications/WeissHapticUnit/WeissHapticUnitApp.h b/source/RobotAPI/applications/WeissHapticUnit/WeissHapticUnitApp.h
index 21d421af42808c8f07035835cef6ffafce438025..9b2cb9e06b8029b90c2277e2bed3ff09d97eb63b 100644
--- a/source/RobotAPI/applications/WeissHapticUnit/WeissHapticUnitApp.h
+++ b/source/RobotAPI/applications/WeissHapticUnit/WeissHapticUnitApp.h
@@ -44,8 +44,8 @@ namespace armarx
void setup(const ManagedIceObjectRegistryInterfacePtr& registry,
Ice::PropertiesPtr properties)
{
- registry->addObject( Component::create<WeissHapticUnit>(properties) );
- registry->addObject( Component::create<HapticObserver>(properties) );
+ registry->addObject(Component::create<WeissHapticUnit>(properties));
+ registry->addObject(Component::create<HapticObserver>(properties));
}
};
}
diff --git a/source/RobotAPI/applications/XsensIMU/XsensIMUApp.h b/source/RobotAPI/applications/XsensIMU/XsensIMUApp.h
index fd3630c57ed183c5dfe4e27b9a32a1cfc8e68ad9..6fcd5818e014d6d66b0eeb373f30f577bced7e32 100644
--- a/source/RobotAPI/applications/XsensIMU/XsensIMUApp.h
+++ b/source/RobotAPI/applications/XsensIMU/XsensIMUApp.h
@@ -47,7 +47,7 @@ namespace armarx
void setup(const ManagedIceObjectRegistryInterfacePtr& registry,
Ice::PropertiesPtr properties)
{
- registry->addObject( Component::create<XsensIMU>(properties) );
+ registry->addObject(Component::create<XsensIMU>(properties));
}
};
}
diff --git a/source/RobotAPI/applications/deprecated/MotionControl/MotionControlApp.h b/source/RobotAPI/applications/deprecated/MotionControl/MotionControlApp.h
index 29a437ebaedd7e511e83b26f05880e3a169eb14f..54c2d4765c771bac03741ed4e388da9c52c7ac3a 100644
--- a/source/RobotAPI/applications/deprecated/MotionControl/MotionControlApp.h
+++ b/source/RobotAPI/applications/deprecated/MotionControl/MotionControlApp.h
@@ -27,11 +27,11 @@
namespace armarx
{
class MotionControlApp :
- virtual public armarx::Application
- {
+ virtual public armarx::Application
+ {
void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
{
- registry->addObject( Component::create<MotionControl::MotionControlOfferer>(properties) );
+ registry->addObject(Component::create<MotionControl::MotionControlOfferer>(properties));
}
};
}
diff --git a/source/RobotAPI/applications/deprecated/MotionControlTest/MotionControlTestApp.h b/source/RobotAPI/applications/deprecated/MotionControlTest/MotionControlTestApp.h
index fb8021dd3901e16c7d040a003d4940543e33c007..cf04af28420ce90829a0fbfba227cb73d6dca2d8 100644
--- a/source/RobotAPI/applications/deprecated/MotionControlTest/MotionControlTestApp.h
+++ b/source/RobotAPI/applications/deprecated/MotionControlTest/MotionControlTestApp.h
@@ -31,7 +31,7 @@ namespace armarx
{
void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
{
- registry->addObject( Component::create<MotionControl::MotionControlOfferer>(properties) );
+ registry->addObject(Component::create<MotionControl::MotionControlOfferer>(properties));
}
};
}
diff --git a/source/RobotAPI/applications/moved_to_robotcomponents_RobotIK/RobotIKApp.h b/source/RobotAPI/applications/moved_to_robotcomponents_RobotIK/RobotIKApp.h
index 4b30dc3ead8d8545bed545e69bb56e24dec6f04d..958331f3ee99014fb9ce027a1895fe0e248959b4 100644
--- a/source/RobotAPI/applications/moved_to_robotcomponents_RobotIK/RobotIKApp.h
+++ b/source/RobotAPI/applications/moved_to_robotcomponents_RobotIK/RobotIKApp.h
@@ -36,10 +36,10 @@ namespace armarx
/**
* @see armarx::Application::setup()
*/
- void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
- {
+ void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
+ {
auto iksolver = Component::create<RobotIK>(properties);
- registry->addObject( iksolver );
+ registry->addObject(iksolver);
}
};
}
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index 88a4f451f093120c9879d29e05432e4195f83c87..87bfcc5e9c1c9e8ee37d942ac7e6c794b4c25858 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -46,1680 +46,1871 @@
using namespace VirtualRobot;
-namespace armarx{
+namespace armarx
+{
-static const std::string DEBUG_LAYER_NAME{"debug"};
+ static const std::string DEBUG_LAYER_NAME
+ {"debug"
+ };
-DebugDrawerComponent::DebugDrawerComponent():
- mutex(new RecursiveMutex()),
- dataUpdateMutex(new RecursiveMutex())
-{
- cycleTimeMS = 1000.0f / 30.0f;
- timerSensor = NULL;
- verbose = true;
+ DebugDrawerComponent::DebugDrawerComponent():
+ mutex(new RecursiveMutex()),
+ dataUpdateMutex(new RecursiveMutex())
+ {
+ cycleTimeMS = 1000.0f / 30.0f;
+ timerSensor = NULL;
+ verbose = true;
- coinVisu = new SoSeparator;
- coinVisu->ref();
+ coinVisu = new SoSeparator;
+ coinVisu->ref();
- layerMainNode = new SoSeparator;
- layerMainNode->ref();
+ layerMainNode = new SoSeparator;
+ layerMainNode->ref();
- SoUnits *u = new SoUnits();
- u->units = SoUnits::MILLIMETERS;
- coinVisu->addChild(u);
- coinVisu->addChild(layerMainNode);
-}
+ SoUnits* u = new SoUnits();
+ u->units = SoUnits::MILLIMETERS;
+ coinVisu->addChild(u);
+ coinVisu->addChild(layerMainNode);
+ }
-void DebugDrawerComponent::setVisuUpdateTime(float visuUpdatesPerSec)
-{
- if (timerSensor)
+ void DebugDrawerComponent::setVisuUpdateTime(float visuUpdatesPerSec)
{
- ARMARX_ERROR << "Cannot change the cycle time, once the thread has been started...";
- return;
+ if (timerSensor)
+ {
+ ARMARX_ERROR << "Cannot change the cycle time, once the thread has been started...";
+ return;
+ }
+
+ if (visuUpdatesPerSec <= 0)
+ {
+ cycleTimeMS = 0;
+ }
+ else
+ {
+ cycleTimeMS = 1000.0f / (float)visuUpdatesPerSec;
+ }
}
- if (visuUpdatesPerSec<=0)
- cycleTimeMS = 0;
- else
- cycleTimeMS = 1000.0f / (float)visuUpdatesPerSec;
-}
-DebugDrawerComponent::~DebugDrawerComponent()
-{
- std::cout << "DESTRUCTOR debug drawer" << std::endl;
- /*{
- ScopedRecursiveLockPtr l = getScopedLock();
- layerMainNode->unref();
- coinVisu->unref();
- }*/
-}
-
-void DebugDrawerComponent::onInitComponent()
-{
- usingTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
- bool enabled = getProperty<bool>("ShowDebugDrawing").getValue();
- if (!enabled)
+ DebugDrawerComponent::~DebugDrawerComponent()
{
- disableAllLayers();
+ std::cout << "DESTRUCTOR debug drawer" << std::endl;
+ /*{
+ ScopedRecursiveLockPtr l = getScopedLock();
+ layerMainNode->unref();
+ coinVisu->unref();
+ }*/
}
- if (cycleTimeMS>0)
+ void DebugDrawerComponent::onInitComponent()
{
- SoSensorManager* sensor_mgr = SoDB::getSensorManager();
- timerSensor = new SoTimerSensor(updateVisualizationCB, this);
- timerSensor->setInterval(SbTime(cycleTimeMS / 1000.0f));
- sensor_mgr->insertTimerSensor(timerSensor);
+ usingTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
+ bool enabled = getProperty<bool>("ShowDebugDrawing").getValue();
+
+ if (!enabled)
+ {
+ disableAllLayers();
+ }
+
+ if (cycleTimeMS > 0)
+ {
+ SoSensorManager* sensor_mgr = SoDB::getSensorManager();
+ timerSensor = new SoTimerSensor(updateVisualizationCB, this);
+ timerSensor->setInterval(SbTime(cycleTimeMS / 1000.0f));
+ sensor_mgr->insertTimerSensor(timerSensor);
+
+ /*execTaskVisuUpdates = new PeriodicTask<DebugDrawerComponent>(this, &DebugDrawerComponent::updateVisualization, cycleTimeMS, false, "DebugDrawerComponentVisuUpdates");
+ execTaskVisuUpdates->start();
+ execTaskVisuUpdates->setDelayWarningTolerance(100);*/
+ }
- /*execTaskVisuUpdates = new PeriodicTask<DebugDrawerComponent>(this, &DebugDrawerComponent::updateVisualization, cycleTimeMS, false, "DebugDrawerComponentVisuUpdates");
- execTaskVisuUpdates->start();
- execTaskVisuUpdates->setDelayWarningTolerance(100);*/
}
-}
+ void DebugDrawerComponent::updateVisualizationCB(void* data, SoSensor* sensor)
+ {
+ DebugDrawerComponent* drawer = static_cast<DebugDrawerComponent*>(data);
-void DebugDrawerComponent::updateVisualizationCB(void* data, SoSensor* sensor)
-{
- DebugDrawerComponent* drawer = static_cast<DebugDrawerComponent*>(data);
- if (!drawer)
- return;
- drawer->updateVisualization();
-}
+ if (!drawer)
+ {
+ return;
+ }
+ drawer->updateVisualization();
+ }
-void DebugDrawerComponent::onConnectComponent()
-{
- verbose = true;
-}
-void DebugDrawerComponent::onDisconnectComponent()
-{
- std::cout << "onDisconnectComponent debug drawer" << std::endl;
- /*verbose = false;
- size_t c = layers.size();
- size_t counter = 0;
- while (layers.size()>0 && counter < 2*c)
+ void DebugDrawerComponent::onConnectComponent()
{
- std::cout << "removing layer " << layers.begin()->first;
- removeLayer(layers.begin()->first);
- counter++; // sec counter
+ verbose = true;
}
- {
- ScopedRecursiveLockPtr l = getScopedLock();
- coinVisu->removeAllChildren();
- }*/
-}
-void DebugDrawerComponent::onExitComponent()
-{
- ARMARX_INFO << "onExitComponent debug drawer";
- verbose = false;
- //ARMARX_DEBUG << "onExitComponent";
- if (timerSensor)
+ void DebugDrawerComponent::onDisconnectComponent()
{
- SoSensorManager* sensor_mgr = SoDB::getSensorManager();
- sensor_mgr->removeTimerSensor(timerSensor);
+ std::cout << "onDisconnectComponent debug drawer" << std::endl;
+ /*verbose = false;
+ size_t c = layers.size();
+ size_t counter = 0;
+ while (layers.size()>0 && counter < 2*c)
+ {
+ std::cout << "removing layer " << layers.begin()->first;
+ removeLayer(layers.begin()->first);
+ counter++; // sec counter
+ }
+ {
+ ScopedRecursiveLockPtr l = getScopedLock();
+ coinVisu->removeAllChildren();
+ }*/
}
- size_t c = layers.size();
- size_t counter = 0;
- while (layers.size()>0 && counter < 2*c)
+ void DebugDrawerComponent::onExitComponent()
{
- ARMARX_INFO << "removing layer " << layers.begin()->first;
- removeLayer(layers.begin()->first);
- counter++; // sec counter
+ ARMARX_INFO << "onExitComponent debug drawer";
+ verbose = false;
+
+ //ARMARX_DEBUG << "onExitComponent";
+ if (timerSensor)
+ {
+ SoSensorManager* sensor_mgr = SoDB::getSensorManager();
+ sensor_mgr->removeTimerSensor(timerSensor);
+ }
+
+ size_t c = layers.size();
+ size_t counter = 0;
+
+ while (layers.size() > 0 && counter < 2 * c)
+ {
+ ARMARX_INFO << "removing layer " << layers.begin()->first;
+ removeLayer(layers.begin()->first);
+ counter++; // sec counter
+ }
+
+ {
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
+ coinVisu->removeAllChildren();
+ layerMainNode->unref();
+ coinVisu->unref();
+ }
}
+ void DebugDrawerComponent::exportScene(const std::string& filename, const Ice::Current&)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- coinVisu->removeAllChildren();
- layerMainNode->unref();
- coinVisu->unref();
+ ARMARX_INFO << "Exporting scene to '" << filename << "'";
+
+ SoWriteAction writeAction;
+ writeAction.getOutput()->openFile(filename.c_str());
+ writeAction.getOutput()->setBinary(false);
+ writeAction.apply(layerMainNode);
+ writeAction.getOutput()->closeFile();
}
-}
-void DebugDrawerComponent::exportScene(const std::string &filename, const Ice::Current &)
-{
- ARMARX_INFO << "Exporting scene to '" << filename << "'";
+ void DebugDrawerComponent::exportLayer(const std::string& filename, const std::string& layerName, const Ice::Current&)
+ {
+ ARMARX_INFO << "Exporting layer '" << layerName << "' to '" << filename << "'";
- SoWriteAction writeAction;
- writeAction.getOutput()->openFile(filename.c_str());
- writeAction.getOutput()->setBinary(false);
- writeAction.apply(layerMainNode);
- writeAction.getOutput()->closeFile();
-}
+ if (!hasLayer(layerName))
+ {
+ ARMARX_WARNING << "Unknown layer to export";
+ return;
+ }
-void DebugDrawerComponent::exportLayer(const std::string &filename, const std::string &layerName, const Ice::Current &)
-{
- ARMARX_INFO << "Exporting layer '" << layerName << "' to '" << filename << "'";
+ SoWriteAction writeAction;
+ writeAction.getOutput()->openFile(filename.c_str());
+ writeAction.getOutput()->setBinary(false);
+ writeAction.apply(layers[layerName].mainNode);
+ writeAction.getOutput()->closeFile();
+ }
- if(!hasLayer(layerName))
+ void DebugDrawerComponent::drawCoordSystem(const CoordData& d)
{
- ARMARX_WARNING << "Unknown layer to export";
- return;
- }
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
+ ARMARX_DEBUG << "drawing coord system";
- SoWriteAction writeAction;
- writeAction.getOutput()->openFile(filename.c_str());
- writeAction.getOutput()->setBinary(false);
- writeAction.apply(layers[layerName].mainNode);
- writeAction.getOutput()->closeFile();
-}
+ auto& layer = requestLayer(d.layerName);
-void DebugDrawerComponent::drawCoordSystem(const CoordData &d)
-{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- ARMARX_DEBUG << "drawing coord system";
+ removeCoordSystem(d.layerName, d.name);
- auto& layer=requestLayer(d.layerName);
+ if (d.scale <= 0 || !d.active)
+ {
+ return;
+ }
- removeCoordSystem(d.layerName, d.name);
+ //if (layer->addedCoordVisualizations.find(name) == layer->addedCoordVisualizations.end())
+ //{
+ SoSeparator* newS = new SoSeparator;
+ SoMatrixTransform* newM = new SoMatrixTransform;
+ newS->addChild(newM);
+ std::string n = d.name;
+ newS->addChild(CoinVisualizationFactory::CreateCoordSystemVisualization(d.scale, &n));
+ layer.addedCoordVisualizations[d.name] = newS;
+ layer.mainNode->addChild(newS);
+ //}
+ SoSeparator* s = layer.addedCoordVisualizations[d.name];
+ SoMatrixTransform* m = (SoMatrixTransform*)(s->getChild(0));
+ SbMatrix mNew = CoinVisualizationFactory::getSbMatrix(d.globalPose);
+ m->matrix.setValue(mNew);
- if (d.scale<=0 || !d.active)
- {
- return;
+ ARMARX_DEBUG << "end";
}
- //if (layer->addedCoordVisualizations.find(name) == layer->addedCoordVisualizations.end())
- //{
- SoSeparator *newS = new SoSeparator;
- SoMatrixTransform* newM = new SoMatrixTransform;
- newS->addChild(newM);
- std::string n = d.name;
- newS->addChild(CoinVisualizationFactory::CreateCoordSystemVisualization(d.scale,&n));
- layer.addedCoordVisualizations[d.name] = newS;
- layer.mainNode->addChild(newS);
- //}
- SoSeparator *s = layer.addedCoordVisualizations[d.name];
- SoMatrixTransform* m = (SoMatrixTransform*)(s->getChild(0));
- SbMatrix mNew = CoinVisualizationFactory::getSbMatrix(d.globalPose);
- m->matrix.setValue(mNew);
+ void DebugDrawerComponent::drawLine(const LineData& d)
+ {
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
+ ARMARX_DEBUG << "drawLine1" << flush;
- ARMARX_DEBUG << "end";
-}
+ auto& layer = requestLayer(d.layerName);
-void DebugDrawerComponent::drawLine(const LineData &d)
-{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- ARMARX_DEBUG << "drawLine1" << flush;
+ removeLine(d.layerName, d.name);
- auto& layer=requestLayer(d.layerName);
+ if (!d.active)
+ {
+ return;
+ }
- removeLine(d.layerName,d.name);
+ SoSeparator* newS = new SoSeparator;
+ Eigen::Matrix4f lp1 = Eigen::Matrix4f::Identity();
+ lp1(0, 3) = d.p1.x();
+ lp1(1, 3) = d.p1.y();
+ lp1(2, 3) = d.p1.z();
+ Eigen::Matrix4f lp2 = Eigen::Matrix4f::Identity();
+ lp2(0, 3) = d.p2.x();
+ lp2(1, 3) = d.p2.y();
+ lp2(2, 3) = d.p2.z();
+ newS->addChild(CoinVisualizationFactory::createCoinLine(lp1, lp2, d.scale, d.color.r, d.color.g, d.color.b));
+ layer.addedLineVisualizations[d.name] = newS;
+ layer.mainNode->addChild(newS);
+ ARMARX_DEBUG << "drawLine2" << flush;
+ }
- if (!d.active)
- return;
+ void DebugDrawerComponent::drawBox(const BoxData& d)
+ {
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
+ ARMARX_DEBUG << "drawBox";
- SoSeparator *newS = new SoSeparator;
- Eigen::Matrix4f lp1 = Eigen::Matrix4f::Identity();
- lp1(0,3) = d.p1.x(); lp1(1,3) = d.p1.y(); lp1(2,3) = d.p1.z();
- Eigen::Matrix4f lp2 = Eigen::Matrix4f::Identity();
- lp2(0,3) = d.p2.x(); lp2(1,3) = d.p2.y(); lp2(2,3) = d.p2.z();
- newS->addChild(CoinVisualizationFactory::createCoinLine(lp1, lp2, d.scale, d.color.r, d.color.g, d.color.b));
- layer.addedLineVisualizations[d.name] = newS;
- layer.mainNode->addChild(newS);
- ARMARX_DEBUG << "drawLine2" << flush;
-}
+ auto& layer = requestLayer(d.layerName);
-void DebugDrawerComponent::drawBox(const BoxData &d)
-{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- ARMARX_DEBUG << "drawBox";
+ removeBox(d.layerName, d.name);
- auto& layer=requestLayer(d.layerName);
+ if (!d.active)
+ {
+ return;
+ }
- removeBox(d.layerName,d.name);
+ SoSeparator* newS = new SoSeparator;
+ Eigen::Matrix4f m = d.globalPose;
+ newS->addChild(CoinVisualizationFactory::getMatrixTransform(m));
- if (!d.active)
- return;
+ SoMaterial* material = new SoMaterial;
+ material->ambientColor.setValue(d.color.r, d.color.g, d.color.b);
+ material->diffuseColor.setValue(d.color.r, d.color.g, d.color.b);
+ material->transparency.setValue(d.color.transparency);
+ newS->addChild(material);
- SoSeparator *newS = new SoSeparator;
- Eigen::Matrix4f m = d.globalPose;
- newS->addChild(CoinVisualizationFactory::getMatrixTransform(m));
+ SoCube* cube = new SoCube;
+ cube->width = d.width;
+ cube->height = d.height;
+ cube->depth = d.depth;
+ newS->addChild(cube);
- SoMaterial *material = new SoMaterial;
- material->ambientColor.setValue(d.color.r, d.color.g, d.color.b);
- material->diffuseColor.setValue(d.color.r, d.color.g, d.color.b);
- material->transparency.setValue(d.color.transparency);
- newS->addChild(material);
+ layer.addedBoxVisualizations[d.name] = newS;
+ layer.mainNode->addChild(newS);
+ }
- SoCube *cube = new SoCube;
- cube->width = d.width;
- cube->height = d.height;
- cube->depth = d.depth;
- newS->addChild(cube);
+ void DebugDrawerComponent::drawText(const TextData& d)
+ {
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
+ ARMARX_DEBUG << "drawText1";
- layer.addedBoxVisualizations[d.name] = newS;
- layer.mainNode->addChild(newS);
-}
+ auto& layer = requestLayer(d.layerName);
-void DebugDrawerComponent::drawText(const TextData &d)
-{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- ARMARX_DEBUG << "drawText1";
+ removeText(d.layerName, d.name);
- auto& layer=requestLayer(d.layerName);
+ if (!d.active)
+ {
+ return;
+ }
- removeText(d.layerName,d.name);
+ SoSeparator* sep = new SoSeparator;
+ SoAnnotation* ann = new SoAnnotation;
+ sep->addChild(ann);
- if (!d.active)
- return;
+ SoMaterial* mat = new SoMaterial;
+ mat->ambientColor.setValue(d.color.r, d.color.g, d.color.b);
+ mat->diffuseColor.setValue(d.color.r, d.color.g, d.color.b);
+ mat->transparency.setValue(d.color.transparency);
+ ann->addChild(mat);
- SoSeparator *sep = new SoSeparator;
- SoAnnotation *ann = new SoAnnotation;
- sep->addChild(ann);
+ SoTransform* tr = new SoTransform;
+ tr->translation.setValue(d.position.x(), d.position.y(), d.position.z());
+ ann->addChild(tr);
- SoMaterial *mat = new SoMaterial;
- mat->ambientColor.setValue(d.color.r, d.color.g, d.color.b);
- mat->diffuseColor.setValue(d.color.r, d.color.g, d.color.b);
- mat->transparency.setValue(d.color.transparency);
- ann->addChild(mat);
+ SoFont* font = new SoFont;
+ font->name.setValue("TGS_Triplex_Roman");
+ font->size = d.size;
- SoTransform *tr = new SoTransform;
- tr->translation.setValue(d.position.x(), d.position.y(), d.position.z());
- ann->addChild(tr);
+ ann->addChild(font);
- SoFont *font = new SoFont;
- font->name.setValue("TGS_Triplex_Roman");
- font->size = d.size;
+ SoText2* te = new SoText2;
+ te->string = d.text.c_str();
+ te->justification = SoText2::CENTER;
+ ann->addChild(te);
- ann->addChild(font);
+ layer.addedTextVisualizations[d.name] = sep;
+ layer.mainNode->addChild(sep);
+ ARMARX_DEBUG << "drawText2";
+ }
- SoText2 *te = new SoText2;
- te->string = d.text.c_str();
- te->justification = SoText2::CENTER;
- ann->addChild(te);
+ void DebugDrawerComponent::drawSphere(const SphereData& d)
+ {
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
+ ARMARX_DEBUG << "drawSphere";
- layer.addedTextVisualizations[d.name] = sep;
- layer.mainNode->addChild(sep);
- ARMARX_DEBUG << "drawText2";
-}
+ auto& layer = requestLayer(d.layerName);
-void DebugDrawerComponent::drawSphere(const SphereData &d)
-{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- ARMARX_DEBUG << "drawSphere";
+ removeSphere(d.layerName, d.name);
- auto& layer=requestLayer(d.layerName);
+ if (!d.active)
+ {
+ return;
+ }
- removeSphere(d.layerName,d.name);
+ SoSeparator* sep = new SoSeparator;
+ SoMaterial* mat = new SoMaterial;
+ mat->ambientColor.setValue(d.color.r, d.color.g, d.color.b);
+ mat->diffuseColor.setValue(d.color.r, d.color.g, d.color.b);
+ mat->transparency.setValue(d.color.transparency);
+ sep->addChild(mat);
- if (!d.active)
- return;
+ SoTransform* tr = new SoTransform;
+ tr->translation.setValue(d.position.x(), d.position.y(), d.position.z());
+ sep->addChild(tr);
- SoSeparator *sep = new SoSeparator;
- SoMaterial *mat = new SoMaterial;
- mat->ambientColor.setValue(d.color.r, d.color.g, d.color.b);
- mat->diffuseColor.setValue(d.color.r, d.color.g, d.color.b);
- mat->transparency.setValue(d.color.transparency);
- sep->addChild(mat);
+ SoSphere* sphere = new SoSphere;
+ sphere->radius = d.radius;
+ sep->addChild(sphere);
- SoTransform *tr = new SoTransform;
- tr->translation.setValue(d.position.x(), d.position.y(), d.position.z());
- sep->addChild(tr);
+ layer.addedSphereVisualizations[d.name] = sep;
+ layer.mainNode->addChild(sep);
+ }
- SoSphere *sphere = new SoSphere;
- sphere->radius = d.radius;
- sep->addChild(sphere);
+ void DebugDrawerComponent::drawCylinder(const DebugDrawerComponent::CylinderData& d)
+ {
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
+ ARMARX_DEBUG << "drawCylinder";
- layer.addedSphereVisualizations[d.name] = sep;
- layer.mainNode->addChild(sep);
-}
+ auto& layer = requestLayer(d.layerName);
-void DebugDrawerComponent::drawCylinder(const DebugDrawerComponent::CylinderData &d)
-{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- ARMARX_DEBUG << "drawCylinder";
+ removeCylinder(d.layerName, d.name);
- auto& layer=requestLayer(d.layerName);
+ if (!d.active)
+ {
+ return;
+ }
- removeCylinder(d.layerName, d.name);
+ SoSeparator* sep = new SoSeparator;
+ SoMaterial* mat = new SoMaterial;
+ mat->ambientColor.setValue(d.color.r, d.color.g, d.color.b);
+ mat->diffuseColor.setValue(d.color.r, d.color.g, d.color.b);
+ mat->transparency.setValue(d.color.transparency);
+ sep->addChild(mat);
- if (!d.active)
- return;
+ // The cylinder extends in y-direction, hence choose the transformation
+ SoTransform* tr = new SoTransform;
+ tr->translation.setValue(d.position.x(), d.position.y(), d.position.z());
+ tr->rotation.setValue(SbRotation(SbVec3f(0, 1, 0), SbVec3f(d.direction.x(), d.direction.y(), d.direction.z())));
+ sep->addChild(tr);
- SoSeparator *sep = new SoSeparator;
- SoMaterial *mat = new SoMaterial;
- mat->ambientColor.setValue(d.color.r, d.color.g, d.color.b);
- mat->diffuseColor.setValue(d.color.r, d.color.g, d.color.b);
- mat->transparency.setValue(d.color.transparency);
- sep->addChild(mat);
+ SoCylinder* cylinder = new SoCylinder;
+ cylinder->height = d.length;
+ cylinder->radius = d.radius;
+ sep->addChild(cylinder);
- // The cylinder extends in y-direction, hence choose the transformation
- SoTransform *tr = new SoTransform;
- tr->translation.setValue(d.position.x(), d.position.y(), d.position.z());
- tr->rotation.setValue(SbRotation(SbVec3f(0, 1, 0), SbVec3f(d.direction.x(), d.direction.y(), d.direction.z())));
- sep->addChild(tr);
+ layer.addedCylinderVisualizations[d.name] = sep;
+ layer.mainNode->addChild(sep);
+ }
- SoCylinder *cylinder = new SoCylinder;
- cylinder->height = d.length;
- cylinder->radius = d.radius;
- sep->addChild(cylinder);
+ void DebugDrawerComponent::drawPointCloud(const PointCloudData& d)
+ {
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
- layer.addedCylinderVisualizations[d.name] = sep;
- layer.mainNode->addChild(sep);
-}
+ auto& layer = requestLayer(d.layerName);
-void DebugDrawerComponent::drawPointCloud(const PointCloudData &d)
-{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ removePointCloud(d.layerName, d.name);
- auto& layer=requestLayer(d.layerName);
+ if (!d.active)
+ {
+ return;
+ }
- removePointCloud(d.layerName,d.name);
+ SoSeparator* sep = new SoSeparator;
+ int sz = d.pointCloud.size();
+ auto coordinates = new float[sz][3];
- if (!d.active)
- return;
+ for (int i = 0; i < sz; i++)
+ {
+ const DebugDrawerPointCloudElement& e = d.pointCloud[i];
+ coordinates[i][0] = e.x;
+ coordinates[i][1] = e.y;
+ coordinates[i][2] = e.z;
+ }
- SoSeparator *sep = new SoSeparator;
- int sz = d.pointCloud.size();
- auto coordinates = new float[sz][3];
+ SoMaterial* material = new SoMaterial;
+ material->ambientColor.setValue(0.2, 0.2, 0.2);
+ material->diffuseColor.setValue(0.2, 0.2, 0.2);
+ sep->addChild(material);
- for(int i = 0; i < sz; i++)
- {
- const DebugDrawerPointCloudElement &e = d.pointCloud[i];
- coordinates[i][0] = e.x;
- coordinates[i][1] = e.y;
- coordinates[i][2] = e.z;
- }
+ SoCoordinate3* coords = new SoCoordinate3;
+ coords->point.setValues(0, sz, coordinates);
+ sep->addChild(coords);
- SoMaterial *material = new SoMaterial;
- material->ambientColor.setValue(0.2, 0.2, 0.2);
- material->diffuseColor.setValue(0.2, 0.2, 0.2);
- sep->addChild(material);
+ SoPointSet* pointSet = new SoPointSet;
+ sep->addChild(pointSet);
- SoCoordinate3 *coords = new SoCoordinate3;
- coords->point.setValues(0, sz, coordinates);
- sep->addChild(coords);
+ layer.addedPointCloudVisualizations[d.name] = sep;
+ layer.mainNode->addChild(sep);
+ }
- SoPointSet *pointSet = new SoPointSet;
- sep->addChild(pointSet);
+ void DebugDrawerComponent::drawPolygon(const DebugDrawerComponent::PolygonData& d)
+ {
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
- layer.addedPointCloudVisualizations[d.name] = sep;
- layer.mainNode->addChild(sep);
-}
+ auto& layer = requestLayer(d.layerName);
-void DebugDrawerComponent::drawPolygon(const DebugDrawerComponent::PolygonData &d)
-{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ removePolygon(d.layerName, d.name);
- auto& layer=requestLayer(d.layerName);
+ if (!d.active)
+ {
+ return;
+ }
- removePolygon(d.layerName,d.name);
+ SoSeparator* sep = VirtualRobot::CoinVisualizationFactory::CreatePolygonVisualization(d.points, d.colorInner, d.colorBorder, d.lineWidth);
+ layer.addedPolygonVisualizations[d.name] = sep;
+ layer.mainNode->addChild(sep);
+ }
- if (!d.active)
- return;
+ void DebugDrawerComponent::drawArrow(const DebugDrawerComponent::ArrowData& d)
+ {
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
- SoSeparator *sep = VirtualRobot::CoinVisualizationFactory::CreatePolygonVisualization(d.points,d.colorInner,d.colorBorder,d.lineWidth);
- layer.addedPolygonVisualizations[d.name] = sep;
- layer.mainNode->addChild(sep);
-}
+ auto& layer = requestLayer(d.layerName);
-void DebugDrawerComponent::drawArrow(const DebugDrawerComponent::ArrowData &d)
-{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ removeArrow(d.layerName, d.name);
- auto& layer=requestLayer(d.layerName);
+ if (!d.active)
+ {
+ return;
+ }
- removeArrow(d.layerName,d.name);
+ SoSeparator* sep = new SoSeparator;
- if (!d.active)
- return;
+ SoTransform* tr = new SoTransform;
+ tr->translation.setValue(d.position.x(), d.position.y(), d.position.z());
+ sep->addChild(tr);
- SoSeparator *sep = new SoSeparator;
+ SoSeparator* sepArrow = VirtualRobot::CoinVisualizationFactory::CreateArrow(d.direction, d.length, d.width, d.color);
+ sep->addChild(sepArrow);
- SoTransform *tr = new SoTransform;
- tr->translation.setValue(d.position.x(), d.position.y(), d.position.z());
- sep->addChild(tr);
+ layer.addedArrowVisualizations[d.name] = sep;
+ layer.mainNode->addChild(sep);
+ }
- SoSeparator *sepArrow = VirtualRobot::CoinVisualizationFactory::CreateArrow(d.direction,d.length,d.width,d.color);
- sep->addChild(sepArrow);
+ void DebugDrawerComponent::drawRobot(const DebugDrawerComponent::RobotData& d)
+ {
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
- layer.addedArrowVisualizations[d.name] = sep;
- layer.mainNode->addChild(sep);
-}
+ auto& layer = requestLayer(d.layerName);
-void DebugDrawerComponent::drawRobot(const DebugDrawerComponent::RobotData &d)
-{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ if (!d.active)
+ {
+ removeRobot(d.layerName, d.name);
+ return;
+ }
- auto& layer=requestLayer(d.layerName);
+ // load or get robot
+ RobotPtr rob = requestRobot(d);
- if (!d.active)
- {
- removeRobot(d.layerName,d.name);
- return;
- }
- // load or get robot
- RobotPtr rob = requestRobot(d);
- if (!rob)
- {
- ARMARX_ERROR << deactivateSpam() << "Could not determine robot " << d.name << " at layer " << d.layerName;
- return;
- }
+ if (!rob)
+ {
+ ARMARX_ERROR << deactivateSpam() << "Could not determine robot " << d.name << " at layer " << d.layerName;
+ return;
+ }
- if (d.updatePose)
- {
- rob->setGlobalPose(d.globalPose);
- }
- if (d.updateConfig)
- {
- rob->setJointValues(d.configuration);
- }
- if (d.updateColor)
- {
- if (layer.addedRobotVisualizations.find(d.name) == layer.addedRobotVisualizations.end())
+ if (d.updatePose)
{
- ARMARX_WARNING << deactivateSpam() << "Internal robot visu error";
- } else
+ rob->setGlobalPose(d.globalPose);
+ }
+
+ if (d.updateConfig)
{
- SoSeparator *sep = layer.addedRobotVisualizations[d.name];
- if (!sep || sep->getNumChildren()<2)
- {
- ARMARX_ERROR << "Internal robot layer error1";
- return;
- }
+ rob->setJointValues(d.configuration);
+ }
- SoMaterial* m = dynamic_cast<SoMaterial*>(sep->getChild(0));
- if (!m)
+ if (d.updateColor)
+ {
+ if (layer.addedRobotVisualizations.find(d.name) == layer.addedRobotVisualizations.end())
{
- ARMARX_ERROR << "Internal robot layer error2";
- return;
+ ARMARX_WARNING << deactivateSpam() << "Internal robot visu error";
}
- if (d.color.isNone())
- {
- m->setOverride(false);
- } else
+ else
{
- if (d.color.r<0 || d.color.g<0 || d.color.b<0)
+ SoSeparator* sep = layer.addedRobotVisualizations[d.name];
+
+ if (!sep || sep->getNumChildren() < 2)
+ {
+ ARMARX_ERROR << "Internal robot layer error1";
+ return;
+ }
+
+ SoMaterial* m = dynamic_cast<SoMaterial*>(sep->getChild(0));
+
+ if (!m)
+ {
+ ARMARX_ERROR << "Internal robot layer error2";
+ return;
+ }
+
+ if (d.color.isNone())
{
+ m->setOverride(false);
+ }
+ else
+ {
+ if (d.color.r < 0 || d.color.g < 0 || d.color.b < 0)
+ {
+ }
+
+ m->diffuseColor = SbColor(d.color.r, d.color.g, d.color.b);
+ m->ambientColor = SbColor(0, 0, 0);
+ m->transparency = std::max(0.0f, d.color.transparency);
+ m->setOverride(true);
}
- m->diffuseColor = SbColor(d.color.r,d.color.g,d.color.b);
- m->ambientColor = SbColor(0,0,0);
- m->transparency = std::max(0.0f,d.color.transparency);
- m->setOverride(true);
}
}
}
-}
-VirtualRobot::RobotPtr DebugDrawerComponent::requestRobot(const DebugDrawerComponent::RobotData &d)
-{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ VirtualRobot::RobotPtr DebugDrawerComponent::requestRobot(const DebugDrawerComponent::RobotData& d)
+ {
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
- auto& layer=requestLayer(d.layerName);
- std::string entryName = "__" + d.layerName + "__" + d.name + "__";
+ auto& layer = requestLayer(d.layerName);
+ std::string entryName = "__" + d.layerName + "__" + d.name + "__";
- ARMARX_DEBUG << "Requesting robot " << entryName;
- if (activeRobots.find(entryName) != activeRobots.end())
- {
- ARMARX_DEBUG << "Found robot " << entryName << ":" << activeRobots[entryName]->getName();
- return activeRobots[entryName];
- }
+ ARMARX_DEBUG << "Requesting robot " << entryName;
- VirtualRobot::RobotPtr result;
+ if (activeRobots.find(entryName) != activeRobots.end())
+ {
+ ARMARX_DEBUG << "Found robot " << entryName << ":" << activeRobots[entryName]->getName();
+ return activeRobots[entryName];
+ }
- if (d.robotFile.empty())
- {
- ARMARX_INFO << deactivateSpam() << "No robot defined for layer " << d.layerName << ", with name " << d.name;
- return result;
- }
+ VirtualRobot::RobotPtr result;
- if (!d.armarxProject.empty())
- {
- ARMARX_INFO << "Adding to datapaths of " << d.armarxProject;
- armarx::CMakePackageFinder finder(d.armarxProject);
- if (!finder.packageFound())
+ if (d.robotFile.empty())
{
- ARMARX_WARNING << "ArmarX Package " << d.armarxProject << " has not been found!";
- } else
+ ARMARX_INFO << deactivateSpam() << "No robot defined for layer " << d.layerName << ", with name " << d.name;
+ return result;
+ }
+
+ if (!d.armarxProject.empty())
{
- ARMARX_INFO << "Adding to datapaths: " << finder.getDataDir();
- armarx::ArmarXDataPath::addDataPaths(finder.getDataDir());
+ ARMARX_INFO << "Adding to datapaths of " << d.armarxProject;
+ armarx::CMakePackageFinder finder(d.armarxProject);
+
+ if (!finder.packageFound())
+ {
+ ARMARX_WARNING << "ArmarX Package " << d.armarxProject << " has not been found!";
+ }
+ else
+ {
+ ARMARX_INFO << "Adding to datapaths: " << finder.getDataDir();
+ armarx::ArmarXDataPath::addDataPaths(finder.getDataDir());
+ }
}
- }
- // load robot
- std::string filename = d.robotFile;
- ArmarXDataPath::getAbsolutePath(filename,filename);
- ARMARX_INFO << "Loading robot from " << filename;
- try
- {
- result = RobotIO::loadRobot(filename);
- } catch (...)
- {
+ // load robot
+ std::string filename = d.robotFile;
+ ArmarXDataPath::getAbsolutePath(filename, filename);
+ ARMARX_INFO << "Loading robot from " << filename;
- }
+ try
+ {
+ result = RobotIO::loadRobot(filename);
+ }
+ catch (...)
+ {
- if (!result)
- {
- ARMARX_IMPORTANT << "Robot loading failed, file:" << filename;
- return result;
- }
+ }
- SoSeparator *sep = new SoSeparator;
- layer.mainNode->addChild(sep);
+ if (!result)
+ {
+ ARMARX_IMPORTANT << "Robot loading failed, file:" << filename;
+ return result;
+ }
- SoMaterial *m = new SoMaterial;
- m->setOverride(false);
- sep->addChild(m);
+ SoSeparator* sep = new SoSeparator;
+ layer.mainNode->addChild(sep);
+ SoMaterial* m = new SoMaterial;
+ m->setOverride(false);
+ sep->addChild(m);
- VirtualRobot::SceneObject::VisualizationType visuType = VirtualRobot::SceneObject::Full;
- if (d.drawStyle == DrawStyle::CollisionModel)
- visuType = VirtualRobot::SceneObject::Collision;
- boost::shared_ptr<CoinVisualization> robVisu = result->getVisualization<CoinVisualization>(visuType);
- if (robVisu)
- {
- SoNode *sepRob = robVisu->getCoinVisualization();
- if (sepRob)
- sep->addChild(sepRob);
- }
- activeRobots[entryName] = result;
- ARMARX_DEBUG << "setting robot to activeRobots, entryName:" << entryName << ", activeRobots.size():" << activeRobots.size();
- layer.addedRobotVisualizations[d.name] = sep;
- ARMARX_DEBUG << "adding sep to layer.addedRobotVisualizations, d.name:" << d.name << ", layer:" << d.layerName << ", layer.addedRobotVisualizations.size():" << layer.addedRobotVisualizations.size();
+ VirtualRobot::SceneObject::VisualizationType visuType = VirtualRobot::SceneObject::Full;
- return result;
-}
+ if (d.drawStyle == DrawStyle::CollisionModel)
+ {
+ visuType = VirtualRobot::SceneObject::Collision;
+ }
-void DebugDrawerComponent::removeLine(const std::string& layerName, const std::string &name)
-{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- if(!hasLayer(layerName))
- {
- return;
- }
- auto& layer=layers.at(layerName);
- if (layer.addedLineVisualizations.find(name) == layer.addedLineVisualizations.end())
- {
- return;
- }
+ boost::shared_ptr<CoinVisualization> robVisu = result->getVisualization<CoinVisualization>(visuType);
- layer.mainNode->removeChild(layer.addedLineVisualizations[name]);
- layer.addedLineVisualizations.erase(name);
-}
+ if (robVisu)
+ {
+ SoNode* sepRob = robVisu->getCoinVisualization();
-void DebugDrawerComponent::removeBox(const std::string& layerName, const std::string &name)
-{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- if(!hasLayer(layerName))
- {
- return;
- }
- auto& layer=layers.at(layerName);
- if(layer.addedBoxVisualizations.find(name) == layer.addedBoxVisualizations.end())
- {
- return;
- }
+ if (sepRob)
+ {
+ sep->addChild(sepRob);
+ }
+ }
- layer.mainNode->removeChild(layer.addedBoxVisualizations[name]);
- layer.addedBoxVisualizations.erase(name);
-}
+ activeRobots[entryName] = result;
+ ARMARX_DEBUG << "setting robot to activeRobots, entryName:" << entryName << ", activeRobots.size():" << activeRobots.size();
+ layer.addedRobotVisualizations[d.name] = sep;
+ ARMARX_DEBUG << "adding sep to layer.addedRobotVisualizations, d.name:" << d.name << ", layer:" << d.layerName << ", layer.addedRobotVisualizations.size():" << layer.addedRobotVisualizations.size();
-void DebugDrawerComponent::removeText(const std::string &layerName, const std::string &name)
-{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- if(!hasLayer(layerName))
- {
- return;
+ return result;
}
- auto& layer=layers.at(layerName);
- if(layer.addedTextVisualizations.find(name) == layer.addedTextVisualizations.end())
+
+ void DebugDrawerComponent::removeLine(const std::string& layerName, const std::string& name)
{
- return;
- }
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
- layer.mainNode->removeChild(layer.addedTextVisualizations[name]);
- layer.addedTextVisualizations.erase(name);
-}
+ if (!hasLayer(layerName))
+ {
+ return;
+ }
-void DebugDrawerComponent::removeSphere(const std::string &layerName, const std::string &name)
-{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- if(!hasLayer(layerName))
- {
- return;
- }
- auto& layer=layers.at(layerName);
- if(layer.addedSphereVisualizations.find(name) == layer.addedSphereVisualizations.end())
- {
- return;
- }
+ auto& layer = layers.at(layerName);
- layer.mainNode->removeChild(layer.addedSphereVisualizations[name]);
- layer.addedSphereVisualizations.erase(name);
-}
+ if (layer.addedLineVisualizations.find(name) == layer.addedLineVisualizations.end())
+ {
+ return;
+ }
-void DebugDrawerComponent::removeCylinder(const std::string &layerName, const std::string &name)
-{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- if(!hasLayer(layerName))
- {
- return;
+ layer.mainNode->removeChild(layer.addedLineVisualizations[name]);
+ layer.addedLineVisualizations.erase(name);
}
- auto& layer=layers.at(layerName);
- if(layer.addedCylinderVisualizations.find(name) == layer.addedCylinderVisualizations.end())
+
+ void DebugDrawerComponent::removeBox(const std::string& layerName, const std::string& name)
{
- return;
- }
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
- layer.mainNode->removeChild(layer.addedCylinderVisualizations[name]);
- layer.addedCylinderVisualizations.erase(name);
-}
+ if (!hasLayer(layerName))
+ {
+ return;
+ }
-void DebugDrawerComponent::removePointCloud(const std::string &layerName, const std::string &name)
-{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- if(!hasLayer(layerName))
- {
- return;
- }
- auto& layer=layers.at(layerName);
- if(layer.addedPointCloudVisualizations.find(name) == layer.addedPointCloudVisualizations.end())
- {
- return;
- }
+ auto& layer = layers.at(layerName);
- layer.mainNode->removeChild(layer.addedPointCloudVisualizations[name]);
- layer.addedPointCloudVisualizations.erase(name);
-}
+ if (layer.addedBoxVisualizations.find(name) == layer.addedBoxVisualizations.end())
+ {
+ return;
+ }
-void DebugDrawerComponent::removePolygon(const std::string &layerName, const std::string &name)
-{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- if(!hasLayer(layerName))
- {
- return;
+ layer.mainNode->removeChild(layer.addedBoxVisualizations[name]);
+ layer.addedBoxVisualizations.erase(name);
}
- auto& layer=layers.at(layerName);
- if(layer.addedPolygonVisualizations.find(name) == layer.addedPolygonVisualizations.end())
+
+ void DebugDrawerComponent::removeText(const std::string& layerName, const std::string& name)
{
- return;
- }
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
+
+ if (!hasLayer(layerName))
+ {
+ return;
+ }
- layer.mainNode->removeChild(layer.addedPolygonVisualizations[name]);
- layer.addedPolygonVisualizations.erase(name);
-}
+ auto& layer = layers.at(layerName);
-void DebugDrawerComponent::removeArrow(const std::string &layerName, const std::string &name)
-{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- if(!hasLayer(layerName))
- {
- return;
+ if (layer.addedTextVisualizations.find(name) == layer.addedTextVisualizations.end())
+ {
+ return;
+ }
+
+ layer.mainNode->removeChild(layer.addedTextVisualizations[name]);
+ layer.addedTextVisualizations.erase(name);
}
- auto& layer=layers.at(layerName);
- if(layer.addedArrowVisualizations.find(name) == layer.addedArrowVisualizations.end())
+
+ void DebugDrawerComponent::removeSphere(const std::string& layerName, const std::string& name)
{
- return;
- }
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
- layer.mainNode->removeChild(layer.addedArrowVisualizations[name]);
- layer.addedArrowVisualizations.erase(name);
-}
+ if (!hasLayer(layerName))
+ {
+ return;
+ }
-void DebugDrawerComponent::removeRobot(const std::string &layerName, const std::string &name)
-{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto& layer = layers.at(layerName);
- // process active robots
- std::string entryName = "__" + layerName + "__" + name + "__";
- ARMARX_DEBUG << "Removing robot " << entryName;
+ if (layer.addedSphereVisualizations.find(name) == layer.addedSphereVisualizations.end())
+ {
+ return;
+ }
- if (activeRobots.find(entryName) != activeRobots.end())
- {
- ARMARX_DEBUG << "Found robot to remove " << entryName;
- activeRobots.erase(entryName);
- ARMARX_DEBUG << "after Found robot to remove, activeRobots.size() = " << activeRobots.size();
+ layer.mainNode->removeChild(layer.addedSphereVisualizations[name]);
+ layer.addedSphereVisualizations.erase(name);
}
- // process visualizations
- if(!hasLayer(layerName))
- {
- ARMARX_DEBUG << "Layer not found " << layerName;
- return;
- }
- auto& layer=layers.at(layerName);
- if(layer.addedRobotVisualizations.find(name) == layer.addedRobotVisualizations.end())
+ void DebugDrawerComponent::removeCylinder(const std::string& layerName, const std::string& name)
{
- ARMARX_INFO << "Could not find robot with name " << name;
- return;
- }
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
+
+ if (!hasLayer(layerName))
+ {
+ return;
+ }
- ARMARX_DEBUG << "Removing visualization for " << entryName;
+ auto& layer = layers.at(layerName);
- ARMARX_DEBUG << "removing sep from layer.addedRobotVisualizations, d.name:" << name << ", layer:" << layerName << ", layer.addedRobotVisualizations.size():" << layer.addedRobotVisualizations.size();
- if (layer.addedRobotVisualizations.find(name) == layer.addedRobotVisualizations.end())
- {
- ARMARX_WARNING << "separator not found...";
- return;
- }
- ARMARX_DEBUG << "removing from layer.mainNode, layer.mainNode->getNumChildren()=" << layer.mainNode->getNumChildren();
- if (layer.mainNode->findChild(layer.addedRobotVisualizations[name])<0)
- {
- ARMARX_WARNING << "separator with wrong index...";
- return;
+ if (layer.addedCylinderVisualizations.find(name) == layer.addedCylinderVisualizations.end())
+ {
+ return;
+ }
+
+ layer.mainNode->removeChild(layer.addedCylinderVisualizations[name]);
+ layer.addedCylinderVisualizations.erase(name);
}
- layer.mainNode->removeChild(layer.addedRobotVisualizations[name]);
- layer.addedRobotVisualizations.erase(name);
- ARMARX_DEBUG << "after removing from layer.mainNode, layer.mainNode->getNumChildren()=" << layer.mainNode->getNumChildren();
- ARMARX_DEBUG << "after removing sep from layer.addedRobotVisualizations, d.name:" << name << ", layer:" << layerName << ", layer.addedRobotVisualizations.size():" << layer.addedRobotVisualizations.size();
-}
-void DebugDrawerComponent::removeCoordSystem(const std::string& layerName, const std::string &name)
-{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- if(!hasLayer(layerName))
- {
- return;
- }
- auto& layer=layers.at(layerName);
- if(layer.addedCoordVisualizations.find(name) == layer.addedCoordVisualizations.end())
+ void DebugDrawerComponent::removePointCloud(const std::string& layerName, const std::string& name)
{
- return;
- }
-
- layer.mainNode->removeChild(layer.addedCoordVisualizations[name]);
- layer.addedCoordVisualizations.erase(name);
-}
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
-void DebugDrawerComponent::setLayerVisibility(const std::string& layerName, bool visible)
-{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- if(!hasLayer(layerName))
- {
- return;
- }
- auto& layer=layers.at(layerName);
- layer.visible=visible;
- if (visible)
- {
- if (layerMainNode->findChild(layer.mainNode)<0)
+ if (!hasLayer(layerName))
{
- layerMainNode->addChild(layer.mainNode);
+ return;
}
- } else
- {
- if (layerMainNode->findChild(layer.mainNode)>=0)
+
+ auto& layer = layers.at(layerName);
+
+ if (layer.addedPointCloudVisualizations.find(name) == layer.addedPointCloudVisualizations.end())
{
- layerMainNode->removeChild(layer.mainNode);
+ return;
}
- }
-}
-
-void DebugDrawerComponent::disableAllLayers(const ::Ice::Current&)
-{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- if (coinVisu->findChild(layerMainNode)>=0)
- {
- coinVisu->removeChild(layerMainNode);
- }
-}
-void DebugDrawerComponent::enableAllLayers(const ::Ice::Current&)
-{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- if (coinVisu->findChild(layerMainNode)<0)
- {
- coinVisu->addChild(layerMainNode);
+ layer.mainNode->removeChild(layer.addedPointCloudVisualizations[name]);
+ layer.addedPointCloudVisualizations.erase(name);
}
-}
-void DebugDrawerComponent::setScaledPoseVisu(const std::string& layerName, const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Float scale, const ::Ice::Current&)
-{
- ARMARX_DEBUG << VAROUT(layerName) << VAROUT(poseName);
- Eigen::Matrix4f gp = PosePtr::dynamicCast(globalPose)->toEigen();
+ void DebugDrawerComponent::removePolygon(const std::string& layerName, const std::string& name)
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
- std::string entryName = "__" + layerName + "__" + poseName + "__";
- CoordData &d = accumulatedUpdateData.coord[entryName];
- d.globalPose = gp;
- d.layerName = layerName;
- d.name = poseName;
- d.scale = scale;
- d.active = true;
- }
-}
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
-void DebugDrawerComponent::setScaledPoseDebugLayerVisu(const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Float scale, const ::Ice::Current&)
-{
- setScaledPoseVisu(DEBUG_LAYER_NAME,poseName,globalPose,scale);
-}
+ if (!hasLayer(layerName))
+ {
+ return;
+ }
-void DebugDrawerComponent::setPoseVisu(const std::string &layerName, const std::string &poseName, const PoseBasePtr &globalPose, const Ice::Current &)
-{
- setScaledPoseVisu(layerName,poseName,globalPose,1.f);
-}
+ auto& layer = layers.at(layerName);
-void DebugDrawerComponent::setPoseDebugLayerVisu(const std::string &poseName, const PoseBasePtr &globalPose, const Ice::Current &)
-{
- setScaledPoseVisu(DEBUG_LAYER_NAME,poseName,globalPose,1.f);
-}
+ if (layer.addedPolygonVisualizations.find(name) == layer.addedPolygonVisualizations.end())
+ {
+ return;
+ }
-void DebugDrawerComponent::removePoseVisu(const std::string &layerName, const std::string &poseName, const Ice::Current &)
-{
- {
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
- std::string entryName = "__" + layerName + "__" + poseName + "__";
- CoordData &d = accumulatedUpdateData.coord[entryName];
- d.layerName = layerName;
- d.name = poseName;
- d.active = false;
+ layer.mainNode->removeChild(layer.addedPolygonVisualizations[name]);
+ layer.addedPolygonVisualizations.erase(name);
}
-}
-void DebugDrawerComponent::removePoseDebugLayerVisu(const std::string &poseName, const Ice::Current &)
-{
- removePoseVisu(DEBUG_LAYER_NAME,poseName);
-}
-
-void DebugDrawerComponent::setLineVisu(const std::string &layerName, const std::string &lineName, const Vector3BasePtr &globalPosition1, const Vector3BasePtr &globalPosition2, float lineWidth, const DrawColor &color, const Ice::Current &)
-{
- Eigen::Vector3f p1 = Vector3Ptr::dynamicCast(globalPosition1)->toEigen();
- Eigen::Vector3f p2 = Vector3Ptr::dynamicCast(globalPosition2)->toEigen();
- VirtualRobot::VisualizationFactory::Color c(color.r,color.g,color.b, 1 - color.a);
+ void DebugDrawerComponent::removeArrow(const std::string& layerName, const std::string& name)
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
- std::string entryName = "__" + layerName + "__" + lineName + "__";
- LineData &d = accumulatedUpdateData.line[entryName];
- d.p1 = p1;
- d.p2 = p2;
- d.layerName = layerName;
- d.name = lineName;
- d.color = c;
- d.scale = lineWidth;
- d.active = true;
- }
-}
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
-void DebugDrawerComponent::setLineDebugLayerVisu(const std::string &lineName, const Vector3BasePtr &globalPosition1, const Vector3BasePtr &globalPosition2, float lineWidth, const DrawColor &color, const Ice::Current &)
-{
- setLineVisu(DEBUG_LAYER_NAME,lineName,globalPosition1,globalPosition2, lineWidth, color);
-}
+ if (!hasLayer(layerName))
+ {
+ return;
+ }
-void DebugDrawerComponent::removeLineVisu(const std::string &layerName, const std::string &lineName, const Ice::Current &)
-{
- {
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
- std::string entryName = "__" + layerName + "__" + lineName + "__";
- LineData &d = accumulatedUpdateData.line[entryName];
- d.layerName = layerName;
- d.name = lineName;
- d.active = false;
- }
-}
+ auto& layer = layers.at(layerName);
-void DebugDrawerComponent::removeLineDebugLayerVisu(const std::string &lineName, const Ice::Current &)
-{
- removeLineVisu(DEBUG_LAYER_NAME,lineName);
-}
+ if (layer.addedArrowVisualizations.find(name) == layer.addedArrowVisualizations.end())
+ {
+ return;
+ }
-void DebugDrawerComponent::setBoxVisu(const std::string &layerName, const std::string &boxName, const PoseBasePtr &globalPose, const Vector3BasePtr &dimensions, const DrawColor &color, const Ice::Current &)
-{
- Eigen::Matrix4f gp = PosePtr::dynamicCast(globalPose)->toEigen();
- VirtualRobot::VisualizationFactory::Color c(color.r,color.g,color.b, 1 - color.a);
- {
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
- std::string entryName = "__" + layerName + "__" + boxName + "__";
- BoxData &d = accumulatedUpdateData.box[entryName];
- d.width = dimensions->x;
- d.height = dimensions->y;
- d.depth = dimensions->z;
- d.layerName = layerName;
- d.name = boxName;
- d.color = c;
- d.globalPose = gp;
- d.active = true;
+ layer.mainNode->removeChild(layer.addedArrowVisualizations[name]);
+ layer.addedArrowVisualizations.erase(name);
}
-}
-
-void DebugDrawerComponent::setBoxDebugLayerVisu(const std::string &boxName, const PoseBasePtr &globalPose, const Vector3BasePtr &dimensions, const DrawColor &color, const Ice::Current &)
-{
- setBoxVisu(DEBUG_LAYER_NAME, boxName, globalPose, dimensions, color);
-}
-void DebugDrawerComponent::removeBoxVisu(const std::string &layerName, const std::string &boxName, const Ice::Current &)
-{
+ void DebugDrawerComponent::removeRobot(const std::string& layerName, const std::string& name)
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
- std::string entryName = "__" + layerName + "__" + boxName + "__";
- BoxData &d = accumulatedUpdateData.box[entryName];
- d.layerName = layerName;
- d.name = boxName;
- d.active = false;
- }
-}
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
-void DebugDrawerComponent::removeBoxDebugLayerVisu(const std::string &boxName, const Ice::Current &)
-{
- removeBoxVisu(DEBUG_LAYER_NAME, boxName);
-}
+ // process active robots
+ std::string entryName = "__" + layerName + "__" + name + "__";
+ ARMARX_DEBUG << "Removing robot " << entryName;
-void DebugDrawerComponent::setTextVisu(const std::string &layerName, const std::string &textName, const std::string &text, const Vector3BasePtr &globalPosition, const DrawColor &color, int size, const Ice::Current &)
-{
- ARMARX_DEBUG << VAROUT(layerName) << VAROUT(textName);
- {
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
- std::string entryName = "__" + layerName + "__" + textName + "__";
- TextData &d = accumulatedUpdateData.text[entryName];
- d.text = text;
- d.position = Vector3Ptr::dynamicCast(globalPosition)->toEigen();
- d.layerName = layerName;
- d.name = textName;
- d.color = VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);
- d.size = size;
- d.active = true;
- }
-}
+ if (activeRobots.find(entryName) != activeRobots.end())
+ {
+ ARMARX_DEBUG << "Found robot to remove " << entryName;
+ activeRobots.erase(entryName);
+ ARMARX_DEBUG << "after Found robot to remove, activeRobots.size() = " << activeRobots.size();
+ }
-void DebugDrawerComponent::setTextDebugLayerVisu(const std::string &textName, const std::string &text, const Vector3BasePtr &globalPosition, const DrawColor &color, int size, const Ice::Current &)
-{
- setTextVisu(DEBUG_LAYER_NAME, textName, text, globalPosition, color, size);
-}
+ // process visualizations
+ if (!hasLayer(layerName))
+ {
+ ARMARX_DEBUG << "Layer not found " << layerName;
+ return;
+ }
-void DebugDrawerComponent::removeTextVisu(const std::string &layerName, const std::string &textName, const Ice::Current &)
-{
- {
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
- std::string entryName = "__" + layerName + "__" + textName + "__";
- TextData &d = accumulatedUpdateData.text[entryName];
- d.layerName = layerName;
- d.name = textName;
- d.active = false;
- }
-}
+ auto& layer = layers.at(layerName);
-void DebugDrawerComponent::removeTextDebugLayerVisu(const std::string &textName, const Ice::Current &)
-{
- removeTextVisu(DEBUG_LAYER_NAME, textName);
-}
+ if (layer.addedRobotVisualizations.find(name) == layer.addedRobotVisualizations.end())
+ {
+ ARMARX_INFO << "Could not find robot with name " << name;
+ return;
+ }
-void DebugDrawerComponent::setSphereVisu(const std::string &layerName, const std::string &sphereName, const Vector3BasePtr &globalPosition, const DrawColor &color, float radius, const Ice::Current &)
-{
- ARMARX_DEBUG << VAROUT(layerName) << VAROUT(sphereName);
- {
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
- std::string entryName = "__" + layerName + "__" + sphereName + "__";
- SphereData &d = accumulatedUpdateData.sphere[entryName];
- d.radius = radius;
- d.position = Vector3Ptr::dynamicCast(globalPosition)->toEigen();
- d.layerName = layerName;
- d.name = sphereName;
- d.color = VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);
- d.active = true;
- }
-}
+ ARMARX_DEBUG << "Removing visualization for " << entryName;
-void DebugDrawerComponent::setSphereDebugLayerVisu(const std::string &sphereName, const Vector3BasePtr &globalPosition, const DrawColor &color, float radius, const Ice::Current &)
-{
- setSphereVisu(DEBUG_LAYER_NAME, sphereName, globalPosition, color, radius);
-}
+ ARMARX_DEBUG << "removing sep from layer.addedRobotVisualizations, d.name:" << name << ", layer:" << layerName << ", layer.addedRobotVisualizations.size():" << layer.addedRobotVisualizations.size();
-void DebugDrawerComponent::removeSphereVisu(const std::string &layerName, const std::string &sphereName, const Ice::Current &)
-{
- {
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
- std::string entryName = "__" + layerName + "__" + sphereName + "__";
- SphereData &d = accumulatedUpdateData.sphere[entryName];
- d.layerName = layerName;
- d.name = sphereName;
- d.active = false;
- }
-}
+ if (layer.addedRobotVisualizations.find(name) == layer.addedRobotVisualizations.end())
+ {
+ ARMARX_WARNING << "separator not found...";
+ return;
+ }
-void DebugDrawerComponent::removeSphereDebugLayerVisu(const std::string &sphereName, const Ice::Current &)
-{
- removeSphereVisu(DEBUG_LAYER_NAME, sphereName);
-}
+ ARMARX_DEBUG << "removing from layer.mainNode, layer.mainNode->getNumChildren()=" << layer.mainNode->getNumChildren();
-void DebugDrawerComponent::setCylinderVisu(const std::string &layerName, const std::string &cylinderName, const Vector3BasePtr &globalPosition, const Vector3BasePtr &direction, float length, float radius, const DrawColor &color, const Ice::Current &)
-{
- ARMARX_DEBUG << VAROUT(layerName) << VAROUT(cylinderName);
- {
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
- std::string entryName = "__" + layerName + "__" + cylinderName + "__";
- CylinderData &d = accumulatedUpdateData.cylinder[entryName];
- d.position = Vector3Ptr::dynamicCast(globalPosition)->toEigen();
- d.direction = Vector3Ptr::dynamicCast(direction)->toEigen();
- d.length = length;
- d.radius = radius;
- d.layerName = layerName;
- d.name = cylinderName;
- d.color = VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);
- d.active = true;
- }
-}
+ if (layer.mainNode->findChild(layer.addedRobotVisualizations[name]) < 0)
+ {
+ ARMARX_WARNING << "separator with wrong index...";
+ return;
+ }
-void DebugDrawerComponent::setCylinderDebugLayerVisu(const std::string &cylinderName, const Vector3BasePtr &globalPosition, const Vector3BasePtr &direction, float length, float radius, const DrawColor &color, const Ice::Current &)
-{
- setCylinderVisu(DEBUG_LAYER_NAME, cylinderName, globalPosition, direction, length, radius, color);
-}
+ layer.mainNode->removeChild(layer.addedRobotVisualizations[name]);
+ layer.addedRobotVisualizations.erase(name);
+ ARMARX_DEBUG << "after removing from layer.mainNode, layer.mainNode->getNumChildren()=" << layer.mainNode->getNumChildren();
+ ARMARX_DEBUG << "after removing sep from layer.addedRobotVisualizations, d.name:" << name << ", layer:" << layerName << ", layer.addedRobotVisualizations.size():" << layer.addedRobotVisualizations.size();
+ }
-void DebugDrawerComponent::removeCylinderVisu(const std::string &layerName, const std::string &cylinderName, const Ice::Current &)
-{
+ void DebugDrawerComponent::removeCoordSystem(const std::string& layerName, const std::string& name)
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
- std::string entryName = "__" + layerName + "__" + cylinderName + "__";
- CylinderData &d = accumulatedUpdateData.cylinder[entryName];
- d.layerName = layerName;
- d.name = cylinderName;
- d.active = false;
- }
-}
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
-void DebugDrawerComponent::removeCylinderDebugLayerVisu(const std::string &cylinderName, const Ice::Current &)
-{
- removeCylinderVisu(DEBUG_LAYER_NAME, cylinderName);
-}
+ if (!hasLayer(layerName))
+ {
+ return;
+ }
-void DebugDrawerComponent::setPointCloudVisu(const std::string &layerName, const std::string &pointCloudName, const DebugDrawerPointCloud &pointCloud, const Ice::Current &)
-{
- ARMARX_DEBUG << VAROUT(layerName) << VAROUT(pointCloudName);
- {
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
- std::string entryName = "__" + layerName + "__" + pointCloudName + "__";
- PointCloudData &d = accumulatedUpdateData.pointcloud[entryName];
- d.pointCloud = pointCloud;
- d.layerName = layerName;
- d.name = pointCloudName;
- d.active = true;
- }
-}
+ auto& layer = layers.at(layerName);
-void DebugDrawerComponent::setPointCloudDebugLayerVisu(const std::string &pointCloudName, const DebugDrawerPointCloud &pointCloud, const Ice::Current &)
-{
- setPointCloudVisu(DEBUG_LAYER_NAME, pointCloudName, pointCloud);
-}
+ if (layer.addedCoordVisualizations.find(name) == layer.addedCoordVisualizations.end())
+ {
+ return;
+ }
-void DebugDrawerComponent::removePointCloudVisu(const std::string &layerName, const std::string &pointCloudName, const Ice::Current &)
-{
- {
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
- std::string entryName = "__" + layerName + "__" + pointCloudName + "__";
- PointCloudData &d = accumulatedUpdateData.pointcloud[entryName];
- d.layerName = layerName;
- d.name = pointCloudName;
- d.active = false;
+ layer.mainNode->removeChild(layer.addedCoordVisualizations[name]);
+ layer.addedCoordVisualizations.erase(name);
}
-}
-
-void DebugDrawerComponent::removePointCloudDebugLayerVisu(const std::string &pointCloudName, const Ice::Current &)
-{
- removePointCloudVisu(DEBUG_LAYER_NAME, pointCloudName);
-}
-void DebugDrawerComponent::setPolygonVisu(const std::string &layerName, const std::string &polygonName, const std::vector<Vector3BasePtr> &polygonPoints, const DrawColor &colorInner, const DrawColor &colorBorder, float lineWidth, const Ice::Current &)
-{
+ void DebugDrawerComponent::setLayerVisibility(const std::string& layerName, bool visible)
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
- std::string entryName = "__" + layerName + "__" + polygonName + "__";
- PolygonData &d = accumulatedUpdateData.polygons[entryName];
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
- std::vector< Eigen::Vector3f > points;
- for (size_t i=0;i<polygonPoints.size();i++)
+ if (!hasLayer(layerName))
{
- Eigen::Vector3f p = Vector3Ptr::dynamicCast(polygonPoints.at(i))->toEigen();;
- points.push_back(p);
+ return;
}
- d.points = points;
- d.colorInner = VirtualRobot::VisualizationFactory::Color(colorInner.r, colorInner.g, colorInner.b, 1 - colorInner.a);;
- d.colorBorder = VirtualRobot::VisualizationFactory::Color(colorBorder.r, colorBorder.g, colorBorder.b, 1 - colorBorder.a);;
+ auto& layer = layers.at(layerName);
+ layer.visible = visible;
- d.lineWidth = lineWidth;
- d.layerName = layerName;
- d.name = polygonName;
- d.active = true;
+ if (visible)
+ {
+ if (layerMainNode->findChild(layer.mainNode) < 0)
+ {
+ layerMainNode->addChild(layer.mainNode);
+ }
+ }
+ else
+ {
+ if (layerMainNode->findChild(layer.mainNode) >= 0)
+ {
+ layerMainNode->removeChild(layer.mainNode);
+ }
+ }
}
-}
-
-void DebugDrawerComponent::setPolygonDebugLayerVisu(const std::string &polygonName, const std::vector<Vector3BasePtr> &polygonPoints, const DrawColor &colorInner, const DrawColor &colorBorder, float lineWidth, const Ice::Current &)
-{
- setPolygonVisu(DEBUG_LAYER_NAME, polygonName, polygonPoints, colorInner, colorBorder, lineWidth);
-}
-void DebugDrawerComponent::removePolygonVisu(const std::string &layerName, const std::string &polygonName, const Ice::Current &)
-{
+ void DebugDrawerComponent::disableAllLayers(const ::Ice::Current&)
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
- std::string entryName = "__" + layerName + "__" + polygonName + "__";
- PolygonData &d = accumulatedUpdateData.polygons[entryName];
- d.layerName = layerName;
- d.name = polygonName;
- d.active = false;
- }
-}
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
-void DebugDrawerComponent::removePolygonDebugLayerVisu(const std::string &polygonName, const Ice::Current &)
-{
- removePolygonVisu(DEBUG_LAYER_NAME, polygonName);
-}
+ if (coinVisu->findChild(layerMainNode) >= 0)
+ {
+ coinVisu->removeChild(layerMainNode);
+ }
+ }
-void DebugDrawerComponent::setArrowVisu(const std::string &layerName, const std::string &arrowName, const Vector3BasePtr &position, const Vector3BasePtr &direction, const DrawColor &color, float length, float width, const Ice::Current &)
-{
+ void DebugDrawerComponent::enableAllLayers(const ::Ice::Current&)
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
- std::string entryName = "__" + layerName + "__" + arrowName + "__";
- ArrowData &d = accumulatedUpdateData.arrows[entryName];
-
- d.position = Vector3Ptr::dynamicCast(position)->toEigen();
- d.direction = Vector3Ptr::dynamicCast(direction)->toEigen();
- d.color = VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);;
- d.length = length;
- d.width = width;
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
- d.layerName = layerName;
- d.name = arrowName;
- d.active = true;
+ if (coinVisu->findChild(layerMainNode) < 0)
+ {
+ coinVisu->addChild(layerMainNode);
+ }
}
-}
-
-void DebugDrawerComponent::setArrowDebugLayerVisu(const std::string &arrowName, const Vector3BasePtr &position, const Vector3BasePtr &direction, const DrawColor &color, float length, float width, const Ice::Current &)
-{
- setArrowVisu(DEBUG_LAYER_NAME, arrowName, position, direction, color, length, width);
-}
-void DebugDrawerComponent::removeArrowVisu(const std::string &layerName, const std::string &arrowName, const Ice::Current &)
-{
+ void DebugDrawerComponent::setScaledPoseVisu(const std::string& layerName, const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Float scale, const ::Ice::Current&)
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
- std::string entryName = "__" + layerName + "__" + arrowName + "__";
- ArrowData &d = accumulatedUpdateData.arrows[entryName];
- d.layerName = layerName;
- d.name = arrowName;
- d.active = false;
+ ARMARX_DEBUG << VAROUT(layerName) << VAROUT(poseName);
+ Eigen::Matrix4f gp = PosePtr::dynamicCast(globalPose)->toEigen();
+ {
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + poseName + "__";
+ CoordData& d = accumulatedUpdateData.coord[entryName];
+ d.globalPose = gp;
+ d.layerName = layerName;
+ d.name = poseName;
+ d.scale = scale;
+ d.active = true;
+ }
}
-}
-
-void DebugDrawerComponent::removeArrowDebugLayerVisu(const std::string &arrowName, const Ice::Current &)
-{
- removeArrowVisu(DEBUG_LAYER_NAME, arrowName);
-}
-
-void DebugDrawerComponent::setRobotVisu(const std::string &layerName, const std::string &robotName, const std::string &robotFile, const std::string &armarxProject, DrawStyle drawStyle, const Ice::Current &)
-{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
- std::string entryName = "__" + layerName + "__" + robotName + "__";
- ARMARX_DEBUG << "setting robot visualization for " << entryName;
- RobotData &d = accumulatedUpdateData.robots[entryName];
-
- d.robotFile = robotFile;
- d.armarxProject = armarxProject;
- d.drawStyle = drawStyle;
-
- d.layerName = layerName;
- d.name = robotName;
- d.active = true;
-}
-void DebugDrawerComponent::updateRobotPose(const std::string &layerName, const std::string &robotName, const PoseBasePtr &globalPose, const Ice::Current &)
-{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
- std::string entryName = "__" + layerName + "__" + robotName + "__";
- ARMARX_DEBUG << "updating robot pose for " << entryName;
- RobotData &d = accumulatedUpdateData.robots[entryName];
-
- // update data
- d.update = true;
- d.updatePose = true;
- d.globalPose = PosePtr::dynamicCast(globalPose)->toEigen();
-
- d.layerName = layerName;
- d.name = robotName;
- d.active = true;
-}
-
-void DebugDrawerComponent::updateRobotConfig(const std::string &layerName, const std::string &robotName, const std::map<std::string, float> &configuration, const Ice::Current &)
-{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
- std::string entryName = "__" + layerName + "__" + robotName + "__";
- ARMARX_DEBUG << "updating robot config for " << entryName;
- RobotData &d = accumulatedUpdateData.robots[entryName];
-
- // update data
- d.update = true;
- d.updateConfig = true;
- d.layerName = layerName;
- d.name = robotName;
- for(auto& it : configuration)
+ void DebugDrawerComponent::setScaledPoseDebugLayerVisu(const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Float scale, const ::Ice::Current&)
{
- d.configuration[it.first] = it.second;
+ setScaledPoseVisu(DEBUG_LAYER_NAME, poseName, globalPose, scale);
}
- d.active = true;
-}
-
-void DebugDrawerComponent::updateRobotColor(const std::string &layerName, const std::string &robotName, const DrawColor &c, const Ice::Current &)
-{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
- std::string entryName = "__" + layerName + "__" + robotName + "__";
- ARMARX_DEBUG << "updating robot color for " << entryName;
- RobotData &d = accumulatedUpdateData.robots[entryName];
-
- // update data
- d.update = true;
- d.updateColor = true;
- d.layerName = layerName;
- d.name = robotName;
- if (c.a == 0 && c.b == 0 && c.r == 0 && c.g == 0)
- d.color = VirtualRobot::VisualizationFactory::Color::None();
- else
- d.color = VirtualRobot::VisualizationFactory::Color(c.r, c.g, c.b, 1 - c.a);
-
- d.active = true;
-}
-
-void DebugDrawerComponent::removeRobotVisu(const std::string &layerName, const std::string &robotName, const Ice::Current &)
-{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
- std::string entryName = "__" + layerName + "__" + robotName + "__";
- ARMARX_DEBUG << "removing robot visu for " << entryName;
- RobotData &d = accumulatedUpdateData.robots[entryName];
- d.layerName = layerName;
- d.name = robotName;
- d.active = false;
-}
-
-void DebugDrawerComponent::clearAll(const Ice::Current &)
-{
- for (auto &i:layers)
+ void DebugDrawerComponent::setPoseVisu(const std::string& layerName, const std::string& poseName, const PoseBasePtr& globalPose, const Ice::Current&)
{
- clearLayer(i.first);
+ setScaledPoseVisu(layerName, poseName, globalPose, 1.f);
}
-}
-void DebugDrawerComponent::clearLayer(const std::string &layerName, const Ice::Current &)
-{
- ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
- ScopedRecursiveLockPtr lockVisu = getScopedVisuLock();
-
- ARMARX_DEBUG << "clearing layer " << layerName;
- if(!hasLayer(layerName))
+ void DebugDrawerComponent::setPoseDebugLayerVisu(const std::string& poseName, const PoseBasePtr& globalPose, const Ice::Current&)
{
- if (verbose)
- ARMARX_VERBOSE << "Layer " << layerName<<" can't be cleared, because it does not exist.";
- return;
+ setScaledPoseVisu(DEBUG_LAYER_NAME, poseName, globalPose, 1.f);
}
- if (verbose)
- ARMARX_VERBOSE << "Clearing layer " << layerName;
-
- removeAccumulatedData(layerName);
-
- auto& layer=layers.at(layerName);
- for (const auto &i:layer.addedCoordVisualizations)
+ void DebugDrawerComponent::removePoseVisu(const std::string& layerName, const std::string& poseName, const Ice::Current&)
{
- removePoseVisu(layerName, i.first);
+ {
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + poseName + "__";
+ CoordData& d = accumulatedUpdateData.coord[entryName];
+ d.layerName = layerName;
+ d.name = poseName;
+ d.active = false;
+ }
}
- for (const auto &i:layer.addedLineVisualizations)
+
+ void DebugDrawerComponent::removePoseDebugLayerVisu(const std::string& poseName, const Ice::Current&)
{
- removeLineVisu(layerName, i.first);
+ removePoseVisu(DEBUG_LAYER_NAME, poseName);
}
- for (const auto &i:layer.addedBoxVisualizations)
+
+ void DebugDrawerComponent::setLineVisu(const std::string& layerName, const std::string& lineName, const Vector3BasePtr& globalPosition1, const Vector3BasePtr& globalPosition2, float lineWidth, const DrawColor& color, const Ice::Current&)
{
- removeBoxVisu(layerName, i.first);
+ Eigen::Vector3f p1 = Vector3Ptr::dynamicCast(globalPosition1)->toEigen();
+ Eigen::Vector3f p2 = Vector3Ptr::dynamicCast(globalPosition2)->toEigen();
+ VirtualRobot::VisualizationFactory::Color c(color.r, color.g, color.b, 1 - color.a);
+ {
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + lineName + "__";
+ LineData& d = accumulatedUpdateData.line[entryName];
+ d.p1 = p1;
+ d.p2 = p2;
+ d.layerName = layerName;
+ d.name = lineName;
+ d.color = c;
+ d.scale = lineWidth;
+ d.active = true;
+ }
}
- for (const auto &i:layer.addedTextVisualizations)
+
+ void DebugDrawerComponent::setLineDebugLayerVisu(const std::string& lineName, const Vector3BasePtr& globalPosition1, const Vector3BasePtr& globalPosition2, float lineWidth, const DrawColor& color, const Ice::Current&)
{
- removeTextVisu(layerName, i.first);
+ setLineVisu(DEBUG_LAYER_NAME, lineName, globalPosition1, globalPosition2, lineWidth, color);
}
- for (const auto &i:layer.addedSphereVisualizations)
+
+ void DebugDrawerComponent::removeLineVisu(const std::string& layerName, const std::string& lineName, const Ice::Current&)
{
- removeSphereVisu(layerName, i.first);
+ {
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + lineName + "__";
+ LineData& d = accumulatedUpdateData.line[entryName];
+ d.layerName = layerName;
+ d.name = lineName;
+ d.active = false;
+ }
}
- for (const auto &i:layer.addedCylinderVisualizations)
+
+ void DebugDrawerComponent::removeLineDebugLayerVisu(const std::string& lineName, const Ice::Current&)
{
- removeCylinderVisu(layerName, i.first);
+ removeLineVisu(DEBUG_LAYER_NAME, lineName);
}
- for (const auto &i:layer.addedPointCloudVisualizations)
+
+ void DebugDrawerComponent::setBoxVisu(const std::string& layerName, const std::string& boxName, const PoseBasePtr& globalPose, const Vector3BasePtr& dimensions, const DrawColor& color, const Ice::Current&)
{
- removePointCloudVisu(layerName, i.first);
+ Eigen::Matrix4f gp = PosePtr::dynamicCast(globalPose)->toEigen();
+ VirtualRobot::VisualizationFactory::Color c(color.r, color.g, color.b, 1 - color.a);
+ {
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + boxName + "__";
+ BoxData& d = accumulatedUpdateData.box[entryName];
+ d.width = dimensions->x;
+ d.height = dimensions->y;
+ d.depth = dimensions->z;
+ d.layerName = layerName;
+ d.name = boxName;
+ d.color = c;
+ d.globalPose = gp;
+ d.active = true;
+ }
}
- for (const auto &i:layer.addedPolygonVisualizations)
+
+ void DebugDrawerComponent::setBoxDebugLayerVisu(const std::string& boxName, const PoseBasePtr& globalPose, const Vector3BasePtr& dimensions, const DrawColor& color, const Ice::Current&)
{
- removePolygonVisu(layerName, i.first);
+ setBoxVisu(DEBUG_LAYER_NAME, boxName, globalPose, dimensions, color);
}
- for (const auto &i:layer.addedArrowVisualizations)
+
+ void DebugDrawerComponent::removeBoxVisu(const std::string& layerName, const std::string& boxName, const Ice::Current&)
{
- removeArrowVisu(layerName, i.first);
+ {
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + boxName + "__";
+ BoxData& d = accumulatedUpdateData.box[entryName];
+ d.layerName = layerName;
+ d.name = boxName;
+ d.active = false;
+ }
}
- for (const auto &i:layer.addedRobotVisualizations)
+
+ void DebugDrawerComponent::removeBoxDebugLayerVisu(const std::string& boxName, const Ice::Current&)
{
- removeRobotVisu(layerName, i.first);
+ removeBoxVisu(DEBUG_LAYER_NAME, boxName);
}
- for (const auto &i:layer.addedCustomVisualizations)
+
+ void DebugDrawerComponent::setTextVisu(const std::string& layerName, const std::string& textName, const std::string& text, const Vector3BasePtr& globalPosition, const DrawColor& color, int size, const Ice::Current&)
{
- removeCustomVisu(layerName, i.first);
+ ARMARX_DEBUG << VAROUT(layerName) << VAROUT(textName);
+ {
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + textName + "__";
+ TextData& d = accumulatedUpdateData.text[entryName];
+ d.text = text;
+ d.position = Vector3Ptr::dynamicCast(globalPosition)->toEigen();
+ d.layerName = layerName;
+ d.name = textName;
+ d.color = VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);
+ d.size = size;
+ d.active = true;
+ }
}
-}
-
-void DebugDrawerComponent::clearDebugLayer(const Ice::Current &)
-{
- clearLayer(DEBUG_LAYER_NAME);
-}
-
-void DebugDrawerComponent::setMutex(boost::shared_ptr<boost::recursive_mutex> m)
-{
- //ARMARX_IMPORTANT << "set mutex3d:" << m.get();
- mutex = m;
-}
-ScopedRecursiveLockPtr DebugDrawerComponent::getScopedVisuLock()
-{
- ScopedRecursiveLockPtr l;
- if (mutex)
+ void DebugDrawerComponent::setTextDebugLayerVisu(const std::string& textName, const std::string& text, const Vector3BasePtr& globalPosition, const DrawColor& color, int size, const Ice::Current&)
{
- //ARMARX_IMPORTANT << mutex.get();
- l.reset(new ScopedRecursiveLock(*mutex));
- } else
- ARMARX_IMPORTANT << deactivateSpam(5) << "NO 3D MUTEX!!!";
-
- return l;
-}
-
-ScopedRecursiveLockPtr DebugDrawerComponent::getScopedAccumulatedDataLock()
-{
- ScopedRecursiveLockPtr l(new ScopedRecursiveLock(*dataUpdateMutex));
- return l;
-}
-
-SoSeparator* DebugDrawerComponent::getVisualization()
-{
- return coinVisu;
-}
+ setTextVisu(DEBUG_LAYER_NAME, textName, text, globalPosition, color, size);
+ }
-DebugDrawerComponent::Layer& DebugDrawerComponent::requestLayer(const std::string& layerName)
-{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- if(hasLayer(layerName))
+ void DebugDrawerComponent::removeTextVisu(const std::string& layerName, const std::string& textName, const Ice::Current&)
{
- return layers.at(layerName);
+ {
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + textName + "__";
+ TextData& d = accumulatedUpdateData.text[entryName];
+ d.layerName = layerName;
+ d.name = textName;
+ d.active = false;
+ }
}
- ARMARX_VERBOSE << "Created layer "<<layerName;
-
- SoSeparator* mainNode=new SoSeparator{};
- mainNode->ref();
- layerMainNode->addChild(mainNode);
- layers[layerName]=Layer();
- layers.at(layerName).mainNode=mainNode;
- layers.at(layerName).visible=true;
- return layers.at(layerName);
-}
-
-void DebugDrawerComponent::updateVisualization()
-{
- ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
- ScopedRecursiveLockPtr lockVisu = getScopedVisuLock();
- for (auto i = accumulatedUpdateData.coord.begin(); i != accumulatedUpdateData.coord.end(); i++)
+ void DebugDrawerComponent::removeTextDebugLayerVisu(const std::string& textName, const Ice::Current&)
{
- drawCoordSystem(i->second);
+ removeTextVisu(DEBUG_LAYER_NAME, textName);
}
- accumulatedUpdateData.coord.clear();
- for (auto i = accumulatedUpdateData.box.begin(); i != accumulatedUpdateData.box.end(); i++)
+ void DebugDrawerComponent::setSphereVisu(const std::string& layerName, const std::string& sphereName, const Vector3BasePtr& globalPosition, const DrawColor& color, float radius, const Ice::Current&)
{
- drawBox(i->second);
+ ARMARX_DEBUG << VAROUT(layerName) << VAROUT(sphereName);
+ {
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + sphereName + "__";
+ SphereData& d = accumulatedUpdateData.sphere[entryName];
+ d.radius = radius;
+ d.position = Vector3Ptr::dynamicCast(globalPosition)->toEigen();
+ d.layerName = layerName;
+ d.name = sphereName;
+ d.color = VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);
+ d.active = true;
+ }
}
- accumulatedUpdateData.box.clear();
- for (auto i = accumulatedUpdateData.line.begin(); i != accumulatedUpdateData.line.end(); i++)
+ void DebugDrawerComponent::setSphereDebugLayerVisu(const std::string& sphereName, const Vector3BasePtr& globalPosition, const DrawColor& color, float radius, const Ice::Current&)
{
- drawLine(i->second);
+ setSphereVisu(DEBUG_LAYER_NAME, sphereName, globalPosition, color, radius);
}
- accumulatedUpdateData.line.clear();
- for (auto i = accumulatedUpdateData.sphere.begin(); i != accumulatedUpdateData.sphere.end(); i++)
+ void DebugDrawerComponent::removeSphereVisu(const std::string& layerName, const std::string& sphereName, const Ice::Current&)
{
- drawSphere(i->second);
+ {
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + sphereName + "__";
+ SphereData& d = accumulatedUpdateData.sphere[entryName];
+ d.layerName = layerName;
+ d.name = sphereName;
+ d.active = false;
+ }
}
- accumulatedUpdateData.sphere.clear();
- for (auto i = accumulatedUpdateData.cylinder.begin(); i != accumulatedUpdateData.cylinder.end(); i++)
+ void DebugDrawerComponent::removeSphereDebugLayerVisu(const std::string& sphereName, const Ice::Current&)
{
- drawCylinder(i->second);
+ removeSphereVisu(DEBUG_LAYER_NAME, sphereName);
}
- accumulatedUpdateData.cylinder.clear();
- for (auto i = accumulatedUpdateData.text.begin(); i != accumulatedUpdateData.text.end(); i++)
+ void DebugDrawerComponent::setCylinderVisu(const std::string& layerName, const std::string& cylinderName, const Vector3BasePtr& globalPosition, const Vector3BasePtr& direction, float length, float radius, const DrawColor& color, const Ice::Current&)
{
- drawText(i->second);
+ ARMARX_DEBUG << VAROUT(layerName) << VAROUT(cylinderName);
+ {
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + cylinderName + "__";
+ CylinderData& d = accumulatedUpdateData.cylinder[entryName];
+ d.position = Vector3Ptr::dynamicCast(globalPosition)->toEigen();
+ d.direction = Vector3Ptr::dynamicCast(direction)->toEigen();
+ d.length = length;
+ d.radius = radius;
+ d.layerName = layerName;
+ d.name = cylinderName;
+ d.color = VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);
+ d.active = true;
+ }
}
- accumulatedUpdateData.text.clear();
- for (auto i = accumulatedUpdateData.pointcloud.begin(); i != accumulatedUpdateData.pointcloud.end(); i++)
+ void DebugDrawerComponent::setCylinderDebugLayerVisu(const std::string& cylinderName, const Vector3BasePtr& globalPosition, const Vector3BasePtr& direction, float length, float radius, const DrawColor& color, const Ice::Current&)
{
- drawPointCloud(i->second);
+ setCylinderVisu(DEBUG_LAYER_NAME, cylinderName, globalPosition, direction, length, radius, color);
}
- accumulatedUpdateData.pointcloud.clear();
- for (auto i = accumulatedUpdateData.polygons.begin(); i != accumulatedUpdateData.polygons.end(); i++)
+ void DebugDrawerComponent::removeCylinderVisu(const std::string& layerName, const std::string& cylinderName, const Ice::Current&)
{
- drawPolygon(i->second);
+ {
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + cylinderName + "__";
+ CylinderData& d = accumulatedUpdateData.cylinder[entryName];
+ d.layerName = layerName;
+ d.name = cylinderName;
+ d.active = false;
+ }
}
- accumulatedUpdateData.polygons.clear();
- for (auto i = accumulatedUpdateData.arrows.begin(); i != accumulatedUpdateData.arrows.end(); i++)
+ void DebugDrawerComponent::removeCylinderDebugLayerVisu(const std::string& cylinderName, const Ice::Current&)
{
- drawArrow(i->second);
+ removeCylinderVisu(DEBUG_LAYER_NAME, cylinderName);
}
- accumulatedUpdateData.arrows.clear();
- for (auto i = accumulatedUpdateData.robots.begin(); i != accumulatedUpdateData.robots.end(); i++)
+ void DebugDrawerComponent::setPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const DebugDrawerPointCloud& pointCloud, const Ice::Current&)
{
- ARMARX_DEBUG << "update visu / drawRobot for robot " << i->first;
-
- drawRobot(i->second);
+ ARMARX_DEBUG << VAROUT(layerName) << VAROUT(pointCloudName);
+ {
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + pointCloudName + "__";
+ PointCloudData& d = accumulatedUpdateData.pointcloud[entryName];
+ d.pointCloud = pointCloud;
+ d.layerName = layerName;
+ d.name = pointCloudName;
+ d.active = true;
+ }
}
- accumulatedUpdateData.robots.clear();
- onUpdateVisualization();
-}
-
-bool DebugDrawerComponent::hasLayer(const std::string& layerName, const ::Ice::Current&)
-{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- return layers.find(layerName)!=layers.end();
-}
+ void DebugDrawerComponent::setPointCloudDebugLayerVisu(const std::string& pointCloudName, const DebugDrawerPointCloud& pointCloud, const Ice::Current&)
+ {
+ setPointCloudVisu(DEBUG_LAYER_NAME, pointCloudName, pointCloud);
+ }
-//todo: in some rare cases the mutex3D lock does not work and results in a broken coin timer setup. No updates of the scene will be drawn in this case
-// -> check for a qt-thread solution
-void DebugDrawerComponent::removeLayer(const std::string& layerName, const ::Ice::Current&)
-{
- if(!hasLayer(layerName))
+ void DebugDrawerComponent::removePointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const Ice::Current&)
{
- if (verbose)
- ARMARX_VERBOSE << "Layer " << layerName<<" can't be removed, because it does not exist.";
- return;
+ {
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + pointCloudName + "__";
+ PointCloudData& d = accumulatedUpdateData.pointcloud[entryName];
+ d.layerName = layerName;
+ d.name = pointCloudName;
+ d.active = false;
+ }
}
- if (verbose)
- ARMARX_VERBOSE << "Removing layer " << layerName;
- clearLayer(layerName);
+ void DebugDrawerComponent::removePointCloudDebugLayerVisu(const std::string& pointCloudName, const Ice::Current&)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- auto& layer=layers.at(layerName);
- layerMainNode->removeChild(layer.mainNode);
- layer.mainNode->unref();
- layers.erase(layerName);
+ removePointCloudVisu(DEBUG_LAYER_NAME, pointCloudName);
}
-}
-void DebugDrawerComponent::removeAccumulatedData(const std::string& layerName)
-{
- ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
- std::string entryName = "__" + layerName + "__";
+ void DebugDrawerComponent::setPolygonVisu(const std::string& layerName, const std::string& polygonName, const std::vector<Vector3BasePtr>& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, float lineWidth, const Ice::Current&)
{
- auto i1 = accumulatedUpdateData.coord.begin();
- while (i1 != accumulatedUpdateData.coord.end())
{
- if (boost::starts_with(i1->first, entryName))
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + polygonName + "__";
+ PolygonData& d = accumulatedUpdateData.polygons[entryName];
+
+ std::vector< Eigen::Vector3f > points;
+
+ for (size_t i = 0; i < polygonPoints.size(); i++)
{
- i1 = accumulatedUpdateData.coord.erase(i1);
- } else
- i1++;
+ Eigen::Vector3f p = Vector3Ptr::dynamicCast(polygonPoints.at(i))->toEigen();;
+ points.push_back(p);
+ }
+
+ d.points = points;
+ d.colorInner = VirtualRobot::VisualizationFactory::Color(colorInner.r, colorInner.g, colorInner.b, 1 - colorInner.a);;
+ d.colorBorder = VirtualRobot::VisualizationFactory::Color(colorBorder.r, colorBorder.g, colorBorder.b, 1 - colorBorder.a);;
+
+ d.lineWidth = lineWidth;
+ d.layerName = layerName;
+ d.name = polygonName;
+ d.active = true;
}
}
+ void DebugDrawerComponent::setPolygonDebugLayerVisu(const std::string& polygonName, const std::vector<Vector3BasePtr>& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, float lineWidth, const Ice::Current&)
+ {
+ setPolygonVisu(DEBUG_LAYER_NAME, polygonName, polygonPoints, colorInner, colorBorder, lineWidth);
+ }
+
+ void DebugDrawerComponent::removePolygonVisu(const std::string& layerName, const std::string& polygonName, const Ice::Current&)
{
- auto i1 = accumulatedUpdateData.box.begin();
- while (i1 != accumulatedUpdateData.box.end())
{
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.box.erase(i1);
- } else
- i1++;
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + polygonName + "__";
+ PolygonData& d = accumulatedUpdateData.polygons[entryName];
+ d.layerName = layerName;
+ d.name = polygonName;
+ d.active = false;
}
}
+ void DebugDrawerComponent::removePolygonDebugLayerVisu(const std::string& polygonName, const Ice::Current&)
+ {
+ removePolygonVisu(DEBUG_LAYER_NAME, polygonName);
+ }
+
+ void DebugDrawerComponent::setArrowVisu(const std::string& layerName, const std::string& arrowName, const Vector3BasePtr& position, const Vector3BasePtr& direction, const DrawColor& color, float length, float width, const Ice::Current&)
{
- auto i1 = accumulatedUpdateData.line.begin();
- while (i1 != accumulatedUpdateData.line.end())
{
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.line.erase(i1);
- } else
- i1++;
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + arrowName + "__";
+ ArrowData& d = accumulatedUpdateData.arrows[entryName];
+
+ d.position = Vector3Ptr::dynamicCast(position)->toEigen();
+ d.direction = Vector3Ptr::dynamicCast(direction)->toEigen();
+ d.color = VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);;
+ d.length = length;
+ d.width = width;
+
+ d.layerName = layerName;
+ d.name = arrowName;
+ d.active = true;
}
}
+ void DebugDrawerComponent::setArrowDebugLayerVisu(const std::string& arrowName, const Vector3BasePtr& position, const Vector3BasePtr& direction, const DrawColor& color, float length, float width, const Ice::Current&)
+ {
+ setArrowVisu(DEBUG_LAYER_NAME, arrowName, position, direction, color, length, width);
+ }
+
+ void DebugDrawerComponent::removeArrowVisu(const std::string& layerName, const std::string& arrowName, const Ice::Current&)
{
- auto i1 = accumulatedUpdateData.sphere.begin();
- while (i1 != accumulatedUpdateData.sphere.end())
{
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.sphere.erase(i1);
- } else
- i1++;
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + arrowName + "__";
+ ArrowData& d = accumulatedUpdateData.arrows[entryName];
+ d.layerName = layerName;
+ d.name = arrowName;
+ d.active = false;
}
}
+ void DebugDrawerComponent::removeArrowDebugLayerVisu(const std::string& arrowName, const Ice::Current&)
+ {
+ removeArrowVisu(DEBUG_LAYER_NAME, arrowName);
+ }
+
+ void DebugDrawerComponent::setRobotVisu(const std::string& layerName, const std::string& robotName, const std::string& robotFile, const std::string& armarxProject, DrawStyle drawStyle, const Ice::Current&)
+ {
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + robotName + "__";
+ ARMARX_DEBUG << "setting robot visualization for " << entryName;
+ RobotData& d = accumulatedUpdateData.robots[entryName];
+
+ d.robotFile = robotFile;
+ d.armarxProject = armarxProject;
+ d.drawStyle = drawStyle;
+
+ d.layerName = layerName;
+ d.name = robotName;
+ d.active = true;
+ }
+
+ void DebugDrawerComponent::updateRobotPose(const std::string& layerName, const std::string& robotName, const PoseBasePtr& globalPose, const Ice::Current&)
{
- auto i1 = accumulatedUpdateData.cylinder.begin();
- while (i1 != accumulatedUpdateData.cylinder.end())
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + robotName + "__";
+ ARMARX_DEBUG << "updating robot pose for " << entryName;
+ RobotData& d = accumulatedUpdateData.robots[entryName];
+
+ // update data
+ d.update = true;
+ d.updatePose = true;
+ d.globalPose = PosePtr::dynamicCast(globalPose)->toEigen();
+
+ d.layerName = layerName;
+ d.name = robotName;
+ d.active = true;
+ }
+
+ void DebugDrawerComponent::updateRobotConfig(const std::string& layerName, const std::string& robotName, const std::map<std::string, float>& configuration, const Ice::Current&)
+ {
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + robotName + "__";
+ ARMARX_DEBUG << "updating robot config for " << entryName;
+ RobotData& d = accumulatedUpdateData.robots[entryName];
+
+ // update data
+ d.update = true;
+ d.updateConfig = true;
+ d.layerName = layerName;
+ d.name = robotName;
+
+ for (auto& it : configuration)
{
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.cylinder.erase(i1);
- } else
- i1++;
+ d.configuration[it.first] = it.second;
}
+
+ d.active = true;
}
+ void DebugDrawerComponent::updateRobotColor(const std::string& layerName, const std::string& robotName, const DrawColor& c, const Ice::Current&)
{
- auto i1 = accumulatedUpdateData.text.begin();
- while (i1 != accumulatedUpdateData.text.end())
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + robotName + "__";
+ ARMARX_DEBUG << "updating robot color for " << entryName;
+ RobotData& d = accumulatedUpdateData.robots[entryName];
+
+ // update data
+ d.update = true;
+ d.updateColor = true;
+ d.layerName = layerName;
+ d.name = robotName;
+
+ if (c.a == 0 && c.b == 0 && c.r == 0 && c.g == 0)
{
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.text.erase(i1);
- } else
- i1++;
+ d.color = VirtualRobot::VisualizationFactory::Color::None();
}
+ else
+ {
+ d.color = VirtualRobot::VisualizationFactory::Color(c.r, c.g, c.b, 1 - c.a);
+ }
+
+ d.active = true;
+ }
+
+ void DebugDrawerComponent::removeRobotVisu(const std::string& layerName, const std::string& robotName, const Ice::Current&)
+ {
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + robotName + "__";
+ ARMARX_DEBUG << "removing robot visu for " << entryName;
+ RobotData& d = accumulatedUpdateData.robots[entryName];
+
+ d.layerName = layerName;
+ d.name = robotName;
+ d.active = false;
}
+ void DebugDrawerComponent::clearAll(const Ice::Current&)
{
- auto i1 = accumulatedUpdateData.pointcloud.begin();
- while (i1 != accumulatedUpdateData.pointcloud.end())
+ for (auto& i : layers)
{
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.pointcloud.erase(i1);
- } else
- i1++;
+ clearLayer(i.first);
}
}
+ void DebugDrawerComponent::clearLayer(const std::string& layerName, const Ice::Current&)
{
- auto i1 = accumulatedUpdateData.polygons.begin();
- while (i1 != accumulatedUpdateData.polygons.end())
+ ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
+ ScopedRecursiveLockPtr lockVisu = getScopedVisuLock();
+
+ ARMARX_DEBUG << "clearing layer " << layerName;
+
+ if (!hasLayer(layerName))
{
- if (boost::starts_with(i1->first, entryName))
+ if (verbose)
{
- i1 = accumulatedUpdateData.polygons.erase(i1);
- } else
- i1++;
+ ARMARX_VERBOSE << "Layer " << layerName << " can't be cleared, because it does not exist.";
+ }
+
+ return;
+ }
+
+ if (verbose)
+ {
+ ARMARX_VERBOSE << "Clearing layer " << layerName;
+ }
+
+ removeAccumulatedData(layerName);
+
+
+ auto& layer = layers.at(layerName);
+
+ for (const auto& i : layer.addedCoordVisualizations)
+ {
+ removePoseVisu(layerName, i.first);
+ }
+
+ for (const auto& i : layer.addedLineVisualizations)
+ {
+ removeLineVisu(layerName, i.first);
+ }
+
+ for (const auto& i : layer.addedBoxVisualizations)
+ {
+ removeBoxVisu(layerName, i.first);
+ }
+
+ for (const auto& i : layer.addedTextVisualizations)
+ {
+ removeTextVisu(layerName, i.first);
+ }
+
+ for (const auto& i : layer.addedSphereVisualizations)
+ {
+ removeSphereVisu(layerName, i.first);
+ }
+
+ for (const auto& i : layer.addedCylinderVisualizations)
+ {
+ removeCylinderVisu(layerName, i.first);
+ }
+
+ for (const auto& i : layer.addedPointCloudVisualizations)
+ {
+ removePointCloudVisu(layerName, i.first);
}
+
+ for (const auto& i : layer.addedPolygonVisualizations)
+ {
+ removePolygonVisu(layerName, i.first);
+ }
+
+ for (const auto& i : layer.addedArrowVisualizations)
+ {
+ removeArrowVisu(layerName, i.first);
+ }
+
+ for (const auto& i : layer.addedRobotVisualizations)
+ {
+ removeRobotVisu(layerName, i.first);
+ }
+
+ for (const auto& i : layer.addedCustomVisualizations)
+ {
+ removeCustomVisu(layerName, i.first);
+ }
+ }
+
+ void DebugDrawerComponent::clearDebugLayer(const Ice::Current&)
+ {
+ clearLayer(DEBUG_LAYER_NAME);
}
+ void DebugDrawerComponent::setMutex(boost::shared_ptr<boost::recursive_mutex> m)
{
- auto i1 = accumulatedUpdateData.arrows.begin();
- while (i1 != accumulatedUpdateData.arrows.end())
+ //ARMARX_IMPORTANT << "set mutex3d:" << m.get();
+ mutex = m;
+ }
+
+ ScopedRecursiveLockPtr DebugDrawerComponent::getScopedVisuLock()
+ {
+ ScopedRecursiveLockPtr l;
+
+ if (mutex)
{
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.arrows.erase(i1);
- } else
- i1++;
+ //ARMARX_IMPORTANT << mutex.get();
+ l.reset(new ScopedRecursiveLock(*mutex));
+ }
+ else
+ {
+ ARMARX_IMPORTANT << deactivateSpam(5) << "NO 3D MUTEX!!!";
+ }
+
+ return l;
+ }
+
+ ScopedRecursiveLockPtr DebugDrawerComponent::getScopedAccumulatedDataLock()
+ {
+ ScopedRecursiveLockPtr l(new ScopedRecursiveLock(*dataUpdateMutex));
+ return l;
+ }
+
+ SoSeparator* DebugDrawerComponent::getVisualization()
+ {
+ return coinVisu;
+ }
+
+ DebugDrawerComponent::Layer& DebugDrawerComponent::requestLayer(const std::string& layerName)
+ {
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
+
+ if (hasLayer(layerName))
+ {
+ return layers.at(layerName);
+ }
+
+ ARMARX_VERBOSE << "Created layer " << layerName;
+
+ SoSeparator* mainNode = new SoSeparator {};
+ mainNode->ref();
+ layerMainNode->addChild(mainNode);
+ layers[layerName] = Layer();
+ layers.at(layerName).mainNode = mainNode;
+ layers.at(layerName).visible = true;
+ return layers.at(layerName);
+ }
+
+ void DebugDrawerComponent::updateVisualization()
+ {
+ ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
+ ScopedRecursiveLockPtr lockVisu = getScopedVisuLock();
+
+ for (auto i = accumulatedUpdateData.coord.begin(); i != accumulatedUpdateData.coord.end(); i++)
+ {
+ drawCoordSystem(i->second);
+ }
+
+ accumulatedUpdateData.coord.clear();
+
+ for (auto i = accumulatedUpdateData.box.begin(); i != accumulatedUpdateData.box.end(); i++)
+ {
+ drawBox(i->second);
+ }
+
+ accumulatedUpdateData.box.clear();
+
+ for (auto i = accumulatedUpdateData.line.begin(); i != accumulatedUpdateData.line.end(); i++)
+ {
+ drawLine(i->second);
+ }
+
+ accumulatedUpdateData.line.clear();
+
+ for (auto i = accumulatedUpdateData.sphere.begin(); i != accumulatedUpdateData.sphere.end(); i++)
+ {
+ drawSphere(i->second);
+ }
+
+ accumulatedUpdateData.sphere.clear();
+
+ for (auto i = accumulatedUpdateData.cylinder.begin(); i != accumulatedUpdateData.cylinder.end(); i++)
+ {
+ drawCylinder(i->second);
+ }
+
+ accumulatedUpdateData.cylinder.clear();
+
+ for (auto i = accumulatedUpdateData.text.begin(); i != accumulatedUpdateData.text.end(); i++)
+ {
+ drawText(i->second);
}
+
+ accumulatedUpdateData.text.clear();
+
+ for (auto i = accumulatedUpdateData.pointcloud.begin(); i != accumulatedUpdateData.pointcloud.end(); i++)
+ {
+ drawPointCloud(i->second);
+ }
+
+ accumulatedUpdateData.pointcloud.clear();
+
+ for (auto i = accumulatedUpdateData.polygons.begin(); i != accumulatedUpdateData.polygons.end(); i++)
+ {
+ drawPolygon(i->second);
+ }
+
+ accumulatedUpdateData.polygons.clear();
+
+ for (auto i = accumulatedUpdateData.arrows.begin(); i != accumulatedUpdateData.arrows.end(); i++)
+ {
+ drawArrow(i->second);
+ }
+
+ accumulatedUpdateData.arrows.clear();
+
+ for (auto i = accumulatedUpdateData.robots.begin(); i != accumulatedUpdateData.robots.end(); i++)
+ {
+ ARMARX_DEBUG << "update visu / drawRobot for robot " << i->first;
+
+ drawRobot(i->second);
+ }
+
+ accumulatedUpdateData.robots.clear();
+
+ onUpdateVisualization();
}
+ bool DebugDrawerComponent::hasLayer(const std::string& layerName, const ::Ice::Current&)
{
- ARMARX_DEBUG << "Removing all accumulated data for robot, nr of robots:" << accumulatedUpdateData.robots.size();
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
+ return layers.find(layerName) != layers.end();
+ }
- auto i1 = accumulatedUpdateData.robots.begin();
- while (i1 != accumulatedUpdateData.robots.end())
+ //todo: in some rare cases the mutex3D lock does not work and results in a broken coin timer setup. No updates of the scene will be drawn in this case
+ // -> check for a qt-thread solution
+ void DebugDrawerComponent::removeLayer(const std::string& layerName, const ::Ice::Current&)
+ {
+ if (!hasLayer(layerName))
{
- ARMARX_DEBUG << "checking accumulated data " << i1->first;
- if (boost::starts_with(i1->first, entryName))
+ if (verbose)
{
- ARMARX_DEBUG << "removing accumulated data for " << i1->first;
- i1 = accumulatedUpdateData.robots.erase(i1);
- } else
- i1++;
+ ARMARX_VERBOSE << "Layer " << layerName << " can't be removed, because it does not exist.";
+ }
+
+ return;
+ }
+
+ if (verbose)
+ {
+ ARMARX_VERBOSE << "Removing layer " << layerName;
+ }
+
+ clearLayer(layerName);
+ {
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto& layer = layers.at(layerName);
+ layerMainNode->removeChild(layer.mainNode);
+ layer.mainNode->unref();
+ layers.erase(layerName);
}
- ARMARX_DEBUG << "end, nr of robots:" << accumulatedUpdateData.robots.size();
}
- onRemoveAccumulatedData(layerName);
+ void DebugDrawerComponent::removeAccumulatedData(const std::string& layerName)
+ {
+ ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__";
+ {
+ auto i1 = accumulatedUpdateData.coord.begin();
-}
+ while (i1 != accumulatedUpdateData.coord.end())
+ {
+ if (boost::starts_with(i1->first, entryName))
+ {
+ i1 = accumulatedUpdateData.coord.erase(i1);
+ }
+ else
+ {
+ i1++;
+ }
+ }
+ }
-void DebugDrawerComponent::enableLayerVisu(const std::string& layerName, bool visible, const ::Ice::Current&)
-{
- setLayerVisibility(layerName,visible);
-}
+ {
+ auto i1 = accumulatedUpdateData.box.begin();
-void DebugDrawerComponent::enableDebugLayerVisu(bool visible, const ::Ice::Current&)
-{
- enableLayerVisu(DEBUG_LAYER_NAME,visible);
-}
+ while (i1 != accumulatedUpdateData.box.end())
+ {
+ if (boost::starts_with(i1->first, entryName))
+ {
+ i1 = accumulatedUpdateData.box.erase(i1);
+ }
+ else
+ {
+ i1++;
+ }
+ }
+ }
-StringSequence DebugDrawerComponent::layerNames(const ::Ice::Current&)
-{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- StringSequence seq{};
- for(const auto& layer:layers)
+ {
+ auto i1 = accumulatedUpdateData.line.begin();
+
+ while (i1 != accumulatedUpdateData.line.end())
+ {
+ if (boost::starts_with(i1->first, entryName))
+ {
+ i1 = accumulatedUpdateData.line.erase(i1);
+ }
+ else
+ {
+ i1++;
+ }
+ }
+ }
+
+ {
+ auto i1 = accumulatedUpdateData.sphere.begin();
+
+ while (i1 != accumulatedUpdateData.sphere.end())
+ {
+ if (boost::starts_with(i1->first, entryName))
+ {
+ i1 = accumulatedUpdateData.sphere.erase(i1);
+ }
+ else
+ {
+ i1++;
+ }
+ }
+ }
+
+ {
+ auto i1 = accumulatedUpdateData.cylinder.begin();
+
+ while (i1 != accumulatedUpdateData.cylinder.end())
+ {
+ if (boost::starts_with(i1->first, entryName))
+ {
+ i1 = accumulatedUpdateData.cylinder.erase(i1);
+ }
+ else
+ {
+ i1++;
+ }
+ }
+ }
+
+ {
+ auto i1 = accumulatedUpdateData.text.begin();
+
+ while (i1 != accumulatedUpdateData.text.end())
+ {
+ if (boost::starts_with(i1->first, entryName))
+ {
+ i1 = accumulatedUpdateData.text.erase(i1);
+ }
+ else
+ {
+ i1++;
+ }
+ }
+ }
+
+ {
+ auto i1 = accumulatedUpdateData.pointcloud.begin();
+
+ while (i1 != accumulatedUpdateData.pointcloud.end())
+ {
+ if (boost::starts_with(i1->first, entryName))
+ {
+ i1 = accumulatedUpdateData.pointcloud.erase(i1);
+ }
+ else
+ {
+ i1++;
+ }
+ }
+ }
+
+ {
+ auto i1 = accumulatedUpdateData.polygons.begin();
+
+ while (i1 != accumulatedUpdateData.polygons.end())
+ {
+ if (boost::starts_with(i1->first, entryName))
+ {
+ i1 = accumulatedUpdateData.polygons.erase(i1);
+ }
+ else
+ {
+ i1++;
+ }
+ }
+ }
+
+ {
+ auto i1 = accumulatedUpdateData.arrows.begin();
+
+ while (i1 != accumulatedUpdateData.arrows.end())
+ {
+ if (boost::starts_with(i1->first, entryName))
+ {
+ i1 = accumulatedUpdateData.arrows.erase(i1);
+ }
+ else
+ {
+ i1++;
+ }
+ }
+ }
+
+ {
+ ARMARX_DEBUG << "Removing all accumulated data for robot, nr of robots:" << accumulatedUpdateData.robots.size();
+
+ auto i1 = accumulatedUpdateData.robots.begin();
+
+ while (i1 != accumulatedUpdateData.robots.end())
+ {
+ ARMARX_DEBUG << "checking accumulated data " << i1->first;
+
+ if (boost::starts_with(i1->first, entryName))
+ {
+ ARMARX_DEBUG << "removing accumulated data for " << i1->first;
+ i1 = accumulatedUpdateData.robots.erase(i1);
+ }
+ else
+ {
+ i1++;
+ }
+ }
+
+ ARMARX_DEBUG << "end, nr of robots:" << accumulatedUpdateData.robots.size();
+ }
+
+ onRemoveAccumulatedData(layerName);
+
+ }
+
+ void DebugDrawerComponent::enableLayerVisu(const std::string& layerName, bool visible, const ::Ice::Current&)
{
- seq.push_back(layer.first);
+ setLayerVisibility(layerName, visible);
}
- return seq;
-}
-::armarx::LayerInformationSequence DebugDrawerComponent::layerInformation(const ::Ice::Current&)
-{
- ::armarx::LayerInformationSequence seq{};
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- for(const auto& layer:layers)
- {
- int count = layer.second.addedCoordVisualizations.size()+
- layer.second.addedLineVisualizations.size()+
- layer.second.addedBoxVisualizations.size()+
- layer.second.addedTextVisualizations.size()+
- layer.second.addedSphereVisualizations.size()+
- layer.second.addedCylinderVisualizations.size()+
- layer.second.addedPointCloudVisualizations.size()+
- layer.second.addedArrowVisualizations.size()+
- layer.second.addedRobotVisualizations.size()+
- layer.second.addedCustomVisualizations.size();
- ::armarx::LayerInformation info={layer.first,layer.second.visible,count};
- seq.push_back(info);
- }
- return seq;
-}
+ void DebugDrawerComponent::enableDebugLayerVisu(bool visible, const ::Ice::Current&)
+ {
+ enableLayerVisu(DEBUG_LAYER_NAME, visible);
+ }
+
+ StringSequence DebugDrawerComponent::layerNames(const ::Ice::Current&)
+ {
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
+ StringSequence seq {};
+
+ for (const auto& layer : layers)
+ {
+ seq.push_back(layer.first);
+ }
+
+ return seq;
+ }
+
+ ::armarx::LayerInformationSequence DebugDrawerComponent::layerInformation(const ::Ice::Current&)
+ {
+ ::armarx::LayerInformationSequence seq {};
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
+
+ for (const auto& layer : layers)
+ {
+ int count = layer.second.addedCoordVisualizations.size() +
+ layer.second.addedLineVisualizations.size() +
+ layer.second.addedBoxVisualizations.size() +
+ layer.second.addedTextVisualizations.size() +
+ layer.second.addedSphereVisualizations.size() +
+ layer.second.addedCylinderVisualizations.size() +
+ layer.second.addedPointCloudVisualizations.size() +
+ layer.second.addedArrowVisualizations.size() +
+ layer.second.addedRobotVisualizations.size() +
+ layer.second.addedCustomVisualizations.size();
+ ::armarx::LayerInformation info = {layer.first, layer.second.visible, count};
+ seq.push_back(info);
+ }
+
+ return seq;
+ }
}//namespace armarx
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
index 6b31222ef1e726cbe7de04b32f0e708a3d057bc0..05a49b0c2830ce6348e8fba35338f1513cc63071 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
@@ -44,404 +44,417 @@
//std
#include <memory>
-namespace armarx{
-
-/*!
- * \class DebugDrawerPropertyDefinitions
- * \brief
- */
-class DebugDrawerPropertyDefinitions:
- public ComponentPropertyDefinitions
+namespace armarx
{
-public:
- DebugDrawerPropertyDefinitions(std::string prefix):
- ComponentPropertyDefinitions(prefix)
- {
- defineOptionalProperty<bool>("ShowDebugDrawing", true, "The simulator implements the DebugDrawerInterface. The debug visualizations (e.g. coordinate systems) can enabled/disbaled with this flag.");
- defineOptionalProperty<std::string>("DebugDrawerTopic", "DebugDrawerUpdates", "Name of the DebugDrawerTopic");
- }
-};
-
-/*!
- * \defgroup Component-DebugDrawerComponent DebugDrawerComponent
- * \ingroup RobotAPI-Components
- * \brief Visualizes debug information.
- *
- * The DebugDrawerComponent implements the DebugDrawerInterface and provides a convenient way for visualizing debug information.
- * It creates a scene graph representation of the debug content and offers it to visualization components.
- * Several GUI plugins visualize this debug scene graph, among others:
- * - RobotViewer (RobotAPI)
- * - WorkingMemoryGui (MemoryX)
- * - SimulatorViewer (ArmarXSimulation)
- *
- * The following example shows an exemplary debug drawing:
-\code
- DebugDrawerInterfacePrx prxDD = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
- if (prxDD)
- {
- // draw in global coordinate system
- Eigen::Matrix4f p;
- p.setIdentity();
- p(2,3) = 1000.0f;
- PosePtr gp(new Pose(p));
- prxDD->setPoseDebugLayerVisu("testPose",gp);
-
- armarx::Vector3Ptr p1(new armarx::Vector3(0, 0, 0));
- armarx::Vector3Ptr p2(new armarx::Vector3(1000, 1000, 1000));
-
- armarx::DrawColor c = {1, 0, 0, 1}; // RGBA
- prxDD->setLineDebugLayerVisu("testLine", p1, p2, 2.0f, c);
- }
-\endcode
- */
-
-/**
- * @brief The DebugDrawerComponent class
- * @ingroup Component-DebugDrawerComponent
- */
-class DebugDrawerComponent :
- virtual public armarx::DebugDrawerInterface,
- virtual public Component
-{
-public:
-
- /*!
- * \brief DebugDrawerComponent Constructs a debug drawer
- */
- DebugDrawerComponent();
-
- /*!
- * \brief setVisuUpdateTime Specifies how often the accumulated draw commands should be applied to the visualization (default 30). Has to be called before init.
- * \param visuUpdatesPerSec All topic requests are collected asynchronously. This parameter specifies how often the rendering should be updated according to the accumulated updates.
- */
- void setVisuUpdateTime(float visuUpdatesPerSec);
-
- // inherited from Component
- virtual std::string getDefaultName() const { return "DebugDrawer"; }
- virtual void onInitComponent();
- virtual void onConnectComponent();
- virtual void onDisconnectComponent();
- virtual void onExitComponent();
-
-
-
- /*!
- * \see PropertyUser::createPropertyDefinitions()
- */
- virtual PropertyDefinitionsPtr createPropertyDefinitions()
- {
- return PropertyDefinitionsPtr(new DebugDrawerPropertyDefinitions(
- getConfigIdentifier()));
- }
-
- /* Inherited from DebugDrawerInterface. */
- virtual void exportScene(const std::string& filename, const ::Ice::Current& = ::Ice::Current());
- virtual void exportLayer(const std::string& filename, const std::string &layerName, const ::Ice::Current& = ::Ice::Current());
-
- virtual void setPoseVisu(const std::string& layerName, const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Current& = ::Ice::Current());
- virtual void setScaledPoseVisu(const std::string& layerName, const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Float scale, const ::Ice::Current& = ::Ice::Current());
- virtual void setPoseDebugLayerVisu(const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Current& = ::Ice::Current());
- virtual void setScaledPoseDebugLayerVisu(const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Float scale, const ::Ice::Current& = ::Ice::Current());
- virtual void removePoseVisu(const std::string& layerName, const std::string& poseName, const ::Ice::Current& = ::Ice::Current());
- virtual void removePoseDebugLayerVisu(const std::string& poseName, const ::Ice::Current& = ::Ice::Current());
-
- virtual void setLineVisu(const std::string& layerName, const std::string& lineName, const ::armarx::Vector3BasePtr& globalPosition1, const ::armarx::Vector3BasePtr& globalPosition2, float lineWidth, const ::armarx::DrawColor &color, const ::Ice::Current& = ::Ice::Current());
- virtual void setLineDebugLayerVisu(const std::string& lineName, const ::armarx::Vector3BasePtr& globalPosition1, const ::armarx::Vector3BasePtr& globalPosition2, float lineWidth, const ::armarx::DrawColor &color, const ::Ice::Current& = ::Ice::Current());
- virtual void removeLineVisu(const std::string& layerName, const std::string& lineName, const ::Ice::Current& = ::Ice::Current());
- virtual void removeLineDebugLayerVisu(const std::string& lineName, const ::Ice::Current& = ::Ice::Current());
-
- virtual void setBoxVisu(const std::string &layerName, const std::string &boxName, const ::armarx::PoseBasePtr &globalPose, const ::armarx::Vector3BasePtr &dimensions, const DrawColor &color, const ::Ice::Current& = ::Ice::Current());
- virtual void setBoxDebugLayerVisu(const std::string &boxName, const ::armarx::PoseBasePtr &globalPose, const ::armarx::Vector3BasePtr &dimensions, const DrawColor &color, const ::Ice::Current& = ::Ice::Current());
- virtual void removeBoxVisu(const std::string &layerName, const std::string &boxName, const ::Ice::Current& = ::Ice::Current());
- virtual void removeBoxDebugLayerVisu(const std::string& boxName, const ::Ice::Current& = ::Ice::Current());
-
- virtual void setTextVisu(const std::string &layerName, const std::string &textName, const std::string &text, const ::armarx::Vector3BasePtr &globalPosition, const DrawColor &color, int size, const ::Ice::Current& = ::Ice::Current());
- virtual void setTextDebugLayerVisu(const std::string &textName, const std::string &text, const ::armarx::Vector3BasePtr &globalPosition, const DrawColor &color, int size, const ::Ice::Current& = ::Ice::Current());
- virtual void removeTextVisu(const std::string &layerName, const std::string &textName, const ::Ice::Current& = ::Ice::Current());
- virtual void removeTextDebugLayerVisu(const std::string& textName, const ::Ice::Current& = ::Ice::Current());
-
- virtual void setSphereVisu(const std::string &layerName, const std::string &sphereName, const ::armarx::Vector3BasePtr &globalPosition, const DrawColor &color, float radius, const ::Ice::Current& = ::Ice::Current());
- virtual void setSphereDebugLayerVisu(const std::string &sphereName, const ::armarx::Vector3BasePtr &globalPosition, const DrawColor &color, float radius, const ::Ice::Current& = ::Ice::Current());
- virtual void removeSphereVisu(const std::string &layerName, const std::string &sphereName, const ::Ice::Current& = ::Ice::Current());
- virtual void removeSphereDebugLayerVisu(const std::string& sphereName, const ::Ice::Current& = ::Ice::Current());
-
- virtual void setCylinderVisu(const std::string &layerName, const std::string &cylinderName, const ::armarx::Vector3BasePtr &globalPosition, const ::armarx::Vector3BasePtr &direction, float length, float radius, const DrawColor &color, const ::Ice::Current& = ::Ice::Current());
- virtual void setCylinderDebugLayerVisu(const std::string &cylinderName, const ::armarx::Vector3BasePtr &globalPosition, const ::armarx::Vector3BasePtr &direction, float length, float radius, const DrawColor &color, const ::Ice::Current& = ::Ice::Current());
- virtual void removeCylinderVisu(const std::string &layerName, const std::string &cylinderName, const ::Ice::Current& = ::Ice::Current());
- virtual void removeCylinderDebugLayerVisu(const std::string& cylinderName, const ::Ice::Current& = ::Ice::Current());
-
- virtual void setPointCloudVisu(const std::string &layerName, const std::string &pointCloudName, const DebugDrawerPointCloud &pointCloud, const ::Ice::Current& = ::Ice::Current());
- virtual void setPointCloudDebugLayerVisu(const std::string &pointCloudName, const DebugDrawerPointCloud &pointCloud, const ::Ice::Current& = ::Ice::Current());
- virtual void removePointCloudVisu(const std::string &layerName, const std::string &pointCloudName, const ::Ice::Current& = ::Ice::Current());
- virtual void removePointCloudDebugLayerVisu(const std::string& pointCloudName, const ::Ice::Current& = ::Ice::Current());
-
- virtual void setPolygonVisu(const std::string &layerName, const std::string &polygonName, const std::vector< ::armarx::Vector3BasePtr > &polygonPoints, const DrawColor &colorInner, const DrawColor &colorBorder, float lineWidth, const ::Ice::Current& = ::Ice::Current());
- virtual void setPolygonDebugLayerVisu(const std::string &polygonName, const std::vector< ::armarx::Vector3BasePtr > &polygonPoints, const DrawColor &colorInner, const DrawColor &colorBorder, float lineWidth, const ::Ice::Current& = ::Ice::Current());
- virtual void removePolygonVisu(const std::string &layerName, const std::string &polygonName, const ::Ice::Current& = ::Ice::Current());
- virtual void removePolygonDebugLayerVisu(const std::string& polygonName, const ::Ice::Current& = ::Ice::Current());
-
- virtual void setArrowVisu(const std::string &layerName, const std::string &arrowName, const ::armarx::Vector3BasePtr &position, const ::armarx::Vector3BasePtr &direction, const DrawColor &color, float length, float width, const ::Ice::Current& = ::Ice::Current());
- virtual void setArrowDebugLayerVisu(const std::string &arrowName, const ::armarx::Vector3BasePtr &position, const ::armarx::Vector3BasePtr &direction, const DrawColor &color, float length, float width, const ::Ice::Current& = ::Ice::Current());
- virtual void removeArrowVisu(const std::string &layerName, const std::string &arrowName, const ::Ice::Current& = ::Ice::Current());
- virtual void removeArrowDebugLayerVisu(const std::string& arrowName, const ::Ice::Current& = ::Ice::Current());
/*!
- * \brief setRobotVisu Initializes a robot visualization
- * \param layerName The layer
- * \param robotName The identifier of the robot
- * \param robotFile The filename of the robot. The robot must be locally present in a project.
- * \param DrawStyle Either FullModel ior CollisionModel.
- * \param armarxProject Additional armarx project that should be used to search the robot. Must be locally available and accessible through the armarx cmake search procedure.
+ * \class DebugDrawerPropertyDefinitions
+ * \brief
*/
- virtual void setRobotVisu(const std::string &layerName, const std::string &robotName, const std::string &robotFile, const std::string &armarxProject, DrawStyle drawStyle, const ::Ice::Current& = ::Ice::Current());
- virtual void updateRobotPose(const std::string &layerName, const std::string &robotName, const ::armarx::PoseBasePtr &globalPose, const ::Ice::Current& = ::Ice::Current());
- virtual void updateRobotConfig(const std::string &layerName, const std::string &robotName, const std::map< std::string, float> &configuration, const ::Ice::Current& = ::Ice::Current());
- /*!
- * \brief updateRobotColor Colorizes the robot visualization
- * \param layerName The layer
- * \param robotName The robot identifyer
- * \param c The draw color, if all is set to 0, the colorization is disabled (i.e. the original vizaulization shows up)
- */
- virtual void updateRobotColor(const std::string &layerName, const std::string &robotName, const armarx::DrawColor &c, const ::Ice::Current& = ::Ice::Current());
- virtual void removeRobotVisu(const std::string &layerName, const std::string &robotName, const ::Ice::Current& = ::Ice::Current());
-
- virtual void clearAll(const ::Ice::Current& = ::Ice::Current());
- virtual void clearLayer(const std::string& layerName, const ::Ice::Current& = ::Ice::Current());
- virtual void clearDebugLayer(const ::Ice::Current& = ::Ice::Current());
-
- virtual bool hasLayer(const std::string& layerName, const ::Ice::Current& = ::Ice::Current());
- virtual void removeLayer(const std::string& layerName, const ::Ice::Current& = ::Ice::Current());
-
- virtual void enableLayerVisu(const std::string& layerName, bool visible, const ::Ice::Current& = ::Ice::Current());
- virtual void enableDebugLayerVisu(bool visible, const ::Ice::Current& = ::Ice::Current());
-
- virtual ::armarx::StringSequence layerNames(const ::Ice::Current& = ::Ice::Current());
- virtual ::armarx::LayerInformationSequence layerInformation(const ::Ice::Current& = ::Ice::Current());
-
- virtual void disableAllLayers(const ::Ice::Current& = ::Ice::Current());
- virtual void enableAllLayers(const ::Ice::Current& = ::Ice::Current());
-
- /*!
- * \brief getScopedLock If using the coin visualization it must be ensured that all rendering calls are protected with this mutex
- * \return The lock that is automatically destructed when leaving the current scope.
- */
- ScopedRecursiveLockPtr getScopedVisuLock();
-
- /*!
- * \brief getVisualization Ensure that all access to this node is protected with the scoped lock mutex.
- * \return The visualization
- */
- SoSeparator* getVisualization();
-
- void setMutex(boost::shared_ptr<boost::recursive_mutex> m);
-
- ~DebugDrawerComponent();
-protected:
-
- static void updateVisualizationCB(void *data, SoSensor *sensor);
-
- // It turned out, that heavy data traffic (resulting in heavy mutex locking) somehow interferes with coin rendering: the
- // SoQt timers are misaligned at some point, could be a bug in SoQt, that results in a situation where the GL window is never updated
- // while the rest of the qt widget works well.
- // The solution for now: collect all data updates and update the coin scene by qt-timed thread (qt events ensure that no drawing event is active)
-
- struct DrawData
- {
- DrawData(){active=false;update=false;}
- std::string layerName;
- std::string name;
- bool active;
- bool update;
- };
-
- struct CoordData : public DrawData
- {
- Eigen::Matrix4f globalPose;
- float scale;
- };
- struct LineData : public DrawData
- {
- Eigen::Vector3f p1;
- Eigen::Vector3f p2;
- float scale;
- VirtualRobot::VisualizationFactory::Color color;
- };
- struct BoxData : public DrawData
- {
- Eigen::Matrix4f globalPose;
- float width;
- float height;
- float depth;
- VirtualRobot::VisualizationFactory::Color color;
- };
- struct TextData : public DrawData
- {
- std::string text;
- Eigen::Vector3f position;
- VirtualRobot::VisualizationFactory::Color color;
- int size;
- };
- struct SphereData : public DrawData
- {
- Eigen::Vector3f position;
- VirtualRobot::VisualizationFactory::Color color;
- float radius;
- };
- struct CylinderData : public DrawData
- {
- Eigen::Vector3f position;
- Eigen::Vector3f direction;
- float length;
- float radius;
- VirtualRobot::VisualizationFactory::Color color;
- };
- struct PointCloudData : public DrawData
- {
- DebugDrawerPointCloud pointCloud;
- };
- struct PolygonData : public DrawData
- {
- std::vector< Eigen::Vector3f > points;
- float lineWidth;
- VirtualRobot::VisualizationFactory::Color colorInner;
- VirtualRobot::VisualizationFactory::Color colorBorder;
- };
- struct ArrowData : public DrawData
- {
- Eigen::Vector3f position;
- Eigen::Vector3f direction;
- float length;
- float width;
- VirtualRobot::VisualizationFactory::Color color;
- };
- struct RobotData : public DrawData
- {
- RobotData(){updateColor = false; updatePose = false; updateConfig = false;}
- std::string robotFile;
- std::string armarxProject;
-
- bool updateColor;
- VirtualRobot::VisualizationFactory::Color color;
-
- bool updatePose;
- Eigen::Matrix4f globalPose;
-
- bool updateConfig;
- std::map < std::string, float > configuration;
-
- armarx::DrawStyle drawStyle;
- };
-
- struct UpdateData
+ class DebugDrawerPropertyDefinitions:
+ public ComponentPropertyDefinitions
{
- std::map<std::string, CoordData> coord;
- std::map<std::string, LineData> line;
- std::map<std::string, BoxData> box;
- std::map<std::string, TextData> text;
- std::map<std::string, SphereData> sphere;
- std::map<std::string, CylinderData> cylinder;
- std::map<std::string, PointCloudData> pointcloud;
- std::map<std::string, PolygonData> polygons;
- std::map<std::string, ArrowData> arrows;
- std::map<std::string, RobotData> robots;
+ public:
+ DebugDrawerPropertyDefinitions(std::string prefix):
+ ComponentPropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<bool>("ShowDebugDrawing", true, "The simulator implements the DebugDrawerInterface. The debug visualizations (e.g. coordinate systems) can enabled/disbaled with this flag.");
+ defineOptionalProperty<std::string>("DebugDrawerTopic", "DebugDrawerUpdates", "Name of the DebugDrawerTopic");
+ }
};
- UpdateData accumulatedUpdateData;
-
- // Reads accumulatedUpdateData, writes all data to coin visualization and clears accumulatedUpdateData
- void updateVisualization();
-
/*!
- * \brief onUpdateVisualization derived methods can overwrite this method to update custom visualizations.
+ * \defgroup Component-DebugDrawerComponent DebugDrawerComponent
+ * \ingroup RobotAPI-Components
+ * \brief Visualizes debug information.
+ *
+ * The DebugDrawerComponent implements the DebugDrawerInterface and provides a convenient way for visualizing debug information.
+ * It creates a scene graph representation of the debug content and offers it to visualization components.
+ * Several GUI plugins visualize this debug scene graph, among others:
+ * - RobotViewer (RobotAPI)
+ * - WorkingMemoryGui (MemoryX)
+ * - SimulatorViewer (ArmarXSimulation)
+ *
+ * The following example shows an exemplary debug drawing:
+ \code
+ DebugDrawerInterfacePrx prxDD = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
+ if (prxDD)
+ {
+ // draw in global coordinate system
+ Eigen::Matrix4f p;
+ p.setIdentity();
+ p(2,3) = 1000.0f;
+ PosePtr gp(new Pose(p));
+ prxDD->setPoseDebugLayerVisu("testPose",gp);
+
+ armarx::Vector3Ptr p1(new armarx::Vector3(0, 0, 0));
+ armarx::Vector3Ptr p2(new armarx::Vector3(1000, 1000, 1000));
+
+ armarx::DrawColor c = {1, 0, 0, 1}; // RGBA
+ prxDD->setLineDebugLayerVisu("testLine", p1, p2, 2.0f, c);
+ }
+ \endcode
*/
- virtual void onUpdateVisualization(){}
- virtual void onRemoveAccumulatedData(const std::string& layerName){}
-
- virtual void removeCustomVisu(const std::string& layerName, const std::string &name) {}
-
- void drawCoordSystem(const CoordData &d);
- void drawLine(const LineData &d);
- void drawBox(const BoxData &d);
- void drawText(const TextData &d);
- void drawSphere(const SphereData &d);
- void drawCylinder(const CylinderData &d);
- void drawPointCloud(const PointCloudData &d);
- void drawPolygon(const PolygonData &d);
- void drawArrow(const ArrowData &d);
- void drawRobot(const RobotData &d);
-
- void removeCoordSystem(const std::string& layerName, const std::string &name);
- void removeLine(const std::string& layerName, const std::string &name);
- void removeBox(const std::string& layerName, const std::string &name);
- void removeText(const std::string& layerName, const std::string &name);
- void removeSphere(const std::string& layerName, const std::string &name);
- void removeCylinder(const std::string& layerName, const std::string &name);
- void removePointCloud(const std::string& layerName, const std::string &name);
- void removePolygon(const std::string& layerName, const std::string &name);
- void removeArrow(const std::string& layerName, const std::string &name);
- void removeRobot(const std::string& layerName, const std::string &name);
-
- void setLayerVisibility(const std::string& layerName, bool visible);
- /*!
- * \brief Contains data for a layer.
+ /**
+ * @brief The DebugDrawerComponent class
+ * @ingroup Component-DebugDrawerComponent
*/
- struct Layer
+ class DebugDrawerComponent :
+ virtual public armarx::DebugDrawerInterface,
+ virtual public Component
{
- SoSeparator* mainNode;
- std::map<std::string, SoSeparator*> addedCoordVisualizations;
- std::map<std::string, SoSeparator*> addedLineVisualizations;
- std::map<std::string, SoSeparator*> addedBoxVisualizations;
- std::map<std::string, SoSeparator*> addedTextVisualizations;
- std::map<std::string, SoSeparator*> addedSphereVisualizations;
- std::map<std::string, SoSeparator*> addedCylinderVisualizations;
- std::map<std::string, SoSeparator*> addedPointCloudVisualizations;
- std::map<std::string, SoSeparator*> addedPolygonVisualizations;
- std::map<std::string, SoSeparator*> addedArrowVisualizations;
- std::map<std::string, SoSeparator*> addedRobotVisualizations;
- std::map<std::string, SoSeparator*> addedCustomVisualizations; // these separators are only used by derived classes and have to start with a material node
-
- bool visible;
+ public:
+
+ /*!
+ * \brief DebugDrawerComponent Constructs a debug drawer
+ */
+ DebugDrawerComponent();
+
+ /*!
+ * \brief setVisuUpdateTime Specifies how often the accumulated draw commands should be applied to the visualization (default 30). Has to be called before init.
+ * \param visuUpdatesPerSec All topic requests are collected asynchronously. This parameter specifies how often the rendering should be updated according to the accumulated updates.
+ */
+ void setVisuUpdateTime(float visuUpdatesPerSec);
+
+ // inherited from Component
+ virtual std::string getDefaultName() const
+ {
+ return "DebugDrawer";
+ }
+ virtual void onInitComponent();
+ virtual void onConnectComponent();
+ virtual void onDisconnectComponent();
+ virtual void onExitComponent();
+
+
+
+ /*!
+ * \see PropertyUser::createPropertyDefinitions()
+ */
+ virtual PropertyDefinitionsPtr createPropertyDefinitions()
+ {
+ return PropertyDefinitionsPtr(new DebugDrawerPropertyDefinitions(
+ getConfigIdentifier()));
+ }
+
+ /* Inherited from DebugDrawerInterface. */
+ virtual void exportScene(const std::string& filename, const ::Ice::Current& = ::Ice::Current());
+ virtual void exportLayer(const std::string& filename, const std::string& layerName, const ::Ice::Current& = ::Ice::Current());
+
+ virtual void setPoseVisu(const std::string& layerName, const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Current& = ::Ice::Current());
+ virtual void setScaledPoseVisu(const std::string& layerName, const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Float scale, const ::Ice::Current& = ::Ice::Current());
+ virtual void setPoseDebugLayerVisu(const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Current& = ::Ice::Current());
+ virtual void setScaledPoseDebugLayerVisu(const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Float scale, const ::Ice::Current& = ::Ice::Current());
+ virtual void removePoseVisu(const std::string& layerName, const std::string& poseName, const ::Ice::Current& = ::Ice::Current());
+ virtual void removePoseDebugLayerVisu(const std::string& poseName, const ::Ice::Current& = ::Ice::Current());
+
+ virtual void setLineVisu(const std::string& layerName, const std::string& lineName, const ::armarx::Vector3BasePtr& globalPosition1, const ::armarx::Vector3BasePtr& globalPosition2, float lineWidth, const ::armarx::DrawColor& color, const ::Ice::Current& = ::Ice::Current());
+ virtual void setLineDebugLayerVisu(const std::string& lineName, const ::armarx::Vector3BasePtr& globalPosition1, const ::armarx::Vector3BasePtr& globalPosition2, float lineWidth, const ::armarx::DrawColor& color, const ::Ice::Current& = ::Ice::Current());
+ virtual void removeLineVisu(const std::string& layerName, const std::string& lineName, const ::Ice::Current& = ::Ice::Current());
+ virtual void removeLineDebugLayerVisu(const std::string& lineName, const ::Ice::Current& = ::Ice::Current());
+
+ virtual void setBoxVisu(const std::string& layerName, const std::string& boxName, const ::armarx::PoseBasePtr& globalPose, const ::armarx::Vector3BasePtr& dimensions, const DrawColor& color, const ::Ice::Current& = ::Ice::Current());
+ virtual void setBoxDebugLayerVisu(const std::string& boxName, const ::armarx::PoseBasePtr& globalPose, const ::armarx::Vector3BasePtr& dimensions, const DrawColor& color, const ::Ice::Current& = ::Ice::Current());
+ virtual void removeBoxVisu(const std::string& layerName, const std::string& boxName, const ::Ice::Current& = ::Ice::Current());
+ virtual void removeBoxDebugLayerVisu(const std::string& boxName, const ::Ice::Current& = ::Ice::Current());
+
+ virtual void setTextVisu(const std::string& layerName, const std::string& textName, const std::string& text, const ::armarx::Vector3BasePtr& globalPosition, const DrawColor& color, int size, const ::Ice::Current& = ::Ice::Current());
+ virtual void setTextDebugLayerVisu(const std::string& textName, const std::string& text, const ::armarx::Vector3BasePtr& globalPosition, const DrawColor& color, int size, const ::Ice::Current& = ::Ice::Current());
+ virtual void removeTextVisu(const std::string& layerName, const std::string& textName, const ::Ice::Current& = ::Ice::Current());
+ virtual void removeTextDebugLayerVisu(const std::string& textName, const ::Ice::Current& = ::Ice::Current());
+
+ virtual void setSphereVisu(const std::string& layerName, const std::string& sphereName, const ::armarx::Vector3BasePtr& globalPosition, const DrawColor& color, float radius, const ::Ice::Current& = ::Ice::Current());
+ virtual void setSphereDebugLayerVisu(const std::string& sphereName, const ::armarx::Vector3BasePtr& globalPosition, const DrawColor& color, float radius, const ::Ice::Current& = ::Ice::Current());
+ virtual void removeSphereVisu(const std::string& layerName, const std::string& sphereName, const ::Ice::Current& = ::Ice::Current());
+ virtual void removeSphereDebugLayerVisu(const std::string& sphereName, const ::Ice::Current& = ::Ice::Current());
+
+ virtual void setCylinderVisu(const std::string& layerName, const std::string& cylinderName, const ::armarx::Vector3BasePtr& globalPosition, const ::armarx::Vector3BasePtr& direction, float length, float radius, const DrawColor& color, const ::Ice::Current& = ::Ice::Current());
+ virtual void setCylinderDebugLayerVisu(const std::string& cylinderName, const ::armarx::Vector3BasePtr& globalPosition, const ::armarx::Vector3BasePtr& direction, float length, float radius, const DrawColor& color, const ::Ice::Current& = ::Ice::Current());
+ virtual void removeCylinderVisu(const std::string& layerName, const std::string& cylinderName, const ::Ice::Current& = ::Ice::Current());
+ virtual void removeCylinderDebugLayerVisu(const std::string& cylinderName, const ::Ice::Current& = ::Ice::Current());
+
+ virtual void setPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const DebugDrawerPointCloud& pointCloud, const ::Ice::Current& = ::Ice::Current());
+ virtual void setPointCloudDebugLayerVisu(const std::string& pointCloudName, const DebugDrawerPointCloud& pointCloud, const ::Ice::Current& = ::Ice::Current());
+ virtual void removePointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const ::Ice::Current& = ::Ice::Current());
+ virtual void removePointCloudDebugLayerVisu(const std::string& pointCloudName, const ::Ice::Current& = ::Ice::Current());
+
+ virtual void setPolygonVisu(const std::string& layerName, const std::string& polygonName, const std::vector< ::armarx::Vector3BasePtr >& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, float lineWidth, const ::Ice::Current& = ::Ice::Current());
+ virtual void setPolygonDebugLayerVisu(const std::string& polygonName, const std::vector< ::armarx::Vector3BasePtr >& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, float lineWidth, const ::Ice::Current& = ::Ice::Current());
+ virtual void removePolygonVisu(const std::string& layerName, const std::string& polygonName, const ::Ice::Current& = ::Ice::Current());
+ virtual void removePolygonDebugLayerVisu(const std::string& polygonName, const ::Ice::Current& = ::Ice::Current());
+
+ virtual void setArrowVisu(const std::string& layerName, const std::string& arrowName, const ::armarx::Vector3BasePtr& position, const ::armarx::Vector3BasePtr& direction, const DrawColor& color, float length, float width, const ::Ice::Current& = ::Ice::Current());
+ virtual void setArrowDebugLayerVisu(const std::string& arrowName, const ::armarx::Vector3BasePtr& position, const ::armarx::Vector3BasePtr& direction, const DrawColor& color, float length, float width, const ::Ice::Current& = ::Ice::Current());
+ virtual void removeArrowVisu(const std::string& layerName, const std::string& arrowName, const ::Ice::Current& = ::Ice::Current());
+ virtual void removeArrowDebugLayerVisu(const std::string& arrowName, const ::Ice::Current& = ::Ice::Current());
+
+ /*!
+ * \brief setRobotVisu Initializes a robot visualization
+ * \param layerName The layer
+ * \param robotName The identifier of the robot
+ * \param robotFile The filename of the robot. The robot must be locally present in a project.
+ * \param DrawStyle Either FullModel ior CollisionModel.
+ * \param armarxProject Additional armarx project that should be used to search the robot. Must be locally available and accessible through the armarx cmake search procedure.
+ */
+ virtual void setRobotVisu(const std::string& layerName, const std::string& robotName, const std::string& robotFile, const std::string& armarxProject, DrawStyle drawStyle, const ::Ice::Current& = ::Ice::Current());
+ virtual void updateRobotPose(const std::string& layerName, const std::string& robotName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Current& = ::Ice::Current());
+ virtual void updateRobotConfig(const std::string& layerName, const std::string& robotName, const std::map< std::string, float>& configuration, const ::Ice::Current& = ::Ice::Current());
+ /*!
+ * \brief updateRobotColor Colorizes the robot visualization
+ * \param layerName The layer
+ * \param robotName The robot identifyer
+ * \param c The draw color, if all is set to 0, the colorization is disabled (i.e. the original vizaulization shows up)
+ */
+ virtual void updateRobotColor(const std::string& layerName, const std::string& robotName, const armarx::DrawColor& c, const ::Ice::Current& = ::Ice::Current());
+ virtual void removeRobotVisu(const std::string& layerName, const std::string& robotName, const ::Ice::Current& = ::Ice::Current());
+
+ virtual void clearAll(const ::Ice::Current& = ::Ice::Current());
+ virtual void clearLayer(const std::string& layerName, const ::Ice::Current& = ::Ice::Current());
+ virtual void clearDebugLayer(const ::Ice::Current& = ::Ice::Current());
+
+ virtual bool hasLayer(const std::string& layerName, const ::Ice::Current& = ::Ice::Current());
+ virtual void removeLayer(const std::string& layerName, const ::Ice::Current& = ::Ice::Current());
+
+ virtual void enableLayerVisu(const std::string& layerName, bool visible, const ::Ice::Current& = ::Ice::Current());
+ virtual void enableDebugLayerVisu(bool visible, const ::Ice::Current& = ::Ice::Current());
+
+ virtual ::armarx::StringSequence layerNames(const ::Ice::Current& = ::Ice::Current());
+ virtual ::armarx::LayerInformationSequence layerInformation(const ::Ice::Current& = ::Ice::Current());
+
+ virtual void disableAllLayers(const ::Ice::Current& = ::Ice::Current());
+ virtual void enableAllLayers(const ::Ice::Current& = ::Ice::Current());
+
+ /*!
+ * \brief getScopedLock If using the coin visualization it must be ensured that all rendering calls are protected with this mutex
+ * \return The lock that is automatically destructed when leaving the current scope.
+ */
+ ScopedRecursiveLockPtr getScopedVisuLock();
+
+ /*!
+ * \brief getVisualization Ensure that all access to this node is protected with the scoped lock mutex.
+ * \return The visualization
+ */
+ SoSeparator* getVisualization();
+
+ void setMutex(boost::shared_ptr<boost::recursive_mutex> m);
+
+ ~DebugDrawerComponent();
+ protected:
+
+ static void updateVisualizationCB(void* data, SoSensor* sensor);
+
+ // It turned out, that heavy data traffic (resulting in heavy mutex locking) somehow interferes with coin rendering: the
+ // SoQt timers are misaligned at some point, could be a bug in SoQt, that results in a situation where the GL window is never updated
+ // while the rest of the qt widget works well.
+ // The solution for now: collect all data updates and update the coin scene by qt-timed thread (qt events ensure that no drawing event is active)
+
+ struct DrawData
+ {
+ DrawData()
+ {
+ active = false;
+ update = false;
+ }
+ std::string layerName;
+ std::string name;
+ bool active;
+ bool update;
+ };
+
+ struct CoordData : public DrawData
+ {
+ Eigen::Matrix4f globalPose;
+ float scale;
+ };
+ struct LineData : public DrawData
+ {
+ Eigen::Vector3f p1;
+ Eigen::Vector3f p2;
+ float scale;
+ VirtualRobot::VisualizationFactory::Color color;
+ };
+ struct BoxData : public DrawData
+ {
+ Eigen::Matrix4f globalPose;
+ float width;
+ float height;
+ float depth;
+ VirtualRobot::VisualizationFactory::Color color;
+ };
+ struct TextData : public DrawData
+ {
+ std::string text;
+ Eigen::Vector3f position;
+ VirtualRobot::VisualizationFactory::Color color;
+ int size;
+ };
+ struct SphereData : public DrawData
+ {
+ Eigen::Vector3f position;
+ VirtualRobot::VisualizationFactory::Color color;
+ float radius;
+ };
+ struct CylinderData : public DrawData
+ {
+ Eigen::Vector3f position;
+ Eigen::Vector3f direction;
+ float length;
+ float radius;
+ VirtualRobot::VisualizationFactory::Color color;
+ };
+ struct PointCloudData : public DrawData
+ {
+ DebugDrawerPointCloud pointCloud;
+ };
+ struct PolygonData : public DrawData
+ {
+ std::vector< Eigen::Vector3f > points;
+ float lineWidth;
+ VirtualRobot::VisualizationFactory::Color colorInner;
+ VirtualRobot::VisualizationFactory::Color colorBorder;
+ };
+ struct ArrowData : public DrawData
+ {
+ Eigen::Vector3f position;
+ Eigen::Vector3f direction;
+ float length;
+ float width;
+ VirtualRobot::VisualizationFactory::Color color;
+ };
+ struct RobotData : public DrawData
+ {
+ RobotData()
+ {
+ updateColor = false;
+ updatePose = false;
+ updateConfig = false;
+ }
+ std::string robotFile;
+ std::string armarxProject;
+
+ bool updateColor;
+ VirtualRobot::VisualizationFactory::Color color;
+
+ bool updatePose;
+ Eigen::Matrix4f globalPose;
+
+ bool updateConfig;
+ std::map < std::string, float > configuration;
+
+ armarx::DrawStyle drawStyle;
+ };
+
+ struct UpdateData
+ {
+ std::map<std::string, CoordData> coord;
+ std::map<std::string, LineData> line;
+ std::map<std::string, BoxData> box;
+ std::map<std::string, TextData> text;
+ std::map<std::string, SphereData> sphere;
+ std::map<std::string, CylinderData> cylinder;
+ std::map<std::string, PointCloudData> pointcloud;
+ std::map<std::string, PolygonData> polygons;
+ std::map<std::string, ArrowData> arrows;
+ std::map<std::string, RobotData> robots;
+ };
+
+ UpdateData accumulatedUpdateData;
+
+ // Reads accumulatedUpdateData, writes all data to coin visualization and clears accumulatedUpdateData
+ void updateVisualization();
+
+ /*!
+ * \brief onUpdateVisualization derived methods can overwrite this method to update custom visualizations.
+ */
+ virtual void onUpdateVisualization() {}
+ virtual void onRemoveAccumulatedData(const std::string& layerName) {}
+
+ virtual void removeCustomVisu(const std::string& layerName, const std::string& name) {}
+
+ void drawCoordSystem(const CoordData& d);
+ void drawLine(const LineData& d);
+ void drawBox(const BoxData& d);
+ void drawText(const TextData& d);
+ void drawSphere(const SphereData& d);
+ void drawCylinder(const CylinderData& d);
+ void drawPointCloud(const PointCloudData& d);
+ void drawPolygon(const PolygonData& d);
+ void drawArrow(const ArrowData& d);
+ void drawRobot(const RobotData& d);
+
+ void removeCoordSystem(const std::string& layerName, const std::string& name);
+ void removeLine(const std::string& layerName, const std::string& name);
+ void removeBox(const std::string& layerName, const std::string& name);
+ void removeText(const std::string& layerName, const std::string& name);
+ void removeSphere(const std::string& layerName, const std::string& name);
+ void removeCylinder(const std::string& layerName, const std::string& name);
+ void removePointCloud(const std::string& layerName, const std::string& name);
+ void removePolygon(const std::string& layerName, const std::string& name);
+ void removeArrow(const std::string& layerName, const std::string& name);
+ void removeRobot(const std::string& layerName, const std::string& name);
+
+ void setLayerVisibility(const std::string& layerName, bool visible);
+
+ /*!
+ * \brief Contains data for a layer.
+ */
+ struct Layer
+ {
+ SoSeparator* mainNode;
+ std::map<std::string, SoSeparator*> addedCoordVisualizations;
+ std::map<std::string, SoSeparator*> addedLineVisualizations;
+ std::map<std::string, SoSeparator*> addedBoxVisualizations;
+ std::map<std::string, SoSeparator*> addedTextVisualizations;
+ std::map<std::string, SoSeparator*> addedSphereVisualizations;
+ std::map<std::string, SoSeparator*> addedCylinderVisualizations;
+ std::map<std::string, SoSeparator*> addedPointCloudVisualizations;
+ std::map<std::string, SoSeparator*> addedPolygonVisualizations;
+ std::map<std::string, SoSeparator*> addedArrowVisualizations;
+ std::map<std::string, SoSeparator*> addedRobotVisualizations;
+ std::map<std::string, SoSeparator*> addedCustomVisualizations; // these separators are only used by derived classes and have to start with a material node
+
+ bool visible;
+ };
+
+ /*!
+ * \brief If a layer with layerName exists it is returned. Otherwise a new layer with given name is created and returned.
+ * \param layerName The layer.
+ * \return The requested layer.
+ */
+ Layer& requestLayer(const std::string& layerName);
+ VirtualRobot::RobotPtr requestRobot(const RobotData& d);
+
+ std::map < std::string, VirtualRobot::RobotPtr > activeRobots;
+ SoSeparator* coinVisu;
+
+ bool verbose;
+
+ /*!
+ * \brief Main node for all layers
+ */
+ SoSeparator* layerMainNode;
+
+ /*!
+ * \brief All existing layers.
+ */
+ std::map<const std::string, Layer> layers;
+
+ //! The coin mutex
+ boost::shared_ptr<boost::recursive_mutex> mutex;
+
+ //! The data update mutex
+ boost::shared_ptr<boost::recursive_mutex> dataUpdateMutex;
+
+ ScopedRecursiveLockPtr getScopedAccumulatedDataLock();
+
+ float cycleTimeMS;
+ //PeriodicTask<DebugDrawerComponent>::pointer_type execTaskVisuUpdates;
+ SoTimerSensor* timerSensor;
+ void removeAccumulatedData(const std::string& layerName);
};
- /*!
- * \brief If a layer with layerName exists it is returned. Otherwise a new layer with given name is created and returned.
- * \param layerName The layer.
- * \return The requested layer.
- */
- Layer &requestLayer(const std::string& layerName);
- VirtualRobot::RobotPtr requestRobot(const RobotData &d);
-
- std::map < std::string, VirtualRobot::RobotPtr > activeRobots;
- SoSeparator* coinVisu;
-
- bool verbose;
-
- /*!
- * \brief Main node for all layers
- */
- SoSeparator* layerMainNode;
-
- /*!
- * \brief All existing layers.
- */
- std::map<const std::string, Layer> layers;
-
- //! The coin mutex
- boost::shared_ptr<boost::recursive_mutex> mutex;
-
- //! The data update mutex
- boost::shared_ptr<boost::recursive_mutex> dataUpdateMutex;
-
- ScopedRecursiveLockPtr getScopedAccumulatedDataLock();
-
- float cycleTimeMS;
- //PeriodicTask<DebugDrawerComponent>::pointer_type execTaskVisuUpdates;
- SoTimerSensor *timerSensor;
- void removeAccumulatedData(const std::string &layerName);
-};
-
-typedef IceInternal::Handle<DebugDrawerComponent> DebugDrawerComponentPtr;
+ typedef IceInternal::Handle<DebugDrawerComponent> DebugDrawerComponentPtr;
}
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index f5220eb3ac826a67c60b6643b65e5ee09348ab7d..10319136430a482f414e9773c574edbabe62b07d 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -51,7 +51,7 @@ namespace armarx
_synchronizedPrx->unref();
}
}
- catch(...)
+ catch (...)
{}
}
@@ -59,13 +59,15 @@ namespace armarx
void RobotStateComponent::onInitComponent()
{
relativeRobotFile = getProperty<std::string>("RobotFileName").getValue();
- if (!ArmarXDataPath::getAbsolutePath(relativeRobotFile,robotFile))
+
+ if (!ArmarXDataPath::getAbsolutePath(relativeRobotFile, robotFile))
{
throw UserException("Could not find robot file " + robotFile);
}
this->_synchronized = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
_synchronized->setName(getProperty<std::string>("AgentName").getValue());
+
if (this->_synchronized)
{
ARMARX_VERBOSE << "Loaded robot from file " << robotFile << ". Robot name: " << this->_synchronized->getName();
@@ -78,15 +80,16 @@ namespace armarx
string robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
- if(robotNodeSetName == "")
+ if (robotNodeSetName == "")
{
throw UserException("RobotNodeSet not defined");
}
VirtualRobot::RobotNodeSetPtr rns = this->_synchronized->getRobotNodeSet(robotNodeSetName);
+
if (!rns)
{
- throw UserException("RobotNodeSet not defined");
+ throw UserException("RobotNodeSet not defined");
}
vector<RobotNodePtr> nodes = rns->getAllRobotNodes();
@@ -128,20 +131,24 @@ namespace armarx
}
- SharedRobotInterfacePrx RobotStateComponent::getSynchronizedRobot(const Current & current) const
+ SharedRobotInterfacePrx RobotStateComponent::getSynchronizedRobot(const Current& current) const
{
if (!this->_synchronizedPrx)
{
throw NotInitializedException("Shared Robot is NULL", "getSynchronizedRobot");
}
+
return this->_synchronizedPrx;
}
- SharedRobotInterfacePrx RobotStateComponent::getRobotSnapshot(const string & time,const Current & current)
+ SharedRobotInterfacePrx RobotStateComponent::getRobotSnapshot(const string& time, const Current& current)
{
- if(!_synchronized)
+ if (!_synchronized)
+ {
throw NotInitializedException("Shared Robot is NULL", "getRobotSnapshot");
+ }
+
auto clone = this->_synchronized->clone(_synchronized->getName());
SharedRobotInterfacePtr p = new SharedRobotServant(clone);
@@ -153,15 +160,15 @@ namespace armarx
void RobotStateComponent::reportJointAngles(const NameValueMap& jointAngles, bool aValueChanged, const Current& c)
{
-// IceUtil::Time start = IceUtil::Time::now();
- if(aValueChanged)
+ // IceUtil::Time start = IceUtil::Time::now();
+ if (aValueChanged)
{
{
WriteLockPtr lock = _synchronized->getWriteLock();
std::vector<float> jv;
std::vector< RobotNodePtr > nodes;
- BOOST_FOREACH(NameValueMap::value_type namedAngle, jointAngles )
+ BOOST_FOREACH(NameValueMap::value_type namedAngle, jointAngles)
{
RobotNodePtr node = this->_synchronized->getRobotNode(namedAngle.first);
nodes.push_back(node);
@@ -169,52 +176,64 @@ namespace armarx
}
_synchronized->setJointValues(nodes, jv);
}
- if(robotStateObs)
+
+ if (robotStateObs)
+ {
robotStateObs->updatePoses();
+ }
}
- if(_sharedRobotServant)
+
+ if (_sharedRobotServant)
+ {
_sharedRobotServant->setTimestamp(IceUtil::Time::now());
-// ARMARX_VERBOSE << deactivateSpam(2) << "my duration: " << (IceUtil::Time::now() - start).toMicroSecondsDouble() << " ms";
+ }
+
+ // ARMARX_VERBOSE << deactivateSpam(2) << "my duration: " << (IceUtil::Time::now() - start).toMicroSecondsDouble() << " ms";
}
std::string RobotStateComponent::getRobotFilename(const Ice::Current&) const
- {
+ {
return relativeRobotFile;
}
- std::vector<string> RobotStateComponent::getArmarXPackages(const Current &) const
+ std::vector<string> RobotStateComponent::getArmarXPackages(const Current&) const
{
std::vector<string> result;
auto packages = armarx::Application::GetProjectDependencies();
packages.push_back(Application::GetProjectName());
- for(const std::string &projectName : packages)
+
+ for (const std::string& projectName : packages)
{
- if(projectName.empty())
+ if (projectName.empty())
+ {
continue;
+ }
result.push_back(projectName);
}
return result;
}
-
- void RobotStateComponent::reportControlModeChanged(const NameControlModeMap& jointModes, bool aValueChanged,const Current& c){}
- void RobotStateComponent::reportJointVelocities(const NameValueMap& jointVelocities, bool aValueChanged,const Current& c)
+
+ void RobotStateComponent::reportControlModeChanged(const NameControlModeMap& jointModes, bool aValueChanged, const Current& c) {}
+ void RobotStateComponent::reportJointVelocities(const NameValueMap& jointVelocities, bool aValueChanged, const Current& c)
{
- if(robotStateObs)
+ if (robotStateObs)
+ {
robotStateObs->updateNodeVelocities(jointVelocities);
+ }
}
- void RobotStateComponent::reportJointTorques(const NameValueMap& jointTorques, bool aValueChanged,const Current& c){}
+ void RobotStateComponent::reportJointTorques(const NameValueMap& jointTorques, bool aValueChanged, const Current& c) {}
- void RobotStateComponent::reportJointAccelerations(const NameValueMap &jointAccelerations, bool aValueChanged, const Current &c)
+ void RobotStateComponent::reportJointAccelerations(const NameValueMap& jointAccelerations, bool aValueChanged, const Current& c)
{
}
- void RobotStateComponent::reportJointCurrents(const NameValueMap& jointCurrents, bool aValueChanged,const Current& c){}
- void RobotStateComponent::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, bool aValueChanged, const Current& c){}
- void RobotStateComponent::reportJointStatuses(const NameStatusMap& jointStatuses, bool aValueChanged, const Current& c){}
+ void RobotStateComponent::reportJointCurrents(const NameValueMap& jointCurrents, bool aValueChanged, const Current& c) {}
+ void RobotStateComponent::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, bool aValueChanged, const Current& c) {}
+ void RobotStateComponent::reportJointStatuses(const NameStatusMap& jointStatuses, bool aValueChanged, const Current& c) {}
/*void RobotStateComponent::reportPlatformPose(float currentPlatformPositionX, float currentPlatformPositionY, float currentPlatformRotation, const Ice::Current& c)
{
@@ -251,12 +270,16 @@ namespace armarx
robotStateObs = observer;
}
- string RobotStateComponent::getRobotName(const Current &) const
+ string RobotStateComponent::getRobotName(const Current&) const
{
- if(_synchronized)
+ if (_synchronized)
+ {
return _synchronizedPrx->getName();
+ }
else
+ {
throw NotInitializedException("Robot Ptr is NULL", "getName");
+ }
}
}
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h
index 6e74c02c23ab540f26c7c318a131b0f78e38584a..617d938e6e3644604f56e7be7fc7e7f750da9e29 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.h
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h
@@ -42,13 +42,13 @@ namespace armarx
* \brief
*/
class RobotStatePropertyDefinitions:
- public ComponentPropertyDefinitions
+ public ComponentPropertyDefinitions
{
public:
RobotStatePropertyDefinitions(std::string prefix):
ComponentPropertyDefinitions(prefix)
{
- defineRequiredProperty<std::string>("RobotNodeSetName","Set of nodes that is controlled by the KinematicUnit");
+ defineRequiredProperty<std::string>("RobotNodeSetName", "Set of nodes that is controlled by the KinematicUnit");
defineRequiredProperty<std::string>("RobotFileName", "Filename of VirtualRobot robot model (e.g. robot_model.xml)");
defineRequiredProperty<std::string>("AgentName", "Name of the agent for which the sensor values are provided");
}
@@ -91,8 +91,8 @@ namespace armarx
/**
* \return Clone of the internal synchronized robot.
*/
- virtual SharedRobotInterfacePrx getRobotSnapshot(const std::string & time, const Ice::Current&);
-
+ virtual SharedRobotInterfacePrx getRobotSnapshot(const std::string& time, const Ice::Current&);
+
/**
* \return the robot xml filename as specified in the configuration
*/
@@ -108,7 +108,7 @@ namespace armarx
*
* \return The name of this robot instance.
*/
- virtual std::string getRobotName(const Ice::Current &) const;
+ virtual std::string getRobotName(const Ice::Current&) const;
/**
* Create an instance of RobotStatePropertyDefinitions.
diff --git a/source/RobotAPI/components/RobotState/SharedRobotServants.cpp b/source/RobotAPI/components/RobotState/SharedRobotServants.cpp
index 8391b2df0d6a7f550e901eebc673e5a3fc114659..ba706483d794fe354d875367db1be3a02e49dea2 100644
--- a/source/RobotAPI/components/RobotState/SharedRobotServants.cpp
+++ b/source/RobotAPI/components/RobotState/SharedRobotServants.cpp
@@ -31,360 +31,407 @@ using namespace Ice;
namespace armarx
{
-typedef std::map<std::string, SharedRobotNodeInterfacePrx> NodeCache;
+ typedef std::map<std::string, SharedRobotNodeInterfacePrx> NodeCache;
-///////////////////////////////
-// SharedObjectBase
-///////////////////////////////
+ ///////////////////////////////
+ // SharedObjectBase
+ ///////////////////////////////
-SharedObjectBase::SharedObjectBase()
-{
- this-> _referenceCount=0;
- #ifdef VERBOSE
- ARMARX_LOG_S << "construct " << this << flush;
- #endif
-}
+ SharedObjectBase::SharedObjectBase()
+ {
+ this-> _referenceCount = 0;
+#ifdef VERBOSE
+ ARMARX_LOG_S << "construct " << this << flush;
+#endif
+ }
-void SharedObjectBase::ref(const Current ¤t)
-{
- boost::lock_guard<boost::mutex> lock(this->_counterMutex);
+ void SharedObjectBase::ref(const Current& current)
+ {
+ boost::lock_guard<boost::mutex> lock(this->_counterMutex);
- _referenceCount++;
+ _referenceCount++;
- #ifdef VERBOSE
- ARMARX_LOG_S << "ref: " << _referenceCount << " " << this << flush;
- #endif
-}
+#ifdef VERBOSE
+ ARMARX_LOG_S << "ref: " << _referenceCount << " " << this << flush;
+#endif
+ }
-void SharedObjectBase::unref(const Current ¤t)
-{
- boost::lock_guard<boost::mutex> lock(this->_counterMutex);
+ void SharedObjectBase::unref(const Current& current)
+ {
+ boost::lock_guard<boost::mutex> lock(this->_counterMutex);
#ifdef VERBOSE
- ARMARX_LOG_S << "unref: " << _referenceCount << " " << this << flush;
+ ARMARX_LOG_S << "unref: " << _referenceCount << " " << this << flush;
#endif
- _referenceCount --;
- if (_referenceCount==0)
- this->destroy(current);
-}
+ _referenceCount --;
-void SharedObjectBase::destroy(const Current ¤t)
-{
- try {
- current.adapter->remove(current.id);
- #ifdef VERBOSE
- ARMARX_ERROR_S << "[SharedObject] destroy " << " " << this << flush;
- #endif
- } catch (const NotRegisteredException&e){
-// ARMARX_INFO_S << "destroy failed with: " << e.what();
- throw ObjectNotExistException(__FILE__, __LINE__);
- };
-}
+ if (_referenceCount == 0)
+ {
+ this->destroy(current);
+ }
+ }
+
+ void SharedObjectBase::destroy(const Current& current)
+ {
+ try
+ {
+ current.adapter->remove(current.id);
+#ifdef VERBOSE
+ ARMARX_ERROR_S << "[SharedObject] destroy " << " " << this << flush;
+#endif
+ }
+ catch (const NotRegisteredException& e)
+ {
+ // ARMARX_INFO_S << "destroy failed with: " << e.what();
+ throw ObjectNotExistException(__FILE__, __LINE__);
+ };
+ }
-///////////////////////////////
-// SharedRobotNodeServant
-///////////////////////////////
+ ///////////////////////////////
+ // SharedRobotNodeServant
+ ///////////////////////////////
-SharedRobotNodeServant::SharedRobotNodeServant(RobotNodePtr node) :
- _node(node)
-{
+ SharedRobotNodeServant::SharedRobotNodeServant(RobotNodePtr node) :
+ _node(node)
+ {
#ifdef VERBOSE
- ARMARX_LOG_S << "[SharedRobotNodeServant] construct " << " " << this << flush;
+ ARMARX_LOG_S << "[SharedRobotNodeServant] construct " << " " << this << flush;
#endif
-}
+ }
-SharedRobotNodeServant::~SharedRobotNodeServant()
-{
+ SharedRobotNodeServant::~SharedRobotNodeServant()
+ {
#ifdef VERBOSE
- ARMARX_FATAL_S << "[SharedRobotNodeServant] destruct " << " " << this << flush;
+ ARMARX_FATAL_S << "[SharedRobotNodeServant] destruct " << " " << this << flush;
#endif
-}
+ }
-float SharedRobotNodeServant::getJointValue(const Current ¤t) const
-{
- ReadLockPtr lock = _node->getRobot()->getReadLock();
- return _node->getJointValue();
-}
+ float SharedRobotNodeServant::getJointValue(const Current& current) const
+ {
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ return _node->getJointValue();
+ }
-string SharedRobotNodeServant::getName(const Current ¤t) const
-{
- ReadLockPtr lock = _node->getRobot()->getReadLock();
- return _node->getName();
-}
+ string SharedRobotNodeServant::getName(const Current& current) const
+ {
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ return _node->getName();
+ }
-PoseBasePtr SharedRobotNodeServant::getLocalTransformation(const Current ¤t) const
-{
- ReadLockPtr lock = _node->getRobot()->getReadLock();
- return new Pose(_node->getLocalTransformation());
-}
+ PoseBasePtr SharedRobotNodeServant::getLocalTransformation(const Current& current) const
+ {
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ return new Pose(_node->getLocalTransformation());
+ }
-FramedPoseBasePtr SharedRobotNodeServant::getGlobalPose(const Current ¤t) const
-{
- ReadLockPtr lock = _node->getRobot()->getReadLock();
- return new FramedPose(_node->getGlobalPose(),
- GlobalFrame,
- "");
-}
+ FramedPoseBasePtr SharedRobotNodeServant::getGlobalPose(const Current& current) const
+ {
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ return new FramedPose(_node->getGlobalPose(),
+ GlobalFrame,
+ "");
+ }
-FramedPoseBasePtr SharedRobotNodeServant::getPoseInRootFrame(const Current ¤t) const
-{
- ReadLockPtr lock = _node->getRobot()->getReadLock();
- return new FramedPose(_node->getPoseInRootFrame(),
- _node->getRobot()->getRootNode()->getName(),
- _node->getRobot()->getName());
-}
+ FramedPoseBasePtr SharedRobotNodeServant::getPoseInRootFrame(const Current& current) const
+ {
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ return new FramedPose(_node->getPoseInRootFrame(),
+ _node->getRobot()->getRootNode()->getName(),
+ _node->getRobot()->getName());
+ }
-JointType SharedRobotNodeServant::getType(const Current ¤t) const
-{
- ReadLockPtr lock = _node->getRobot()->getReadLock();
- if (_node->isRotationalJoint()) return eRevolute;
- else if (_node->isTranslationalJoint()) return ePrismatic;
- else return eFixed;
-}
+ JointType SharedRobotNodeServant::getType(const Current& current) const
+ {
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
-Vector3BasePtr SharedRobotNodeServant::getJointTranslationDirection(const Current ¤t) const
-{
- ReadLockPtr lock = _node->getRobot()->getReadLock();
- boost::shared_ptr<RobotNodePrismatic> prismatic = dynamic_pointer_cast<RobotNodePrismatic>(_node);
- if (prismatic)
- return new Vector3(prismatic->getJointTranslationDirection());
- else
- return new Vector3;
-}
+ if (_node->isRotationalJoint())
+ {
+ return eRevolute;
+ }
+ else if (_node->isTranslationalJoint())
+ {
+ return ePrismatic;
+ }
+ else
+ {
+ return eFixed;
+ }
+ }
-Vector3BasePtr SharedRobotNodeServant::getJointRotationAxis(const Current ¤t) const
-{
- ReadLockPtr lock = _node->getRobot()->getReadLock();
- boost::shared_ptr<RobotNodeRevolute> revolute = dynamic_pointer_cast<RobotNodeRevolute>(_node);
- if (revolute)
- return new Vector3(revolute->getJointRotationAxis());
- else
- return new Vector3;
-}
+ Vector3BasePtr SharedRobotNodeServant::getJointTranslationDirection(const Current& current) const
+ {
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ boost::shared_ptr<RobotNodePrismatic> prismatic = dynamic_pointer_cast<RobotNodePrismatic>(_node);
+
+ if (prismatic)
+ {
+ return new Vector3(prismatic->getJointTranslationDirection());
+ }
+ else
+ {
+ return new Vector3;
+ }
+ }
+ Vector3BasePtr SharedRobotNodeServant::getJointRotationAxis(const Current& current) const
+ {
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ boost::shared_ptr<RobotNodeRevolute> revolute = dynamic_pointer_cast<RobotNodeRevolute>(_node);
-bool SharedRobotNodeServant::hasChild(const string &name, bool recursive, const Current ¤t) const
-{
- ReadLockPtr lock = _node->getRobot()->getReadLock();
- //return _node->hasChild(name,recursive);
- return false;
-}
+ if (revolute)
+ {
+ return new Vector3(revolute->getJointRotationAxis());
+ }
+ else
+ {
+ return new Vector3;
+ }
+ }
-string SharedRobotNodeServant::getParent(const Current ¤t) const
-{
- ReadLockPtr lock = _node->getRobot()->getReadLock();
- SceneObjectPtr parent = _node->getParent();
- if (!parent) throw UserException("This RobotNode has no parent.");
- return parent->getName();
-}
-NameList SharedRobotNodeServant::getChildren(const Current ¤t) const
-{
- ReadLockPtr lock = _node->getRobot()->getReadLock();
- vector<SceneObjectPtr> children = _node->getChildren();
- NameList names;
- BOOST_FOREACH(SceneObjectPtr node, children){
- names.push_back(node->getName());
+ bool SharedRobotNodeServant::hasChild(const string& name, bool recursive, const Current& current) const
+ {
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ //return _node->hasChild(name,recursive);
+ return false;
}
- return names;
-}
-NameList SharedRobotNodeServant::getAllParents(const std::string &name, const Ice::Current ¤t) const
-{
- ReadLockPtr lock = _node->getRobot()->getReadLock();
- vector<RobotNodePtr> parents = _node->getAllParents(_node->getRobot()->getRobotNodeSet(name));
- NameList names;
- BOOST_FOREACH(RobotNodePtr node, parents){
- names.push_back(node->getName());
+ string SharedRobotNodeServant::getParent(const Current& current) const
+ {
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ SceneObjectPtr parent = _node->getParent();
+
+ if (!parent)
+ {
+ throw UserException("This RobotNode has no parent.");
+ }
+
+ return parent->getName();
}
- return names;
-}
-float SharedRobotNodeServant::getJointValueOffest(const Current ¤t) const
-{
- ReadLockPtr lock = _node->getRobot()->getReadLock();
- return _node->getJointValueOffset();
-}
+ NameList SharedRobotNodeServant::getChildren(const Current& current) const
+ {
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ vector<SceneObjectPtr> children = _node->getChildren();
+ NameList names;
+ BOOST_FOREACH(SceneObjectPtr node, children)
+ {
+ names.push_back(node->getName());
+ }
+ return names;
+ }
-float SharedRobotNodeServant::getJointLimitHigh(const Current ¤t) const
-{
- ReadLockPtr lock = _node->getRobot()->getReadLock();
- return _node->getJointLimitHigh();
-}
+ NameList SharedRobotNodeServant::getAllParents(const std::string& name, const Ice::Current& current) const
+ {
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ vector<RobotNodePtr> parents = _node->getAllParents(_node->getRobot()->getRobotNodeSet(name));
+ NameList names;
+ BOOST_FOREACH(RobotNodePtr node, parents)
+ {
+ names.push_back(node->getName());
+ }
+ return names;
+ }
-float SharedRobotNodeServant::getJointLimitLow(const Current ¤t) const
-{
- ReadLockPtr lock = _node->getRobot()->getReadLock();
- return _node->getJointLimitLow();
-}
+ float SharedRobotNodeServant::getJointValueOffest(const Current& current) const
+ {
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ return _node->getJointValueOffset();
+ }
+ float SharedRobotNodeServant::getJointLimitHigh(const Current& current) const
+ {
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ return _node->getJointLimitHigh();
+ }
-///////////////////////////////
-// SharedRobotServant
-///////////////////////////////
+ float SharedRobotNodeServant::getJointLimitLow(const Current& current) const
+ {
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ return _node->getJointLimitLow();
+ }
-SharedRobotServant::SharedRobotServant(RobotPtr robot)
- : _robot(robot)
-{
+
+ ///////////////////////////////
+ // SharedRobotServant
+ ///////////////////////////////
+
+ SharedRobotServant::SharedRobotServant(RobotPtr robot)
+ : _robot(robot)
+ {
#ifdef VERBOSE
- ARMARX_WARNING_S << "construct " << this << flush;
+ ARMARX_WARNING_S << "construct " << this << flush;
#endif
-}
+ }
-SharedRobotServant::~SharedRobotServant()
-{
+ SharedRobotServant::~SharedRobotServant()
+ {
#ifdef VERBOSE
- ARMARX_WARNING_S << "destruct " << this << flush;
+ ARMARX_WARNING_S << "destruct " << this << flush;
#endif
- boost::recursive_mutex::scoped_lock cloneLock(m);
+ boost::recursive_mutex::scoped_lock cloneLock(m);
- for(auto value: this->_cachedNodes)
- {
- try
+ for (auto value : this->_cachedNodes)
{
- value.second->unref();
+ try
+ {
+ value.second->unref();
+ }
+ catch (...) {}
}
- catch(...){}
}
-}
-SharedRobotNodeInterfacePrx SharedRobotServant::getRobotNode(const string &name, const Current ¤t)
-{
-// ARMARX_LOG_S << "Looking for node: " << name << flush;
- assert(_robot);
- boost::recursive_mutex::scoped_lock cloneLock(m);
- SharedRobotNodeInterfacePrx prx;
- if (this->_cachedNodes.find(name) == this->_cachedNodes.end()){
- RobotNodePtr robotNode = _robot->getRobotNode(name);
- if (!robotNode){
- ARMARX_WARNING_S << "RobotNode \"" + name + "\" not defined.";
- throw UserException("RobotNode \"" + name + "\" not defined.");
+ SharedRobotNodeInterfacePrx SharedRobotServant::getRobotNode(const string& name, const Current& current)
+ {
+ // ARMARX_LOG_S << "Looking for node: " << name << flush;
+ assert(_robot);
+ boost::recursive_mutex::scoped_lock cloneLock(m);
+ SharedRobotNodeInterfacePrx prx;
+
+ if (this->_cachedNodes.find(name) == this->_cachedNodes.end())
+ {
+ RobotNodePtr robotNode = _robot->getRobotNode(name);
+
+ if (!robotNode)
+ {
+ ARMARX_WARNING_S << "RobotNode \"" + name + "\" not defined.";
+ throw UserException("RobotNode \"" + name + "\" not defined.");
+ }
+
+ SharedRobotNodeInterfacePtr servant = new SharedRobotNodeServant(
+ _robot->getRobotNode(name));
+ //servant->ref();
+ prx = SharedRobotNodeInterfacePrx::uncheckedCast(current.adapter->addWithUUID(servant));
+ prx->ref();
+ // return prx;
+ this->_cachedNodes[name] = prx;
}
- SharedRobotNodeInterfacePtr servant = new SharedRobotNodeServant(
- _robot->getRobotNode(name));
- //servant->ref();
- prx = SharedRobotNodeInterfacePrx::uncheckedCast(current.adapter->addWithUUID(servant));
- prx->ref();
-// return prx;
- this->_cachedNodes[name] = prx;
+ return this->_cachedNodes[name];
}
- return this->_cachedNodes[name];
-}
-
-SharedRobotNodeInterfacePrx SharedRobotServant::getRootNode(const Current ¤t)
-{
- assert(_robot);
- boost::recursive_mutex::scoped_lock cloneLock(m);
- string name = _robot->getRootNode()/*,current*/->getName();
- return this->getRobotNode(name, current);
-}
+ SharedRobotNodeInterfacePrx SharedRobotServant::getRootNode(const Current& current)
+ {
+ assert(_robot);
+ boost::recursive_mutex::scoped_lock cloneLock(m);
+ string name = _robot->getRootNode()/*,current*/->getName();
+ return this->getRobotNode(name, current);
+ }
-bool SharedRobotServant::hasRobotNode(const string & name, const Current ¤t)
-{
- return _robot->hasRobotNode(name);
-}
-NameList SharedRobotServant::getRobotNodes(const Current ¤t)
-{
- vector<RobotNodePtr> robotNodes = _robot->getRobotNodes();
- NameList names;
- BOOST_FOREACH(RobotNodePtr node, robotNodes){
- names.push_back(node->getName());
+ bool SharedRobotServant::hasRobotNode(const string& name, const Current& current)
+ {
+ return _robot->hasRobotNode(name);
}
- return names;
-}
-RobotNodeSetInfoPtr SharedRobotServant::getRobotNodeSet(const string & name, const Current ¤t)
-{
- RobotNodeSetPtr robotNodeSet = _robot->getRobotNodeSet(name);
- if (!robotNodeSet) throw UserException("RobotNodeSet \""+name+"\" not defined");
- vector<RobotNodePtr> robotNodes = robotNodeSet->getAllRobotNodes();
- NameList names;
- BOOST_FOREACH(RobotNodePtr node, robotNodes){
- names.push_back(node->getName());
+ NameList SharedRobotServant::getRobotNodes(const Current& current)
+ {
+ vector<RobotNodePtr> robotNodes = _robot->getRobotNodes();
+ NameList names;
+ BOOST_FOREACH(RobotNodePtr node, robotNodes)
+ {
+ names.push_back(node->getName());
+ }
+ return names;
}
- RobotNodeSetInfoPtr info = new RobotNodeSetInfo;
- info->names = names;
- info->name = name;
- info->tcpName = robotNodeSet->getTCP()->getName();
- info->rootName = robotNodeSet->getKinematicRoot()->getName();
+ RobotNodeSetInfoPtr SharedRobotServant::getRobotNodeSet(const string& name, const Current& current)
+ {
+ RobotNodeSetPtr robotNodeSet = _robot->getRobotNodeSet(name);
+
+ if (!robotNodeSet)
+ {
+ throw UserException("RobotNodeSet \"" + name + "\" not defined");
+ }
- return info;
+ vector<RobotNodePtr> robotNodes = robotNodeSet->getAllRobotNodes();
+ NameList names;
+ BOOST_FOREACH(RobotNodePtr node, robotNodes)
+ {
+ names.push_back(node->getName());
+ }
-}
+ RobotNodeSetInfoPtr info = new RobotNodeSetInfo;
+ info->names = names;
+ info->name = name;
+ info->tcpName = robotNodeSet->getTCP()->getName();
+ info->rootName = robotNodeSet->getKinematicRoot()->getName();
+
+ return info;
-NameList SharedRobotServant::getRobotNodeSets(const Current ¤t)
-{
- vector<RobotNodeSetPtr> robotNodeSets = _robot->getRobotNodeSets();
- NameList names;
- BOOST_FOREACH(RobotNodeSetPtr set, robotNodeSets){
- names.push_back(set->getName());
}
- return names;
-}
+ NameList SharedRobotServant::getRobotNodeSets(const Current& current)
+ {
+ vector<RobotNodeSetPtr> robotNodeSets = _robot->getRobotNodeSets();
+ NameList names;
+ BOOST_FOREACH(RobotNodeSetPtr set, robotNodeSets)
+ {
+ names.push_back(set->getName());
+ }
-bool SharedRobotServant::hasRobotNodeSet(const string & name, const Current ¤t)
-{
- return _robot->hasRobotNodeSet(name);
-}
+ return names;
+ }
-string SharedRobotServant::getName(const Current ¤t)
-{
- //ARMARX_VERBOSE_S << "SharedRobotServant::getname:" << _robot->getName();
- return _robot->getName();
-}
+ bool SharedRobotServant::hasRobotNodeSet(const string& name, const Current& current)
+ {
+ return _robot->hasRobotNodeSet(name);
+ }
-string SharedRobotServant::getType(const Current ¤t)
-{
- return _robot->getType();
-}
+ string SharedRobotServant::getName(const Current& current)
+ {
+ //ARMARX_VERBOSE_S << "SharedRobotServant::getname:" << _robot->getName();
+ return _robot->getName();
+ }
-void SharedRobotServant::setTimestamp(const IceUtil::Time &updateTime)
-{
- updateTimestamp = updateTime;
-}
+ string SharedRobotServant::getType(const Current& current)
+ {
+ return _robot->getType();
+ }
-PoseBasePtr SharedRobotServant::getGlobalPose(const Current ¤t)
-{
- ReadLockPtr lock = _robot->getReadLock();
- return new Pose(_robot->getGlobalPose());
-}
+ void SharedRobotServant::setTimestamp(const IceUtil::Time& updateTime)
+ {
+ updateTimestamp = updateTime;
+ }
-NameValueMap SharedRobotServant::getConfig(const Current ¤t)
-{
- if(!_robot)
+ PoseBasePtr SharedRobotServant::getGlobalPose(const Current& current)
{
- ARMARX_WARNING_S << "no robot set";
- return NameValueMap();
+ ReadLockPtr lock = _robot->getReadLock();
+ return new Pose(_robot->getGlobalPose());
}
- ReadLockPtr lock = _robot->getReadLock();
- std::map < std::string, float > values = _robot->getConfig()->getRobotNodeJointValueMap();
- NameValueMap result(values.begin(), values.end());
- return result;
-}
-TimestampBasePtr SharedRobotServant::getTimestamp(const Current &) const
-{
- return new TimestampVariant(updateTimestamp);
-}
+ NameValueMap SharedRobotServant::getConfig(const Current& current)
+ {
+ if (!_robot)
+ {
+ ARMARX_WARNING_S << "no robot set";
+ return NameValueMap();
+ }
-void SharedRobotServant::setGlobalPose(const armarx::PoseBasePtr &pose, const Current &)
-{
- WriteLockPtr lock = _robot->getWriteLock();
- _robot->setGlobalPose(PosePtr::dynamicCast(pose)->toEigen(), true);
-}
+ ReadLockPtr lock = _robot->getReadLock();
+ std::map < std::string, float > values = _robot->getConfig()->getRobotNodeJointValueMap();
+ NameValueMap result(values.begin(), values.end());
+ return result;
+ }
-RobotPtr SharedRobotServant::getRobot() const
-{
- return this->_robot;
-}
+ TimestampBasePtr SharedRobotServant::getTimestamp(const Current&) const
+ {
+ return new TimestampVariant(updateTimestamp);
+ }
+
+ void SharedRobotServant::setGlobalPose(const armarx::PoseBasePtr& pose, const Current&)
+ {
+ WriteLockPtr lock = _robot->getWriteLock();
+ _robot->setGlobalPose(PosePtr::dynamicCast(pose)->toEigen(), true);
+ }
+
+ RobotPtr SharedRobotServant::getRobot() const
+ {
+ return this->_robot;
+ }
}
diff --git a/source/RobotAPI/components/RobotState/SharedRobotServants.h b/source/RobotAPI/components/RobotState/SharedRobotServants.h
index ba01ebe81ef083154b73f054396f66fa3199dae9..845ed5465c4d5a4a43ad52f7ad9551b9c878f340 100644
--- a/source/RobotAPI/components/RobotState/SharedRobotServants.h
+++ b/source/RobotAPI/components/RobotState/SharedRobotServants.h
@@ -18,7 +18,8 @@ namespace VirtualRobot
typedef boost::shared_ptr<RobotNode> RobotNodePtr;
}
-namespace armarx {
+namespace armarx
+{
// Zugriff muss mutex geschuetzt werden!
/**
@@ -29,14 +30,14 @@ namespace armarx {
class SharedObjectBase :
virtual public SharedObjectInterface
{
- public:
- virtual void ref(const Ice::Current &c = Ice::Current());
- virtual void unref(const Ice::Current &c = Ice::Current());
- void destroy(const Ice::Current &c = Ice::Current());
- SharedObjectBase();
- private:
- unsigned int _referenceCount;
- boost::mutex _counterMutex;
+ public:
+ virtual void ref(const Ice::Current& c = Ice::Current());
+ virtual void unref(const Ice::Current& c = Ice::Current());
+ void destroy(const Ice::Current& c = Ice::Current());
+ SharedObjectBase();
+ private:
+ unsigned int _referenceCount;
+ boost::mutex _counterMutex;
};
/**
@@ -54,25 +55,25 @@ namespace armarx {
SharedRobotNodeServant(VirtualRobot::RobotNodePtr node /*,const Ice::Current & current = Ice::Current()*/);
~SharedRobotNodeServant();
- virtual float getJointValue(const Ice::Current & current = Ice::Current()) const;
- virtual std::string getName(const Ice::Current & current = Ice::Current()) const;
+ virtual float getJointValue(const Ice::Current& current = Ice::Current()) const;
+ virtual std::string getName(const Ice::Current& current = Ice::Current()) const;
- virtual PoseBasePtr getLocalTransformation(const Ice::Current & current = Ice::Current()) const;
- virtual FramedPoseBasePtr getGlobalPose(const Ice::Current & current = Ice::Current()) const;
- virtual FramedPoseBasePtr getPoseInRootFrame(const Ice::Current ¤t = Ice::Current()) const;
+ virtual PoseBasePtr getLocalTransformation(const Ice::Current& current = Ice::Current()) const;
+ virtual FramedPoseBasePtr getGlobalPose(const Ice::Current& current = Ice::Current()) const;
+ virtual FramedPoseBasePtr getPoseInRootFrame(const Ice::Current& current = Ice::Current()) const;
- virtual JointType getType(const Ice::Current & current = Ice::Current()) const;
- virtual Vector3BasePtr getJointTranslationDirection(const Ice::Current & current = Ice::Current()) const;
- virtual Vector3BasePtr getJointRotationAxis(const Ice::Current & current = Ice::Current()) const;
+ virtual JointType getType(const Ice::Current& current = Ice::Current()) const;
+ virtual Vector3BasePtr getJointTranslationDirection(const Ice::Current& current = Ice::Current()) const;
+ virtual Vector3BasePtr getJointRotationAxis(const Ice::Current& current = Ice::Current()) const;
- virtual bool hasChild(const std::string &name, bool recursive, const Ice::Current & current = Ice::Current()) const;
- virtual std::string getParent(const Ice::Current & current = Ice::Current()) const;
- virtual NameList getChildren(const Ice::Current & current = Ice::Current()) const;
- virtual NameList getAllParents(const std::string &name, const Ice::Current & current = Ice::Current()) const;
+ virtual bool hasChild(const std::string& name, bool recursive, const Ice::Current& current = Ice::Current()) const;
+ virtual std::string getParent(const Ice::Current& current = Ice::Current()) const;
+ virtual NameList getChildren(const Ice::Current& current = Ice::Current()) const;
+ virtual NameList getAllParents(const std::string& name, const Ice::Current& current = Ice::Current()) const;
- virtual float getJointValueOffest(const Ice::Current & current = Ice::Current()) const;
- virtual float getJointLimitHigh(const Ice::Current & current = Ice::Current()) const;
- virtual float getJointLimitLow(const Ice::Current & current = Ice::Current()) const;
+ virtual float getJointValueOffest(const Ice::Current& current = Ice::Current()) const;
+ virtual float getJointLimitHigh(const Ice::Current& current = Ice::Current()) const;
+ virtual float getJointLimitLow(const Ice::Current& current = Ice::Current()) const;
@@ -96,26 +97,26 @@ namespace armarx {
SharedRobotServant(VirtualRobot::RobotPtr robot);
~SharedRobotServant();
- virtual SharedRobotNodeInterfacePrx getRobotNode(const std::string & name, const Ice::Current & current = Ice::Current());
- virtual SharedRobotNodeInterfacePrx getRootNode(const Ice::Current & current = Ice::Current());
- virtual bool hasRobotNode(const std::string & name, const Ice::Current & current = Ice::Current());
+ virtual SharedRobotNodeInterfacePrx getRobotNode(const std::string& name, const Ice::Current& current = Ice::Current());
+ virtual SharedRobotNodeInterfacePrx getRootNode(const Ice::Current& current = Ice::Current());
+ virtual bool hasRobotNode(const std::string& name, const Ice::Current& current = Ice::Current());
- virtual NameList getRobotNodes(const Ice::Current & current = Ice::Current());
- virtual RobotNodeSetInfoPtr getRobotNodeSet(const std::string & name, const Ice::Current & current = Ice::Current());
- virtual NameList getRobotNodeSets(const Ice::Current & current = Ice::Current());
- virtual bool hasRobotNodeSet(const std::string & name, const Ice::Current & current = Ice::Current());
+ virtual NameList getRobotNodes(const Ice::Current& current = Ice::Current());
+ virtual RobotNodeSetInfoPtr getRobotNodeSet(const std::string& name, const Ice::Current& current = Ice::Current());
+ virtual NameList getRobotNodeSets(const Ice::Current& current = Ice::Current());
+ virtual bool hasRobotNodeSet(const std::string& name, const Ice::Current& current = Ice::Current());
- virtual std::string getName(const Ice::Current & current = Ice::Current());
- virtual std::string getType(const Ice::Current & current = Ice::Current());
+ virtual std::string getName(const Ice::Current& current = Ice::Current());
+ virtual std::string getType(const Ice::Current& current = Ice::Current());
- virtual PoseBasePtr getGlobalPose(const Ice::Current & current = Ice::Current());
- NameValueMap getConfig(const Ice::Current & current = Ice::Current());
- virtual void setGlobalPose(const PoseBasePtr& pose, const Ice::Current ¤t);
+ virtual PoseBasePtr getGlobalPose(const Ice::Current& current = Ice::Current());
+ NameValueMap getConfig(const Ice::Current& current = Ice::Current());
+ virtual void setGlobalPose(const PoseBasePtr& pose, const Ice::Current& current);
VirtualRobot::RobotPtr getRobot() const;
- void setTimestamp(const IceUtil::Time &updateTime);
- TimestampBasePtr getTimestamp(const Ice::Current &) const;
+ void setTimestamp(const IceUtil::Time& updateTime);
+ TimestampBasePtr getTimestamp(const Ice::Current&) const;
protected:
VirtualRobot::RobotPtr _robot;
boost::recursive_mutex m;
diff --git a/source/RobotAPI/components/units/BusInspectionUnit.cpp b/source/RobotAPI/components/units/BusInspectionUnit.cpp
index 30acee8e89df979ae920eb8d78f393cdb1e414b6..6bf0479059deefcdb3ef94496ff7d3878c41e69d 100644
--- a/source/RobotAPI/components/units/BusInspectionUnit.cpp
+++ b/source/RobotAPI/components/units/BusInspectionUnit.cpp
@@ -31,12 +31,14 @@ using namespace std;
using namespace armarx;
-void BusInspectionUnit::onInitComponent() {
- this->onInitBusInspectionUnit();
- offeringTopic("HardwareValues");
+void BusInspectionUnit::onInitComponent()
+{
+ this->onInitBusInspectionUnit();
+ offeringTopic("HardwareValues");
}
-void BusInspectionUnit::onConnectComponent() {
+void BusInspectionUnit::onConnectComponent()
+{
ARMARX_INFO << flush;
@@ -51,24 +53,29 @@ void BusInspectionUnit::onConnectComponent() {
this->onConnectBusInspectionUnit();
}
-void BusInspectionUnit::onExitComponent() {
- this->onExitBusInspectionUnit();
+void BusInspectionUnit::onExitComponent()
+{
+ this->onExitBusInspectionUnit();
}
-BusNames BusInspectionUnit::getConfiguredBusses( const Ice::Current& c) {
+BusNames BusInspectionUnit::getConfiguredBusses(const Ice::Current& c)
+{
return *(new vector<string>());
}
-BusInformation BusInspectionUnit::getBusInformation(const std::string& bus, const Ice::Current& c){
+BusInformation BusInspectionUnit::getBusInformation(const std::string& bus, const Ice::Current& c)
+{
return *(new BusInformation);
-
+
}
-DeviceNames BusInspectionUnit::getDevicesOnBus(const std::string &bus, const Ice::Current& c ){
- return *(new vector<string>());
+DeviceNames BusInspectionUnit::getDevicesOnBus(const std::string& bus, const Ice::Current& c)
+{
+ return *(new vector<string>());
}
-DeviceInformation BusInspectionUnit::getDeviceInformation(const std::string& device, const Ice::Current& c ) {
- return *(new DeviceInformation);
+DeviceInformation BusInspectionUnit::getDeviceInformation(const std::string& device, const Ice::Current& c)
+{
+ return *(new DeviceInformation);
}
Ice::Int BusInspectionUnit::performDeviceOperation(BasicOperation operation, Ice::Int device, const Ice::Current& c)
@@ -76,7 +83,7 @@ Ice::Int BusInspectionUnit::performDeviceOperation(BasicOperation operation, Ice
return 0;
}
-Ice::Int BusInspectionUnit::performBusOperation(BasicOperation operation, const std::string &bus, const Ice::Current& c)
+Ice::Int BusInspectionUnit::performBusOperation(BasicOperation operation, const std::string& bus, const Ice::Current& c)
{
return 0;
}
@@ -87,7 +94,8 @@ bool BusInspectionUnit::isInRealTimeMode(const Ice::Current& c)
}
-std::string BusInspectionUnit::sendCommand(const std::string& command, Ice::Int device, const Ice::Current& c ) {
+std::string BusInspectionUnit::sendCommand(const std::string& command, Ice::Int device, const Ice::Current& c)
+{
return "";
}
@@ -95,7 +103,7 @@ std::string BusInspectionUnit::sendCommand(const std::string& command, Ice::Int
PropertyDefinitionsPtr BusInspectionUnit::createPropertyDefinitions()
{
return PropertyDefinitionsPtr(new BusInspectionUnitPropertyDefinitions(
- getConfigIdentifier()));
+ getConfigIdentifier()));
}
diff --git a/source/RobotAPI/components/units/BusInspectionUnit.h b/source/RobotAPI/components/units/BusInspectionUnit.h
index 668d80518f781b00a4ef73c3e255bb02870cc4ca..98a3dcaa788c7df5198b24b99cb45aea39d561e8 100644
--- a/source/RobotAPI/components/units/BusInspectionUnit.h
+++ b/source/RobotAPI/components/units/BusInspectionUnit.h
@@ -38,15 +38,15 @@ namespace armarx
* \brief
*/
class BusInspectionUnitPropertyDefinitions:
- public ComponentPropertyDefinitions
+ public ComponentPropertyDefinitions
{
- public:
- BusInspectionUnitPropertyDefinitions(std::string prefix) :
- ComponentPropertyDefinitions(prefix)
- {
- }
+ public:
+ BusInspectionUnitPropertyDefinitions(std::string prefix) :
+ ComponentPropertyDefinitions(prefix)
+ {
+ }
};
-
+
/**
* \class BusInspectionUnit
* \ingroup RobotAPI-SensorActorUnits
@@ -55,40 +55,43 @@ namespace armarx
* BusInspectionUnit is the base class of a unit that allows the introspection of a connected bus system.
*/
class BusInspectionUnit :
- virtual public BusInspectionInterface, virtual public Component
+ virtual public BusInspectionInterface, virtual public Component
+ {
+ public:
+ // inherited from Component
+ virtual std::string getDefaultName() const
{
- public:
- // inherited from Component
- virtual std::string getDefaultName() const { return "BusInspectionUnit"; }
- virtual void onInitComponent();
- virtual void onConnectComponent();
- virtual void onExitComponent();
-
- virtual void onInitBusInspectionUnit() = 0;
- virtual void onConnectBusInspectionUnit() = 0;
- virtual void onExitBusInspectionUnit() = 0;
-
- virtual BusNames getConfiguredBusses( const Ice::Current& c = ::Ice::Current());
- virtual BusInformation getBusInformation(const std::string &bus, const Ice::Current& c = ::Ice::Current());
- virtual DeviceNames getDevicesOnBus(const std::string &bus, const Ice::Current& c = ::Ice::Current());
- virtual DeviceInformation getDeviceInformation(const std::string &device, const Ice::Current& c = ::Ice::Current());
-
- virtual Ice::Int performDeviceOperation(BasicOperation operation, Ice::Int device, const Ice::Current& c = ::Ice::Current());
- virtual Ice::Int performBusOperation(BasicOperation operation, const std::string &bus, const Ice::Current& c = ::Ice::Current());
- virtual bool isInRealTimeMode(const Ice::Current& c = ::Ice::Current());
-
- virtual std::string sendCommand(const std::string &command, Ice::Int device, const Ice::Current& c = ::Ice::Current());
-
- /**
- * \see PropertyUser::createPropertyDefinitions()
- */
- virtual PropertyDefinitionsPtr createPropertyDefinitions();
-
- protected:
- std::string channel;
- BusInspectionInterfacePrx busInspectionValuePrx; // gets commands from hardware
-
- };
+ return "BusInspectionUnit";
+ }
+ virtual void onInitComponent();
+ virtual void onConnectComponent();
+ virtual void onExitComponent();
+
+ virtual void onInitBusInspectionUnit() = 0;
+ virtual void onConnectBusInspectionUnit() = 0;
+ virtual void onExitBusInspectionUnit() = 0;
+
+ virtual BusNames getConfiguredBusses(const Ice::Current& c = ::Ice::Current());
+ virtual BusInformation getBusInformation(const std::string& bus, const Ice::Current& c = ::Ice::Current());
+ virtual DeviceNames getDevicesOnBus(const std::string& bus, const Ice::Current& c = ::Ice::Current());
+ virtual DeviceInformation getDeviceInformation(const std::string& device, const Ice::Current& c = ::Ice::Current());
+
+ virtual Ice::Int performDeviceOperation(BasicOperation operation, Ice::Int device, const Ice::Current& c = ::Ice::Current());
+ virtual Ice::Int performBusOperation(BasicOperation operation, const std::string& bus, const Ice::Current& c = ::Ice::Current());
+ virtual bool isInRealTimeMode(const Ice::Current& c = ::Ice::Current());
+
+ virtual std::string sendCommand(const std::string& command, Ice::Int device, const Ice::Current& c = ::Ice::Current());
+
+ /**
+ * \see PropertyUser::createPropertyDefinitions()
+ */
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+ protected:
+ std::string channel;
+ BusInspectionInterfacePrx busInspectionValuePrx; // gets commands from hardware
+
+ };
}
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.cpp b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
index 85634e8f6d28ac6f15b127260ac40b9f20353ac2..2fa220da656ab2d002693284a68f4e4ab660f74e 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
@@ -56,10 +56,14 @@ void ForceTorqueObserver::setTopicName(std::string topicName)
void ForceTorqueObserver::onInitObserver()
{
- if(topicName.empty())
+ if (topicName.empty())
+ {
usingTopic(getProperty<std::string>("ForceTorqueTopicName").getValue());
+ }
else
+ {
usingTopic(topicName);
+ }
// register all checks
offerConditionCheck("updated", new ConditionCheckUpdated());
@@ -83,20 +87,20 @@ void ForceTorqueObserver::onConnectObserver()
PropertyDefinitionsPtr ForceTorqueObserver::createPropertyDefinitions()
{
return PropertyDefinitionsPtr(new ForceTorqueObserverPropertyDefinitions(
- getConfigIdentifier()));
+ getConfigIdentifier()));
}
-void ForceTorqueObserver::offerValue(std::string nodeName, const std::string &type, FramedDirectionBasePtr value, const DataFieldIdentifierPtr& id)
+void ForceTorqueObserver::offerValue(std::string nodeName, const std::string& type, FramedDirectionBasePtr value, const DataFieldIdentifierPtr& id)
{
FramedDirectionPtr vec = FramedDirectionPtr::dynamicCast(value);
- if(!existsChannel(id->channelName))
+ if (!existsChannel(id->channelName))
{
offerChannel(id->channelName, type + " vectors on specific parts of the robot.");
}
- if(!existsDataField(id->channelName, id->datafieldName))
+ if (!existsDataField(id->channelName, id->datafieldName))
{
offerDataFieldWithDefault(id->channelName, id->datafieldName, Variant(value), "3D vector for " + type + " of " + nodeName);
}
@@ -108,39 +112,44 @@ void ForceTorqueObserver::offerValue(std::string nodeName, const std::string &ty
// pod = plain old data
std::string pod_channelName = id->channelName + POD_SUFFIX;
- if(!existsChannel(pod_channelName) )
+
+ if (!existsChannel(pod_channelName))
{
offerChannel(pod_channelName, id->datafieldName + " on " + nodeName + " as plain old data (pod)");
}
+
offerOrUpdateDataField(pod_channelName, id->datafieldName + "_x", Variant(vec->x), type + " on X axis");
offerOrUpdateDataField(pod_channelName, id->datafieldName + "_y", Variant(vec->y), type + " on Y axis");
offerOrUpdateDataField(pod_channelName, id->datafieldName + "_z", Variant(vec->z), type + " on Z axis");
offerOrUpdateDataField(pod_channelName, id->datafieldName + "_frame", Variant(vec->frame), "Frame of " + value->frame);
}
-void armarx::ForceTorqueObserver::reportSensorValues(const std::string &sensorNodeName, const FramedDirectionBasePtr &forces, const FramedDirectionBasePtr &torques, const Ice::Current &)
+void armarx::ForceTorqueObserver::reportSensorValues(const std::string& sensorNodeName, const FramedDirectionBasePtr& forces, const FramedDirectionBasePtr& torques, const Ice::Current&)
{
ScopedLock lock(dataMutex);
try
{
DataFieldIdentifierPtr id;
- if(forces)
+
+ if (forces)
{
id = getForceDatafieldId(sensorNodeName, forces->frame);
std::string type = "forces";
offerValue(sensorNodeName, type, forces, id);
}
- if(torques)
+
+ if (torques)
{
id = getTorqueDatafieldId(sensorNodeName, torques->frame);
std::string type = "torques";
offerValue(sensorNodeName, type, torques, id);
}
+
updateChannel(id->channelName);
updateChannel(id->channelName + POD_SUFFIX);
}
- catch(std::exception&e)
+ catch (std::exception& e)
{
ARMARX_ERROR << "Reporting force torque for " << sensorNodeName << " failed! ";
handleExceptions();
@@ -148,53 +157,77 @@ void armarx::ForceTorqueObserver::reportSensorValues(const std::string &sensorNo
}
-DatafieldRefBasePtr armarx::ForceTorqueObserver::createNulledDatafield(const DatafieldRefBasePtr &forceTorqueDatafieldRef, const Ice::Current &)
+DatafieldRefBasePtr armarx::ForceTorqueObserver::createNulledDatafield(const DatafieldRefBasePtr& forceTorqueDatafieldRef, const Ice::Current&)
{
return createFilteredDatafield(new filters::OffsetFilter(), forceTorqueDatafieldRef);
}
-DatafieldRefBasePtr ForceTorqueObserver::getForceDatafield(const std::string &nodeName, const Ice::Current &)
+DatafieldRefBasePtr ForceTorqueObserver::getForceDatafield(const std::string& nodeName, const Ice::Current&)
{
auto id = getForceDatafieldId(nodeName, nodeName);
- if(!existsChannel(id->channelName))
+
+ if (!existsChannel(id->channelName))
+ {
throw UserException("No sensor for node '" + nodeName + "'available: channel " + id->channelName);
- if(!existsDataField(id->channelName, id->datafieldName))
+ }
+
+ if (!existsDataField(id->channelName, id->datafieldName))
+ {
throw UserException("No sensor for node '" + nodeName + "'available: datafield " + id->datafieldName);
+ }
return new DatafieldRef(this, id->channelName, id->datafieldName);
}
-DatafieldRefBasePtr ForceTorqueObserver::getTorqueDatafield(const std::string &nodeName, const Ice::Current &)
+DatafieldRefBasePtr ForceTorqueObserver::getTorqueDatafield(const std::string& nodeName, const Ice::Current&)
{
auto id = getTorqueDatafieldId(nodeName, nodeName);
- if(!existsChannel(id->channelName))
+
+ if (!existsChannel(id->channelName))
+ {
throw UserException("No sensor for node '" + nodeName + "'available: channel " + id->channelName);
- if(!existsDataField(id->channelName, id->datafieldName))
+ }
+
+ if (!existsDataField(id->channelName, id->datafieldName))
+ {
throw UserException("No sensor for node '" + nodeName + "'available: datafield " + id->datafieldName);
+ }
return new DatafieldRef(this, id->channelName, id->datafieldName);
}
-DataFieldIdentifierPtr ForceTorqueObserver::getForceDatafieldId(const std::string &nodeName, const std::string &frame)
+DataFieldIdentifierPtr ForceTorqueObserver::getForceDatafieldId(const std::string& nodeName, const std::string& frame)
{
std::string channelName;
- if(frame == nodeName)
+
+ if (frame == nodeName)
+ {
channelName = nodeName;
+ }
else
+ {
channelName = nodeName + "_" + frame;
+ }
+
std::string datafieldName = "forces";
DataFieldIdentifierPtr id = new DataFieldIdentifier(getName(), channelName, datafieldName);
return id;
}
-DataFieldIdentifierPtr ForceTorqueObserver::getTorqueDatafieldId(const std::string &nodeName, const std::string &frame)
+DataFieldIdentifierPtr ForceTorqueObserver::getTorqueDatafieldId(const std::string& nodeName, const std::string& frame)
{
std::string channelName;
- if(frame == nodeName)
+
+ if (frame == nodeName)
+ {
channelName = nodeName;
+ }
else
+ {
channelName = nodeName + "_" + frame;
+ }
+
std::string datafieldName = "torques";
DataFieldIdentifierPtr id = new DataFieldIdentifier(getName(), channelName, datafieldName);
return id;
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.h b/source/RobotAPI/components/units/ForceTorqueObserver.h
index d64ba77c16c918ff51d101d1df4a1d336c396f3c..8aa5dbd05cf2ebcd1b8ebe227c6fe70823811215 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.h
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.h
@@ -34,13 +34,13 @@ namespace armarx
* \brief
*/
class ForceTorqueObserverPropertyDefinitions:
- public ComponentPropertyDefinitions
+ public ComponentPropertyDefinitions
{
public:
ForceTorqueObserverPropertyDefinitions(std::string prefix):
ComponentPropertyDefinitions(prefix)
{
- defineRequiredProperty<std::string>("ForceTorqueTopicName","Name of the ForceTorqueUnit Topic");
+ defineRequiredProperty<std::string>("ForceTorqueTopicName", "Name of the ForceTorqueUnit Topic");
}
};
@@ -53,8 +53,8 @@ namespace armarx
* Available condition checks are: *updated*, *larger*, *equals*, *smaller* and *magnitudelarger*.
*/
class ForceTorqueObserver :
- virtual public Observer,
- virtual public ForceTorqueUnitObserverInterface
+ virtual public Observer,
+ virtual public ForceTorqueUnitObserverInterface
{
public:
ForceTorqueObserver();
@@ -62,11 +62,14 @@ namespace armarx
void setTopicName(std::string topicName);
// framework hooks
- virtual std::string getDefaultName() const { return "ForceTorqueUnitObserver"; }
+ virtual std::string getDefaultName() const
+ {
+ return "ForceTorqueUnitObserver";
+ }
void onInitObserver();
void onConnectObserver();
- virtual void reportSensorValues(const std::string &sensorNodeName, const FramedDirectionBasePtr &forces, const FramedDirectionBasePtr &torques, const Ice::Current &);
+ virtual void reportSensorValues(const std::string& sensorNodeName, const FramedDirectionBasePtr& forces, const FramedDirectionBasePtr& torques, const Ice::Current&);
/**
* @see PropertyUser::createPropertyDefinitions()
@@ -83,17 +86,17 @@ namespace armarx
std::string topicName;
- void offerValue(std::string nodeName, const std::string &type, FramedDirectionBasePtr value, const DataFieldIdentifierPtr& id);
+ void offerValue(std::string nodeName, const std::string& type, FramedDirectionBasePtr value, const DataFieldIdentifierPtr& id);
// ForceTorqueUnitObserverInterface interface
public:
- DatafieldRefBasePtr createNulledDatafield(const DatafieldRefBasePtr &forceTorqueDatafieldRef, const Ice::Current &);
+ DatafieldRefBasePtr createNulledDatafield(const DatafieldRefBasePtr& forceTorqueDatafieldRef, const Ice::Current&);
- DatafieldRefBasePtr getForceDatafield(const std::string &nodeName, const Ice::Current &);
- DatafieldRefBasePtr getTorqueDatafield(const std::string &nodeName, const Ice::Current &);
+ DatafieldRefBasePtr getForceDatafield(const std::string& nodeName, const Ice::Current&);
+ DatafieldRefBasePtr getTorqueDatafield(const std::string& nodeName, const Ice::Current&);
- DataFieldIdentifierPtr getForceDatafieldId(const std::string &nodeName, const std::string &frame);
- DataFieldIdentifierPtr getTorqueDatafieldId(const std::string &nodeName, const std::string &frame);
+ DataFieldIdentifierPtr getForceDatafieldId(const std::string& nodeName, const std::string& frame);
+ DataFieldIdentifierPtr getTorqueDatafieldId(const std::string& nodeName, const std::string& frame);
};
}
diff --git a/source/RobotAPI/components/units/ForceTorqueUnit.h b/source/RobotAPI/components/units/ForceTorqueUnit.h
index fbe8b41b701a59f3b468ef7ba05fabebd28ddd18..2ec0ae0c4d7eed1c3210fede45a023322131fc0a 100644
--- a/source/RobotAPI/components/units/ForceTorqueUnit.h
+++ b/source/RobotAPI/components/units/ForceTorqueUnit.h
@@ -41,18 +41,18 @@ namespace armarx
*/
class ForceTorqueUnitPropertyDefinitions : public ComponentPropertyDefinitions
{
- public:
- ForceTorqueUnitPropertyDefinitions(std::string prefix) :
- ComponentPropertyDefinitions(prefix)
- {
- defineRequiredProperty<std::string>("AgentName", "Name of the agent for which the sensor values are provided");
- defineOptionalProperty<std::string>("ForceTorqueTopicName","ForceTorqueValues", "Name of the topic on which the sensor values are provided");
+ public:
+ ForceTorqueUnitPropertyDefinitions(std::string prefix) :
+ ComponentPropertyDefinitions(prefix)
+ {
+ defineRequiredProperty<std::string>("AgentName", "Name of the agent for which the sensor values are provided");
+ defineOptionalProperty<std::string>("ForceTorqueTopicName", "ForceTorqueValues", "Name of the topic on which the sensor values are provided");
- // No required properties
- }
+ // No required properties
+ }
};
-
+
/**
* \defgroup Component-ForceTorqueUnit ForceTorqueUnit
* \ingroup RobotAPI-SensorActorUnits
@@ -72,22 +72,25 @@ namespace armarx
virtual public ForceTorqueUnitInterface,
virtual public SensorActorUnit
{
- public:
- virtual std::string getDefaultName() const { return "ForceTorqueUnit"; }
-
- virtual void onInitComponent();
- virtual void onConnectComponent();
- virtual void onExitComponent();
-
- virtual void onInitForceTorqueUnit() = 0;
- virtual void onStartForceTorqueUnit() = 0;
- virtual void onExitForceTorqueUnit() = 0;
-
- virtual PropertyDefinitionsPtr createPropertyDefinitions();
-
- protected:
- ForceTorqueUnitListenerPrx listenerPrx;
- std::string listenerName;
+ public:
+ virtual std::string getDefaultName() const
+ {
+ return "ForceTorqueUnit";
+ }
+
+ virtual void onInitComponent();
+ virtual void onConnectComponent();
+ virtual void onExitComponent();
+
+ virtual void onInitForceTorqueUnit() = 0;
+ virtual void onStartForceTorqueUnit() = 0;
+ virtual void onExitForceTorqueUnit() = 0;
+
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+ protected:
+ ForceTorqueUnitListenerPrx listenerPrx;
+ std::string listenerName;
};
}
diff --git a/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp b/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp
index 6b0330f628e0d0b94ee2d0f3c99b7b0aeebcdb72..e0a44e3f16e29f25a5322ec6b247c2e9ba36a152 100644
--- a/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp
@@ -39,7 +39,7 @@ void ForceTorqueUnitSimulation::onInitForceTorqueUnit()
//{
// forces[*s] = new armarx::FramedDirection(Eigen::Vector3f(0, 0, 0), *s);
// torques[*s] = new armarx::FramedDirection(Eigen::Vector3f(0, 0, 0), *s);
- // }
+ // }
forces = new armarx::FramedDirection(Eigen::Vector3f(0, 0, 0), sensorName, getProperty<std::string>("AgentName").getValue());
torques = new armarx::FramedDirection(Eigen::Vector3f(0, 0, 0), sensorName, getProperty<std::string>("AgentName").getValue());
@@ -67,12 +67,12 @@ void ForceTorqueUnitSimulation::simulationFunction()
//listenerPrx->reportSensorValues(torques);
}
-void ForceTorqueUnitSimulation::setOffset(const FramedDirectionBasePtr &forceOffsets, const FramedDirectionBasePtr &torqueOffsets, const Ice::Current &c)
+void ForceTorqueUnitSimulation::setOffset(const FramedDirectionBasePtr& forceOffsets, const FramedDirectionBasePtr& torqueOffsets, const Ice::Current& c)
{
// Ignore
}
-void ForceTorqueUnitSimulation::setToNull(const Ice::Current &c)
+void ForceTorqueUnitSimulation::setToNull(const Ice::Current& c)
{
// Ignore
}
diff --git a/source/RobotAPI/components/units/ForceTorqueUnitSimulation.h b/source/RobotAPI/components/units/ForceTorqueUnitSimulation.h
index 12c499a6d3057355c5e5e470231a67dfcb9776e9..cc4505a6cf781a0d1f5cd5dbb4170ed6e4d0e1a7 100644
--- a/source/RobotAPI/components/units/ForceTorqueUnitSimulation.h
+++ b/source/RobotAPI/components/units/ForceTorqueUnitSimulation.h
@@ -40,7 +40,7 @@ namespace armarx
* \brief
*/
class ForceTorqueUnitSimulationPropertyDefinitions :
- public ForceTorqueUnitPropertyDefinitions
+ public ForceTorqueUnitPropertyDefinitions
{
public:
ForceTorqueUnitSimulationPropertyDefinitions(std::string prefix) :
@@ -77,7 +77,7 @@ namespace armarx
/**
* \warning Not implemented yet
*/
- virtual void setOffset(const FramedDirectionBasePtr &forceOffsets, const FramedDirectionBasePtr &torqueOffsets, const Ice::Current& c = ::Ice::Current());
+ virtual void setOffset(const FramedDirectionBasePtr& forceOffsets, const FramedDirectionBasePtr& torqueOffsets, const Ice::Current& c = ::Ice::Current());
/**
* \warning Not implemented yet
diff --git a/source/RobotAPI/components/units/HandUnit.cpp b/source/RobotAPI/components/units/HandUnit.cpp
index 73d02c74a9a5a1329a00af40be6187e4559159b0..b30f96136afd126e2f7b90e7b5ef4b0c4aae6b31 100644
--- a/source/RobotAPI/components/units/HandUnit.cpp
+++ b/source/RobotAPI/components/units/HandUnit.cpp
@@ -50,37 +50,47 @@ void HandUnit::onInitComponent()
{
robot = VirtualRobot::RobotIO::loadRobot(endeffectorFile, VirtualRobot::RobotIO::eStructure);
}
- catch(VirtualRobot::VirtualRobotException& e)
+ catch (VirtualRobot::VirtualRobotException& e)
{
throw UserException(e.what());
}
- if(endeffectorName == "")
+ if (endeffectorName == "")
{
throw UserException("EndeffectorName not defined");
}
- if(!robot->hasEndEffector(endeffectorName))
+
+ if (!robot->hasEndEffector(endeffectorName))
{
throw UserException("Robot does not contain an endeffector with name: " + endeffectorName);
}
+
eef = robot->getEndEffector(endeffectorName);
robotName = robot->getName();
+
if (eef->getTcp())
+ {
tcpName = robot->getEndEffector(endeffectorName)->getTcp()->getName();
+ }
else
+ {
throw UserException("Endeffector without TCP: " + endeffectorName);
+ }
// get all joints
std::vector<EndEffectorActorPtr> actors;
eef->getActors(actors);
- for (size_t i=0;i<actors.size();i++)
+
+ for (size_t i = 0; i < actors.size(); i++)
{
EndEffectorActorPtr a = actors[i];
std::vector<EndEffectorActor::ActorDefinition> ads;// = a->getDefinition();
- for (size_t j=0;j<ads.size();j++)
+
+ for (size_t j = 0; j < ads.size(); j++)
{
EndEffectorActor::ActorDefinition ad = ads[j];
- if (ad.directionAndSpeed!=0 && ad.robotNode)
+
+ if (ad.directionAndSpeed != 0 && ad.robotNode)
{
handJoints[ad.robotNode->getName()] = ad.directionAndSpeed;
}
@@ -91,7 +101,8 @@ void HandUnit::onInitComponent()
shapeNames = new SingleTypeVariantList(VariantType::String);
std::vector<std::string>::const_iterator iter = endeffectorPreshapes.begin();
- while(iter != endeffectorPreshapes.end())
+
+ while (iter != endeffectorPreshapes.end())
{
Variant currentPreshape;
currentPreshape.setString(*iter);
@@ -138,34 +149,36 @@ SingleTypeVariantListBasePtr HandUnit::getShapeNames(const Ice::Current& c)
PropertyDefinitionsPtr HandUnit::createPropertyDefinitions()
{
return PropertyDefinitionsPtr(new HandUnitPropertyDefinitions(
- getConfigIdentifier()));
+ getConfigIdentifier()));
}
-NameValueMap HandUnit::getShapeJointValues(const std::string &shapeName, const Ice::Current &)
+NameValueMap HandUnit::getShapeJointValues(const std::string& shapeName, const Ice::Current&)
{
EndEffectorPtr efp = robot->getEndEffector(getProperty<std::string>("EndeffectorName").getValue());
RobotConfigPtr rc = efp->getPreshape(shapeName);
return rc->getRobotNodeJointValueMap();
}
-NameValueMap HandUnit::getCurrentJointValues(const Ice::Current &c)
+NameValueMap HandUnit::getCurrentJointValues(const Ice::Current& c)
{
NameValueMap result;
+
for (auto j : handJoints)
{
result[j.first] = 0.0f;
}
+
return result;
}
-void HandUnit::setObjectGrasped(const std::string &objectName, const Ice::Current &)
+void HandUnit::setObjectGrasped(const std::string& objectName, const Ice::Current&)
{
- ARMARX_INFO << "Object grasped " << objectName << flush;
- graspedObject = objectName;
+ ARMARX_INFO << "Object grasped " << objectName << flush;
+ graspedObject = objectName;
}
-void HandUnit::setObjectReleased(const std::string &objectName, const Ice::Current &)
+void HandUnit::setObjectReleased(const std::string& objectName, const Ice::Current&)
{
ARMARX_INFO << "Object released " << objectName << flush;
graspedObject = "";
@@ -173,13 +186,13 @@ void HandUnit::setObjectReleased(const std::string &objectName, const Ice::Curre
-std::string armarx::HandUnit::getHandName(const Ice::Current &)
+std::string armarx::HandUnit::getHandName(const Ice::Current&)
{
return eef->getName();
}
-void armarx::HandUnit::setJointAngles(const armarx::NameValueMap& targetJointAngles, const Ice::Current & c)
+void armarx::HandUnit::setJointAngles(const armarx::NameValueMap& targetJointAngles, const Ice::Current& c)
{
}
diff --git a/source/RobotAPI/components/units/HandUnit.h b/source/RobotAPI/components/units/HandUnit.h
index 6bc729ddc7442585926aeaeffe4c73cdcbcd6581..1fd2c51bfa6d13018024dae430a9346d0e58bb12 100644
--- a/source/RobotAPI/components/units/HandUnit.h
+++ b/source/RobotAPI/components/units/HandUnit.h
@@ -47,7 +47,7 @@ namespace armarx
* \brief Defines all necessary properties for armarx::HandUnit
*/
class HandUnitPropertyDefinitions:
- public ComponentPropertyDefinitions
+ public ComponentPropertyDefinitions
{
public:
HandUnitPropertyDefinitions(std::string prefix):
@@ -124,12 +124,12 @@ namespace armarx
*/
virtual SingleTypeVariantListBasePtr getShapeNames(const Ice::Current& c = ::Ice::Current());
- NameValueMap getShapeJointValues(const std::string &shapeName, const Ice::Current & c = ::Ice::Current());
- virtual NameValueMap getCurrentJointValues(const Ice::Current & c = ::Ice::Current());
+ NameValueMap getShapeJointValues(const std::string& shapeName, const Ice::Current& c = ::Ice::Current());
+ virtual NameValueMap getCurrentJointValues(const Ice::Current& c = ::Ice::Current());
- void setObjectGrasped(const std::string& objectName, const Ice::Current &);
- void setObjectReleased(const std::string& objectName, const Ice::Current &);
- void setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current &);
+ void setObjectGrasped(const std::string& objectName, const Ice::Current&);
+ void setObjectReleased(const std::string& objectName, const Ice::Current&);
+ void setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current&);
/**
* \see armarx::PropertyUser::createPropertyDefinitions()
@@ -165,7 +165,7 @@ namespace armarx
// HandUnitInterface interface
public:
- std::string getHandName(const Ice::Current &);
+ std::string getHandName(const Ice::Current&);
};
diff --git a/source/RobotAPI/components/units/HandUnitSimulation.cpp b/source/RobotAPI/components/units/HandUnitSimulation.cpp
index c6e8e534b3f34b0df3625616cac62cf0a24ed31f..96adc2d15d3ab7003a340ac74de95bd23356be9d 100644
--- a/source/RobotAPI/components/units/HandUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/HandUnitSimulation.cpp
@@ -56,7 +56,7 @@ void HandUnitSimulation::setShape(const std::string& shapeName, const Ice::Curre
-void armarx::HandUnitSimulation::setJointAngles(const NameValueMap& jointAngles, const Ice::Current &)
+void armarx::HandUnitSimulation::setJointAngles(const NameValueMap& jointAngles, const Ice::Current&)
{
}
diff --git a/source/RobotAPI/components/units/HandUnitSimulation.h b/source/RobotAPI/components/units/HandUnitSimulation.h
index 74d5aefa29d3b5cf6c3307355cbd29fc251043fd..84fa0dd706d6dabda20f2d782747fa3f8b24ca29 100644
--- a/source/RobotAPI/components/units/HandUnitSimulation.h
+++ b/source/RobotAPI/components/units/HandUnitSimulation.h
@@ -33,14 +33,14 @@ namespace armarx
* \brief Defines all necessary properties for armarx::HandUnitSimulation
*/
class HandUnitSimulationPropertyDefinitions:
- public ComponentPropertyDefinitions
+ public ComponentPropertyDefinitions
{
public:
HandUnitSimulationPropertyDefinitions(std::string prefix):
ComponentPropertyDefinitions(prefix)
{
- defineRequiredProperty<std::string>("RobotFileName","VirtualRobot XML file in which the endeffector is is stored.");
- defineRequiredProperty<std::string>("EndeffectorName","Name of the endeffector as stored in the XML file (will publish values on EndeffectorName + 'State')");
+ defineRequiredProperty<std::string>("RobotFileName", "VirtualRobot XML file in which the endeffector is is stored.");
+ defineRequiredProperty<std::string>("EndeffectorName", "Name of the endeffector as stored in the XML file (will publish values on EndeffectorName + 'State')");
}
};
diff --git a/source/RobotAPI/components/units/HapticObserver.cpp b/source/RobotAPI/components/units/HapticObserver.cpp
index 3b8cd1fac82619900a15502f85ff8b6d4593bca7..569894080e054db4adcfd7e6c8639341dc21996a 100644
--- a/source/RobotAPI/components/units/HapticObserver.cpp
+++ b/source/RobotAPI/components/units/HapticObserver.cpp
@@ -25,10 +25,14 @@ void HapticObserver::setTopicName(std::string topicName)
void HapticObserver::onInitObserver()
{
- if(topicName.empty())
+ if (topicName.empty())
+ {
usingTopic(getProperty<std::string>("HapticTopicName").getValue());
+ }
else
+ {
usingTopic(topicName);
+ }
// register all checks
offerConditionCheck("updated", new ConditionCheckUpdated());
@@ -57,7 +61,8 @@ void HapticObserver::reportSensorValues(const std::string& device, const std::st
float max = eigenMatrix.maxCoeff();
float mean = eigenMatrix.mean();
std::string channelName = name;
- if(!existsChannel(channelName))
+
+ if (!existsChannel(channelName))
{
offerChannel(channelName, "Haptic data");
offerDataFieldWithDefault(channelName, "device", Variant(device), "Device of the tactile sensor");
@@ -78,6 +83,7 @@ void HapticObserver::reportSensorValues(const std::string& device, const std::st
setDataField(channelName, "mean", Variant(mean));
setDataField(channelName, "timestamp", timestampPtr);
}
+
/*if(statistics.count(device) > 0)
{
statistics.at(device).add(timestamp->timestamp);
diff --git a/source/RobotAPI/components/units/HapticObserver.h b/source/RobotAPI/components/units/HapticObserver.h
index 6a73f77c9bbf43ae5c0eb2fdab2009899d2a2b28..160d1cf81192edb6db02cc1c6596c5a261b1ef10 100644
--- a/source/RobotAPI/components/units/HapticObserver.h
+++ b/source/RobotAPI/components/units/HapticObserver.h
@@ -37,7 +37,7 @@ namespace armarx
* \brief
*/
class HapticObserverPropertyDefinitions:
- public ComponentPropertyDefinitions
+ public ComponentPropertyDefinitions
{
public:
HapticObserverPropertyDefinitions(std::string prefix):
@@ -64,7 +64,8 @@ namespace armarx
void add(long timestamp)
{
long delta = timestamp - lastTimestamp;
- if(deltas.size() < count)
+
+ if (deltas.size() < count)
{
deltas.push_back(delta);
}
@@ -73,6 +74,7 @@ namespace armarx
deltas.at(pos) = delta;
pos = (pos + 1) % count;
}
+
lastTimestamp = timestamp;
}
@@ -88,15 +90,18 @@ namespace armarx
long average()
{
- if(deltas.size() == 0)
+ if (deltas.size() == 0)
{
return 0;
}
+
long sum = 0;
- for(std::vector<long>::iterator it = deltas.begin(); it != deltas.end(); ++it)
+
+ for (std::vector<long>::iterator it = deltas.begin(); it != deltas.end(); ++it)
{
sum += *it;
}
+
return sum / deltas.size();
}
@@ -116,8 +121,8 @@ namespace armarx
* Available condition checks are: *updated*, *larger*, *equals* and *smaller*.
*/
class HapticObserver :
- virtual public Observer,
- virtual public HapticUnitObserverInterface
+ virtual public Observer,
+ virtual public HapticUnitObserverInterface
{
public:
HapticObserver();
@@ -125,7 +130,10 @@ namespace armarx
void setTopicName(std::string topicName);
// framework hooks
- virtual std::string getDefaultName() const { return "HapticUnitObserver"; }
+ virtual std::string getDefaultName() const
+ {
+ return "HapticUnitObserver";
+ }
void onInitObserver();
void onConnectObserver();
void onExitObserver();
diff --git a/source/RobotAPI/components/units/HapticUnit.h b/source/RobotAPI/components/units/HapticUnit.h
index e3aefb2e7eca0579541804d5faad3fc64cc8acf9..d3daeb10a1cd26c227bad58336e1ff0d233158b7 100644
--- a/source/RobotAPI/components/units/HapticUnit.h
+++ b/source/RobotAPI/components/units/HapticUnit.h
@@ -40,14 +40,14 @@ namespace armarx
*/
class HapticUnitPropertyDefinitions : public ComponentPropertyDefinitions
{
- public:
- HapticUnitPropertyDefinitions(std::string prefix) :
- ComponentPropertyDefinitions(prefix)
- {
- defineOptionalProperty<std::string>("HapticTopicName", "HapticValues", "Name of the Haptic Topic.");
- }
+ public:
+ HapticUnitPropertyDefinitions(std::string prefix) :
+ ComponentPropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<std::string>("HapticTopicName", "HapticValues", "Name of the Haptic Topic.");
+ }
};
-
+
/**
* \defgroup Component-HapticUnit HapticUnit
* \ingroup RobotAPI-SensorActorUnits
@@ -62,21 +62,24 @@ namespace armarx
virtual public HapticUnitInterface,
virtual public SensorActorUnit
{
- public:
- virtual std::string getDefaultName() const { return "HapticUnit"; }
-
- virtual void onInitComponent();
- virtual void onConnectComponent();
- virtual void onExitComponent();
-
- virtual void onInitHapticUnit() = 0;
- virtual void onStartHapticUnit() = 0;
- virtual void onExitHapticUnit() = 0;
-
- virtual PropertyDefinitionsPtr createPropertyDefinitions();
-
- protected:
- HapticUnitListenerPrx hapticTopicPrx;
+ public:
+ virtual std::string getDefaultName() const
+ {
+ return "HapticUnit";
+ }
+
+ virtual void onInitComponent();
+ virtual void onConnectComponent();
+ virtual void onExitComponent();
+
+ virtual void onInitHapticUnit() = 0;
+ virtual void onStartHapticUnit() = 0;
+ virtual void onExitHapticUnit() = 0;
+
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+ protected:
+ HapticUnitListenerPrx hapticTopicPrx;
};
}
diff --git a/source/RobotAPI/components/units/HeadIKUnit.cpp b/source/RobotAPI/components/units/HeadIKUnit.cpp
index acba6b5dadb484cc1b581c92863133613ee49b0a..653a3fd7a27c418e6e9c8f01322c51ab7541da3f 100644
--- a/source/RobotAPI/components/units/HeadIKUnit.cpp
+++ b/source/RobotAPI/components/units/HeadIKUnit.cpp
@@ -53,7 +53,12 @@ namespace armarx
//localRobot->setConfig(robotSnapshot->getConfig());
requested = false;
- if (execTask) execTask->stop();
+
+ if (execTask)
+ {
+ execTask->stop();
+ }
+
execTask = new PeriodicTask<HeadIKUnit>(this, &HeadIKUnit::periodicExec, cycleTime, false, "HeadIKCalculator");
execTask->setDelayWarningTolerance(300);
execTask->start();
@@ -63,13 +68,19 @@ namespace armarx
void HeadIKUnit::onDisconnectComponent()
{
release();
+
//ScopedLock lock(accessMutex);
- if(drawer)
+ if (drawer)
+ {
drawer->removeSphereDebugLayerVisu("HeadViewTarget");
+ }
- if (execTask) execTask->stop();
+ if (execTask)
+ {
+ execTask->stop();
+ }
}
void HeadIKUnit::onExitComponent()
@@ -78,12 +89,16 @@ namespace armarx
- void HeadIKUnit::setCycleTime(Ice::Int milliseconds, const Ice::Current & c)
+ void HeadIKUnit::setCycleTime(Ice::Int milliseconds, const Ice::Current& c)
{
ScopedLock lock(accessMutex);
cycleTime = milliseconds;
- if (execTask) execTask->changeInterval(cycleTime);
+
+ if (execTask)
+ {
+ execTask->changeInterval(cycleTime);
+ }
}
@@ -97,11 +112,12 @@ namespace armarx
this->targetPosition->z = targetPosition->z;
this->targetPosition->frame = targetPosition->frame;
FramedPositionPtr globalTarget = FramedPositionPtr::dynamicCast(targetPosition)->toGlobal(robotStateComponentPrx->getSynchronizedRobot());
- if(drawer)
+
+ if (drawer)
{
drawer->setSphereDebugLayerVisu("HeadViewTarget",
globalTarget,
- DrawColor{1,0,0,0.7},
+ DrawColor {1, 0, 0, 0.7},
15);
@@ -115,30 +131,33 @@ namespace armarx
- void HeadIKUnit::init(const Ice::Current & c)
+ void HeadIKUnit::init(const Ice::Current& c)
{
}
- void HeadIKUnit::start(const Ice::Current & c)
+ void HeadIKUnit::start(const Ice::Current& c)
{
}
- void HeadIKUnit::stop(const Ice::Current & c)
+ void HeadIKUnit::stop(const Ice::Current& c)
{
}
- UnitExecutionState HeadIKUnit::getExecutionState(const Ice::Current & c)
+ UnitExecutionState HeadIKUnit::getExecutionState(const Ice::Current& c)
{
switch (getState())
{
case eManagedIceObjectStarted:
return eUnitStarted;
+
case eManagedIceObjectInitialized:
case eManagedIceObjectStarting:
return eUnitInitialized;
+
case eManagedIceObjectExiting:
case eManagedIceObjectExited:
return eUnitStopped;
+
default:
return eUnitConstructed;
}
@@ -147,13 +166,18 @@ namespace armarx
- void HeadIKUnit::request(const Ice::Current & c)
+ void HeadIKUnit::request(const Ice::Current& c)
{
ScopedLock lock(accessMutex);
requested = true;
ARMARX_IMPORTANT << "Requesting HeadIKUnit";
- if(execTask) execTask->stop();
+
+ if (execTask)
+ {
+ execTask->stop();
+ }
+
execTask = new PeriodicTask<HeadIKUnit>(this, &HeadIKUnit::periodicExec, cycleTime, false, "TCPVelocityCalculator");
execTask->start();
ARMARX_IMPORTANT << "Requested HeadIKUnit";
@@ -162,15 +186,17 @@ namespace armarx
- void HeadIKUnit::release(const Ice::Current & c)
+ void HeadIKUnit::release(const Ice::Current& c)
{
ScopedLock lock(accessMutex);
ARMARX_INFO << "Releasing HeadIKUnit";
requested = false;
- if(drawer)
+ if (drawer)
+ {
drawer->removeSphereDebugLayerVisu("HeadViewTarget");
+ }
// why do we stop the kin unit?
/*try
@@ -194,6 +220,7 @@ namespace armarx
{
ScopedTryLock lock(accessMutex);
+
if (lock.owns_lock())
{
doCalculation = requested && newTargetSet;
@@ -209,31 +236,37 @@ namespace armarx
if (doCalculation)
{
ScopedLock lock(accessMutex);
- RemoteRobot::synchronizeLocalClone(localRobot,robotStateComponentPrx);
+ RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponentPrx);
//VirtualRobot::RobotPtr robotSnapshot(new RemoteRobot(remoteRobotPrx));
//localRobot->setConfig(robotSnapshot->getConfig());
VirtualRobot::RobotNodeSetPtr kinematicChain = localRobot->getRobotNodeSet(robotNodeSetName);
VirtualRobot::RobotNodePrismaticPtr virtualJoint;
- for (unsigned int i=0; i<kinematicChain->getSize(); i++)
+
+ for (unsigned int i = 0; i < kinematicChain->getSize(); i++)
{
if (kinematicChain->getNode(i)->getName().compare("VirtualCentralGaze") == 0)
{
virtualJoint = boost::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(kinematicChain->getNode(i));
}
}
+
ARMARX_DEBUG << deactivateSpam(5) << VAROUT(virtualJoint->getName()) << " " << VAROUT(kinematicChain->getName());
VirtualRobot::GazeIK ikSolver(kinematicChain, virtualJoint);
ikSolver.enableJointLimitAvoidance(true);
ikSolver.setup(10, 30, 20);
-// ikSolver.setVerbose(true);
+ // ikSolver.setVerbose(true);
auto globalPos = targetPosition->toGlobal(localRobot);
ARMARX_DEBUG << "Calculating IK for target position " << globalPos->output();
auto start = IceUtil::Time::now();
bool solutionFound = ikSolver.solve(globalPos->toEigen());
- auto duration = (IceUtil::Time::now()-start);
- if(duration.toMilliSeconds() > 500)
+ auto duration = (IceUtil::Time::now() - start);
+
+ if (duration.toMilliSeconds() > 500)
+ {
ARMARX_INFO << "Calculating Gaze IK took " << duration.toMilliSeconds() << " ms";
+ }
+
if (!solutionFound)
{
ARMARX_WARNING << "IKSolver found no solution!";
@@ -242,18 +275,19 @@ namespace armarx
{
ARMARX_DEBUG << "Solution found";
- if(drawer && localRobot->hasRobotNode("Cameras"))
+ if (drawer && localRobot->hasRobotNode("Cameras"))
{
Vector3Ptr startPos = new Vector3(localRobot->getRobotNode("Cameras")->getGlobalPose());
drawer->setSphereDebugLayerVisu("HeadViewTarget",
startPos,
- DrawColor{0,1,1,0.2},
+ DrawColor {0, 1, 1, 0.2},
17);
}
NameValueMap targetJointAngles;
NameControlModeMap controlModes;
+
for (int i = 0; i < (signed int)kinematicChain->getSize(); i++)
{
if (kinematicChain->getNode(i)->getName().compare("VirtualCentralGaze") != 0)
@@ -261,14 +295,17 @@ namespace armarx
targetJointAngles[kinematicChain->getNode(i)->getName()] = kinematicChain->getNode(i)->getJointValue();
controlModes[kinematicChain->getNode(i)->getName()] = ePositionControl;
}
+
ARMARX_DEBUG << kinematicChain->getNode(i)->getName() << ": " << kinematicChain->getNode(i)->getJointValue();
}
+
try
{
kinematicUnitPrx->switchControlMode(controlModes);
ARMARX_DEBUG << "setting new joint angles" << targetJointAngles;
kinematicUnitPrx->setJointAngles(targetJointAngles);
- } catch (...)
+ }
+ catch (...)
{
ARMARX_IMPORTANT << "setJointAngles failed";
}
diff --git a/source/RobotAPI/components/units/HeadIKUnit.h b/source/RobotAPI/components/units/HeadIKUnit.h
index 81dcee3eef24784aca6922bd032f0d208b605208..c9f137bb7781b59c860a76efee8629767cf934db 100644
--- a/source/RobotAPI/components/units/HeadIKUnit.h
+++ b/source/RobotAPI/components/units/HeadIKUnit.h
@@ -40,14 +40,14 @@ namespace armarx
* \brief
*/
class HeadIKUnitPropertyDefinitions:
- public ComponentPropertyDefinitions
+ public ComponentPropertyDefinitions
{
public:
HeadIKUnitPropertyDefinitions(std::string prefix):
ComponentPropertyDefinitions(prefix)
{
- defineRequiredProperty<std::string>("KinematicUnitName","Name of the KinematicUnit Proxy");
- defineOptionalProperty<std::string>("RobotStateTopicName","RobotState","Name of the RobotComponent State topic.");
+ defineRequiredProperty<std::string>("KinematicUnitName", "Name of the KinematicUnit Proxy");
+ defineOptionalProperty<std::string>("RobotStateTopicName", "RobotState", "Name of the RobotComponent State topic.");
defineOptionalProperty<int>("CycleTime", 30, "Cycle time of the tcp control in ms");
}
};
@@ -70,21 +70,24 @@ namespace armarx
void onConnectComponent();
void onDisconnectComponent();
void onExitComponent();
- std::string getDefaultName() const{ return "HeadIKUnit"; }
+ std::string getDefaultName() const
+ {
+ return "HeadIKUnit";
+ }
// HeadIKUnitInterface interface
- void setCycleTime(Ice::Int milliseconds, const Ice::Current &c = Ice::Current());
- void setHeadTarget(const std::string& robotNodeSetName, const FramedPositionBasePtr &targetPosition, const Ice::Current &c = Ice::Current());
+ void setCycleTime(Ice::Int milliseconds, const Ice::Current& c = Ice::Current());
+ void setHeadTarget(const std::string& robotNodeSetName, const FramedPositionBasePtr& targetPosition, const Ice::Current& c = Ice::Current());
// UnitExecutionManagementInterface interface
- void init(const Ice::Current &c = Ice::Current());
- void start(const Ice::Current &c = Ice::Current());
- void stop(const Ice::Current &c = Ice::Current());
- UnitExecutionState getExecutionState(const Ice::Current &c = Ice::Current());
+ void init(const Ice::Current& c = Ice::Current());
+ void start(const Ice::Current& c = Ice::Current());
+ void stop(const Ice::Current& c = Ice::Current());
+ UnitExecutionState getExecutionState(const Ice::Current& c = Ice::Current());
// UnitResourceManagementInterface interface
- void request(const Ice::Current &c = Ice::Current());
- void release(const Ice::Current &c = Ice::Current());
+ void request(const Ice::Current& c = Ice::Current());
+ void release(const Ice::Current& c = Ice::Current());
// PropertyUser interface
PropertyDefinitionsPtr createPropertyDefinitions();
diff --git a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
index 9eeb61928b79f5c812344dd4d2d636b02b5ac030..8feff3d67002613b2b5269b3671182a5aed60bc1 100644
--- a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
+++ b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
@@ -41,7 +41,7 @@ void InertialMeasurementUnitObserver::onExitObserver()
debugDrawerPrx->removeLineVisu("IMU", "acceleration");
}
-void InertialMeasurementUnitObserver::reportSensorValues(const std::string &device, const std::string &name, const IMUData &values, const TimestampBasePtr ×tamp, const Ice::Current &c)
+void InertialMeasurementUnitObserver::reportSensorValues(const std::string& device, const std::string& name, const IMUData& values, const TimestampBasePtr& timestamp, const Ice::Current& c)
{
ScopedLock lock(dataMutex);
@@ -52,9 +52,10 @@ void InertialMeasurementUnitObserver::reportSensorValues(const std::string &devi
Vector3Ptr magneticRotation = new Vector3(values.magneticRotation.at(0), values.magneticRotation.at(1), values.magneticRotation.at(2));
QuaternionPtr orientationQuaternion = new Quaternion(values.orientationQuaternion.at(0), values.orientationQuaternion.at(1), values.orientationQuaternion.at(2), values.orientationQuaternion.at(3));
- if(!existsChannel(device))
+ if (!existsChannel(device))
{
- offerChannel(device, "IMU data");\
+ offerChannel(device, "IMU data");
+ \
//todo remove
offerDataFieldWithDefault(device, "name", Variant(name), "Name of the IMU sensor");
offerDataFieldWithDefault(device, "acceleration", acceleration, "acceleration values");
@@ -72,6 +73,7 @@ void InertialMeasurementUnitObserver::reportSensorValues(const std::string &devi
setDataField(device, "orientationQuaternion", orientationQuaternion);
setDataField(device, "timestamp", timestampPtr);
}
+
updateChannel(device);
Eigen::Vector3f zero;
diff --git a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
index a19daa9851108c60a63d991c61cc62bf24cdd5e7..26bee065be5154cdc9bfb6ecd4f67dee3040efa2 100644
--- a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
+++ b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
@@ -35,7 +35,7 @@ namespace armarx
* \brief
*/
class InertialMeasurementUnitObserverPropertyDefinitions:
- public ComponentPropertyDefinitions
+ public ComponentPropertyDefinitions
{
public:
InertialMeasurementUnitObserverPropertyDefinitions(std::string prefix):
@@ -56,18 +56,21 @@ namespace armarx
* Available condition checks are: *updated*, *larger*, *equals* and *smaller*.
*/
class InertialMeasurementUnitObserver :
- virtual public Observer,
- virtual public InertialMeasurementUnitObserverInterface
+ virtual public Observer,
+ virtual public InertialMeasurementUnitObserverInterface
{
public:
- InertialMeasurementUnitObserver(){}
+ InertialMeasurementUnitObserver() {}
- virtual std::string getDefaultName() const { return "InertialMeasurementUnitObserver"; }
+ virtual std::string getDefaultName() const
+ {
+ return "InertialMeasurementUnitObserver";
+ }
virtual void onInitObserver();
virtual void onConnectObserver();
virtual void onExitObserver();
- void reportSensorValues(const std::string &device, const std::string &name, const IMUData &values, const TimestampBasePtr ×tamp, const Ice::Current &c = ::Ice::Current());
+ void reportSensorValues(const std::string& device, const std::string& name, const IMUData& values, const TimestampBasePtr& timestamp, const Ice::Current& c = ::Ice::Current());
/**
* @see PropertyUser::createPropertyDefinitions()
@@ -75,9 +78,9 @@ namespace armarx
virtual PropertyDefinitionsPtr createPropertyDefinitions();
- private:
- Mutex dataMutex;
- DebugDrawerInterfacePrx debugDrawerPrx;
+ private:
+ Mutex dataMutex;
+ DebugDrawerInterfacePrx debugDrawerPrx;
};
diff --git a/source/RobotAPI/components/units/KinematicUnit.cpp b/source/RobotAPI/components/units/KinematicUnit.cpp
index 1de87940111bbb4323e48b7c0bec22c524daa21f..d5c08b7a95a0dd28558f217e1ff5862c2c087854 100644
--- a/source/RobotAPI/components/units/KinematicUnit.cpp
+++ b/source/RobotAPI/components/units/KinematicUnit.cpp
@@ -56,7 +56,7 @@ void KinematicUnit::onInitComponent()
includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
}
- if (!ArmarXDataPath::getAbsolutePath(robotFile,robotFile,includePaths))
+ if (!ArmarXDataPath::getAbsolutePath(robotFile, robotFile, includePaths))
{
throw UserException("Could not find robot file " + robotFile);
}
@@ -66,11 +66,12 @@ void KinematicUnit::onInitComponent()
{
robot = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
}
- catch(VirtualRobot::VirtualRobotException& e)
+ catch (VirtualRobot::VirtualRobotException& e)
{
throw UserException(e.what());
}
- if(robotNodeSetName == "")
+
+ if (robotNodeSetName == "")
{
throw UserException("RobotNodeSet not defined");
}
@@ -126,5 +127,5 @@ void KinematicUnit::releaseKinematicUnit(const StringSequence& nodes, const Ice:
PropertyDefinitionsPtr KinematicUnit::createPropertyDefinitions()
{
return PropertyDefinitionsPtr(new KinematicUnitPropertyDefinitions(
- getConfigIdentifier()));
+ getConfigIdentifier()));
}
diff --git a/source/RobotAPI/components/units/KinematicUnit.h b/source/RobotAPI/components/units/KinematicUnit.h
index e91ef47c346be52b73d4ea0dd8aa1487042b2628..c3dd87e08a5b36e1c50c3bdcb14164b162aef89e 100644
--- a/source/RobotAPI/components/units/KinematicUnit.h
+++ b/source/RobotAPI/components/units/KinematicUnit.h
@@ -41,13 +41,13 @@ namespace armarx
* \brief
*/
class KinematicUnitPropertyDefinitions:
- public ComponentPropertyDefinitions
+ public ComponentPropertyDefinitions
{
public:
KinematicUnitPropertyDefinitions(std::string prefix):
ComponentPropertyDefinitions(prefix)
{
- defineRequiredProperty<std::string>("RobotNodeSetName","Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
+ defineRequiredProperty<std::string>("RobotNodeSetName", "Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
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)");
}
@@ -76,7 +76,10 @@ namespace armarx
{
public:
// inherited from Component
- virtual std::string getDefaultName() const { return "KinematicUnit"; }
+ virtual std::string getDefaultName() const
+ {
+ return "KinematicUnit";
+ }
virtual void onInitComponent();
virtual void onConnectComponent();
diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.cpp b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
index 65ab623d4393bad0a661c86f1843171cf4851f52..19673d07830dfa863605cb1648c0f2b4902d30a0 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
@@ -79,28 +79,30 @@ void KinematicUnitObserver::onConnectObserver()
includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
}
- if (!ArmarXDataPath::getAbsolutePath(robotFile,robotFile,includePaths))
+ if (!ArmarXDataPath::getAbsolutePath(robotFile, robotFile, includePaths))
{
throw UserException("Could not find robot file " + robotFile);
}
VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
- if(robotNodeSetName == "")
+
+ if (robotNodeSetName == "")
{
throw UserException("RobotNodeSet not defined");
}
+
VirtualRobot::RobotNodeSetPtr robotNodeSetPtr = robot->getRobotNodeSet(robotNodeSetName);
std::vector< VirtualRobot::RobotNodePtr > robotNodes;
robotNodes = robotNodeSetPtr->getAllRobotNodes();
// register all channels
- offerChannel("jointangles","Joint values of the " + robotNodeSetName + " kinematic chain");
- offerChannel("jointvelocities","Joint velocities of the " + robotNodeSetName + "kinematic chain");
- offerChannel("jointtorques","Joint torques of the" + robotNodeSetName + " kinematic chain");
- offerChannel("jointcurrents","Joint currents of the " + robotNodeSetName + " kinematic chain");
- offerChannel("jointmotortemperatures","Joint motor temperatures of the " + robotNodeSetName + " kinematic chain");
- offerChannel("jointcontrolmodes","Joint motor temperatures of the " + robotNodeSetName + " kinematic chain");
+ offerChannel("jointangles", "Joint values of the " + robotNodeSetName + " kinematic chain");
+ offerChannel("jointvelocities", "Joint velocities of the " + robotNodeSetName + "kinematic chain");
+ offerChannel("jointtorques", "Joint torques of the" + robotNodeSetName + " kinematic chain");
+ offerChannel("jointcurrents", "Joint currents of the " + robotNodeSetName + " kinematic chain");
+ offerChannel("jointmotortemperatures", "Joint motor temperatures of the " + robotNodeSetName + " kinematic chain");
+ offerChannel("jointcontrolmodes", "Joint motor temperatures of the " + robotNodeSetName + " kinematic chain");
// register all data fields
@@ -115,6 +117,7 @@ void KinematicUnitObserver::onConnectObserver()
offerDataField("jointcurrents", jointName, VariantType::Float, "Joint current of the " + jointName + " joint");
offerDataField("jointmotortemperatures", jointName, VariantType::Float, "Joint motor temperature of the " + jointName + " joint");
}
+
updateChannel("jointcontrolmodes");
}
@@ -125,18 +128,23 @@ void KinematicUnitObserver::onConnectObserver()
// ********************************************************************
void KinematicUnitObserver::reportControlModeChanged(const NameControlModeMap& jointModes, bool aValueChanged, const Ice::Current& c)
{
- try{
+ try
+ {
- if(jointModes.size() == 0)
+ if (jointModes.size() == 0)
+ {
return;
- for(auto elem : jointModes)
+ }
+
+ for (auto elem : jointModes)
{
setDataFieldFlatCopy("jointcontrolmodes", elem.first, new Variant(ControlModeToString(elem.second)));
}
+
updateChannel("jointcontrolmodes");
}
- catch(...)
+ catch (...)
{
handleExceptions();
}
@@ -144,18 +152,21 @@ void KinematicUnitObserver::reportControlModeChanged(const NameControlModeMap& j
void KinematicUnitObserver::reportJointAngles(const NameValueMap& jointAngles, bool aValueChanged, const Ice::Current& c)
{
- try{
+ try
+ {
- if(jointAngles.size() == 0)
+ if (jointAngles.size() == 0)
+ {
return;
+ }
nameValueMapToDataFields("jointangles", jointAngles);
updateChannel("jointangles");
}
- catch(...)
+ catch (...)
{
handleExceptions();
}
@@ -164,13 +175,17 @@ void KinematicUnitObserver::reportJointAngles(const NameValueMap& jointAngles,
void KinematicUnitObserver::reportJointVelocities(const NameValueMap& jointVelocities, bool aValueChanged, const Ice::Current& c)
{
- try{
- if(jointVelocities.size() == 0)
+ try
+ {
+ if (jointVelocities.size() == 0)
+ {
return;
+ }
+
nameValueMapToDataFields("jointvelocities", jointVelocities);
updateChannel("jointvelocities");
}
- catch(...)
+ catch (...)
{
handleExceptions();
}
@@ -178,32 +193,40 @@ void KinematicUnitObserver::reportJointVelocities(const NameValueMap& jointVeloc
void KinematicUnitObserver::reportJointTorques(const NameValueMap& jointTorques, bool aValueChanged, const Ice::Current& c)
{
- try{
- if(jointTorques.size() == 0)
+ try
+ {
+ if (jointTorques.size() == 0)
+ {
return;
+ }
+
nameValueMapToDataFields("jointtorques", jointTorques);
updateChannel("jointtorques");
}
- catch(...)
+ catch (...)
{
handleExceptions();
}
}
-void KinematicUnitObserver::reportJointAccelerations(const NameValueMap &jointAccelerations, bool aValueChanged, const Ice::Current &c)
+void KinematicUnitObserver::reportJointAccelerations(const NameValueMap& jointAccelerations, bool aValueChanged, const Ice::Current& c)
{
}
void KinematicUnitObserver::reportJointCurrents(const NameValueMap& jointCurrents, bool aValueChanged, const Ice::Current& c)
{
- try{
- if(jointCurrents.size() == 0)
+ try
+ {
+ if (jointCurrents.size() == 0)
+ {
return;
+ }
+
nameValueMapToDataFields("jointcurrents", jointCurrents);
updateChannel("jointcurrents");
}
- catch(...)
+ catch (...)
{
handleExceptions();
}
@@ -211,30 +234,34 @@ void KinematicUnitObserver::reportJointCurrents(const NameValueMap& jointCurrent
void KinematicUnitObserver::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, bool aValueChanged, const Ice::Current& c)
{
- try{
- if(jointMotorTemperatures.size() == 0)
+ try
+ {
+ if (jointMotorTemperatures.size() == 0)
+ {
return;
+ }
+
nameValueMapToDataFields("jointmotortemperatures", jointMotorTemperatures);
updateChannel("jointmotortemperatures");
}
- catch(...)
+ catch (...)
{
handleExceptions();
}
}
-void KinematicUnitObserver::reportJointStatuses(const NameStatusMap &jointStatuses, bool aValueChanged, const Ice::Current &c)
+void KinematicUnitObserver::reportJointStatuses(const NameStatusMap& jointStatuses, bool aValueChanged, const Ice::Current& c)
{
}
// ********************************************************************
// private methods
// ********************************************************************
-void KinematicUnitObserver::nameValueMapToDataFields(const std::string &channelName, const NameValueMap& nameValueMap)
+void KinematicUnitObserver::nameValueMapToDataFields(const std::string& channelName, const NameValueMap& nameValueMap)
{
NameValueMap::const_iterator iter = nameValueMap.begin();
- while(iter != nameValueMap.end())
+ while (iter != nameValueMap.end())
{
setDataFieldFlatCopy(channelName, iter->first, new Variant(iter->second));
iter++;
diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.h b/source/RobotAPI/components/units/KinematicUnitObserver.h
index bb772980ad212758c097273d5d052e0f744ff82f..ecbc321b7be827e6c33bd460076c7c1cc925921a 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.h
+++ b/source/RobotAPI/components/units/KinematicUnitObserver.h
@@ -39,19 +39,19 @@ namespace armarx
ARMARX_CREATE_CHECK(KinematicUnitObserver, equals)
ARMARX_CREATE_CHECK(KinematicUnitObserver, larger)
- /**
- * \class KinematicUnitObserver
- * \ingroup RobotAPI-SensorActorUnits-observers
- * \brief Observer monitoring kinematic sensor and actor values
- *
- * The KinematicUnitObserver allows to install conditions on all channel reported by the KinematicUnit.
- * These include joint angles, velocities, torques and motor temperatures
- *
- * The KinematicUnitObserver retrieves its configuration from a VirtualRobot robot model. Within the model, the joints
- * which are observer by the unit are define by a robotnodeset.
- *
- * Available condition checks are: *valid*, *updated*, *equals*, *inrange*, *approx*, *larger* and *smaller*.
- */
+ /**
+ * \class KinematicUnitObserver
+ * \ingroup RobotAPI-SensorActorUnits-observers
+ * \brief Observer monitoring kinematic sensor and actor values
+ *
+ * The KinematicUnitObserver allows to install conditions on all channel reported by the KinematicUnit.
+ * These include joint angles, velocities, torques and motor temperatures
+ *
+ * The KinematicUnitObserver retrieves its configuration from a VirtualRobot robot model. Within the model, the joints
+ * which are observer by the unit are define by a robotnodeset.
+ *
+ * Available condition checks are: *valid*, *updated*, *equals*, *inrange*, *approx*, *larger* and *smaller*.
+ */
class ARMARXCORE_IMPORT_EXPORT KinematicUnitObserver :
virtual public Observer,
virtual public KinematicUnitObserverInterface
@@ -84,40 +84,55 @@ namespace armarx
static std::string ControlModeToString(ControlMode mode)
{
- switch(mode)
+ switch (mode)
{
- case eDisabled:
- return "Disabled";
- case ePositionControl:
- return "PositionControl";
- case eVelocityControl:
- return "VelocityControl";
- case eTorqueControl:
- return "TorqueControl";
- case ePositionVelocityControl:
- return "PositionVelocityControl";
- case eUnknown:
- default:
- return "Unknown";
+ case eDisabled:
+ return "Disabled";
+
+ case ePositionControl:
+ return "PositionControl";
+
+ case eVelocityControl:
+ return "VelocityControl";
+
+ case eTorqueControl:
+ return "TorqueControl";
+
+ case ePositionVelocityControl:
+ return "PositionVelocityControl";
+
+ case eUnknown:
+ default:
+ return "Unknown";
}
}
- static ControlMode StringToControlMode(const std::string &mode)
+ static ControlMode StringToControlMode(const std::string& mode)
{
- if(mode == "Disabled")
+ if (mode == "Disabled")
+ {
return eDisabled;
+ }
- if(mode == "PositionControl")
+ if (mode == "PositionControl")
+ {
return ePositionControl;
+ }
- if(mode == "VelocityControl")
+ if (mode == "VelocityControl")
+ {
return eVelocityControl;
+ }
- if(mode == "TorqueControl")
+ if (mode == "TorqueControl")
+ {
return eTorqueControl;
+ }
- if(mode == "PositionVelocityControl")
+ if (mode == "PositionVelocityControl")
+ {
return ePositionVelocityControl;
+ }
return eUnknown;
}
@@ -127,35 +142,41 @@ namespace armarx
private:
std::string robotNodeSetName;
-};
-
+ };
-/**
- @class KinematicUnitDatafieldCreator
- @brief
- @ingroup RobotAPI-SensorActorUnits-util
- */
-class KinematicUnitDatafieldCreator
-{
-public:
- KinematicUnitDatafieldCreator(const std::string &channelName): _channelName(channelName){}
/**
- * @brief Function to create a Channel Representation for a KinematicUnitObserver
- * @param kinematicUnitOberserverName Name of the KinematicUnitObserver
- * @param jointName Name of a joint of the robot like it is specified
- * in the simox-robot-xml-file
- * @return returns a ChannelRepresentationPtr
+ @class KinematicUnitDatafieldCreator
+ @brief
+ @ingroup RobotAPI-SensorActorUnits-util
*/
- DataFieldIdentifier getDatafield(const std::string &kinematicUnitObserverName, const std::string &jointName) const
+ class KinematicUnitDatafieldCreator
+ {
+ public:
+ KinematicUnitDatafieldCreator(const std::string& channelName): _channelName(channelName) {}
+
+ /**
+ * @brief Function to create a Channel Representation for a KinematicUnitObserver
+ * @param kinematicUnitOberserverName Name of the KinematicUnitObserver
+ * @param jointName Name of a joint of the robot like it is specified
+ * in the simox-robot-xml-file
+ * @return returns a ChannelRepresentationPtr
+ */
+ DataFieldIdentifier getDatafield(const std::string& kinematicUnitObserverName, const std::string& jointName) const
{
- if(kinematicUnitObserverName.empty())
+ if (kinematicUnitObserverName.empty())
+ {
throw LocalException("kinematicUnitObserverName must not be empty!");
- if(jointName.empty())
+ }
+
+ if (jointName.empty())
+ {
throw LocalException("jointName must not be empty!");
+ }
+
return DataFieldIdentifier(kinematicUnitObserverName, _channelName, jointName);
}
@@ -164,9 +185,11 @@ public:
};
- namespace channels{
+ namespace channels
+ {
- namespace KinematicUnitObserver{
+ namespace KinematicUnitObserver
+ {
const KinematicUnitDatafieldCreator jointAngles("jointAngles");
const KinematicUnitDatafieldCreator jointVelocities("jointVelocities");
const KinematicUnitDatafieldCreator jointTorques("jointTorques");
diff --git a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
index fce1e50257f0796706de3f64f237f59bb0fd2f7a..411fb9bf187fca618596e97e4cf4c2ed29213320 100644
--- a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
@@ -41,8 +41,9 @@ void KinematicUnitSimulation::onInitKinematicUnit()
for (std::vector<VirtualRobot::RobotNodePtr>::iterator it = robotNodes.begin(); it != robotNodes.end(); it++)
{
std::string jointName = (*it)->getName();
- jointInfos[jointName].limitLo = (*it)->getJointLimitLo();
- jointInfos[jointName].limitHi = (*it)->getJointLimitHi();
+ jointInfos[jointName].limitLo = (*it)->getJointLimitLo();
+ jointInfos[jointName].limitHi = (*it)->getJointLimitHi();
+
if ((*it)->isRotationalJoint())
{
if (jointInfos[jointName].limitHi - jointInfos[jointName].limitLo >= float(M_PI * 2))
@@ -50,12 +51,14 @@ void KinematicUnitSimulation::onInitKinematicUnit()
jointInfos[jointName].reset();
}
}
+
ARMARX_VERBOSE << jointName << " with limits: lo: " << jointInfos[jointName].limitLo << " hi: " << jointInfos[jointName].limitHi;
}
{
// duplicate the loop because of locking
boost::mutex::scoped_lock lock(jointStatesMutex);
+
// initialize JoinStates
for (std::vector<VirtualRobot::RobotNodePtr>::iterator it = robotNodes.begin(); it != robotNodes.end(); it++)
{
@@ -64,8 +67,9 @@ void KinematicUnitSimulation::onInitKinematicUnit()
jointStates[jointName].init();
}
}
+
noise = getProperty<float>("Noise").getValue() / 180.f * M_PI;
- distribution = std::normal_distribution<double>(0,noise);
+ distribution = std::normal_distribution<double>(0, noise);
intervalMs = getProperty<int>("IntervalMs").getValue();
ARMARX_VERBOSE << "Starting kinematic unit simulation with " << intervalMs << " ms interval";
simulationTask = new PeriodicTask<KinematicUnitSimulation>(this, &KinematicUnitSimulation::simulationFunction, intervalMs, false, "KinematicUnitSimulation");
@@ -98,12 +102,14 @@ void KinematicUnitSimulation::simulationFunction()
{
boost::mutex::scoped_lock lock(jointStatesMutex);
- for ( KinematicUnitSimulationJointStates::iterator it = jointStates.begin(); it!=jointStates.end(); it++ )
+
+ for (KinematicUnitSimulationJointStates::iterator it = jointStates.begin(); it != jointStates.end(); it++)
{
float newAngle = 0.0f;
+
// calculate joint positions if joint is in velocity control mode
- if ( it->second.controlMode == eVelocityControl )
+ if (it->second.controlMode == eVelocityControl)
{
double change = (it->second.velocity * timeDeltaInSeconds);
@@ -111,13 +117,13 @@ void KinematicUnitSimulation::simulationFunction()
change += randomNoiseValue;
newAngle = it->second.angle + change;
}
- else if(it->second.controlMode == ePositionControl)
+ else if (it->second.controlMode == ePositionControl)
{
newAngle = it->second.angle;
}
// constrain the angle
- KinematicUnitSimulationJointInfo & jointInfo = jointInfos[it->first];
+ KinematicUnitSimulationJointInfo& jointInfo = jointInfos[it->first];
float maxAngle = (jointInfo.hasLimits()) ? jointInfo.limitHi : 2.0 * M_PI;
float minAngle = (jointInfo.hasLimits()) ? jointInfo.limitLo : -2.0 * M_PI;
newAngle = std::max<float>(newAngle, minAngle);
@@ -125,20 +131,22 @@ void KinematicUnitSimulation::simulationFunction()
// Check if joint existed before
KinematicUnitSimulationJointStates::iterator itPrev = previousJointStates.find(it->first);
- if(itPrev == previousJointStates.end())
+
+ if (itPrev == previousJointStates.end())
{
aPosValueChanged = aVelValueChanged = true;
}
// check if the angle has changed
- if(fabs(previousJointStates[it->first].angle - newAngle) > 0.00000001 )
+ if (fabs(previousJointStates[it->first].angle - newAngle) > 0.00000001)
{
aPosValueChanged = true;
// set the new joint angle locally for the next simulation iteration
it->second.angle = newAngle;
}
+
// check if velocities have changed
- if(fabs(previousJointStates[it->first].velocity - it->second.velocity) > 0.00000001 )
+ if (fabs(previousJointStates[it->first].velocity - it->second.velocity) > 0.00000001)
{
aVelValueChanged = true;
}
@@ -147,6 +155,7 @@ void KinematicUnitSimulation::simulationFunction()
actualJointAngles[it->first] = newAngle;
actualJointVelocities[it->first] = it->second.velocity;
}
+
previousJointStates = jointStates;
}
@@ -160,7 +169,8 @@ void KinematicUnitSimulation::switchControlMode(const NameControlModeMap& target
NameControlModeMap changedControlModes;
{
boost::mutex::scoped_lock lock(jointStatesMutex);
- for ( NameControlModeMap::const_iterator it = targetJointModes.begin(); it!=targetJointModes.end(); it++ )
+
+ for (NameControlModeMap::const_iterator it = targetJointModes.begin(); it != targetJointModes.end(); it++)
{
// target values
std::string targetJointName = it->first;
@@ -169,15 +179,20 @@ void KinematicUnitSimulation::switchControlMode(const NameControlModeMap& target
KinematicUnitSimulationJointStates::iterator iterJoints = jointStates.find(targetJointName);
// check whether there is a joint with this name and set joint angle
- if(iterJoints != jointStates.end())
+ if (iterJoints != jointStates.end())
{
- if(iterJoints->second.controlMode != targetControlMode)
+ if (iterJoints->second.controlMode != targetControlMode)
+ {
aValueChanged = true;
+ }
+
iterJoints->second.controlMode = targetControlMode;
changedControlModes.insert(*it);
}
else
+ {
ARMARX_WARNING << "Could not find joint with name " << targetJointName;
+ }
}
}
// only report control modes which have been changed
@@ -192,7 +207,8 @@ void KinematicUnitSimulation::setJointAngles(const NameValueMap& targetJointAngl
// name value maps for reporting
NameValueMap actualJointAngles;
bool aValueChanged = false;
- for ( NameValueMap::const_iterator it = targetJointAngles.begin(); it!=targetJointAngles.end(); it++ )
+
+ for (NameValueMap::const_iterator it = targetJointAngles.begin(); it != targetJointAngles.end(); it++)
{
// target values
std::string targetJointName = it->first;
@@ -200,23 +216,33 @@ void KinematicUnitSimulation::setJointAngles(const NameValueMap& targetJointAngl
// check whether there is a joint with this name and set joint angle
KinematicUnitSimulationJointStates::iterator iterJoints = jointStates.find(targetJointName);
- if(iterJoints != jointStates.end())
+
+ if (iterJoints != jointStates.end())
{
- if (fabs(iterJoints->second.angle - targetJointAngle)>0)
+ if (fabs(iterJoints->second.angle - targetJointAngle) > 0)
+ {
aValueChanged = true;
+ }
+
if (jointInfos[targetJointName].hasLimits())
{
targetJointAngle = std::max<float>(targetJointAngle, jointInfos[targetJointName].limitLo);
targetJointAngle = std::min<float>(targetJointAngle, jointInfos[targetJointName].limitHi);
}
+
iterJoints->second.angle = targetJointAngle;
actualJointAngles[targetJointName] = iterJoints->second.angle;
}
else
+ {
ARMARX_WARNING << deactivateSpam(1) << "Joint '" << targetJointName << "' not found!";
+ }
}
+
if (aValueChanged)
+ {
listenerPrx->reportJointAngles(actualJointAngles, aValueChanged);
+ }
}
void KinematicUnitSimulation::setJointVelocities(const NameValueMap& targetJointVelocities, const Ice::Current& c)
@@ -224,24 +250,32 @@ void KinematicUnitSimulation::setJointVelocities(const NameValueMap& targetJoint
boost::mutex::scoped_lock lock(jointStatesMutex);
NameValueMap actualJointVelocities;
bool aValueChanged = false;
- for ( NameValueMap::const_iterator it = targetJointVelocities.begin(); it!=targetJointVelocities.end(); it++ )
+
+ for (NameValueMap::const_iterator it = targetJointVelocities.begin(); it != targetJointVelocities.end(); it++)
{
// target values
std::string targetJointName = it->first;
float targetJointVelocity = it->second;
KinematicUnitSimulationJointStates::iterator iterJoints = jointStates.find(targetJointName);
+
// check whether there is a joint with this name and set joint angle
- if(iterJoints != jointStates.end())
+ if (iterJoints != jointStates.end())
{
if (fabs(iterJoints->second.velocity - targetJointVelocity) > 0)
+ {
aValueChanged = true;
+ }
+
iterJoints->second.velocity = targetJointVelocity;
actualJointVelocities[targetJointName] = iterJoints->second.velocity;
}
}
+
if (aValueChanged)
+ {
listenerPrx->reportJointVelocities(actualJointVelocities, aValueChanged);
+ }
}
void KinematicUnitSimulation::setJointTorques(const NameValueMap& targetJointTorques, const Ice::Current& c)
@@ -249,33 +283,43 @@ void KinematicUnitSimulation::setJointTorques(const NameValueMap& targetJointTor
boost::mutex::scoped_lock lock(jointStatesMutex);
NameValueMap actualJointTorques;
bool aValueChanged = false;
- for ( NameValueMap::const_iterator it = targetJointTorques.begin(); it!=targetJointTorques.end(); it++ )
+
+ for (NameValueMap::const_iterator it = targetJointTorques.begin(); it != targetJointTorques.end(); it++)
{
// target values
std::string targetJointName = it->first;
float targetJointTorque = it->second;
KinematicUnitSimulationJointStates::iterator iterJoints = jointStates.find(targetJointName);
+
// check whether there is a joint with this name and set joint angle
- if(iterJoints != jointStates.end())
+ if (iterJoints != jointStates.end())
{
if (fabs(iterJoints->second.torque - targetJointTorque) > 0)
+ {
aValueChanged = true;
+ }
+
iterJoints->second.torque = targetJointTorque;
actualJointTorques[targetJointName] = iterJoints->second.torque;
}
}
+
if (aValueChanged)
+ {
listenerPrx->reportJointTorques(actualJointTorques, aValueChanged);
+ }
}
NameControlModeMap KinematicUnitSimulation::getControlModes(const Ice::Current& c)
{
NameControlModeMap result;
- for ( KinematicUnitSimulationJointStates::iterator it = jointStates.begin(); it!=jointStates.end(); it++ )
+
+ for (KinematicUnitSimulationJointStates::iterator it = jointStates.begin(); it != jointStates.end(); it++)
{
result[it->first] = it->second.controlMode;
}
+
return result;
}
@@ -287,14 +331,16 @@ void KinematicUnitSimulation::setJointDecelerations(const NameValueMap& targetJo
{
}
-void KinematicUnitSimulation::stop(const Ice::Current &c)
+void KinematicUnitSimulation::stop(const Ice::Current& c)
{
boost::mutex::scoped_lock lock(jointStatesMutex);
- for ( KinematicUnitSimulationJointStates::iterator it = jointStates.begin(); it!=jointStates.end(); it++ )
+
+ for (KinematicUnitSimulationJointStates::iterator it = jointStates.begin(); it != jointStates.end(); it++)
{
it->second.velocity = 0;
}
+
SensorActorUnit::stop(c);
}
@@ -316,6 +362,7 @@ void KinematicUnitSimulation::requestJoints(const StringSequence& joints, const
ARMARX_INFO << flush;
// if one of the joints belongs to this unit, lock access to this unit for other components except for the requesting one
KinematicUnitSimulationJointStates::const_iterator it = std::find_first_of(jointStates.begin(), jointStates.end(), joints.begin(), joints.end(), mapEntryEqualsString);
+
if (jointStates.end() != it)
{
KinematicUnit::request(c);
@@ -327,6 +374,7 @@ void KinematicUnitSimulation::releaseJoints(const StringSequence& joints, const
ARMARX_INFO << flush;
// if one of the joints belongs to this unit, unlock access
KinematicUnitSimulationJointStates::const_iterator it = std::find_first_of(jointStates.begin(), jointStates.end(), joints.begin(), joints.end(), mapEntryEqualsString);
+
if (jointStates.end() != it)
{
KinematicUnit::release(c);
diff --git a/source/RobotAPI/components/units/KinematicUnitSimulation.h b/source/RobotAPI/components/units/KinematicUnitSimulation.h
index ece22420037000179824d457208bb8a9b84d6159..236bda6325f8101369929f5b595ccfdd04b90198 100644
--- a/source/RobotAPI/components/units/KinematicUnitSimulation.h
+++ b/source/RobotAPI/components/units/KinematicUnitSimulation.h
@@ -102,7 +102,7 @@ namespace armarx
* \brief
*/
class KinematicUnitSimulationPropertyDefinitions :
- public KinematicUnitPropertyDefinitions
+ public KinematicUnitPropertyDefinitions
{
public:
KinematicUnitSimulationPropertyDefinitions(std::string prefix) :
@@ -123,7 +123,10 @@ namespace armarx
{
public:
// inherited from Component
- virtual std::string getDefaultName() const { return "KinematicUnitSimulation"; }
+ virtual std::string getDefaultName() const
+ {
+ return "KinematicUnitSimulation";
+ }
virtual void onInitKinematicUnit();
virtual void onStartKinematicUnit();
@@ -151,7 +154,7 @@ namespace armarx
*/
virtual void setJointDecelerations(const NameValueMap& targetJointDecelerations, const Ice::Current& c = ::Ice::Current());
- void stop(const Ice::Current &c = Ice::Current());
+ void stop(const Ice::Current& c = Ice::Current());
/**
* \see PropertyUser::createPropertyDefinitions()
diff --git a/source/RobotAPI/components/units/PlatformUnit.cpp b/source/RobotAPI/components/units/PlatformUnit.cpp
index cb244890358ad29214c761e9cb09a4f862719f2e..7a0e6c3e8b8096fd6a8f74f666612a9a3fa66b84 100644
--- a/source/RobotAPI/components/units/PlatformUnit.cpp
+++ b/source/RobotAPI/components/units/PlatformUnit.cpp
@@ -62,7 +62,7 @@ void PlatformUnit::onExitComponent()
}
-void PlatformUnit::moveTo(Ice::Float targetPlatformPositionX, Ice::Float targetPlatformPositionY, Ice::Float targetPlatformRotation, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current &c)
+void PlatformUnit::moveTo(Ice::Float targetPlatformPositionX, Ice::Float targetPlatformPositionY, Ice::Float targetPlatformRotation, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c)
{
}
@@ -70,5 +70,5 @@ void PlatformUnit::moveTo(Ice::Float targetPlatformPositionX, Ice::Float targetP
PropertyDefinitionsPtr PlatformUnit::createPropertyDefinitions()
{
return PropertyDefinitionsPtr(new PlatformUnitPropertyDefinitions(
- getConfigIdentifier()));
+ getConfigIdentifier()));
}
diff --git a/source/RobotAPI/components/units/PlatformUnit.h b/source/RobotAPI/components/units/PlatformUnit.h
index 8d2b53baa04461d0f856a7cee4faa6c808c00c38..3b49d038d9ac02503d4857394b8bb7faa6e72a63 100644
--- a/source/RobotAPI/components/units/PlatformUnit.h
+++ b/source/RobotAPI/components/units/PlatformUnit.h
@@ -39,7 +39,7 @@ namespace armarx
* \brief Defines all necessary properties for armarx::PlatformUnit
*/
class PlatformUnitPropertyDefinitions:
- public ComponentPropertyDefinitions
+ public ComponentPropertyDefinitions
{
public:
PlatformUnitPropertyDefinitions(std::string prefix):
@@ -106,7 +106,7 @@ namespace armarx
virtual void moveTo(Ice::Float targetPlatformPositionX, Ice::Float targetPlatformPositionY, Ice::Float targetPlatformRotation,
Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c = ::Ice::Current());
- void stopPlatform(const Ice::Current &c = Ice::Current()){}
+ void stopPlatform(const Ice::Current& c = Ice::Current()) {}
/**
* \see armarx::PropertyUser::createPropertyDefinitions()
*/
diff --git a/source/RobotAPI/components/units/PlatformUnitObserver.cpp b/source/RobotAPI/components/units/PlatformUnitObserver.cpp
index 8b9d63b0623f6a05943f97d2e3a649cddfabe237..01c6bfd20e173a4ad2893ebd99782958d47d7898 100644
--- a/source/RobotAPI/components/units/PlatformUnitObserver.cpp
+++ b/source/RobotAPI/components/units/PlatformUnitObserver.cpp
@@ -111,5 +111,5 @@ void PlatformUnitObserver::nameValueMapToDataFields(std::string channelName, ::I
PropertyDefinitionsPtr PlatformUnitObserver::createPropertyDefinitions()
{
return PropertyDefinitionsPtr(new PlatformUnitPropertyDefinitions(
- getConfigIdentifier()));
+ getConfigIdentifier()));
}
diff --git a/source/RobotAPI/components/units/PlatformUnitObserver.h b/source/RobotAPI/components/units/PlatformUnitObserver.h
index b1a9649839fb588af50c62f0c06e9d4d95909b76..5589e366c3d49e004e83fce534fe842b0c7117b4 100644
--- a/source/RobotAPI/components/units/PlatformUnitObserver.h
+++ b/source/RobotAPI/components/units/PlatformUnitObserver.h
@@ -41,19 +41,19 @@ namespace armarx
ARMARX_CREATE_CHECK(PlatformUnitObserver, larger)
ARMARX_CREATE_CHECK(PlatformUnitObserver, inrange)
- /**
- * \class PlatformUnitObserver
- * \ingroup RobotAPI-SensorActorUnits-observers
- * \brief Observer monitoring platform-related sensor values.
- *
- * The PlatformUnitObserver allows the installation of conditions on all channel reported by the PlatformUnit.
- * These include current and target position.
- *
- * The PlatformUnitObserver retrieves its platform configuration via properties.
- * The name must exist in the used Simox robot model file.
- *
- * Available condition checks are: *valid*, *updated*, *equals*, *inrange*, *approx*, *larger* and *smaller*.
- */
+ /**
+ * \class PlatformUnitObserver
+ * \ingroup RobotAPI-SensorActorUnits-observers
+ * \brief Observer monitoring platform-related sensor values.
+ *
+ * The PlatformUnitObserver allows the installation of conditions on all channel reported by the PlatformUnit.
+ * These include current and target position.
+ *
+ * The PlatformUnitObserver retrieves its platform configuration via properties.
+ * The name must exist in the used Simox robot model file.
+ *
+ * Available condition checks are: *valid*, *updated*, *equals*, *inrange*, *approx*, *larger* and *smaller*.
+ */
class ARMARXCORE_IMPORT_EXPORT PlatformUnitObserver :
virtual public Observer,
virtual public PlatformUnitObserverInterface
@@ -93,7 +93,7 @@ namespace armarx
class PlatformUnitDatafieldCreator
{
public:
- PlatformUnitDatafieldCreator(const std::string &channelName) :
+ PlatformUnitDatafieldCreator(const std::string& channelName) :
channelName(channelName)
{}
@@ -106,10 +106,16 @@ namespace armarx
*/
DataFieldIdentifier getDatafield(const std::string& platformUnitObserverName, const std::string& platformName) const
{
- if(platformUnitObserverName.empty())
+ if (platformUnitObserverName.empty())
+ {
throw LocalException("kinematicUnitObserverName must not be empty!");
- if(platformName.empty())
+ }
+
+ if (platformName.empty())
+ {
throw LocalException("jointName must not be empty!");
+ }
+
return DataFieldIdentifier(platformUnitObserverName, channelName, platformName);
}
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
index dd76eba8164aec7d3ddf88f2fc71e19584273d00..382a28c0a801c6fb13700d6b57fa6cee309f4f35 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
@@ -53,15 +53,19 @@ void PlatformUnitSimulation::onStartPlatformUnit()
void PlatformUnitSimulation::onStopPlatformUnit()
{
- if(simulationTask)
+ if (simulationTask)
+ {
simulationTask->stop();
+ }
}
void PlatformUnitSimulation::onExitPlatformUnit()
{
- if(simulationTask)
+ if (simulationTask)
+ {
simulationTask->stop();
+ }
}
@@ -74,29 +78,41 @@ void PlatformUnitSimulation::simulationFunction()
{
ScopedLock lock(currentPoseMutex);
float diff = fabs(targetPositionX - currentPositionX);
+
if (diff > positionalAccuracy)
{
int sign = copysignf(1.0f, (targetPositionX - currentPositionX));
currentPositionX += sign * std::min<float>(linearVelocity * timeDeltaInSeconds, diff);
vel[0] = linearVelocity;
}
+
diff = fabs(targetPositionY - currentPositionY);
+
if (diff > positionalAccuracy)
{
int sign = copysignf(1.0f, (targetPositionY - currentPositionY));
currentPositionY += sign * std::min<float>(linearVelocity * timeDeltaInSeconds, diff);
vel[1] = linearVelocity;
}
+
diff = fabs(targetRotation - currentRotation);
+
if (diff > orientationalAccuracy)
{
int sign = copysignf(1.0f, (targetRotation - currentRotation));
currentRotation += sign * std::min<float>(angularVelocity * timeDeltaInSeconds, diff);
+
// stay between +/- M_2_PI
- if(currentRotation > 2 * M_PI)
+ if (currentRotation > 2 * M_PI)
+ {
currentRotation -= 2 * M_PI;
- if(currentRotation < -2 * M_PI)
+ }
+
+ if (currentRotation < -2 * M_PI)
+ {
currentRotation += 2 * M_PI;
+ }
+
vel[2] = angularVelocity;
}
@@ -126,12 +142,12 @@ PropertyDefinitionsPtr PlatformUnitSimulation::createPropertyDefinitions()
}
-void armarx::PlatformUnitSimulation::move(float targetPlatformVelocityX, float targetPlatformVelocityY, float targetPlatformVelocityRotation, const Ice::Current &c)
+void armarx::PlatformUnitSimulation::move(float targetPlatformVelocityX, float targetPlatformVelocityY, float targetPlatformVelocityRotation, const Ice::Current& c)
{
throw LocalException("NYI");
}
-void PlatformUnitSimulation::moveRelative(float targetPlatformOffsetX, float targetPlatformOffsetY, float targetPlatformOffsetRotation, float positionalAccuracy, float orientationalAccuracy, const Ice::Current &c)
+void PlatformUnitSimulation::moveRelative(float targetPlatformOffsetX, float targetPlatformOffsetY, float targetPlatformOffsetRotation, float positionalAccuracy, float orientationalAccuracy, const Ice::Current& c)
{
float targetPositionX, targetPositionY, targetRotation;
{
@@ -143,7 +159,7 @@ void PlatformUnitSimulation::moveRelative(float targetPlatformOffsetX, float tar
moveTo(targetPositionX, targetPositionY, targetRotation, positionalAccuracy, orientationalAccuracy);
}
-void PlatformUnitSimulation::setMaxVelocities(float positionalVelocity, float orientaionalVelocity, const Ice::Current &c)
+void PlatformUnitSimulation::setMaxVelocities(float positionalVelocity, float orientaionalVelocity, const Ice::Current& c)
{
ScopedLock lock(currentPoseMutex);
linearVelocity = positionalVelocity;
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.h b/source/RobotAPI/components/units/PlatformUnitSimulation.h
index 8724c8000032e36a28f9dd70cd7db8b2b65232a3..0e9bb1fc683262e4117b7fae14d1a83ce9ec4a87 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.h
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.h
@@ -39,7 +39,7 @@ namespace armarx
* \brief
*/
class PlatformUnitSimulationPropertyDefinitions :
- public PlatformUnitPropertyDefinitions
+ public PlatformUnitPropertyDefinitions
{
public:
PlatformUnitSimulationPropertyDefinitions(std::string prefix) :
@@ -83,10 +83,10 @@ namespace armarx
/**
* \warning Not yet implemented!
*/
- void move(float targetPlatformVelocityX, float targetPlatformVelocityY, float targetPlatformVelocityRotation, const Ice::Current &c = Ice::Current());
+ void move(float targetPlatformVelocityX, float targetPlatformVelocityY, float targetPlatformVelocityRotation, const Ice::Current& c = Ice::Current());
- void moveRelative(float targetPlatformOffsetX, float targetPlatformOffsetY, float targetPlatformOffsetRotation, float positionalAccuracy, float orientationalAccuracy, const Ice::Current &c = Ice::Current());
- void setMaxVelocities(float positionalVelocity, float orientaionalVelocity, const Ice::Current &c = Ice::Current());
+ void moveRelative(float targetPlatformOffsetX, float targetPlatformOffsetY, float targetPlatformOffsetRotation, float positionalAccuracy, float orientationalAccuracy, const Ice::Current& c = Ice::Current());
+ void setMaxVelocities(float positionalVelocity, float orientaionalVelocity, const Ice::Current& c = Ice::Current());
/**
* \see PropertyUser::createPropertyDefinitions()
diff --git a/source/RobotAPI/components/units/SensorActorUnit.cpp b/source/RobotAPI/components/units/SensorActorUnit.cpp
index 7565ca674be7b575ce0c6d39dac1a260e225c637..c716b3f708759d67825dd565a1d2d14979a0825e 100644
--- a/source/RobotAPI/components/units/SensorActorUnit.cpp
+++ b/source/RobotAPI/components/units/SensorActorUnit.cpp
@@ -39,71 +39,77 @@ SensorActorUnit::~SensorActorUnit()
void SensorActorUnit::init(const Ice::Current& c)
{
- std::string currentName = c.adapter->getName();
- Ice::Identity currentId = c.adapter->getCommunicator()->stringToIdentity(currentName);
- if(ownerId == currentId)
- {
+ std::string currentName = c.adapter->getName();
+ Ice::Identity currentId = c.adapter->getCommunicator()->stringToIdentity(currentName);
+
+ if (ownerId == currentId)
+ {
CALLINFO
- if(executionState == eUnitConstructed)
- {
- executionState = eUnitInitialized;
- onInit();
- }
- else
- {
- ARMARX_LOG << armarx::eWARN << "Unit " << getName() << " is already initialized." << armarx::flush;
- }
- }
+
+ if (executionState == eUnitConstructed)
+ {
+ executionState = eUnitInitialized;
+ onInit();
+ }
+ else
+ {
+ ARMARX_LOG << armarx::eWARN << "Unit " << getName() << " is already initialized." << armarx::flush;
+ }
+ }
else
- {
+ {
ARMARX_LOG << armarx::eWARN << "Unit " << getName() << " can not be initialized while unrequested." << armarx::flush;
- }
+ }
}
void SensorActorUnit::start(const Ice::Current& c)
{
- std::string currentName = c.adapter->getName();
- Ice::Identity currentId = c.adapter->getCommunicator()->stringToIdentity(currentName);
- if(ownerId == currentId)
- {
+ std::string currentName = c.adapter->getName();
+ Ice::Identity currentId = c.adapter->getCommunicator()->stringToIdentity(currentName);
+
+ if (ownerId == currentId)
+ {
CALLINFO
- if( (executionState == eUnitInitialized ) || (executionState == eUnitStopped) )
- {
+
+ if ((executionState == eUnitInitialized) || (executionState == eUnitStopped))
+ {
executionState = eUnitStarted;
onStart();
- }
- else
- {
- ARMARX_LOG << armarx::eWARN << "Unit " << getName() << " could not be started." << armarx::flush;
- }
- }
+ }
+ else
+ {
+ ARMARX_LOG << armarx::eWARN << "Unit " << getName() << " could not be started." << armarx::flush;
+ }
+ }
else
- {
+ {
ARMARX_LOG << armarx::eWARN << "Unit " << getName() << " can not be started while unrequested." << armarx::flush;
- }
+ }
}
void SensorActorUnit::stop(const Ice::Current& c)
{
- std::string currentName = c.adapter->getName();
- Ice::Identity currentId = c.adapter->getCommunicator()->stringToIdentity(currentName);
- if(ownerId == currentId)
- {
+ std::string currentName = c.adapter->getName();
+ Ice::Identity currentId = c.adapter->getCommunicator()->stringToIdentity(currentName);
+
+ if (ownerId == currentId)
+ {
CALLINFO
- if(executionState == eUnitStarted)
- {
- executionState = eUnitStopped;
- onStop();
- }
- else
- {
- ARMARX_LOG << armarx::eWARN << "Unit " << getName() << " could not be stopped." << armarx::flush;
- }
- }
+
+ if (executionState == eUnitStarted)
+ {
+ executionState = eUnitStopped;
+ onStop();
+ }
+ else
+ {
+ ARMARX_LOG << armarx::eWARN << "Unit " << getName() << " could not be stopped." << armarx::flush;
+ }
+ }
else
- {
+ {
ARMARX_LOG << armarx::eWARN << "Unit " << getName() << " can not be stopped while unrequested. " << armarx::flush;
- }
+ }
}
UnitExecutionState SensorActorUnit::getExecutionState(const Ice::Current& c)
@@ -116,8 +122,9 @@ UnitExecutionState SensorActorUnit::getExecutionState(const Ice::Current& c)
void SensorActorUnit::request(const Ice::Current& c)
{
CALLINFO
+
// try to lock unit
- if(!unitMutex.try_lock())
+ if (!unitMutex.try_lock())
{
ARMARX_LOG << armarx::eERROR << "Trying to request already owned unit " << getName() << "\n" << armarx::flush;
throw ResourceUnavailableException("Trying to request already owned unit");
@@ -127,7 +134,7 @@ void SensorActorUnit::request(const Ice::Current& c)
std::string ownerName = c.adapter->getName();
ownerId = c.adapter->getCommunicator()->stringToIdentity(ownerName);
- ARMARX_INFO << "unit requested by "<< ownerName << flush;
+ ARMARX_INFO << "unit requested by " << ownerName << flush;
}
void SensorActorUnit::release(const Ice::Current& c)
@@ -137,7 +144,7 @@ void SensorActorUnit::release(const Ice::Current& c)
std::string callerName = c.adapter->getName();
Ice::Identity callerId = c.adapter->getCommunicator()->stringToIdentity(callerName);
- if(!(ownerId == callerId))
+ if (!(ownerId == callerId))
{
ARMARX_LOG << armarx::eERROR << "Trying to release unit owned by another component" << armarx::flush;
throw ResourceNotOwnedException("Trying to release unit owned by another component");
diff --git a/source/RobotAPI/components/units/SensorActorUnit.h b/source/RobotAPI/components/units/SensorActorUnit.h
index 8c82ff86271221d5a0156b04295a350852b5bb86..db78c6f821ff6b51336a79c10f7387c53eaca2af 100644
--- a/source/RobotAPI/components/units/SensorActorUnit.h
+++ b/source/RobotAPI/components/units/SensorActorUnit.h
@@ -43,91 +43,91 @@ namespace armarx
virtual public SensorActorUnitInterface,
virtual public Component
{
- public:
- /**
- * Constructs a SensorActorUnit.
- */
- SensorActorUnit();
- /**
- * Destructor of SensorActorUnit.
- */
- virtual ~SensorActorUnit();
+ public:
+ /**
+ * Constructs a SensorActorUnit.
+ */
+ SensorActorUnit();
+ /**
+ * Destructor of SensorActorUnit.
+ */
+ virtual ~SensorActorUnit();
- /**
- * Set execution state to eInitialized.
- *
- * Assures that init is called only once and the calls subclass method onInit().
- *
- * \param c Ice context provided by the Ice framework
- */
- virtual void init(const Ice::Current& c = ::Ice::Current());
+ /**
+ * Set execution state to eInitialized.
+ *
+ * Assures that init is called only once and the calls subclass method onInit().
+ *
+ * \param c Ice context provided by the Ice framework
+ */
+ virtual void init(const Ice::Current& c = ::Ice::Current());
- /**
- * Set execution state to eStarted
- *
- * Start streaming of sensory data and acceptance of control data.
- *
- * Start can be called if the unit is initialized and not started yet (stopped).
- * Calls subclass method inStart().
- *
- * \param c Ice context provided by the Ice framework
- */
- virtual void start(const Ice::Current& c = ::Ice::Current());
+ /**
+ * Set execution state to eStarted
+ *
+ * Start streaming of sensory data and acceptance of control data.
+ *
+ * Start can be called if the unit is initialized and not started yet (stopped).
+ * Calls subclass method inStart().
+ *
+ * \param c Ice context provided by the Ice framework
+ */
+ virtual void start(const Ice::Current& c = ::Ice::Current());
- /**
- * Set execution state to eStopped
- *
- * Stop streaming of sensory data and acceptance of control data.
- *
- * Stop can be called if the unit is started.
- * Calls subclass method onStop()
- *
- * \param c Ice context provided by the Ice framework
- */
- virtual void stop(const Ice::Current& c = ::Ice::Current());
+ /**
+ * Set execution state to eStopped
+ *
+ * Stop streaming of sensory data and acceptance of control data.
+ *
+ * Stop can be called if the unit is started.
+ * Calls subclass method onStop()
+ *
+ * \param c Ice context provided by the Ice framework
+ */
+ virtual void stop(const Ice::Current& c = ::Ice::Current());
- /**
- * Retrieve current execution state
- *
- * \param c Ice context provided by the Ice framework
- * \return current execution state
- */
- UnitExecutionState getExecutionState(const Ice::Current& c = ::Ice::Current());
+ /**
+ * Retrieve current execution state
+ *
+ * \param c Ice context provided by the Ice framework
+ * \return current execution state
+ */
+ UnitExecutionState getExecutionState(const Ice::Current& c = ::Ice::Current());
- /**
- * Request exclusive access to current unit. Throws ResourceUnavailableException on error.
- *
- * \param c Ice context provided by the Ice framework
- */
- virtual void request(const Ice::Current& c = ::Ice::Current());
+ /**
+ * Request exclusive access to current unit. Throws ResourceUnavailableException on error.
+ *
+ * \param c Ice context provided by the Ice framework
+ */
+ virtual void request(const Ice::Current& c = ::Ice::Current());
- /**
- * Release exclusive access to current unit. Throws ResourceUnavailableException on error.
- *
- * \param c Ice context provided by the Ice framework
- */
- virtual void release(const Ice::Current& c = ::Ice::Current());
+ /**
+ * Release exclusive access to current unit. Throws ResourceUnavailableException on error.
+ *
+ * \param c Ice context provided by the Ice framework
+ */
+ virtual void release(const Ice::Current& c = ::Ice::Current());
- protected:
- void onExitComponent();
- /**
- * callback onInit for subclass hook. See init().
- */
- virtual void onInit() {};
+ protected:
+ void onExitComponent();
+ /**
+ * callback onInit for subclass hook. See init().
+ */
+ virtual void onInit() {};
- /**
- * callback onStart for subclass hook. See start().
- */
- virtual void onStart() {};
+ /**
+ * callback onStart for subclass hook. See start().
+ */
+ virtual void onStart() {};
- /**
- * callback onStop for subclass hook. See stop().
- */
- virtual void onStop() {};
+ /**
+ * callback onStop for subclass hook. See stop().
+ */
+ virtual void onStop() {};
- boost::mutex unitMutex;
- Ice::Identity ownerId;
- UnitExecutionState executionState;
+ boost::mutex unitMutex;
+ Ice::Identity ownerId;
+ UnitExecutionState executionState;
};
}
diff --git a/source/RobotAPI/components/units/TCPControlUnit.cpp b/source/RobotAPI/components/units/TCPControlUnit.cpp
index 846a9ce54a71e7fff482c28ed3f4097fecbbb992..8ddedde1f105cd85d190fe028c57bded83d8b8ad 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/components/units/TCPControlUnit.cpp
@@ -42,7 +42,7 @@ namespace armarx
{
TCPControlUnit::TCPControlUnit() :
- maxJointVelocity(30.f/180*3.141),
+ maxJointVelocity(30.f / 180 * 3.141),
cycleTime(30),
requested(false),
calculationRunning(false)
@@ -80,15 +80,19 @@ namespace armarx
localReportRobot = localRobot->clone(localRobot->getName());
std::string nodesetsString = getProperty<std::string>("TCPsToReport").getValue();
- if(!nodesetsString.empty())
+
+ if (!nodesetsString.empty())
{
- if(nodesetsString=="*")
+ if (nodesetsString == "*")
{
auto nodesets = localReportRobot->getRobotNodeSets();
- for(RobotNodeSetPtr& set : nodesets)
+
+ for (RobotNodeSetPtr& set : nodesets)
{
- if(set->getTCP())
+ if (set->getTCP())
+ {
nodesToReport.push_back(set->getTCP());
+ }
}
}
else
@@ -98,19 +102,26 @@ namespace armarx
nodesetsString,
boost::is_any_of(","),
boost::token_compress_on);
- for(auto& name : nodesetNames)
+
+ for (auto& name : nodesetNames)
{
auto node = localReportRobot->getRobotNode(name);
- if(node)
+
+ if (node)
+ {
nodesToReport.push_back(node);
+ }
else
+ {
ARMARX_ERROR << "Could not find node set with name: " << name;
+ }
}
}
}
std::vector<RobotNodePtr> nodes = localRobot->getRobotNodes();
- for(unsigned int i=0; i < nodes.size(); i++)
+
+ for (unsigned int i = 0; i < nodes.size(); i++)
{
ARMARX_VERBOSE << nodes.at(i)->getName();
}
@@ -120,8 +131,12 @@ namespace armarx
listener = getTopic<TCPControlUnitListenerPrx>(topicName);
requested = false;
- if(execTask)
+
+ if (execTask)
+ {
execTask->stop();
+ }
+
execTask = new PeriodicTask<TCPControlUnit>(this, &TCPControlUnit::periodicExec, cycleTime, false, "TCPVelocityCalculator");
execTask->start();
execTask->setDelayWarningTolerance(100);
@@ -135,13 +150,15 @@ namespace armarx
{
release();
}
- catch(std::exception&e)
+ catch (std::exception& e)
{
ARMARX_WARNING << "Releasing TCP Unit failed";
}
- if(execTask)
+ if (execTask)
+ {
execTask->stop();
+ }
}
void TCPControlUnit::onExitComponent()
@@ -150,30 +167,39 @@ namespace armarx
- void TCPControlUnit::setCycleTime(Ice::Int milliseconds, const Ice::Current & c)
+ void TCPControlUnit::setCycleTime(Ice::Int milliseconds, const Ice::Current& c)
{
ScopedLock lock(taskMutex);
cycleTime = milliseconds;
- if(execTask)
+
+ if (execTask)
+ {
execTask->changeInterval(cycleTime);
+ }
}
- void TCPControlUnit::setTCPVelocity(const std::string &nodeSetName, const std::string &tcpName, const FramedDirectionBasePtr &translationVelocity, const FramedDirectionBasePtr &orientationVelocityRPY, const Ice::Current &c)
+ void TCPControlUnit::setTCPVelocity(const std::string& nodeSetName, const std::string& tcpName, const FramedDirectionBasePtr& translationVelocity, const FramedDirectionBasePtr& orientationVelocityRPY, const Ice::Current& c)
{
ScopedLock lock(dataMutex);
ARMARX_CHECK_EXPRESSION_W_HINT(jointExistenceCheckRobot->hasRobotNodeSet(nodeSetName), "The robot does not have the node set: " + nodeSetName);
- if(translationVelocity)
+ if (translationVelocity)
+ {
ARMARX_DEBUG << "Setting new Velocity for " << nodeSetName << " in frame " << translationVelocity->frame << ":\n" << FramedDirectionPtr::dynamicCast(translationVelocity)->toEigen();
- if(orientationVelocityRPY)
+ }
+
+ if (orientationVelocityRPY)
+ {
ARMARX_DEBUG << "Orientation Velo in frame " << orientationVelocityRPY->frame << ": \n" << FramedDirectionPtr::dynamicCast(orientationVelocityRPY)->toEigen();
+ }
TCPVelocityData data;
data.nodeSetName = nodeSetName;
data.translationVelocity = FramedDirectionPtr::dynamicCast(translationVelocity);
data.orientationVelocity = FramedDirectionPtr::dynamicCast(orientationVelocityRPY);
- if(tcpName.empty())
+
+ if (tcpName.empty())
{
data.tcpName = jointExistenceCheckRobot->getRobotNodeSet(nodeSetName)->getTCP()->getName();
}
@@ -183,50 +209,60 @@ namespace armarx
data.tcpName = tcpName;
}
+
tcpVelocitiesMap[data.tcpName] = data;
lastCommandTime = IceUtil::Time::now();
}
- void TCPControlUnit::init(const Ice::Current & c)
+ void TCPControlUnit::init(const Ice::Current& c)
{
}
- void TCPControlUnit::start(const Ice::Current & c)
+ void TCPControlUnit::start(const Ice::Current& c)
{
}
- void TCPControlUnit::stop(const Ice::Current & c)
+ void TCPControlUnit::stop(const Ice::Current& c)
{
}
- UnitExecutionState TCPControlUnit::getExecutionState(const Ice::Current & c)
+ UnitExecutionState TCPControlUnit::getExecutionState(const Ice::Current& c)
{
switch (getState())
{
case eManagedIceObjectStarted:
return eUnitStarted;
+
case eManagedIceObjectInitialized:
case eManagedIceObjectStarting:
return eUnitInitialized;
+
case eManagedIceObjectExiting:
case eManagedIceObjectExited:
return eUnitStopped;
+
default:
return eUnitConstructed;
}
}
- void TCPControlUnit::request(const Ice::Current & c)
+ void TCPControlUnit::request(const Ice::Current& c)
{
ARMARX_IMPORTANT << "Requesting TCPControlUnit";
ScopedLock lock(dataMutex);
requested = true;
- if(execTask)
+
+ if (execTask)
{
- while (calculationRunning) IceUtil::ThreadControl::yield();
+ while (calculationRunning)
+ {
+ IceUtil::ThreadControl::yield();
+ }
+
execTask->stop();
}
+
lastCommandTime = IceUtil::Time::now();
execTask = new PeriodicTask<TCPControlUnit>(this, &TCPControlUnit::periodicExec, cycleTime, false, "TCPVelocityCalculator");
execTask->start();
@@ -234,11 +270,16 @@ namespace armarx
ARMARX_IMPORTANT << "Requested TCPControlUnit";
}
- void TCPControlUnit::release(const Ice::Current & c)
+ void TCPControlUnit::release(const Ice::Current& c)
{
ARMARX_IMPORTANT << "Releasing TCPControlUnit";
ScopedLock lock(dataMutex);
- while (calculationRunning) IceUtil::ThreadControl::yield();
+
+ while (calculationRunning)
+ {
+ IceUtil::ThreadControl::yield();
+ }
+
tcpVelocitiesMap.clear();
localTcpVelocitiesMap.clear();
requested = false;
@@ -257,21 +298,23 @@ namespace armarx
{
localTcpVelocitiesMap.clear();
TCPVelocityDataMap::iterator it = tcpVelocitiesMap.begin();
- for(; it != tcpVelocitiesMap.end(); it++)
+
+ for (; it != tcpVelocitiesMap.end(); it++)
{
localTcpVelocitiesMap[it->first] = it->second;
}
localDIKMap.clear();
DIKMap::iterator itDIK = dIKMap.begin();
- for(; itDIK != dIKMap.end(); itDIK++)
+
+ for (; itDIK != dIKMap.end(); itDIK++)
{
localDIKMap[itDIK->first] = itDIK->second;
}
//ARMARX_DEBUG << "RN TCP R pose1:" << localRobot->getRobotNode("TCP R")->getGlobalPose();
- RemoteRobot::synchronizeLocalClone(localRobot,robotStateComponentPrx);
+ RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponentPrx);
//ARMARX_DEBUG << "RN TCP R pose2:" << localRobot->getRobotNode("TCP R")->getGlobalPose();
}
}
@@ -287,7 +330,7 @@ namespace armarx
// release(); <---- this is a potential deadlock
// }
// else
- calcAndSetVelocities();
+ calcAndSetVelocities();
}
calculationRunning = false;
@@ -298,14 +341,19 @@ namespace armarx
void TCPControlUnit::calcAndSetVelocities()
{
TCPVelocityDataMap::iterator it = localTcpVelocitiesMap.begin();
- for(; it != localTcpVelocitiesMap.end(); it++)
+
+ for (; it != localTcpVelocitiesMap.end(); it++)
{
const TCPVelocityData& data = it->second;
RobotNodeSetPtr nodeSet = localRobot->getRobotNodeSet(data.nodeSetName);
-// std::string refFrame = nodeSet->getTCP()->getName();
+ // std::string refFrame = nodeSet->getTCP()->getName();
DIKMap::iterator itDIK = localDIKMap.find(data.nodeSetName);
- if(itDIK == localDIKMap.end())
+
+ if (itDIK == localDIKMap.end())
+ {
localDIKMap[data.nodeSetName].reset(new EDifferentialIK(nodeSet, localRobot->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+ }
+
EDifferentialIKPtr dIk = boost::dynamic_pointer_cast<EDifferentialIK>(localDIKMap[data.nodeSetName]);
dIk->clearGoals();
}
@@ -313,25 +361,31 @@ namespace armarx
using namespace Eigen;
it = localTcpVelocitiesMap.begin();
- for(; it != localTcpVelocitiesMap.end(); it++)
+
+ for (; it != localTcpVelocitiesMap.end(); it++)
{
TCPVelocityData& data = it->second;
-// RobotNodeSetPtr nodeSet = localRobot->getRobotNodeSet(data.nodeSetName);
+ // RobotNodeSetPtr nodeSet = localRobot->getRobotNodeSet(data.nodeSetName);
std::string refFrame = localRobot->getRootNode()->getName();
IKSolver::CartesianSelection mode;
- if(data.translationVelocity && data.orientationVelocity)
+
+ if (data.translationVelocity && data.orientationVelocity)
{
mode = IKSolver::All;
-// ARMARX_DEBUG << deactivateSpam(4) << "FullMode";
+ // ARMARX_DEBUG << deactivateSpam(4) << "FullMode";
}
- else if(data.translationVelocity && !data.orientationVelocity)
+ else if (data.translationVelocity && !data.orientationVelocity)
+ {
mode = IKSolver::Position;
- else if(!data.translationVelocity && data.orientationVelocity)
+ }
+ else if (!data.translationVelocity && data.orientationVelocity)
+ {
mode = IKSolver::Orientation;
+ }
else
{
-// ARMARX_VERBOSE << deactivateSpam(2) << "No mode feasible for " << data.nodeSetName << " - skipping";
+ // ARMARX_VERBOSE << deactivateSpam(2) << "No mode feasible for " << data.nodeSetName << " - skipping";
continue;
}
@@ -339,76 +393,91 @@ namespace armarx
Eigen::Matrix4f m;
m.setIdentity();
- if(data.orientationVelocity)
+ if (data.orientationVelocity)
{
Eigen::Matrix3f rotInOriginalFrame, rotInRefFrame;
- rotInOriginalFrame = Eigen::AngleAxisf(data.orientationVelocity->z*cycleTime*0.001, Eigen::Vector3f::UnitZ())
- * Eigen::AngleAxisf(data.orientationVelocity->y*cycleTime*0.001, Eigen::Vector3f::UnitY())
- * Eigen::AngleAxisf(data.orientationVelocity->x*cycleTime*0.001, Eigen::Vector3f::UnitX());
+ rotInOriginalFrame = Eigen::AngleAxisf(data.orientationVelocity->z * cycleTime * 0.001, Eigen::Vector3f::UnitZ())
+ * Eigen::AngleAxisf(data.orientationVelocity->y * cycleTime * 0.001, Eigen::Vector3f::UnitY())
+ * Eigen::AngleAxisf(data.orientationVelocity->x * cycleTime * 0.001, Eigen::Vector3f::UnitX());
+
if (data.orientationVelocity->frame != refFrame)
{
Eigen::Matrix4f trafoOriginalFrameToGlobal = Eigen::Matrix4f::Identity();
- if (data.orientationVelocity->frame != GlobalFrame) trafoOriginalFrameToGlobal = localRobot->getRobotNode(data.orientationVelocity->frame)->getGlobalPose();
+
+ if (data.orientationVelocity->frame != GlobalFrame)
+ {
+ trafoOriginalFrameToGlobal = localRobot->getRobotNode(data.orientationVelocity->frame)->getGlobalPose();
+ }
+
Eigen::Matrix4f trafoRefFrameToGlobal = Eigen::Matrix4f::Identity();
- if (refFrame != GlobalFrame) trafoRefFrameToGlobal = localRobot->getRobotNode(refFrame)->getGlobalPose();
+
+ if (refFrame != GlobalFrame)
+ {
+ trafoRefFrameToGlobal = localRobot->getRobotNode(refFrame)->getGlobalPose();
+ }
+
Eigen::Matrix4f trafoOriginalToRef = trafoRefFrameToGlobal.inverse() * trafoOriginalFrameToGlobal;
- rotInRefFrame = trafoOriginalToRef.block<3,3>(0,0) * rotInOriginalFrame * trafoOriginalToRef.block<3,3>(0,0).inverse();
+ rotInRefFrame = trafoOriginalToRef.block<3, 3>(0, 0) * rotInOriginalFrame * trafoOriginalToRef.block<3, 3>(0, 0).inverse();
}
else
{
rotInRefFrame = rotInOriginalFrame;
}
- m.block(0,0,3,3) = rotInRefFrame * (tcpNode->getGlobalPose().block(0,0,3,3) );
+
+ m.block(0, 0, 3, 3) = rotInRefFrame * (tcpNode->getGlobalPose().block(0, 0, 3, 3));
}
-// ARMARX_VERBOSE << deactivateSpam(1) << "Delta Mat: \n" << m;
+ // ARMARX_VERBOSE << deactivateSpam(1) << "Delta Mat: \n" << m;
-// m = m * tcpNode->getGlobalPose();
+ // m = m * tcpNode->getGlobalPose();
- if(data.translationVelocity)
+ if (data.translationVelocity)
{
data.translationVelocity = FramedDirection::ChangeFrame(localRobot, *data.translationVelocity, refFrame);
ARMARX_INFO << deactivateSpam(1) << "Translation in " << refFrame << ": " << data.translationVelocity->toEigen();
- m.block(0,3,3,1) = tcpNode->getGlobalPose().block(0,3,3,1) + data.translationVelocity->toEigen()*cycleTime*0.001;
+ m.block(0, 3, 3, 1) = tcpNode->getGlobalPose().block(0, 3, 3, 1) + data.translationVelocity->toEigen() * cycleTime * 0.001;
}
DifferentialIKPtr dIK = localDIKMap[data.nodeSetName];
- if(!dIK)
+
+ if (!dIK)
{
ARMARX_WARNING << deactivateSpam(1) << "DiffIK is NULL for robot node set: " << data.nodeSetName;
continue;
}
-// ARMARX_VERBOSE << deactivateSpam(1) << "Old Pos: \n" << tcpNode->getGlobalPose();
-// ARMARX_VERBOSE << deactivateSpam(1) << "New Goal: \n" << m;
-// if(tcpNode != nodeSet->getTCP())
-// {
-// mode = IKSolver::Z;
-// Eigen::VectorXf weight(1);
-// weight << 0.2;
-// boost::shared_dynamic_cast<EDifferentialIK>(dIK)->setGoal(m, tcpNode,mode, 0.001, 0.001/180.0f*3.14159, weight);
-// }
-// else
- dIK->setGoal(m, tcpNode,mode, 0.001, 0.001/180.0f*3.14159);
-
-// ARMARX_VERBOSE << deactivateSpam(1) << "Delta to Goal: " << dIK->getDeltaToGoal(tcpNode);
+
+ // ARMARX_VERBOSE << deactivateSpam(1) << "Old Pos: \n" << tcpNode->getGlobalPose();
+ // ARMARX_VERBOSE << deactivateSpam(1) << "New Goal: \n" << m;
+ // if(tcpNode != nodeSet->getTCP())
+ // {
+ // mode = IKSolver::Z;
+ // Eigen::VectorXf weight(1);
+ // weight << 0.2;
+ // boost::shared_dynamic_cast<EDifferentialIK>(dIK)->setGoal(m, tcpNode,mode, 0.001, 0.001/180.0f*3.14159, weight);
+ // }
+ // else
+ dIK->setGoal(m, tcpNode, mode, 0.001, 0.001 / 180.0f * 3.14159);
+
+ // ARMARX_VERBOSE << deactivateSpam(1) << "Delta to Goal: " << dIK->getDeltaToGoal(tcpNode);
}
NameValueMap targetVelocities;
NameControlModeMap controlModes;
DIKMap::iterator itDIK = localDIKMap.begin();
- for(; itDIK != localDIKMap.end(); itDIK++)
+
+ for (; itDIK != localDIKMap.end(); itDIK++)
{
RobotNodeSetPtr robotNodeSet = localRobot->getRobotNodeSet(itDIK->first);
EDifferentialIKPtr dIK = boost::dynamic_pointer_cast<EDifferentialIK>(itDIK->second);
-// ARMARX_VERBOSE << deactivateSpam(1) << "Old Pos: \n" << robotNodeSet->getTCP()->getGlobalPose();
-// dIK->setVerbose(true);
+ // ARMARX_VERBOSE << deactivateSpam(1) << "Old Pos: \n" << robotNodeSet->getTCP()->getGlobalPose();
+ // dIK->setVerbose(true);
Eigen::VectorXf jointDelta;
dIK->computeSteps(jointDelta, 0.8f, 0.001, 1, &EDifferentialIK::computeStep); // 1.0, 0.00001, 50
-// ARMARX_VERBOSE << deactivateSpam(1) << "New Goal: \n" << robotNodeSet->getTCP()->getGlobalPose();
+ // ARMARX_VERBOSE << deactivateSpam(1) << "New Goal: \n" << robotNodeSet->getTCP()->getGlobalPose();
MatrixXf fullJac = dIK->calcFullJacobian();
MatrixXf fullJacInv = dIK->computePseudoInverseJacobianMatrix(fullJac);
@@ -435,24 +504,25 @@ namespace armarx
if (jointLimitCompensation(0) == jointLimitCompensation(0))
{
- if (jointLimitCompensation.norm() > maxJointLimitCompensationRatio*jointDelta.norm())
+ if (jointLimitCompensation.norm() > maxJointLimitCompensationRatio * jointDelta.norm())
{
- jointLimitCompensation = maxJointLimitCompensationRatio*jointDelta.norm()/jointLimitCompensation.norm() * jointLimitCompensation;
+ jointLimitCompensation = maxJointLimitCompensationRatio * jointDelta.norm() / jointLimitCompensation.norm() * jointLimitCompensation;
}
+
jointDelta += jointLimitCompensation;
}
- jointDelta /=(cycleTime*0.001);
+ jointDelta /= (cycleTime * 0.001);
jointDelta = applyMaxJointVelocity(jointDelta, maxJointVelocity);
-// Eigen::Vector3f currentTCPPosition = robotNodeSet->getTCP()->getGlobalPose().block(0,3,3,1);
-// const Eigen::Matrix4f mat = robotNodeSet->getTCP()->getGlobalPose() * lastTCPPose.inverse();
-// const Eigen::Matrix3f rot = currentPose.block(0,0,3,3) * lastPose->toEigen().block(0,0,3,3).inverse();
-// Eigen::Vector3f rpy;
-// VirtualRobot::MathTools::eigen4f2rpy(mat, rpy);
-// ARMARX_DEBUG << deactivateSpam(2) << "Current TCP Position: \n" << currentTCPPosition;
-// ARMARX_VERBOSE << deactivateSpam(2)<< "ActualTCPVelocity: " << (rpy/(0.001*cycleTime));
+ // Eigen::Vector3f currentTCPPosition = robotNodeSet->getTCP()->getGlobalPose().block(0,3,3,1);
+ // const Eigen::Matrix4f mat = robotNodeSet->getTCP()->getGlobalPose() * lastTCPPose.inverse();
+ // const Eigen::Matrix3f rot = currentPose.block(0,0,3,3) * lastPose->toEigen().block(0,0,3,3).inverse();
+ // Eigen::Vector3f rpy;
+ // VirtualRobot::MathTools::eigen4f2rpy(mat, rpy);
+ // ARMARX_DEBUG << deactivateSpam(2) << "Current TCP Position: \n" << currentTCPPosition;
+ // ARMARX_VERBOSE << deactivateSpam(2)<< "ActualTCPVelocity: " << (rpy/(0.001*cycleTime));
lastTCPPose = robotNodeSet->getTCP()->getGlobalPose();
// build name value map
@@ -460,10 +530,14 @@ namespace armarx
const std::vector< VirtualRobot::RobotNodePtr > nodes = robotNodeSet->getAllRobotNodes();
std::vector< VirtualRobot::RobotNodePtr >::const_iterator iter = nodes.begin();
int i = 0;
+
while (iter != nodes.end() && i < jointDelta.rows())
{
- if(targetVelocities.find((*iter)->getName()) != targetVelocities.end())
+ if (targetVelocities.find((*iter)->getName()) != targetVelocities.end())
+ {
ARMARX_WARNING << deactivateSpam(2) << (*iter)->getName() << " is set from two joint delta calculations - overwriting first value";
+ }
+
targetVelocities.insert(std::make_pair((*iter)->getName(), jointDelta(i)));
controlModes.insert(std::make_pair((*iter)->getName(), eVelocityControl));
@@ -479,29 +553,35 @@ namespace armarx
- void TCPControlUnit::ContinuousAngles(const Eigen::AngleAxisf& oldAngle, Eigen::AngleAxisf& newAngle, double & offset)
+ void TCPControlUnit::ContinuousAngles(const Eigen::AngleAxisf& oldAngle, Eigen::AngleAxisf& newAngle, double& offset)
{
- // if(oldAngle.axis().isApprox(newAngle.axis()*-1))
+ // if(oldAngle.axis().isApprox(newAngle.axis()*-1))
const Eigen::Vector3f& v1 = oldAngle.axis();
const Eigen::Vector3f& v2 = newAngle.axis();
- const Eigen::Vector3f& v2i = newAngle.axis()*-1;
+ const Eigen::Vector3f& v2i = newAngle.axis() * -1;
double angle = acos(v1.dot(v2) / (v1.norm() * v2.norm()));
double angleInv = acos(v1.dot(v2i) / (v1.norm() * v2i.norm()));
-// std::cout << "angle1: " << angle << std::endl;
-// std::cout << "angleInv: " << angleInv << std::endl;
+ // std::cout << "angle1: " << angle << std::endl;
+ // std::cout << "angleInv: " << angleInv << std::endl;
+
+ // Eigen::AngleAxisd result;
+ if (angle > angleInv)
+ {
+ // ARMARX_IMPORTANT_S << "inversion needed" << std::endl;
+ newAngle = Eigen::AngleAxisf(2.0 * M_PI - newAngle.angle(), newAngle.axis() * -1);
+ }
+
+ // else newAngle = newAngle;
-// Eigen::AngleAxisd result;
- if(angle > angleInv)
+ if (fabs(newAngle.angle() + offset - oldAngle.angle()) > fabs(newAngle.angle() + offset - (oldAngle.angle() + M_PI * 2)))
+ {
+ offset -= M_PI * 2;
+ }
+ else if (fabs(newAngle.angle() + offset - oldAngle.angle()) > fabs((newAngle.angle() + M_PI * 2 + offset) - oldAngle.angle()))
{
-// ARMARX_IMPORTANT_S << "inversion needed" << std::endl;
- newAngle = Eigen::AngleAxisf(2.0*M_PI - newAngle.angle(), newAngle.axis()*-1);
+ offset += M_PI * 2;
}
-// else newAngle = newAngle;
- if(fabs(newAngle.angle()+offset - oldAngle.angle()) > fabs(newAngle.angle()+offset - (oldAngle.angle() + M_PI*2)) )
- offset -= M_PI*2;
- else if(fabs(newAngle.angle()+offset - oldAngle.angle()) > fabs((newAngle.angle()+M_PI*2+offset) - oldAngle.angle()) )
- offset += M_PI*2;
newAngle.angle() += offset;
}
@@ -510,12 +590,13 @@ namespace armarx
{
std::vector< VirtualRobot::RobotNodePtr > nodes = robotNodeSet->getAllRobotNodes();
Eigen::VectorXf actualJointValues(nodes.size());
- if(desiredJointValues.rows() == 0)
+
+ if (desiredJointValues.rows() == 0)
{
desiredJointValues.resize(nodes.size());
- for(unsigned int i=0; i < nodes.size(); i++)
+ for (unsigned int i = 0; i < nodes.size(); i++)
{
VirtualRobot::RobotNodePtr node = nodes.at(i);
desiredJointValues(i) = (node->getJointLimitHigh() - node->getJointLimitLow()) * 0.5f + node->getJointLimitLow();
@@ -523,23 +604,26 @@ namespace armarx
}
-// ARMARX_IMPORTANT << deactivateSpam(true, 0.5) << "desiredJointValues: " << desiredJointValues;
+
+ // ARMARX_IMPORTANT << deactivateSpam(true, 0.5) << "desiredJointValues: " << desiredJointValues;
Eigen::VectorXf jointCompensationDeltas(desiredJointValues.rows());
- for(unsigned int i = 0; i < desiredJointValues.rows(); i++)
+
+ for (unsigned int i = 0; i < desiredJointValues.rows(); i++)
{
VirtualRobot::RobotNodePtr node = nodes.at(i);
actualJointValues(i) = node->getJointValue();
- jointCompensationDeltas(i) = (node->getJointValue() - desiredJointValues(i) ) / (node->getJointLimitHigh() - node->getJointLimitLow());
- jointCompensationDeltas(i) = -pow(jointCompensationDeltas(i), 3)*pow(nodes.size()-i,2);
+ jointCompensationDeltas(i) = (node->getJointValue() - desiredJointValues(i)) / (node->getJointLimitHigh() - node->getJointLimitLow());
+ jointCompensationDeltas(i) = -pow(jointCompensationDeltas(i), 3) * pow(nodes.size() - i, 2);
}
-// ARMARX_IMPORTANT << deactivateSpam(true, 0.5) << "actualJointValues: " << actualJointValues;
- return CalcNullspaceJointDeltas(jointCompensationDeltas,jacobian, jacobianInverse);
+ // ARMARX_IMPORTANT << deactivateSpam(true, 0.5) << "actualJointValues: " << actualJointValues;
+
+ return CalcNullspaceJointDeltas(jointCompensationDeltas, jacobian, jacobianInverse);
}
Eigen::VectorXf TCPControlUnit::CalcNullspaceJointDeltas(const Eigen::VectorXf& desiredJointDeltas, const Eigen::MatrixXf& jacobian, const Eigen::MatrixXf& jacobianInverse)
{
- Eigen::MatrixXf I(jacobianInverse.rows(),jacobian.cols());
+ Eigen::MatrixXf I(jacobianInverse.rows(), jacobian.cols());
I.setIdentity();
Eigen::MatrixXf nullspaceProjection = I - jacobianInverse * jacobian;
@@ -552,18 +636,21 @@ namespace armarx
{
double currentMaxJointVel = std::numeric_limits<double>::min();
unsigned int rows = jointVelocity.rows();
- for(unsigned int i=0; i < rows; i++)
+
+ for (unsigned int i = 0; i < rows; i++)
{
currentMaxJointVel = std::max(fabs(jointVelocity(i)), currentMaxJointVel);
}
- if(currentMaxJointVel > maxJointVelocity)
+ if (currentMaxJointVel > maxJointVelocity)
{
ARMARX_IMPORTANT << deactivateSpam(1) << "max joint velocity too high: " << currentMaxJointVel << " allowed: " << maxJointVelocity;
- return jointVelocity * (maxJointVelocity/currentMaxJointVel);
+ return jointVelocity * (maxJointVelocity / currentMaxJointVel);
}
else
+ {
return jointVelocity;
+ }
}
@@ -583,27 +670,36 @@ namespace armarx
Eigen::MatrixXf EDifferentialIK::calcFullJacobian()
{
- if (nRows==0) this->setNRows();
+ if (nRows == 0)
+ {
+ this->setNRows();
+ }
+
size_t nDoF = nodes.size();
using namespace Eigen;
- MatrixXf Jacobian(nRows,nDoF);
+ MatrixXf Jacobian(nRows, nDoF);
+
+ size_t index = 0;
- size_t index=0;
- for (size_t i=0; i<tcp_set.size();i++)
+ for (size_t i = 0; i < tcp_set.size(); i++)
{
SceneObjectPtr tcp = tcp_set[i];
- if (this->targets.find(tcp)!=this->targets.end())
+
+ if (this->targets.find(tcp) != this->targets.end())
{
IKSolver::CartesianSelection mode = this->modes[tcp];
- MatrixXf partJacobian = this->getJacobianMatrix(tcp,mode);
- Jacobian.block(index,0,partJacobian.rows(),nDoF) = partJacobian;
+ MatrixXf partJacobian = this->getJacobianMatrix(tcp, mode);
+ Jacobian.block(index, 0, partJacobian.rows(), nDoF) = partJacobian;
}
else
- VR_ERROR << "Internal error?!" << endl; // Error
+ {
+ VR_ERROR << "Internal error?!" << endl; // Error
+ }
}
+
return Jacobian;
}
@@ -622,14 +718,16 @@ namespace armarx
this->coordSystem = coordSystem;
}
- void EDifferentialIK::setGoal(const Matrix4f &goal, RobotNodePtr tcp, IKSolver::CartesianSelection mode, float tolerancePosition, float toleranceRotation, VectorXf tcpWeight)
+ void EDifferentialIK::setGoal(const Matrix4f& goal, RobotNodePtr tcp, IKSolver::CartesianSelection mode, float tolerancePosition, float toleranceRotation, VectorXf tcpWeight)
{
- if(mode <= IKSolver::Z)
+ if (mode <= IKSolver::Z)
ARMARX_CHECK_EXPRESSION_W_HINT(tcpWeight.rows() == 1, "The tcpWeight vector must be of size 1")
- else if(mode <= IKSolver::Orientation)
- ARMARX_CHECK_EXPRESSION_W_HINT(tcpWeight.rows() == 3, "The tcpWeight vector must be of size 3")
- else if(mode == IKSolver::All)
- ARMARX_CHECK_EXPRESSION_W_HINT(tcpWeight.rows() == 6, "The tcpWeight vector must be of size 6");
+ else if (mode <= IKSolver::Orientation)
+ ARMARX_CHECK_EXPRESSION_W_HINT(tcpWeight.rows() == 3, "The tcpWeight vector must be of size 3")
+ else if (mode == IKSolver::All)
+ {
+ ARMARX_CHECK_EXPRESSION_W_HINT(tcpWeight.rows() == 6, "The tcpWeight vector must be of size 6");
+ }
tcpWeights[tcp] = tcpWeight;
DifferentialIK::setGoal(goal, tcp, mode, tolerancePosition, toleranceRotation);
@@ -646,19 +744,19 @@ namespace armarx
return computeSteps(jointDelta, stepSize, mininumChange, maxNStep, &DifferentialIK::computeStep);
}
-// void EDifferentialIK::setTCPWeights(Eigen::VectorXf tcpWeights)
-// {
-// this->tcpWeights = tcpWeights;
-// }
+ // void EDifferentialIK::setTCPWeights(Eigen::VectorXf tcpWeights)
+ // {
+ // this->tcpWeights = tcpWeights;
+ // }
- bool EDifferentialIK::computeSteps(VectorXf &resultJointDelta, float stepSize, float mininumChange, int maxNStep, ComputeFunction computeFunction)
+ bool EDifferentialIK::computeSteps(VectorXf& resultJointDelta, float stepSize, float mininumChange, int maxNStep, ComputeFunction computeFunction)
{
VR_ASSERT(rns);
VR_ASSERT(nodes.size() == rns->getSize());
//std::vector<RobotNodePtr> rn = this->nodes;
RobotPtr robot = rns->getRobot();
VR_ASSERT(robot);
- std::vector<float> jv(nodes.size(),0.0f);
+ std::vector<float> jv(nodes.size(), 0.0f);
std::vector<float> jvBest = rns->getJointValues();
int step = 0;
checkTolerances();
@@ -668,76 +766,89 @@ namespace armarx
VectorXf oldJointValues;
rns->getJointValues(oldJointValues);
VectorXf tempDOFWeights = dofWeights;
-// VectorXf dThetaSum(nodes.size());
+ // VectorXf dThetaSum(nodes.size());
resultJointDelta.resize(nodes.size());
resultJointDelta.setZero();
+
do
{
VectorXf dTheta = (this->*computeFunction)(stepSize);
- if(adjustDOFWeightsToJointLimits(dTheta))
+
+ if (adjustDOFWeightsToJointLimits(dTheta))
+ {
dTheta = computeStep(stepSize);
+ }
- for (unsigned int i=0; i<nodes.size();i++)
+ for (unsigned int i = 0; i < nodes.size(); i++)
{
ARMARX_DEBUG << VAROUT(nodes[i]->getJointValue()) << VAROUT(dTheta[i]);
jv[i] = (nodes[i]->getJointValue() + dTheta[i]);
+
if (boost::math::isnan(jv[i]) || boost::math::isinf(jv[i]))
{
ARMARX_WARNING_S << "Aborting, invalid joint value (nan)" << endl;
dofWeights = tempDOFWeights;
return false;
}
+
//nodes[i]->setJointValue(nodes[i]->getJointValue() + dTheta[i]);
}
- robot->setJointValues(rns,jv);
+ robot->setJointValues(rns, jv);
VectorXf newJointValues;
rns->getJointValues(newJointValues);
resultJointDelta = newJointValues - oldJointValues;
-// ARMARX_DEBUG << "joint angle deltas:\n" << dThetaSum;
+ // ARMARX_DEBUG << "joint angle deltas:\n" << dThetaSum;
// check tolerances
if (checkTolerances())
{
if (verbose)
+ {
ARMARX_INFO << deactivateSpam(1) << "Tolerances ok, loop:" << step << endl;
+ }
+
break;
}
+
float d = dTheta.norm();
float posDist = getMeanErrorPosition();
float oriErr = getErrorRotation(rns->getTCP());
- if (dTheta.norm()<mininumChange)
+
+ if (dTheta.norm() < mininumChange)
{
-// if (verbose)
- ARMARX_INFO << deactivateSpam(1) << "Could not improve result any more (dTheta.norm()=" << d << "), loop:" << step << " Resulting error: pos " << posDist << " orientation: " << oriErr << endl;
+ // if (verbose)
+ ARMARX_INFO << deactivateSpam(1) << "Could not improve result any more (dTheta.norm()=" << d << "), loop:" << step << " Resulting error: pos " << posDist << " orientation: " << oriErr << endl;
break;
}
- if (checkImprovement && posDist>lastDist)
+ if (checkImprovement && posDist > lastDist)
{
-// if (verbose)
- ARMARX_INFO << deactivateSpam(1) << "Could not improve result any more (current position error=" << posDist << ", last loop's error:" << lastDist << "), loop:" << step << endl;
- robot->setJointValues(rns,jvBest);
+ // if (verbose)
+ ARMARX_INFO << deactivateSpam(1) << "Could not improve result any more (current position error=" << posDist << ", last loop's error:" << lastDist << "), loop:" << step << endl;
+ robot->setJointValues(rns, jvBest);
break;
}
+
jvBest = jv;
lastDist = posDist;
step++;
jointDeltaIterations.push_back(std::make_pair(getWeightedError(), resultJointDelta));
}
- while (step<maxNStep);
+ while (step < maxNStep);
float bestJVError = std::numeric_limits<float>::max();
- for(unsigned int i=0; i < jointDeltaIterations.size(); i++)
+
+ for (unsigned int i = 0; i < jointDeltaIterations.size(); i++)
{
- if(jointDeltaIterations.at(i).first < bestJVError)
+ if (jointDeltaIterations.at(i).first < bestJVError)
{
bestJVError = jointDeltaIterations.at(i).first;
resultJointDelta = jointDeltaIterations.at(i).second;
@@ -746,15 +857,16 @@ namespace armarx
}
}
- robot->setJointValues(rns,oldJointValues + resultJointDelta);
+ robot->setJointValues(rns, oldJointValues + resultJointDelta);
-// ARMARX_DEBUG << "best try: " << bestIndex << " with error: " << bestJVError;
-// rns->setJointValues(oldJointValues);
-// Matrix4f oldPose = rns->getTCP()->getGlobalPose();
-// rns->setJointValues(oldJointValues+dThetaSum);
-// Matrix4f newPose = rns->getTCP()->getGlobalPose();
-// ARMARX_IMPORTANT << "tcp delta:\n" << newPose.block(0,3,3,1) - oldPose.block(0,3,3,1);
+ // ARMARX_DEBUG << "best try: " << bestIndex << " with error: " << bestJVError;
+ // rns->setJointValues(oldJointValues);
+ // Matrix4f oldPose = rns->getTCP()->getGlobalPose();
+ // rns->setJointValues(oldJointValues+dThetaSum);
+ // Matrix4f newPose = rns->getTCP()->getGlobalPose();
+ // ARMARX_IMPORTANT << "tcp delta:\n" << newPose.block(0,3,3,1) - oldPose.block(0,3,3,1);
dofWeights = tempDOFWeights;
+
if (step >= maxNStep && verbose)
{
ARMARX_INFO << deactivateSpam(1) << "IK failed, loop:" << step << endl;
@@ -766,58 +878,70 @@ namespace armarx
return true;
}
- VectorXf EDifferentialIK::computeStep(float stepSize )
+ VectorXf EDifferentialIK::computeStep(float stepSize)
{
- if (nRows==0) this->setNRows();
+ if (nRows == 0)
+ {
+ this->setNRows();
+ }
+
size_t nDoF = nodes.size();
- MatrixXf Jacobian(nRows,nDoF);
+ MatrixXf Jacobian(nRows, nDoF);
VectorXf error(nRows);
- size_t index=0;
- for (size_t i=0; i<tcp_set.size();i++)
+ size_t index = 0;
+
+ for (size_t i = 0; i < tcp_set.size(); i++)
{
SceneObjectPtr tcp = tcp_set[i];
- if (this->targets.find(tcp)!=this->targets.end())
+
+ if (this->targets.find(tcp) != this->targets.end())
{
Eigen::VectorXf delta = getDeltaToGoal(tcp);
//ARMARX_DEBUG << VAROUT(delta);
IKSolver::CartesianSelection mode = this->modes[tcp];
- MatrixXf partJacobian = this->getJacobianMatrix(tcp,mode);
+ MatrixXf partJacobian = this->getJacobianMatrix(tcp, mode);
//ARMARX_DEBUG << VAROUT(partJacobian);
- Jacobian.block(index,0,partJacobian.rows(),nDoF) = partJacobian;
+ Jacobian.block(index, 0, partJacobian.rows(), nDoF) = partJacobian;
Vector3f position = delta.head(3);
- position *=stepSize;
+ position *= stepSize;
+
if (mode & IKSolver::X)
{
error(index) = position(0);
index++;
}
+
if (mode & IKSolver::Y)
{
error(index) = position(1);
index++;
}
+
if (mode & IKSolver::Z)
{
error(index) = position(2);
index++;
}
+
if (mode & IKSolver::Orientation)
{
- error.segment(index,3) = delta.tail(3)*stepSize;
- index+=3;
+ error.segment(index, 3) = delta.tail(3) * stepSize;
+ index += 3;
}
}
else
- VR_ERROR << "Internal error?!" << endl; // Error
+ {
+ VR_ERROR << "Internal error?!" << endl; // Error
+ }
}
-// applyDOFWeightsToJacobian(Jacobian);
+ // applyDOFWeightsToJacobian(Jacobian);
ARMARX_DEBUG << VAROUT(Jacobian);
MatrixXf pseudo = computePseudoInverseJacobianMatrix(Jacobian);
ARMARX_DEBUG << VAROUT(pseudo);
@@ -826,72 +950,91 @@ namespace armarx
}
- VectorXf EDifferentialIK::computeStepIndependently(float stepSize )
+ VectorXf EDifferentialIK::computeStepIndependently(float stepSize)
{
- if (nRows==0) this->setNRows();
+ if (nRows == 0)
+ {
+ this->setNRows();
+ }
+
size_t nDoF = nodes.size();
std::map<float, MatrixXf> partJacobians;
VectorXf thetaSum(nDoF);
thetaSum.setZero();
- size_t index=0;
- for (size_t i=0; i<tcp_set.size();i++)
+ size_t index = 0;
+
+ for (size_t i = 0; i < tcp_set.size(); i++)
{
SceneObjectPtr tcp = tcp_set[i];
- if (this->targets.find(tcp)!=this->targets.end())
+
+ if (this->targets.find(tcp) != this->targets.end())
{
Eigen::VectorXf delta = getDeltaToGoal(tcp);
IKSolver::CartesianSelection mode = this->modes[tcp];
- MatrixXf partJacobian = this->getJacobianMatrix(tcp,mode);
+ MatrixXf partJacobian = this->getJacobianMatrix(tcp, mode);
// apply tcp weights
-// applyTCPWeights(tcp, partJacobian);
+ // applyTCPWeights(tcp, partJacobian);
int tcpDOF = 0;
- if(mode <= IKSolver::Z)
+
+ if (mode <= IKSolver::Z)
+ {
tcpDOF = 1;
- else if(mode <= IKSolver::Orientation)
+ }
+ else if (mode <= IKSolver::Orientation)
+ {
tcpDOF = 3;
- else if(mode == IKSolver::All)
+ }
+ else if (mode == IKSolver::All)
+ {
tcpDOF = 6;
+ }
+
index = 0;
VectorXf partError(tcpDOF);
Vector3f position = delta.head(3);
-// ARMARX_DEBUG << tcp->getName() << "'s error: " << position << " weighted error: " << position*stepSize;
- position *=stepSize;
+ // ARMARX_DEBUG << tcp->getName() << "'s error: " << position << " weighted error: " << position*stepSize;
+ position *= stepSize;
+
if (mode & IKSolver::X)
{
partError(index) = position(0);
index++;
}
+
if (mode & IKSolver::Y)
{
partError(index) = position(1);
index++;
}
+
if (mode & IKSolver::Z)
{
partError(index) = position(2);
index++;
}
+
if (mode & IKSolver::Orientation)
{
- partError.segment(index,3) = delta.tail(3)*stepSize;
- index+=3;
+ partError.segment(index, 3) = delta.tail(3) * stepSize;
+ index += 3;
}
ARMARX_DEBUG << deactivateSpam(0.05) << "step size adjusted error to goal:\n" << partError;
applyDOFWeightsToJacobian(partJacobian);
-// ARMARX_DEBUG << "Jac:\n" << partJacobian;
+ // ARMARX_DEBUG << "Jac:\n" << partJacobian;
MatrixXf pseudo = computePseudoInverseJacobianMatrix(partJacobian);
Eigen::VectorXf tcpWeightVec;
std::map<VirtualRobot:: SceneObjectPtr, Eigen::VectorXf>::iterator it = tcpWeights.find(tcp);
- if(it != tcpWeights.end())
+
+ if (it != tcpWeights.end())
{
tcpWeightVec = it->second;
}
@@ -899,40 +1042,51 @@ namespace armarx
{
IKSolver::CartesianSelection mode = modes[tcp_set.at(i)];
int size = 1;
- if(mode <= IKSolver::Z)
+
+ if (mode <= IKSolver::Z)
+ {
size = 1;
- else if(mode <= IKSolver::Orientation)
+ }
+ else if (mode <= IKSolver::Orientation)
+ {
size = 3;
- else if(mode == IKSolver::All)
+ }
+ else if (mode == IKSolver::All)
+ {
size = 6;
+ }
Eigen::VectorXf tmptcpWeightVec(size);
tmptcpWeightVec.setOnes();
- tcpWeightVec= tmptcpWeightVec;
+ tcpWeightVec = tmptcpWeightVec;
}
- if(pseudo.cols() == tcpWeightVec.rows())
+ if (pseudo.cols() == tcpWeightVec.rows())
{
Eigen::MatrixXf weightMat(tcpWeightVec.rows(), tcpWeightVec.rows());
weightMat.setIdentity();
weightMat.diagonal() = tcpWeightVec;
- // ARMARX_DEBUG << /*deactivateSpam(1) <<*/ "tcpWeightVec:\n" << tcpWeightVec;
- // ARMARX_DEBUG << /*deactivateSpam(1) << */"InvJac Before:\n" << invJacobian;
+ // ARMARX_DEBUG << /*deactivateSpam(1) <<*/ "tcpWeightVec:\n" << tcpWeightVec;
+ // ARMARX_DEBUG << /*deactivateSpam(1) << */"InvJac Before:\n" << invJacobian;
pseudo = pseudo * weightMat;
-// ARMARX_DEBUG << /*deactivateSpam(1) <<*/ "InvJac after:\n" << pseudo;
+ // ARMARX_DEBUG << /*deactivateSpam(1) <<*/ "InvJac after:\n" << pseudo;
}
else
+ {
ARMARX_WARNING << deactivateSpam(3) << "Wrong dimension of tcp weights: " << tcpWeightVec.rows() << ", but should be: " << pseudo.cols();
+ }
thetaSum += pseudo * partError;
}
else
- VR_ERROR << "Internal error?!" << endl; // Error
+ {
+ VR_ERROR << "Internal error?!" << endl; // Error
+ }
}
@@ -946,28 +1100,33 @@ namespace armarx
rns->getJointValues(jointValuesBefore);
Eigen::VectorXf resultJointDelta;
Eigen::VectorXf jointDeltaAlternative;
- bool result = computeSteps(resultJointDelta, stepSize,minChange, maxSteps);
+ bool result = computeSteps(resultJointDelta, stepSize, minChange, maxSteps);
float error = getWeightedError();
- if(useAlternativeOnFail && error > 5)
- result = computeSteps(jointDeltaAlternative,stepSize,minChange, maxSteps, &EDifferentialIK::computeStepIndependently);
- if(getWeightedError() < error)
+
+ if (useAlternativeOnFail && error > 5)
+ {
+ result = computeSteps(jointDeltaAlternative, stepSize, minChange, maxSteps, &EDifferentialIK::computeStepIndependently);
+ }
+
+ if (getWeightedError() < error)
{
resultJointDelta = jointDeltaAlternative;
}
+
return result;
}
- void EDifferentialIK::applyDOFWeightsToJacobian(MatrixXf &Jacobian)
+ void EDifferentialIK::applyDOFWeightsToJacobian(MatrixXf& Jacobian)
{
- if(dofWeights.rows() == Jacobian.cols())
+ if (dofWeights.rows() == Jacobian.cols())
{
Eigen::MatrixXf weightMat(dofWeights.rows(), dofWeights.rows());
weightMat.setIdentity();
weightMat.diagonal() = dofWeights;
-// ARMARX_DEBUG << deactivateSpam(1) << "Jac before:\n" << Jacobian;
-// ARMARX_DEBUG << deactivateSpam(1) << "Weights:\n" << weightMat;
+ // ARMARX_DEBUG << deactivateSpam(1) << "Jac before:\n" << Jacobian;
+ // ARMARX_DEBUG << deactivateSpam(1) << "Weights:\n" << weightMat;
Jacobian = Jacobian * weightMat;
-// ARMARX_DEBUG << deactivateSpam(1) << "Jac after:\n" << Jacobian;
+ // ARMARX_DEBUG << deactivateSpam(1) << "Jac after:\n" << Jacobian;
}
}
@@ -976,10 +1135,12 @@ namespace armarx
{
std::map<VirtualRobot:: SceneObjectPtr, Eigen::VectorXf>::iterator it = tcpWeights.find(tcp);
- if(it != tcpWeights.end())
+
+ if (it != tcpWeights.end())
{
Eigen::VectorXf& tcpWeightVec = it->second;
- if(partJacobian.rows() == tcpWeightVec.rows())
+
+ if (partJacobian.rows() == tcpWeightVec.rows())
{
Eigen::MatrixXf weightMat(tcpWeightVec.rows(), tcpWeightVec.rows());
@@ -990,39 +1151,58 @@ namespace armarx
ARMARX_DEBUG << deactivateSpam(1) << "Jac after: " << partJacobian;
}
else
+ {
ARMARX_WARNING << deactivateSpam(3) << "Wrong dimension of tcp weights: " << tcpWeightVec.rows() << ", but should be: " << partJacobian.rows();
+ }
}
}
void EDifferentialIK::applyTCPWeights(MatrixXf& invJacobian)
{
- if(tcpWeightVec.rows() == 0)
- for (size_t i=0; i<tcp_set.size();i++)
+ if (tcpWeightVec.rows() == 0)
+ for (size_t i = 0; i < tcp_set.size(); i++)
{
std::map<VirtualRobot:: SceneObjectPtr, Eigen::VectorXf>::iterator it = tcpWeights.find(tcp_set.at(i));
- if(it != tcpWeights.end())
+
+ if (it != tcpWeights.end())
{
Eigen::VectorXf& tmptcpWeightVec = it->second;
Eigen::VectorXf oldVec = tcpWeightVec;
- tcpWeightVec.resize(tcpWeightVec.rows()+tmptcpWeightVec.rows());
- if(oldVec.rows()>0)
+ tcpWeightVec.resize(tcpWeightVec.rows() + tmptcpWeightVec.rows());
+
+ if (oldVec.rows() > 0)
+ {
tcpWeightVec.head(oldVec.rows()) = oldVec;
+ }
+
tcpWeightVec.tail(tmptcpWeightVec.rows()) = tmptcpWeightVec;
}
else
{
IKSolver::CartesianSelection mode = modes[tcp_set.at(i)];
int size = 1;
- if(mode <= IKSolver::Z)
+
+ if (mode <= IKSolver::Z)
+ {
size = 1;
- else if(mode <= IKSolver::Orientation)
+ }
+ else if (mode <= IKSolver::Orientation)
+ {
size = 3;
- else if(mode == IKSolver::All)
+ }
+ else if (mode == IKSolver::All)
+ {
size = 6;
+ }
+
Eigen::VectorXf oldVec = tcpWeightVec;
- tcpWeightVec.resize(tcpWeightVec.rows()+size);
- if(oldVec.rows()>0)
+ tcpWeightVec.resize(tcpWeightVec.rows() + size);
+
+ if (oldVec.rows() > 0)
+ {
tcpWeightVec.head(oldVec.rows()) = oldVec;
+ }
+
Eigen::VectorXf tmptcpWeightVec(size);
tmptcpWeightVec.setOnes();
tcpWeightVec.tail(size) = tmptcpWeightVec;
@@ -1030,72 +1210,91 @@ namespace armarx
}
}
- if(invJacobian.cols() == tcpWeightVec.rows())
+ if (invJacobian.cols() == tcpWeightVec.rows())
{
Eigen::MatrixXf weightMat(tcpWeightVec.rows(), tcpWeightVec.rows());
weightMat.setIdentity();
weightMat.diagonal() = tcpWeightVec;
-// ARMARX_DEBUG << /*deactivateSpam(1) <<*/ "tcpWeightVec:\n" << tcpWeightVec;
-// ARMARX_DEBUG << /*deactivateSpam(1) << */"InvJac Before:\n" << invJacobian;
+ // ARMARX_DEBUG << /*deactivateSpam(1) <<*/ "tcpWeightVec:\n" << tcpWeightVec;
+ // ARMARX_DEBUG << /*deactivateSpam(1) << */"InvJac Before:\n" << invJacobian;
invJacobian = invJacobian * weightMat;
ARMARX_DEBUG << /*deactivateSpam(1) <<*/ "InvJac after:\n" << invJacobian;
}
else
+ {
ARMARX_WARNING << deactivateSpam(3) << "Wrong dimension of tcp weights: " << tcpWeightVec.rows() << ", but should be: " << invJacobian.cols();
+ }
}
float EDifferentialIK::getWeightedError()
{
float result = 0.0f;
- float positionOrientationRatio = 3.f/180.f*M_PI;
- for (size_t i=0; i<tcp_set.size();i++){
+ float positionOrientationRatio = 3.f / 180.f * M_PI;
+
+ for (size_t i = 0; i < tcp_set.size(); i++)
+ {
SceneObjectPtr tcp = tcp_set[i];
- result += getWeightedErrorPosition(tcp) + getErrorRotation(tcp)*positionOrientationRatio;
+ result += getWeightedErrorPosition(tcp) + getErrorRotation(tcp) * positionOrientationRatio;
}
+
return result;
}
float EDifferentialIK::getWeightedErrorPosition(SceneObjectPtr tcp)
{
if (modes[tcp] == IKSolver::Orientation)
- return 0.0f; // ignoring position
+ {
+ return 0.0f; // ignoring position
+ }
if (!tcp)
+ {
tcp = getDefaultTCP();
- Vector3f position = targets[tcp].block(0,3,3,1) - tcp->getGlobalPose().block(0,3,3,1);
+ }
+
+ Vector3f position = targets[tcp].block(0, 3, 3, 1) - tcp->getGlobalPose().block(0, 3, 3, 1);
//cout << "Error Position <" << tcp->getName() << ">: " << position.norm() << endl;
//cout << boost::format("Error Position < %1% >: %2%") % tcp->getName() % position.norm() << std::endl;
float result = 0.0f;
Eigen::VectorXf tcpWeight(3);
- if(tcpWeights.find(tcp) != tcpWeights.end())
+
+ if (tcpWeights.find(tcp) != tcpWeights.end())
+ {
tcpWeight = tcpWeights.find(tcp)->second;
+ }
else
+ {
tcpWeight.setOnes();
+ }
+
int weightIndex = 0;
+
if (modes[tcp] & IKSolver::X)
{
-// ARMARX_DEBUG << "error x: " << position(0)*tcpWeight(weightIndex);
+ // ARMARX_DEBUG << "error x: " << position(0)*tcpWeight(weightIndex);
result += position(0) * position(0) * tcpWeight(weightIndex++);
}
+
if (modes[tcp] & IKSolver::Y)
{
-// ARMARX_DEBUG << "error y: " << position(1)*tcpWeight(weightIndex);
+ // ARMARX_DEBUG << "error y: " << position(1)*tcpWeight(weightIndex);
result += position(1) * position(1) * tcpWeight(weightIndex++);
}
+
if (modes[tcp] & IKSolver::Z)
{
-// ARMARX_DEBUG << "error z: " << position(2)*tcpWeight(weightIndex);
+ // ARMARX_DEBUG << "error z: " << position(2)*tcpWeight(weightIndex);
result += position(2) * position(2) * tcpWeight(weightIndex++);
}
return sqrtf(result);
}
- bool EDifferentialIK::adjustDOFWeightsToJointLimits(const VectorXf &plannedJointDeltas)
+ bool EDifferentialIK::adjustDOFWeightsToJointLimits(const VectorXf& plannedJointDeltas)
{
- if(dofWeights.rows() != plannedJointDeltas.rows())
+ if (dofWeights.rows() != plannedJointDeltas.rows())
{
dofWeights.resize(plannedJointDeltas.rows());
dofWeights.setOnes();
@@ -1103,16 +1302,19 @@ namespace armarx
}
bool result = false;
- for (unsigned int i=0; i<nodes.size();i++)
+
+ for (unsigned int i = 0; i < nodes.size(); i++)
{
- float angle = nodes[i]->getJointValue() + plannedJointDeltas[i]*0.1;
- if(angle > nodes[i]->getJointLimitHi() || angle < nodes[i]->getJointLimitLo())
+ float angle = nodes[i]->getJointValue() + plannedJointDeltas[i] * 0.1;
+
+ if (angle > nodes[i]->getJointLimitHi() || angle < nodes[i]->getJointLimitLo())
{
ARMARX_VERBOSE << deactivateSpam(3) << nodes[i]->getName() << " joint deactivated because of joint limit";
dofWeights(i) = 0;
result = true;
}
}
+
return result;
}
@@ -1123,104 +1325,120 @@ namespace armarx
-void armarx::TCPControlUnit::reportControlModeChanged(const NameControlModeMap &, bool, const Ice::Current &)
+void armarx::TCPControlUnit::reportControlModeChanged(const NameControlModeMap&, bool, const Ice::Current&)
{
}
-void armarx::TCPControlUnit::reportJointAngles(const NameValueMap &jointAngles, bool, const Ice::Current &)
+void armarx::TCPControlUnit::reportJointAngles(const NameValueMap& jointAngles, bool, const Ice::Current&)
{
ScopedLock lock(reportMutex);
FramedPoseBaseMap tcpPoses;
std::string rootFrame = localReportRobot->getRootNode()->getName();
localReportRobot->setJointValues(jointAngles);
+
//IceUtil::Time start = IceUtil::Time::now();
- for(unsigned int i=0; i < nodesToReport.size(); i++)
+ for (unsigned int i = 0; i < nodesToReport.size(); i++)
{
RobotNodePtr& node = nodesToReport.at(i);
- const std::string &tcpName = node->getName();
- const Eigen::Matrix4f ¤tPose = node->getPoseInRootFrame();
+ const std::string& tcpName = node->getName();
+ const Eigen::Matrix4f& currentPose = node->getPoseInRootFrame();
tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame, localReportRobot->getName());
}
-// ARMARX_DEBUG << deactivateSpam(5) << "TCP Pose Calc took: " << ( IceUtil::Time::now() - start).toMilliSecondsDouble();
+
+ // ARMARX_DEBUG << deactivateSpam(5) << "TCP Pose Calc took: " << ( IceUtil::Time::now() - start).toMilliSecondsDouble();
listener->reportTCPPose(tcpPoses);
}
-void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap &jointVel, bool, const Ice::Current &)
+void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap& jointVel, bool, const Ice::Current&)
{
- if((IceUtil::Time::now()-lastTopicReportTime).toMilliSeconds() < cycleTime )
+ if ((IceUtil::Time::now() - lastTopicReportTime).toMilliSeconds() < cycleTime)
+ {
return;
+ }
+
lastTopicReportTime = IceUtil::Time::now();
ScopedLock lock(reportMutex);
- if(!localVelReportRobot)
+
+ if (!localVelReportRobot)
+ {
localVelReportRobot = localReportRobot->clone(localReportRobot->getName());
-// ARMARX_DEBUG << jointVel; FramedPoseBaseMap tcpPoses;
+ }
+
+ // ARMARX_DEBUG << jointVel; FramedPoseBaseMap tcpPoses;
FramedDirectionMap tcpTranslationVelocities;
FramedDirectionMap tcpOrientationVelocities;
std::string rootFrame = localReportRobot->getRootNode()->getName();
NameValueMap tempJointAngles = localReportRobot->getConfig()->getRobotNodeJointValueMap();
FramedPoseBaseMap tcpPoses;
- for(unsigned int i=0; i < nodesToReport.size(); i++)
+
+ for (unsigned int i = 0; i < nodesToReport.size(); i++)
{
RobotNodePtr node = nodesToReport.at(i);
- const std::string &tcpName = node->getName();
- const Eigen::Matrix4f ¤tPose = node->getPoseInRootFrame();
+ const std::string& tcpName = node->getName();
+ const Eigen::Matrix4f& currentPose = node->getPoseInRootFrame();
tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame, localReportRobot->getName());
}
double tDelta = 0.001;
- for(NameValueMap::iterator it = tempJointAngles.begin(); it != tempJointAngles.end(); it++)
+
+ for (NameValueMap::iterator it = tempJointAngles.begin(); it != tempJointAngles.end(); it++)
{
NameValueMap::const_iterator itSrc = jointVel.find(it->first);
- if(itSrc != jointVel.end())
+
+ if (itSrc != jointVel.end())
+ {
it->second += itSrc->second * tDelta;
+ }
}
localVelReportRobot->setJointValues(tempJointAngles);
+
//IceUtil::Time start = IceUtil::Time::now();
- for(unsigned int i=0; i < nodesToReport.size(); i++)
+ for (unsigned int i = 0; i < nodesToReport.size(); i++)
{
RobotNodePtr node = localVelReportRobot->getRobotNode(nodesToReport.at(i)->getName());
- const std::string &tcpName = node->getName();
- const Eigen::Matrix4f ¤tPose = node->getPoseInRootFrame();
+ const std::string& tcpName = node->getName();
+ const Eigen::Matrix4f& currentPose = node->getPoseInRootFrame();
FramedPosePtr lastPose = FramedPosePtr::dynamicCast(tcpPoses[tcpName]);
- tcpTranslationVelocities[tcpName] = new FramedDirection((currentPose.block(0,3,3,1) - lastPose->toEigen().block(0,3,3,1))/tDelta, rootFrame, localReportRobot->getName());
+ tcpTranslationVelocities[tcpName] = new FramedDirection((currentPose.block(0, 3, 3, 1) - lastPose->toEigen().block(0, 3, 3, 1)) / tDelta, rootFrame, localReportRobot->getName());
const Eigen::Matrix4f mat = currentPose * lastPose->toEigen().inverse();
-// const Eigen::Matrix3f rot = currentPose.block(0,0,3,3) * lastPose->toEigen().block(0,0,3,3).inverse();
+ // const Eigen::Matrix3f rot = currentPose.block(0,0,3,3) * lastPose->toEigen().block(0,0,3,3).inverse();
Eigen::Vector3f rpy;
VirtualRobot::MathTools::eigen4f2rpy(mat, rpy);
-// Eigen::AngleAxisf orient(rot.block<3,3>(0,0));
+ // Eigen::AngleAxisf orient(rot.block<3,3>(0,0));
- tcpOrientationVelocities[tcpName] = new FramedDirection(rpy/tDelta, rootFrame, localReportRobot->getName());
+ tcpOrientationVelocities[tcpName] = new FramedDirection(rpy / tDelta, rootFrame, localReportRobot->getName());
}
-// ARMARX_DEBUG << deactivateSpam(5) << "TCP Pose Vel Calc took: " << ( IceUtil::Time::now() - start).toMilliSecondsDouble();
+
+ // ARMARX_DEBUG << deactivateSpam(5) << "TCP Pose Vel Calc took: " << ( IceUtil::Time::now() - start).toMilliSecondsDouble();
listener->reportTCPVelocities(tcpTranslationVelocities, tcpOrientationVelocities);
}
-void armarx::TCPControlUnit::reportJointTorques(const NameValueMap &, bool, const Ice::Current &)
+void armarx::TCPControlUnit::reportJointTorques(const NameValueMap&, bool, const Ice::Current&)
{
}
-void armarx::TCPControlUnit::reportJointAccelerations(const armarx::NameValueMap &jointAccelerations, bool aValueChanged, const Ice::Current &c)
+void armarx::TCPControlUnit::reportJointAccelerations(const armarx::NameValueMap& jointAccelerations, bool aValueChanged, const Ice::Current& c)
{
}
-void armarx::TCPControlUnit::reportJointCurrents(const NameValueMap &, bool, const Ice::Current &)
+void armarx::TCPControlUnit::reportJointCurrents(const NameValueMap&, bool, const Ice::Current&)
{
}
-void armarx::TCPControlUnit::reportJointMotorTemperatures(const NameValueMap &, bool, const Ice::Current &)
+void armarx::TCPControlUnit::reportJointMotorTemperatures(const NameValueMap&, bool, const Ice::Current&)
{
}
-void armarx::TCPControlUnit::reportJointStatuses(const NameStatusMap &, bool, const Ice::Current &)
+void armarx::TCPControlUnit::reportJointStatuses(const NameStatusMap&, bool, const Ice::Current&)
{
}
diff --git a/source/RobotAPI/components/units/TCPControlUnit.h b/source/RobotAPI/components/units/TCPControlUnit.h
index 96b5a5dfe7fe281ab019eb7f902626f3c635bea2..31ddf540cc143fb5ad18dd90d268ccbf242481af 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.h
+++ b/source/RobotAPI/components/units/TCPControlUnit.h
@@ -39,15 +39,15 @@ namespace armarx
* \brief
*/
class TCPControlUnitPropertyDefinitions:
- public ComponentPropertyDefinitions
+ public ComponentPropertyDefinitions
{
public:
TCPControlUnitPropertyDefinitions(std::string prefix):
ComponentPropertyDefinitions(prefix)
{
- defineRequiredProperty<std::string>("KinematicUnitName","Name of the KinematicUnit Proxy");
- defineOptionalProperty<std::string>("RobotStateTopicName","RobotState","Name of the RobotComponent State topic.");
- defineOptionalProperty<float>("MaxJointVelocity",30.f/180*3.141, "Maximal joint velocity in rad/sec");
+ defineRequiredProperty<std::string>("KinematicUnitName", "Name of the KinematicUnit Proxy");
+ defineOptionalProperty<std::string>("RobotStateTopicName", "RobotState", "Name of the RobotComponent State topic.");
+ defineOptionalProperty<float>("MaxJointVelocity", 30.f / 180 * 3.141, "Maximal joint velocity in rad/sec");
defineRequiredProperty<std::string>("RobotFileName", "Robot file name, e.g. robot_model.xml");
defineOptionalProperty<int>("CycleTime", 30, "Cycle time of the tcp control in ms");
defineOptionalProperty<float>("MaximumCommandDelay", 20000, "Delay after which the TCP Control unit releases itself if no new velocity have been set.");
@@ -81,8 +81,8 @@ namespace armarx
* @brief The TCPControlUnit class
*/
class TCPControlUnit :
- virtual public Component,
- virtual public TCPControlUnitInterface
+ virtual public Component,
+ virtual public TCPControlUnitInterface
{
public:
TCPControlUnit();
@@ -94,7 +94,7 @@ namespace armarx
* \param milliseconds New cycle time.
* \param c Ice Context, leave blank.
*/
- void setCycleTime(Ice::Int milliseconds, const Ice::Current &c = Ice::Current());
+ void setCycleTime(Ice::Int milliseconds, const Ice::Current& c = Ice::Current());
/**
* \brief Sets the cartesian velocity of a node in a nodeset for translation and/or orientation.
@@ -111,32 +111,32 @@ namespace armarx
*
* \see request(), release()
*/
- void setTCPVelocity(const std::string &nodeSetName, const std::string &tcpName, const::armarx::FramedDirectionBasePtr &translationVelocity, const::armarx::FramedDirectionBasePtr &orientationVelocityRPY, const Ice::Current &c = Ice::Current());
+ void setTCPVelocity(const std::string& nodeSetName, const std::string& tcpName, const::armarx::FramedDirectionBasePtr& translationVelocity, const::armarx::FramedDirectionBasePtr& orientationVelocityRPY, const Ice::Current& c = Ice::Current());
// UnitExecutionManagementInterface interface
/**
* \brief Does not do anything at the moment.
* \param c
*/
- void init(const Ice::Current &c = Ice::Current());
+ void init(const Ice::Current& c = Ice::Current());
/**
* \brief Does not do anything at the moment.
* \param c
*/
- void start(const Ice::Current &c = Ice::Current());
+ void start(const Ice::Current& c = Ice::Current());
/**
* \brief Does not do anything at the moment.
* \param c
*/
- void stop(const Ice::Current &c = Ice::Current());
- UnitExecutionState getExecutionState(const Ice::Current &c = Ice::Current());
+ void stop(const Ice::Current& c = Ice::Current());
+ UnitExecutionState getExecutionState(const Ice::Current& c = Ice::Current());
// UnitResourceManagementInterface interface
/**
* \brief Triggers the calculation loop for using cartesian velocity. Call once before/after setting a tcp velocity with SetTCPVelocity.
* \param c Ice Context, leave blank.
*/
- void request(const Ice::Current &c = Ice::Current());
+ void request(const Ice::Current& c = Ice::Current());
/**
* \brief Releases and stops the recalculation and updating of joint velocities.
@@ -144,7 +144,7 @@ namespace armarx
* all node set will be deleted in this function.
* \param c Ice Context, leave blank.
*/
- void release(const Ice::Current &c = Ice::Current());
+ void release(const Ice::Current& c = Ice::Current());
protected:
@@ -152,20 +152,23 @@ namespace armarx
void onConnectComponent();
void onDisconnectComponent();
void onExitComponent();
- std::string getDefaultName() const{return "TCPControlUnit";}
+ std::string getDefaultName() const
+ {
+ return "TCPControlUnit";
+ }
// PropertyUser interface
PropertyDefinitionsPtr createPropertyDefinitions();
- static Eigen::VectorXf CalcNullspaceJointDeltas(const Eigen::VectorXf &desiredJointDeltas, const Eigen::MatrixXf &jacobian, const Eigen::MatrixXf &jacobianInverse);
- static Eigen::VectorXf CalcJointLimitAvoidanceDeltas(VirtualRobot::RobotNodeSetPtr robotNodeSet, const Eigen::MatrixXf &jacobian, const Eigen::MatrixXf &jacobianInverse, Eigen::VectorXf desiredJointValues = Eigen::VectorXf());
+ static Eigen::VectorXf CalcNullspaceJointDeltas(const Eigen::VectorXf& desiredJointDeltas, const Eigen::MatrixXf& jacobian, const Eigen::MatrixXf& jacobianInverse);
+ static Eigen::VectorXf CalcJointLimitAvoidanceDeltas(VirtualRobot::RobotNodeSetPtr robotNodeSet, const Eigen::MatrixXf& jacobian, const Eigen::MatrixXf& jacobianInverse, Eigen::VectorXf desiredJointValues = Eigen::VectorXf());
void calcAndSetVelocities();
private:
- static void ContinuousAngles(const Eigen::AngleAxisf &oldAngle, Eigen::AngleAxisf &newAngle, double & offset);
+ static void ContinuousAngles(const Eigen::AngleAxisf& oldAngle, Eigen::AngleAxisf& newAngle, double& offset);
void periodicExec();
void periodicReport();
Eigen::VectorXf calcJointVelocities(VirtualRobot::RobotNodeSetPtr robotNodeSet, Eigen::VectorXf tcpDelta, VirtualRobot::RobotNodePtr refFrame, VirtualRobot::IKSolver::CartesianSelection mode);
- Eigen::VectorXf applyMaxJointVelocity(const Eigen::VectorXf &jointVelocity, float maxJointVelocity);
+ Eigen::VectorXf applyMaxJointVelocity(const Eigen::VectorXf& jointVelocity, float maxJointVelocity);
float maxJointVelocity;
int cycleTime;
@@ -221,48 +224,54 @@ namespace armarx
// KinematicUnitListener interface
protected:
- void reportControlModeChanged(const NameControlModeMap &, bool, const Ice::Current &);
- void reportJointAngles(const NameValueMap &jointAngles, bool, const Ice::Current &);
- void reportJointVelocities(const NameValueMap &jointVel, bool, const Ice::Current &);
- void reportJointTorques(const NameValueMap &, bool, const Ice::Current &);
+ void reportControlModeChanged(const NameControlModeMap&, bool, const Ice::Current&);
+ void reportJointAngles(const NameValueMap& jointAngles, bool, const Ice::Current&);
+ void reportJointVelocities(const NameValueMap& jointVel, bool, const Ice::Current&);
+ void reportJointTorques(const NameValueMap&, bool, const Ice::Current&);
void reportJointAccelerations(const NameValueMap& jointAccelerations, bool aValueChanged, const Ice::Current& c);
- void reportJointCurrents(const NameValueMap &, bool, const Ice::Current &);
- void reportJointMotorTemperatures(const NameValueMap &, bool, const Ice::Current &);
- void reportJointStatuses(const NameStatusMap &, bool, const Ice::Current &);
+ void reportJointCurrents(const NameValueMap&, bool, const Ice::Current&);
+ void reportJointMotorTemperatures(const NameValueMap&, bool, const Ice::Current&);
+ void reportJointStatuses(const NameStatusMap&, bool, const Ice::Current&);
};
class EDifferentialIK : public VirtualRobot::DifferentialIK, virtual public Logging
{
public:
- typedef Eigen::VectorXf (EDifferentialIK::*ComputeFunction)(float);
+ typedef Eigen::VectorXf(EDifferentialIK::*ComputeFunction)(float);
- EDifferentialIK(VirtualRobot::RobotNodeSetPtr rns,VirtualRobot:: RobotNodePtr coordSystem = VirtualRobot::RobotNodePtr(), VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod = eSVD);
+ EDifferentialIK(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot:: RobotNodePtr coordSystem = VirtualRobot::RobotNodePtr(), VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod = eSVD);
- VirtualRobot::RobotNodePtr getRefFrame(){return coordSystem;}
- int getJacobianRows(){ return nRows;}
+ VirtualRobot::RobotNodePtr getRefFrame()
+ {
+ return coordSystem;
+ }
+ int getJacobianRows()
+ {
+ return nRows;
+ }
Eigen::MatrixXf calcFullJacobian();
void clearGoals();
void setRefFrame(VirtualRobot::RobotNodePtr coordSystem);
- void setGoal(const Eigen::Matrix4f &goal, VirtualRobot::RobotNodePtr tcp, VirtualRobot::IKSolver::CartesianSelection mode, float tolerancePosition, float toleranceRotation, Eigen::VectorXf tcpWeight);
+ void setGoal(const Eigen::Matrix4f& goal, VirtualRobot::RobotNodePtr tcp, VirtualRobot::IKSolver::CartesianSelection mode, float tolerancePosition, float toleranceRotation, Eigen::VectorXf tcpWeight);
void setDOFWeights(Eigen::VectorXf dofWeights);
-// void setTCPWeights(Eigen::VectorXf tcpWeights);
+ // void setTCPWeights(Eigen::VectorXf tcpWeights);
bool computeSteps(float stepSize = 1.f, float mininumChange = 0.01f, int maxNStep = 50);
bool computeSteps(Eigen::VectorXf& resultJointDelta, float stepSize = 1.f, float mininumChange = 0.01f, int maxNStep = 50, ComputeFunction computeFunction = &DifferentialIK::computeStep);
Eigen::VectorXf computeStep(float stepSize);
- void applyDOFWeightsToJacobian(Eigen::MatrixXf &Jacobian);
- void applyTCPWeights(VirtualRobot::RobotNodePtr tcp, Eigen::MatrixXf &partJacobian);
- void applyTCPWeights(Eigen::MatrixXf &invJacobian);
+ void applyDOFWeightsToJacobian(Eigen::MatrixXf& Jacobian);
+ void applyTCPWeights(VirtualRobot::RobotNodePtr tcp, Eigen::MatrixXf& partJacobian);
+ void applyTCPWeights(Eigen::MatrixXf& invJacobian);
float getWeightedError();
float getWeightedErrorPosition(VirtualRobot::SceneObjectPtr tcp);
Eigen::VectorXf computeStepIndependently(float stepSize);
bool solveIK(float stepSize = 1, float minChange = 0.01f, int maxSteps = 50, bool useAlternativeOnFail = false);
protected:
- bool adjustDOFWeightsToJointLimits(const Eigen::VectorXf &plannedJointDeltas);
+ bool adjustDOFWeightsToJointLimits(const Eigen::VectorXf& plannedJointDeltas);
Eigen::VectorXf dofWeights;
std::map<VirtualRobot:: SceneObjectPtr, Eigen::VectorXf> tcpWeights;
diff --git a/source/RobotAPI/components/units/TCPControlUnitObserver.cpp b/source/RobotAPI/components/units/TCPControlUnitObserver.cpp
index b8931ef7067d08c18b77ca8f60a6106d0851c81f..2c34c81588359653d6c2592568a0c822a8f7a1b1 100644
--- a/source/RobotAPI/components/units/TCPControlUnitObserver.cpp
+++ b/source/RobotAPI/components/units/TCPControlUnitObserver.cpp
@@ -35,7 +35,8 @@
#define TCP_POSE_CHANNEL "TCPPose"
#define TCP_TRANS_VELOCITIES_CHANNEL "TCPVelocities"
-namespace armarx {
+namespace armarx
+{
TCPControlUnitObserver::TCPControlUnitObserver()
{
@@ -50,7 +51,7 @@ namespace armarx {
offerConditionCheck("larger", new ConditionCheckLarger());
offerConditionCheck("equals", new ConditionCheckEquals());
offerConditionCheck("smaller", new ConditionCheckSmaller());
- // offerConditionCheck("magnitudeinrange", new ConditionCheckInRange());
+ // offerConditionCheck("magnitudeinrange", new ConditionCheckInRange());
offerConditionCheck("magnitudelarger", new ConditionCheckMagnitudeLarger());
offerConditionCheck("approx", new ConditionCheckApproxPose());
@@ -68,95 +69,116 @@ namespace armarx {
PropertyDefinitionsPtr TCPControlUnitObserver::createPropertyDefinitions()
{
return PropertyDefinitionsPtr(new TCPControlUnitObserverPropertyDefinitions(
- getConfigIdentifier()));
+ getConfigIdentifier()));
}
- void TCPControlUnitObserver::reportTCPPose(const FramedPoseBaseMap &poseMap, const Ice::Current &)
+ void TCPControlUnitObserver::reportTCPPose(const FramedPoseBaseMap& poseMap, const Ice::Current&)
{
ScopedLock lock(dataMutex);
-// ARMARX_INFO << deactivateSpam() << "new tcp poses reported";
+ // ARMARX_INFO << deactivateSpam() << "new tcp poses reported";
FramedPoseBaseMap::const_iterator it = poseMap.begin();
- for(; it != poseMap.end(); it++)
+
+ for (; it != poseMap.end(); it++)
{
FramedPosePtr vec = FramedPosePtr::dynamicCast(it->second);
- const std::string &tcpName = it->first;
- if(!existsDataField(TCP_POSE_CHANNEL, tcpName))
+ const std::string& tcpName = it->first;
+
+ if (!existsDataField(TCP_POSE_CHANNEL, tcpName))
+ {
offerDataFieldWithDefault(TCP_POSE_CHANNEL, tcpName, Variant(it->second), "Pose of " + tcpName);
+ }
else
+ {
setDataField(TCP_POSE_CHANNEL, tcpName, Variant(it->second));
+ }
+
updateChannel(TCP_POSE_CHANNEL);
- if(!existsChannel(tcpName))
+
+ if (!existsChannel(tcpName))
{
offerChannel(tcpName, "pose components of " + tcpName);
- offerDataFieldWithDefault(tcpName,"x", Variant(vec->position->x), "X axis");
- offerDataFieldWithDefault(tcpName,"y", Variant(vec->position->y), "Y axis");
- offerDataFieldWithDefault(tcpName,"z", Variant(vec->position->z), "Z axis");
- offerDataFieldWithDefault(tcpName,"qx", Variant(vec->orientation->qx), "Quaternion part x");
- offerDataFieldWithDefault(tcpName,"qy", Variant(vec->orientation->qy), "Quaternion part y");
- offerDataFieldWithDefault(tcpName,"qz", Variant(vec->orientation->qz), "Quaternion part z");
- offerDataFieldWithDefault(tcpName,"qw", Variant(vec->orientation->qw), "Quaternion part w");
- offerDataFieldWithDefault(tcpName,"frame", Variant(vec->frame), "Reference Frame");
+ offerDataFieldWithDefault(tcpName, "x", Variant(vec->position->x), "X axis");
+ offerDataFieldWithDefault(tcpName, "y", Variant(vec->position->y), "Y axis");
+ offerDataFieldWithDefault(tcpName, "z", Variant(vec->position->z), "Z axis");
+ offerDataFieldWithDefault(tcpName, "qx", Variant(vec->orientation->qx), "Quaternion part x");
+ offerDataFieldWithDefault(tcpName, "qy", Variant(vec->orientation->qy), "Quaternion part y");
+ offerDataFieldWithDefault(tcpName, "qz", Variant(vec->orientation->qz), "Quaternion part z");
+ offerDataFieldWithDefault(tcpName, "qw", Variant(vec->orientation->qw), "Quaternion part w");
+ offerDataFieldWithDefault(tcpName, "frame", Variant(vec->frame), "Reference Frame");
}
else
{
- setDataField(tcpName,"x", Variant(vec->position->x));
- setDataField(tcpName,"y", Variant(vec->position->y));
- setDataField(tcpName,"z", Variant(vec->position->z));
- setDataField(tcpName,"qx", Variant(vec->orientation->qx));
- setDataField(tcpName,"qy", Variant(vec->orientation->qy));
- setDataField(tcpName,"qz", Variant(vec->orientation->qz));
- setDataField(tcpName,"qw", Variant(vec->orientation->qw));
- setDataField(tcpName,"frame", Variant(vec->frame));
+ setDataField(tcpName, "x", Variant(vec->position->x));
+ setDataField(tcpName, "y", Variant(vec->position->y));
+ setDataField(tcpName, "z", Variant(vec->position->z));
+ setDataField(tcpName, "qx", Variant(vec->orientation->qx));
+ setDataField(tcpName, "qy", Variant(vec->orientation->qy));
+ setDataField(tcpName, "qz", Variant(vec->orientation->qz));
+ setDataField(tcpName, "qw", Variant(vec->orientation->qw));
+ setDataField(tcpName, "frame", Variant(vec->frame));
}
+
updateChannel(tcpName);
}
}
- void TCPControlUnitObserver::reportTCPVelocities(const FramedDirectionMap &tcpTranslationVelocities, const FramedDirectionMap &tcpOrientationVelocities, const Ice::Current &)
+ void TCPControlUnitObserver::reportTCPVelocities(const FramedDirectionMap& tcpTranslationVelocities, const FramedDirectionMap& tcpOrientationVelocities, const Ice::Current&)
{
ScopedLock lock(dataMutex);
FramedDirectionMap::const_iterator it = tcpTranslationVelocities.begin();
- for(; it != tcpTranslationVelocities.end(); it++)
+
+ for (; it != tcpTranslationVelocities.end(); it++)
{
FramedDirectionPtr vec = FramedDirectionPtr::dynamicCast(it->second);
FramedDirectionPtr vecOri;
FramedDirectionMap::const_iterator itOri = tcpOrientationVelocities.find(it->first);
- if(itOri != tcpOrientationVelocities.end())
+
+ if (itOri != tcpOrientationVelocities.end())
+ {
vecOri = FramedDirectionPtr::dynamicCast(itOri->second);
- const std::string &tcpName = it->first;
+ }
+
+ const std::string& tcpName = it->first;
ARMARX_CHECK_EXPRESSION(vec->frame == vecOri->frame);
- if(!existsDataField(TCP_TRANS_VELOCITIES_CHANNEL, tcpName))
+ if (!existsDataField(TCP_TRANS_VELOCITIES_CHANNEL, tcpName))
+ {
offerDataFieldWithDefault(TCP_TRANS_VELOCITIES_CHANNEL, tcpName, Variant(it->second), "Pose of " + tcpName);
+ }
else
+ {
setDataField(TCP_TRANS_VELOCITIES_CHANNEL, tcpName, Variant(it->second));
+ }
+
updateChannel(TCP_TRANS_VELOCITIES_CHANNEL);
- const std::string channelName = tcpName+"Velocities";
- if(!existsChannel(channelName))
+ const std::string channelName = tcpName + "Velocities";
+
+ if (!existsChannel(channelName))
{
offerChannel(channelName, "pose components of " + tcpName);
- offerDataFieldWithDefault(channelName,"x", Variant(vec->x), "X axis");
- offerDataFieldWithDefault(channelName,"y", Variant(vec->y), "Y axis");
- offerDataFieldWithDefault(channelName,"z", Variant(vec->z), "Z axis");
- offerDataFieldWithDefault(channelName,"roll", Variant(vecOri->x), "Roll");
- offerDataFieldWithDefault(channelName,"pitch", Variant(vecOri->y), "Pitch");
- offerDataFieldWithDefault(channelName,"yaw", Variant(vecOri->z), "Yaw");
- offerDataFieldWithDefault(channelName,"frame", Variant(vecOri->frame), "Reference Frame");
+ offerDataFieldWithDefault(channelName, "x", Variant(vec->x), "X axis");
+ offerDataFieldWithDefault(channelName, "y", Variant(vec->y), "Y axis");
+ offerDataFieldWithDefault(channelName, "z", Variant(vec->z), "Z axis");
+ offerDataFieldWithDefault(channelName, "roll", Variant(vecOri->x), "Roll");
+ offerDataFieldWithDefault(channelName, "pitch", Variant(vecOri->y), "Pitch");
+ offerDataFieldWithDefault(channelName, "yaw", Variant(vecOri->z), "Yaw");
+ offerDataFieldWithDefault(channelName, "frame", Variant(vecOri->frame), "Reference Frame");
}
else
{
- setDataField(channelName,"x", Variant(vec->x));
- setDataField(channelName,"y", Variant(vec->y));
- setDataField(channelName,"z", Variant(vec->z));
- setDataField(channelName,"roll", Variant(vecOri->x));
- setDataField(channelName,"pitch", Variant(vecOri->y));
- setDataField(channelName,"yaw", Variant(vecOri->z));
- setDataField(channelName,"frame", Variant(vec->frame));
+ setDataField(channelName, "x", Variant(vec->x));
+ setDataField(channelName, "y", Variant(vec->y));
+ setDataField(channelName, "z", Variant(vec->z));
+ setDataField(channelName, "roll", Variant(vecOri->x));
+ setDataField(channelName, "pitch", Variant(vecOri->y));
+ setDataField(channelName, "yaw", Variant(vecOri->z));
+ setDataField(channelName, "frame", Variant(vec->frame));
}
+
updateChannel(channelName);
}
diff --git a/source/RobotAPI/components/units/TCPControlUnitObserver.h b/source/RobotAPI/components/units/TCPControlUnitObserver.h
index ac8a6bbae10f6fda3ea0eb24a2967d6f0b1233d3..e3903b39e30d60d91a30acaea2d75e245f5e824f 100644
--- a/source/RobotAPI/components/units/TCPControlUnitObserver.h
+++ b/source/RobotAPI/components/units/TCPControlUnitObserver.h
@@ -25,19 +25,20 @@
#include <ArmarXCore/observers/Observer.h>
#include <RobotAPI/interface/units/TCPControlUnit.h>
-namespace armarx {
+namespace armarx
+{
/**
* \class TCPControlUnitObserverPropertyDefinitions
* \brief
*/
class TCPControlUnitObserverPropertyDefinitions:
- public ComponentPropertyDefinitions
+ public ComponentPropertyDefinitions
{
public:
TCPControlUnitObserverPropertyDefinitions(std::string prefix):
ComponentPropertyDefinitions(prefix)
{
- defineRequiredProperty<std::string>("TCPControlUnitName","Name of the TCPControlUnit");
+ defineRequiredProperty<std::string>("TCPControlUnitName", "Name of the TCPControlUnit");
}
};
@@ -52,14 +53,17 @@ namespace armarx {
* Available condition checks are: *updated*, *equals*, *approx*, *larger*, *smaller* and *magnitudelarger*.
*/
class TCPControlUnitObserver :
- virtual public Observer,
- virtual public TCPControlUnitObserverInterface
+ virtual public Observer,
+ virtual public TCPControlUnitObserverInterface
{
public:
TCPControlUnitObserver();
// framework hooks
- virtual std::string getDefaultName() const { return "TCPControlUnitObserver"; }
+ virtual std::string getDefaultName() const
+ {
+ return "TCPControlUnitObserver";
+ }
void onInitObserver();
void onConnectObserver();
@@ -70,8 +74,8 @@ namespace armarx {
public:
// TCPControlUnitListener interface
- void reportTCPPose(const FramedPoseBaseMap &poseMap, const Ice::Current &c = Ice::Current());
- void reportTCPVelocities(const FramedDirectionMap &tcpTranslationVelocities, const FramedDirectionMap &tcpOrientationVelocities, const Ice::Current &c = Ice::Current());
+ void reportTCPPose(const FramedPoseBaseMap& poseMap, const Ice::Current& c = Ice::Current());
+ void reportTCPVelocities(const FramedDirectionMap& tcpTranslationVelocities, const FramedDirectionMap& tcpOrientationVelocities, const Ice::Current& c = Ice::Current());
Mutex dataMutex;
};
diff --git a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.cpp b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.cpp
index b99d8db65ae5697043cc5f89503956abe30af4d9..1c29574a9c4a0e85d4de52ed1528f5beba14066a 100644
--- a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.cpp
+++ b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.cpp
@@ -26,7 +26,7 @@
#include <RobotAPI/components/units/HandUnit.h>
#include <IceUtil/UUID.h>
-armarx::HandUnitConfigDialog::HandUnitConfigDialog(QWidget *parent) :
+armarx::HandUnitConfigDialog::HandUnitConfigDialog(QWidget* parent) :
QDialog(parent),
ui(new Ui::HandUnitConfigDialog),
uuid(IceUtil::generateUUID())
diff --git a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.h b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.h
index df5a39243cd9604b7325e07449395c597e124c97..3dad9189a3231bb20cd479fff5b09ff09efc6749 100644
--- a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.h
+++ b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.h
@@ -29,7 +29,8 @@
#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/IceProxyFinder.h>
-namespace Ui {
+namespace Ui
+{
class HandUnitConfigDialog;
}
@@ -42,17 +43,20 @@ namespace armarx
Q_OBJECT
public:
- explicit HandUnitConfigDialog(QWidget *parent = 0);
+ explicit HandUnitConfigDialog(QWidget* parent = 0);
~HandUnitConfigDialog();
protected:
// ManagedIceObject interface
- std::string getDefaultName() const { return "HandUnitConfigDialog" + uuid;}
+ std::string getDefaultName() const
+ {
+ return "HandUnitConfigDialog" + uuid;
+ }
void onInitComponent();
void onConnectComponent();
private:
- Ui::HandUnitConfigDialog *ui;
+ Ui::HandUnitConfigDialog* ui;
IceProxyFinderBase* proxyFinder;
std::string uuid;
diff --git a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp
index 5dbf0b8abf3fc213dc83fd677aef923a3132ce93..11628ca2e20f32473b81d9bdc6f1b30a2d60ae5b 100644
--- a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp
@@ -50,7 +50,7 @@ void HandUnitWidget::onConnectComponent()
handName = handUnitProxy->getHandName();
// @@@ In simulation hand is called 'Hand L'/'Hand R'. On 3b Hand is called 'TCP R'
- if(handName != "Hand L" && handName != "Hand R" && handName != "TCP L" && handName != "TCP R")
+ if (handName != "Hand L" && handName != "Hand R" && handName != "TCP L" && handName != "TCP R")
{
//QMessageBox::warning(NULL, "Hand not supported", QString("Hand with name \"") + QString::fromStdString(handName) + " \" is not suppored.");
ARMARX_WARNING << "Hand with name \"" << handName << "\" is not supported.";
@@ -61,11 +61,13 @@ void HandUnitWidget::onConnectComponent()
SingleTypeVariantListPtr preshapeStrings = SingleTypeVariantListPtr::dynamicCast(handUnitProxy->getShapeNames());
QStringList list;
int preshapeCount = preshapeStrings->getSize();
+
for (int i = 0; i < preshapeCount; ++i)
{
std::string shape = ((preshapeStrings->getVariant(i))->get<std::string>());
list << QString::fromStdString(shape);
}
+
ui.comboPreshapes->clear();
ui.comboPreshapes->addItems(list);
}
@@ -81,12 +83,13 @@ void HandUnitWidget::onExitComponent()
QPointer<QDialog> HandUnitWidget::getConfigDialog(QWidget* parent)
{
- if(!dialog)
+ if (!dialog)
{
dialog = new HandUnitConfigDialog(parent);
//dialog->ui->editHandUnitName->setText(QString::fromStdString(handUnitProxyName));
//dialog->ui->editHandName->setText(QString::fromStdString(handName));
}
+
return qobject_cast<HandUnitConfigDialog*>(dialog);
}
@@ -102,21 +105,22 @@ void HandUnitWidget::preshapeHand()
void HandUnitWidget::setJointAngles()
{
- if(!handUnitProxy)
+ if (!handUnitProxy)
{
ARMARX_WARNING << "invalid proxy";
return;
}
- if(!setJointAnglesFlag)
+ if (!setJointAnglesFlag)
{
return;
}
+
setJointAnglesFlag = false;
NameValueMap ja;
- if(handName == "Hand L" || handName == "TCP L")
+ if (handName == "Hand L" || handName == "TCP L")
{
ja["Hand Palm 2 L"] = ui.horizontalSliderPalm->value() * M_PI / 180;
ja["Index L J0"] = ui.horizontalSliderIndexJ0->value() * M_PI / 180;
@@ -131,7 +135,7 @@ void HandUnitWidget::setJointAngles()
ja["Pinky L J0"] = rinkyValue;
ja["Pinky L J1"] = rinkyValue;
}
- else if(handName == "Hand R" || handName == "TCP R")
+ else if (handName == "Hand R" || handName == "TCP R")
{
ja["Hand Palm 2 R"] = ui.horizontalSliderPalm->value() * M_PI / 180;
ja["Index R J0"] = ui.horizontalSliderIndexJ0->value() * M_PI / 180;
@@ -185,13 +189,13 @@ void HandUnitWidget::setPreshape(std::string preshape)
handUnitProxy->setShape(preshape);
}
-void HandUnitWidget::loadSettings(QSettings *settings)
+void HandUnitWidget::loadSettings(QSettings* settings)
{
handUnitProxyName = settings->value("handUnitProxyName", QString::fromStdString(handUnitProxyName)).toString().toStdString();
handName = settings->value("handName", QString::fromStdString(handName)).toString().toStdString();
}
-void HandUnitWidget::saveSettings(QSettings *settings)
+void HandUnitWidget::saveSettings(QSettings* settings)
{
settings->setValue("handUnitProxyName", QString::fromStdString(handUnitProxyName));
settings->setValue("handName", QString::fromStdString(handName));
@@ -221,26 +225,26 @@ void HandUnitWidget::connectSlots()
-void armarx::HandUnitWidget::reportHandShaped(const std::string &handName, const std::string &handShapeName, const Ice::Current &)
+void armarx::HandUnitWidget::reportHandShaped(const std::string& handName, const std::string& handShapeName, const Ice::Current&)
{
ARMARX_IMPORTANT << handName << ": " << handShapeName;
}
-void armarx::HandUnitWidget::reportNewHandShapeName(const std::string &handName, const std::string &handShapeName, const Ice::Current &)
+void armarx::HandUnitWidget::reportNewHandShapeName(const std::string& handName, const std::string& handShapeName, const Ice::Current&)
{
ARMARX_IMPORTANT << handName << ": " << handShapeName;
}
-void armarx::HandUnitWidget::reportJointAngles(const armarx::NameValueMap &actualJointAngles, const Ice::Current &c)
+void armarx::HandUnitWidget::reportJointAngles(const armarx::NameValueMap& actualJointAngles, const Ice::Current& c)
{
}
-void armarx::HandUnitWidget::reportJointPressures(const armarx::NameValueMap &actualJointPressures, const Ice::Current &c)
+void armarx::HandUnitWidget::reportJointPressures(const armarx::NameValueMap& actualJointPressures, const Ice::Current& c)
{
}
diff --git a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.h b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.h
index 60f53f01f0655418790f5a3c4ccb7fd47f3f5f7d..b28665be31d178f915f633b45e15bb14b678976a 100644
--- a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.h
@@ -50,7 +50,7 @@ namespace armarx
\see HandUnitWidget
*/
class HandUnitGuiPlugin :
- public ArmarXGuiPlugin
+ public ArmarXGuiPlugin
{
public:
HandUnitGuiPlugin();
@@ -67,14 +67,14 @@ namespace armarx
* You can either select a preshape from the drop-down-menu on top or set each
* joint individually.
* When you add the widget to the Gui, you need to specify the following parameters:
- *
+ *
* Parameter Name | Example Value | Required? | Description
* :---------------- | :-------------: | :-------------- |:--------------------
* Proxy | LeftHandUnit | Yes | The hand unit you want to control.
*/
class HandUnitWidget :
- public ArmarXComponentWidgetController,
- public HandUnitListener
+ public ArmarXComponentWidgetController,
+ public HandUnitListener
{
Q_OBJECT
public:
@@ -89,17 +89,23 @@ namespace armarx
virtual void onExitComponent();
// HandUnitListener interface
- void reportHandShaped(const std::string &, const std::string &, const Ice::Current &);
- void reportNewHandShapeName(const std::string &, const std::string &, const Ice::Current &);
+ void reportHandShaped(const std::string&, const std::string&, const Ice::Current&);
+ void reportNewHandShapeName(const std::string&, const std::string&, const Ice::Current&);
// inherited of ArmarXWidget
- virtual QString getWidgetName() const { return "RobotControl.HandUnitGUI"; }
- virtual QIcon getWidgetIcon() const { return QIcon("://icons/hand.svg"); }
+ virtual QString getWidgetName() const
+ {
+ return "RobotControl.HandUnitGUI";
+ }
+ virtual QIcon getWidgetIcon() const
+ {
+ return QIcon("://icons/hand.svg");
+ }
QPointer<QDialog> getConfigDialog(QWidget* parent = 0);
- virtual void loadSettings(QSettings * settings);
- virtual void saveSettings(QSettings * settings);
+ virtual void loadSettings(QSettings* settings);
+ virtual void saveSettings(QSettings* settings);
void configured();
public slots:
@@ -136,8 +142,8 @@ namespace armarx
// HandUnitListener interface
public:
- void reportJointAngles(const::armarx::NameValueMap &actualJointAngles, const Ice::Current &);
- void reportJointPressures(const::armarx::NameValueMap &actualJointPressures, const Ice::Current &);
+ void reportJointAngles(const::armarx::NameValueMap& actualJointAngles, const Ice::Current&);
+ void reportJointPressures(const::armarx::NameValueMap& actualJointPressures, const Ice::Current&);
};
typedef boost::shared_ptr<HandUnitWidget> HandUnitGuiPluginPtr;
}
diff --git a/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitConfigDialog.cpp b/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitConfigDialog.cpp
index 155b2cfd68483560b1c6f04f33b9df98482513bb..d450ab1607339c95e83fe9eea17976c5d1ab35b7 100644
--- a/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitConfigDialog.cpp
+++ b/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitConfigDialog.cpp
@@ -23,7 +23,7 @@
#include "HapticUnitConfigDialog.h"
#include "ui_HapticUnitConfigDialog.h"
-armarx::HapticUnitConfigDialog::HapticUnitConfigDialog(QWidget *parent) :
+armarx::HapticUnitConfigDialog::HapticUnitConfigDialog(QWidget* parent) :
QDialog(parent),
ui(new Ui::HapticUnitConfigDialog)
{
diff --git a/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitConfigDialog.h b/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitConfigDialog.h
index 2b34936eb3fb5e9d2065fe18a2fb60612c7c6b76..3592cf3244d74024dc6ac1bb486ba892b1dea84c 100644
--- a/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitConfigDialog.h
+++ b/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitConfigDialog.h
@@ -25,7 +25,8 @@
#include <QDialog>
-namespace Ui {
+namespace Ui
+{
class HapticUnitConfigDialog;
}
@@ -37,11 +38,11 @@ namespace armarx
Q_OBJECT
public:
- explicit HapticUnitConfigDialog(QWidget *parent = 0);
+ explicit HapticUnitConfigDialog(QWidget* parent = 0);
~HapticUnitConfigDialog();
private:
- Ui::HapticUnitConfigDialog *ui;
+ Ui::HapticUnitConfigDialog* ui;
//friend class HapticUnitWidget;
};
diff --git a/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.cpp
index 4cd5b9428ac4a8f5b00a55f649cb521c7f0d17da..a3437fbd79e0b255c956d7bfc3383a9746faf56b 100644
--- a/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.cpp
@@ -75,10 +75,11 @@ void HapticUnitWidget::onDisconnectComponent()
QPointer<QDialog> HapticUnitWidget::getConfigDialog(QWidget* parent)
{
- if(!dialog)
+ if (!dialog)
{
dialog = new HapticUnitConfigDialog(parent);
}
+
return qobject_cast<HapticUnitConfigDialog*>(dialog);
}
@@ -91,7 +92,7 @@ void HapticUnitWidget::configured()
void HapticUnitWidget::updateData()
{
- for(std::pair<std::string, MatrixDatafieldDisplayWidget*> pair : matrixDisplays)
+ for (std::pair<std::string, MatrixDatafieldDisplayWidget*> pair : matrixDisplays)
{
//MatrixFloatPtr matrix = VariantPtr::dynamicCast(hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, pair.first, "matrix")))->get<MatrixFloat>();
std::string name = hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, pair.first, "name"))->getString();
@@ -109,7 +110,11 @@ void HapticUnitWidget::onContextMenu(QPoint point)
{
QMenu* contextMenu = new QMenu(getWidget());
MatrixDatafieldDisplayWidget* matrixDisplay = qobject_cast<MatrixDatafieldDisplayWidget*>(sender());
- if(!matrixDisplay) return;
+
+ if (!matrixDisplay)
+ {
+ return;
+ }
QAction* setDeviceTag = contextMenu->addAction(tr("Set Device Tag"));
@@ -118,14 +123,15 @@ void HapticUnitWidget::onContextMenu(QPoint point)
std::string channelName = channelNameReverseMap.at(matrixDisplay);
std::string deviceName = deviceNameReverseMap.at(matrixDisplay);
- if(action == setDeviceTag)
+ if (action == setDeviceTag)
{
std::string tag = hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, channelName, "name"))->getString();
bool ok;
QString newTag = QInputDialog::getText(getWidget(), tr("Set Device Tag"),
- QString::fromStdString("New Tag for Device '" + deviceName + "'"), QLineEdit::Normal,
- QString::fromStdString(tag), &ok);
+ QString::fromStdString("New Tag for Device '" + deviceName + "'"), QLineEdit::Normal,
+ QString::fromStdString(tag), &ok);
+
if (ok && !newTag.isEmpty())
{
ARMARX_IMPORTANT_S << "requesting to set new device tag for " << deviceName << ": " << newTag.toStdString();
@@ -137,12 +143,12 @@ void HapticUnitWidget::onContextMenu(QPoint point)
}
-void HapticUnitWidget::loadSettings(QSettings *settings)
+void HapticUnitWidget::loadSettings(QSettings* settings)
{
}
-void HapticUnitWidget::saveSettings(QSettings *settings)
+void HapticUnitWidget::saveSettings(QSettings* settings)
{
}
@@ -162,14 +168,17 @@ void HapticUnitWidget::createMatrixWidgets()
void HapticUnitWidget::updateDisplayWidgets()
{
- QLayoutItem *child;
- while ((child = ui.gridLayoutDisplay->takeAt(0)) != 0) {
+ QLayoutItem* child;
+
+ while ((child = ui.gridLayoutDisplay->takeAt(0)) != 0)
+ {
delete child;
}
int i = 0;
ChannelRegistry channels = hapticObserverProxy->getAvailableChannels(false);
- for(std::pair<std::string, ChannelRegistryEntry> pair : channels)
+
+ for (std::pair<std::string, ChannelRegistryEntry> pair : channels)
{
std::string channelName = pair.first;
MatrixDatafieldDisplayWidget* matrixDisplay = new MatrixDatafieldDisplayWidget(new DatafieldRef(hapticObserverProxy, channelName, "matrix"), hapticObserverProxy, getWidget());
@@ -188,7 +197,7 @@ void HapticUnitWidget::updateDisplayWidgets()
void HapticUnitWidget::onCheckBoxOffsetFilterStateChanged(int state)
{
//ARMARX_IMPORTANT << "onCheckBoxOffsetFilterToggled: " << state;
- for(std::pair<std::string, MatrixDatafieldDisplayWidget*> pair : matrixDisplays)
+ for (std::pair<std::string, MatrixDatafieldDisplayWidget*> pair : matrixDisplays)
{
pair.second->enableOffsetFilter(ui.checkBoxOffsetFilter->isChecked());
}
diff --git a/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.h b/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.h
index 6b2a9f629030b4bdac48eb25393522a43de5a13c..36201d76340544e05115f1666599fb957a946e99 100644
--- a/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.h
@@ -46,7 +46,7 @@ namespace armarx
class HapticUnitConfigDialog;
class HapticUnitGuiPlugin :
- public ArmarXGuiPlugin
+ public ArmarXGuiPlugin
{
public:
HapticUnitGuiPlugin();
@@ -61,7 +61,7 @@ namespace armarx
\brief With this widget the HapticUnit can be controlled.
*/
class HapticUnitWidget :
- public ArmarXComponentWidgetController
+ public ArmarXComponentWidgetController
{
Q_OBJECT
public:
@@ -76,12 +76,18 @@ namespace armarx
virtual void onDisconnectComponent();
// inherited of ArmarXWidget
- virtual QString getWidgetName() const { return "RobotControl.HapticUnitGUI"; }
- virtual QIcon getWidgetIcon() const { return QIcon("://icons/haptic-hand.png"); }
+ virtual QString getWidgetName() const
+ {
+ return "RobotControl.HapticUnitGUI";
+ }
+ virtual QIcon getWidgetIcon() const
+ {
+ return QIcon("://icons/haptic-hand.png");
+ }
QPointer<QDialog> getConfigDialog(QWidget* parent = 0);
- virtual void loadSettings(QSettings * settings);
- virtual void saveSettings(QSettings * settings);
+ virtual void loadSettings(QSettings* settings);
+ virtual void saveSettings(QSettings* settings);
void configured();
public slots:
diff --git a/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.cpp b/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.cpp
index 655237021a4aa994631d08be2a760feb7eea6ab1..ba3f893e94292aaedab26bf463ee311196d20c40 100644
--- a/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.cpp
+++ b/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.cpp
@@ -20,7 +20,7 @@ void MatrixDatafieldDisplayWidget::updateRequested()
update();
}
-MatrixDatafieldDisplayWidget::MatrixDatafieldDisplayWidget(DatafieldRefBasePtr matrixDatafield, ObserverInterfacePrx observer, QWidget *parent) :
+MatrixDatafieldDisplayWidget::MatrixDatafieldDisplayWidget(DatafieldRefBasePtr matrixDatafield, ObserverInterfacePrx observer, QWidget* parent) :
QWidget(parent)
{
this->matrixDatafield = DatafieldRefPtr::dynamicCast(matrixDatafield);
@@ -31,7 +31,8 @@ MatrixDatafieldDisplayWidget::MatrixDatafieldDisplayWidget(DatafieldRefBasePtr m
this->data(0, 0) = 0;
enableOffsetFilter(false);
QColor c[] = {QColor::fromHsv(0, 0, 0), QColor::fromHsv(240, 255, 255), QColor::fromHsv(270, 255, 255), QColor::fromHsv(300, 255, 255),
- QColor::fromHsv(0, 255, 255), QColor::fromHsv(30, 255, 255), QColor::fromHsv(60, 255, 255)};
+ QColor::fromHsv(0, 255, 255), QColor::fromHsv(30, 255, 255), QColor::fromHsv(60, 255, 255)
+ };
this->colors = std::valarray<QColor>(c, sizeof c / sizeof c[0]);
//connect(this, SIGNAL(updateData(MatrixXf)), SLOT(setData(MatrixXf)), Qt::QueuedConnection);
@@ -43,13 +44,13 @@ MatrixDatafieldDisplayWidget::~MatrixDatafieldDisplayWidget()
//delete ui;
}
-void MatrixDatafieldDisplayWidget::paintEvent(QPaintEvent *)
+void MatrixDatafieldDisplayWidget::paintEvent(QPaintEvent*)
{
mtx.lock();
int paddingBottom = 40;
QPainter painter(this);
- painter.fillRect(rect(), QColor::fromRgb(0,0,0));
+ painter.fillRect(rect(), QColor::fromRgb(0, 0, 0));
int matrixHeight = (height() - paddingBottom) * 8 / 10;
int percentilesHeight = (height() - paddingBottom) - matrixHeight;
drawMatrix(QRect(0, 0, width(), matrixHeight), painter);
@@ -64,7 +65,7 @@ void MatrixDatafieldDisplayWidget::paintEvent(QPaintEvent *)
void MatrixDatafieldDisplayWidget::enableOffsetFilter(bool enabled)
{
- if(enabled)
+ if (enabled)
{
this->matrixDatafieldOffsetFiltered = DatafieldRefPtr::dynamicCast(observer->createFilteredDatafield(DatafieldFilterBasePtr(new filters::OffsetFilter()), matrixDatafield));
}
@@ -72,23 +73,33 @@ void MatrixDatafieldDisplayWidget::enableOffsetFilter(bool enabled)
{
this->matrixDatafieldOffsetFiltered = this->matrixDatafield;
}
+
this->percentilesDatafield = DatafieldRefPtr::dynamicCast(observer->createFilteredDatafield(DatafieldFilterBasePtr(new filters::MatrixPercentilesFilter(50)), matrixDatafieldOffsetFiltered));
}
QColor MatrixDatafieldDisplayWidget::getColor(float value, float min, float max)
{
value = (value - min) / (max - min) * (colors.size() - 1);
- if(value < 0) return colors[0];
- if(value >= colors.size() - 1) return colors[colors.size() - 1];
+
+ if (value < 0)
+ {
+ return colors[0];
+ }
+
+ if (value >= colors.size() - 1)
+ {
+ return colors[colors.size() - 1];
+ }
+
int i = (int)value;
float f2 = value - i;
- float f1 = 1- f2;
+ float f1 = 1 - f2;
QColor c1 = colors[i];
QColor c2 = colors[i + 1];
return QColor((int)(c1.red() * f1 + c2.red() * f2), (int)(c1.green() * f1 + c2.green() * f2), (int)(c1.blue() * f1 + c2.blue() * f2));
}
-void MatrixDatafieldDisplayWidget::drawMatrix(const QRect &target, QPainter &painter)
+void MatrixDatafieldDisplayWidget::drawMatrix(const QRect& target, QPainter& painter)
{
int pixelSize = std::min(target.width() / data.cols(), target.height() / data.rows());
int dx = (target.width() - pixelSize * data.cols()) / 2;
@@ -96,9 +107,9 @@ void MatrixDatafieldDisplayWidget::drawMatrix(const QRect &target, QPainter &pai
painter.setFont(QFont("Arial", 8));
painter.setPen(QColor(Qt::GlobalColor::gray));
- for(int x = 0; x < data.cols(); x++)
+ for (int x = 0; x < data.cols(); x++)
{
- for(int y = 0; y < data.rows(); y++)
+ for (int y = 0; y < data.rows(); y++)
{
QRect target = QRect(dx + x * pixelSize, dy + y * pixelSize, pixelSize, pixelSize);
painter.fillRect(target, getColor(data(y, x), min, max));
@@ -107,12 +118,13 @@ void MatrixDatafieldDisplayWidget::drawMatrix(const QRect &target, QPainter &pai
}
}
-void MatrixDatafieldDisplayWidget::drawPercentiles(const QRect &target, QPainter &painter)
+void MatrixDatafieldDisplayWidget::drawPercentiles(const QRect& target, QPainter& painter)
{
painter.setPen(QColor(Qt::GlobalColor::gray));
painter.drawRect(target);
painter.setPen(QColor(Qt::GlobalColor::red));
- for(int i = 0; i < (int)percentiles.size() - 1; i++)
+
+ for (int i = 0; i < (int)percentiles.size() - 1; i++)
{
int x1 = i * target.width() / (percentiles.size() - 1);
int x2 = (i + 1) * target.width() / (percentiles.size() - 1);
diff --git a/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.h b/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.h
index ef9f0f56ba986a255158a15aa125b77e8924191d..76c0d573ccc91d4d850e96476ec4604b05fb1d31 100644
--- a/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.h
+++ b/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.h
@@ -11,8 +11,9 @@
using Eigen::MatrixXf;
-namespace Ui {
-class MatrixDatafieldDisplayWidget;
+namespace Ui
+{
+ class MatrixDatafieldDisplayWidget;
}
namespace armarx
@@ -28,9 +29,9 @@ namespace armarx
void updateRequested();
public:
- explicit MatrixDatafieldDisplayWidget(DatafieldRefBasePtr matrixDatafield, ObserverInterfacePrx observer, QWidget *parent = 0);
+ explicit MatrixDatafieldDisplayWidget(DatafieldRefBasePtr matrixDatafield, ObserverInterfacePrx observer, QWidget* parent = 0);
~MatrixDatafieldDisplayWidget();
- void paintEvent(QPaintEvent *);
+ void paintEvent(QPaintEvent*);
void setRange(float min, float max)
@@ -41,7 +42,8 @@ namespace armarx
void enableOffsetFilter(bool enabled);
QColor getColor(float value, float min, float max);
- void invokeUpdate(){
+ void invokeUpdate()
+ {
emit doUpdate();
}
void setInfoOverlay(QString infoOverlay)
@@ -52,11 +54,11 @@ namespace armarx
}
private:
- void drawMatrix(const QRect &target, QPainter &painter);
- void drawPercentiles(const QRect &target, QPainter &painter);
+ void drawMatrix(const QRect& target, QPainter& painter);
+ void drawPercentiles(const QRect& target, QPainter& painter);
private:
- Ui::MatrixDatafieldDisplayWidget *ui;
+ Ui::MatrixDatafieldDisplayWidget* ui;
MatrixXf data;
std::vector<float> percentiles;
float min, max;
diff --git a/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.cpp b/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.cpp
index dc1124675d390a9ba43e9a57726cc3321e67f998..85bfd0506081b94bb2f9664ce9fc8948646d3a33 100644
--- a/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.cpp
+++ b/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.cpp
@@ -8,7 +8,7 @@
using namespace std;
using namespace armarx;
-MatrixDisplayWidget::MatrixDisplayWidget(QWidget *parent) :
+MatrixDisplayWidget::MatrixDisplayWidget(QWidget* parent) :
QWidget(parent),
ui(new Ui::MatrixDisplayWidget)
{
@@ -18,7 +18,8 @@ MatrixDisplayWidget::MatrixDisplayWidget(QWidget *parent) :
this->data = MatrixXf(1, 1);
this->data(0, 0) = 0;
QColor c[] = {QColor::fromHsv(0, 0, 0), QColor::fromHsv(240, 255, 255), QColor::fromHsv(270, 255, 255), QColor::fromHsv(300, 255, 255),
- QColor::fromHsv(0, 255, 255), QColor::fromHsv(30, 255, 255), QColor::fromHsv(60, 255, 255)};
+ QColor::fromHsv(0, 255, 255), QColor::fromHsv(30, 255, 255), QColor::fromHsv(60, 255, 255)
+ };
this->colors = std::valarray<QColor>(c, sizeof c / sizeof c[0]);
//connect(this, SIGNAL(updateData(MatrixXf)), SLOT(setData(MatrixXf)), Qt::QueuedConnection);
@@ -30,7 +31,7 @@ MatrixDisplayWidget::~MatrixDisplayWidget()
delete ui;
}
-void MatrixDisplayWidget::paintEvent(QPaintEvent *)
+void MatrixDisplayWidget::paintEvent(QPaintEvent*)
{
mtx.lock();
MatrixXf data = this->data;
@@ -41,18 +42,19 @@ void MatrixDisplayWidget::paintEvent(QPaintEvent *)
int dx = (width() - pixelSize * data.cols()) / 2;
int dy = (height() - pixelSize * data.rows()) / 2;
QPainter painter(this);
- painter.fillRect(rect(), QColor::fromRgb(0,0,0));
+ painter.fillRect(rect(), QColor::fromRgb(0, 0, 0));
painter.setFont(QFont("Arial", 8));
- for(int x = 0; x < data.cols(); x++)
+ for (int x = 0; x < data.cols(); x++)
{
- for(int y = 0; y < data.rows(); y++)
+ for (int y = 0; y < data.rows(); y++)
{
QRect target = QRect(dx + x * pixelSize, dy + y * pixelSize, pixelSize, pixelSize);
painter.fillRect(target, getColor(data(y, x), min, max));
painter.drawText(target, Qt::AlignCenter, QString::number(data(y, x)));
}
}
+
painter.setFont(QFont("Arial", 12));
painter.drawText(rect(), Qt::AlignBottom | Qt::AlignRight, infoOverlay);
@@ -62,11 +64,20 @@ void MatrixDisplayWidget::paintEvent(QPaintEvent *)
QColor MatrixDisplayWidget::getColor(float value, float min, float max)
{
value = (value - min) / (max - min) * (colors.size() - 1);
- if(value < 0) return colors[0];
- if(value >= colors.size() - 1) return colors[colors.size() - 1];
+
+ if (value < 0)
+ {
+ return colors[0];
+ }
+
+ if (value >= colors.size() - 1)
+ {
+ return colors[colors.size() - 1];
+ }
+
int i = (int)value;
float f2 = value - i;
- float f1 = 1- f2;
+ float f1 = 1 - f2;
QColor c1 = colors[i];
QColor c2 = colors[i + 1];
return QColor((int)(c1.red() * f1 + c2.red() * f2), (int)(c1.green() * f1 + c2.green() * f2), (int)(c1.blue() * f1 + c2.blue() * f2));
diff --git a/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.h b/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.h
index 33480b6a827ba490812895638a9ca7febca3124c..6354d65c7d01d0e15cade9187c41dbd4823a99d1 100644
--- a/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.h
+++ b/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.h
@@ -8,8 +8,9 @@
using Eigen::MatrixXf;
-namespace Ui {
-class MatrixDisplayWidget;
+namespace Ui
+{
+ class MatrixDisplayWidget;
}
namespace armarx
@@ -31,9 +32,9 @@ namespace armarx
}
public:
- explicit MatrixDisplayWidget(QWidget *parent = 0);
+ explicit MatrixDisplayWidget(QWidget* parent = 0);
~MatrixDisplayWidget();
- void paintEvent(QPaintEvent *);
+ void paintEvent(QPaintEvent*);
@@ -44,7 +45,8 @@ namespace armarx
}
QColor getColor(float value, float min, float max);
- void invokeUpdate(){
+ void invokeUpdate()
+ {
emit doUpdate();
}
void setInfoOverlay(QString infoOverlay)
@@ -55,7 +57,7 @@ namespace armarx
}
private:
- Ui::MatrixDisplayWidget *ui;
+ Ui::MatrixDisplayWidget* ui;
MatrixXf data;
float min, max;
std::valarray<QColor> colors;
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp
index 03ed91cef871b53d259d73e3069fb2bda696522b..80817194c02ed3b42a3d0b7e21a16293bc45348e 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp
@@ -37,7 +37,7 @@
using namespace armarx;
-KinematicUnitConfigDialog::KinematicUnitConfigDialog(QWidget *parent) :
+KinematicUnitConfigDialog::KinematicUnitConfigDialog(QWidget* parent) :
QDialog(parent),
ui(new Ui::KinematicUnitConfigDialog),
uuid(IceUtil::generateUUID())
@@ -48,18 +48,18 @@ KinematicUnitConfigDialog::KinematicUnitConfigDialog(QWidget *parent) :
fileDialog->setFileMode(QFileDialog::ExistingFiles);
QStringList fileTypes;
fileTypes << tr("XML (*.xml)")
- << tr("All Files (*.*)");
+ << tr("All Files (*.*)");
fileDialog->setNameFilters(fileTypes);
connect(ui->btnSelectModelFile, SIGNAL(clicked()), fileDialog, SLOT(show()));
connect(fileDialog, SIGNAL(accepted()), this, SLOT(modelFileSelected()));
connect(this->ui->buttonBox, SIGNAL(accepted()), this, SLOT(verifyConfig()));
- ui->buttonBox->button( QDialogButtonBox::Ok )->setDefault(true);
+ ui->buttonBox->button(QDialogButtonBox::Ok)->setDefault(true);
proxyFinder = new IceProxyFinder<KinematicUnitInterfacePrx>(this);
proxyFinder->setSearchMask("*");
- ui->gridLayout->addWidget(proxyFinder, 2,1,1,2);
+ ui->gridLayout->addWidget(proxyFinder, 2, 1, 1, 2);
topicFinder = new IceTopicFinder(this);
topicFinder->setSearchMask("*RobotState");
- ui->gridLayout->addWidget(topicFinder, 3,1,1,2);
+ ui->gridLayout->addWidget(topicFinder, 3, 1, 1, 2);
}
@@ -94,14 +94,18 @@ void KinematicUnitConfigDialog::modelFileSelected()
void KinematicUnitConfigDialog::verifyConfig()
{
- if(proxyFinder->getSelectedProxyName().trimmed().length() == 0)
+ if (proxyFinder->getSelectedProxyName().trimmed().length() == 0)
{
QMessageBox::critical(this, "Invalid Configuration", "The proxy name must not be empty");
}
- else if(topicFinder->getSelectedProxyName().trimmed().length() == 0)
+ else if (topicFinder->getSelectedProxyName().trimmed().length() == 0)
+ {
QMessageBox::critical(this, "Invalid Configuration", "The topic name must not be empty");
+ }
else
+ {
this->accept();
+ }
}
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.h b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.h
index 4a0e95672ef50d4975dbafa3e6ab5ea1455d1817..a6990bacf5483eea1b60311a1aff04bae05fecf3 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.h
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.h
@@ -31,11 +31,13 @@
#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/IceProxyFinder.h>
-namespace Ui {
+namespace Ui
+{
class KinematicUnitConfigDialog;
}
-namespace armarx{
+namespace armarx
+{
class KinematicUnitConfigDialog :
public QDialog,
virtual public ManagedIceObject
@@ -43,11 +45,14 @@ namespace armarx{
Q_OBJECT
public:
- explicit KinematicUnitConfigDialog(QWidget *parent = 0);
+ explicit KinematicUnitConfigDialog(QWidget* parent = 0);
~KinematicUnitConfigDialog();
// inherited from ManagedIceObject
- std::string getDefaultName() const { return "KinematicUnitConfigDialog" + uuid;}
+ std::string getDefaultName() const
+ {
+ return "KinematicUnitConfigDialog" + uuid;
+ }
void onInitComponent();
void onConnectComponent();
void onExitComponent();
@@ -62,7 +67,7 @@ namespace armarx{
IceProxyFinderBase* proxyFinder;
IceProxyFinderBase* topicFinder;
- Ui::KinematicUnitConfigDialog *ui;
+ Ui::KinematicUnitConfigDialog* ui;
QFileDialog* fileDialog;
std::string uuid;
friend class KinematicUnitWidgetController;
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index 9137ccf3004a58e93cb6132fa86a61ea600b4317..1779a712b377711c34e23a7fa15dfe152611cc47 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -67,14 +67,14 @@ KinematicUnitWidgetController::KinematicUnitWidgetController() :
void KinematicUnitWidgetController::onInitComponent()
{
- dirty_squaresum_old.resize(5,0);
+ dirty_squaresum_old.resize(5, 0);
ARMARX_INFO << flush;
verbose = true;
-// // parsing properties from ice config
-// kinematicUnitFile = getProperty<std::string>("RobotFileName").getValue();
-// robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
-// kinematicUnitName = getProperty<std::string>("KinematicUnitName").getValue();
+ // // parsing properties from ice config
+ // kinematicUnitFile = getProperty<std::string>("RobotFileName").getValue();
+ // robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
+ // kinematicUnitName = getProperty<std::string>("KinematicUnitName").getValue();
ARMARX_INFO << "RobotFileName: " << kinematicUnitFile << flush;
ARMARX_INFO << "RobotNodeSetName: " << robotNodeSetName << flush;
@@ -84,11 +84,16 @@ void KinematicUnitWidgetController::onInitComponent()
// init
robot = loadRobotFile(kinematicUnitFile);
- if(!robot)
+
+ if (!robot)
{
getObjectScheduler()->terminate();
- if(getWidget()->parentWidget())
+
+ if (getWidget()->parentWidget())
+ {
getWidget()->parentWidget()->close();
+ }
+
std::cout << "returning" << std::endl;
return;
}
@@ -102,7 +107,7 @@ void KinematicUnitWidgetController::onInitComponent()
robotNodeSet = getRobotNodeSet(robot, robotNodeSetName);
- if(!robotNodeSet)
+ if (!robotNodeSet)
{
ARMARX_ERROR << "Failed to obtain RobotNodeSet '" << robotNodeSetName << "'";
}
@@ -110,13 +115,18 @@ void KinematicUnitWidgetController::onInitComponent()
// create the debugdrawer component
std::string debugDrawerComponentName = "KinemticUnitGUIDebugDrawer_" + getName();
ARMARX_INFO << "Creating component " << debugDrawerComponentName;
- debugDrawer = Component::create<DebugDrawerComponent>(properties,debugDrawerComponentName);
+ debugDrawer = Component::create<DebugDrawerComponent>(properties, debugDrawerComponentName);
+
if (mutex3D)
{
//ARMARX_IMPORTANT << "mutex3d:" << mutex3D.get();
debugDrawer->setMutex(mutex3D);
- } else
+ }
+ else
+ {
ARMARX_ERROR << " No 3d mutex available...";
+ }
+
ArmarXManagerPtr m = getArmarXManager();
m->addObject(debugDrawer, false);
@@ -143,8 +153,11 @@ void KinematicUnitWidgetController::onInitComponent()
void KinematicUnitWidgetController::onConnectComponent()
{
- if(!robot)
+ if (!robot)
+ {
return;
+ }
+
ARMARX_INFO << flush;
// get proxy to send commands to the kinematic unit
@@ -181,36 +194,38 @@ void KinematicUnitWidgetController::onExitComponent()
}
}
-/*
- if (debugDrawer && debugDrawer->getObjectScheduler())
- {
- ARMARX_INFO << "Removing DebugDrawer component...";
- debugDrawer->getObjectScheduler()->terminate();
- ARMARX_INFO << "Removing DebugDrawer component...done";
- }
-*/
+ /*
+ if (debugDrawer && debugDrawer->getObjectScheduler())
+ {
+ ARMARX_INFO << "Removing DebugDrawer component...";
+ debugDrawer->getObjectScheduler()->terminate();
+ ARMARX_INFO << "Removing DebugDrawer component...done";
+ }
+ */
}
QPointer<QDialog> KinematicUnitWidgetController::getConfigDialog(QWidget* parent)
{
- if(!dialog)
+ if (!dialog)
{
dialog = new KinematicUnitConfigDialog(parent);
dialog->setName(dialog->getDefaultName());
-
+
armarx::CMakePackageFinder finder(KINEMATIC_UNIT_FILE_DEFAULT_PACKAGE);
+
if (!finder.packageFound())
{
ARMARX_WARNING << "ArmarX Package " << KINEMATIC_UNIT_FILE_DEFAULT_PACKAGE << " has not been found!";
- } else
+ }
+ else
{
ARMARX_INFO << "Adding to datapaths: " << finder.getDataDir();
armarx::ArmarXDataPath::addDataPaths(finder.getDataDir());
}
-
+
std::string filename = KINEMATIC_UNIT_FILE_DEFAULT;
- ArmarXDataPath::getAbsolutePath(filename,filename);
-
+ ArmarXDataPath::getAbsolutePath(filename, filename);
+
boost::filesystem::path dir(filename);
@@ -219,22 +234,23 @@ QPointer<QDialog> KinematicUnitWidgetController::getConfigDialog(QWidget* parent
dialog->ui->ediRobotNodeSetName->setText(KINEMATIC_UNIT_NAME_DEFAULT);
}
+
return qobject_cast<KinematicUnitConfigDialog*>(dialog);
}
- void KinematicUnitWidgetController::configured()
- {
- ARMARX_VERBOSE << "KinematicUnitWidget::configured()";
- kinematicUnitFile = dialog->ui->editRobotFilepath->text().toStdString();
- robotNodeSetName = dialog->ui->ediRobotNodeSetName->text().toStdString();
- kinematicUnitName = dialog->proxyFinder->getSelectedProxyName().toStdString();
- topicName = dialog->topicFinder->getSelectedProxyName().toStdString();
- }
+void KinematicUnitWidgetController::configured()
+{
+ ARMARX_VERBOSE << "KinematicUnitWidget::configured()";
+ kinematicUnitFile = dialog->ui->editRobotFilepath->text().toStdString();
+ robotNodeSetName = dialog->ui->ediRobotNodeSetName->text().toStdString();
+ kinematicUnitName = dialog->proxyFinder->getSelectedProxyName().toStdString();
+ topicName = dialog->topicFinder->getSelectedProxyName().toStdString();
+}
-void KinematicUnitWidgetController::loadSettings(QSettings *settings)
+void KinematicUnitWidgetController::loadSettings(QSettings* settings)
{
kinematicUnitFile = settings->value("kinematicUnitFile", QString::fromStdString(KINEMATIC_UNIT_FILE_DEFAULT)).toString().toStdString();
ArmarXDataPath::getAbsolutePath(kinematicUnitFile, kinematicUnitFile);
@@ -243,13 +259,15 @@ void KinematicUnitWidgetController::loadSettings(QSettings *settings)
topicName = settings->value("topicName", TOPIC_NAME_DEFAULT).toString().toStdString();
}
-void KinematicUnitWidgetController::saveSettings(QSettings *settings)
+void KinematicUnitWidgetController::saveSettings(QSettings* settings)
{
std::string robFile = kinematicUnitFile;
+
try
{
robFile = ArmarXDataPath::getRelativeArmarXPath(robFile);
- } catch (...)
+ }
+ catch (...)
{
ARMARX_WARNING << "Could not get relative filename, using full filename:" << robFile;
}
@@ -265,7 +283,7 @@ void KinematicUnitWidgetController::showVisuLayers(bool show)
{
if (debugDrawer)
{
- if(show)
+ if (show)
{
debugDrawer->enableAllLayers();
}
@@ -273,7 +291,7 @@ void KinematicUnitWidgetController::showVisuLayers(bool show)
{
debugDrawer->disableAllLayers();
}
- }
+ }
}
void KinematicUnitWidgetController::updateGuiElements()
@@ -286,7 +304,7 @@ void KinematicUnitWidgetController::modelUpdateCB()
}
-SoNode *KinematicUnitWidgetController::getScene()
+SoNode* KinematicUnitWidgetController::getScene()
{
//if(kinematicUnitNode)
// std::cout << "returning scene=" << kinematicUnitNode->getName() << std::endl;
@@ -311,12 +329,12 @@ void KinematicUnitWidgetController::connectSlots()
connect(ui.showDebugLayer, SIGNAL(toggled(bool)), this, SLOT(showVisuLayers(bool)), Qt::QueuedConnection);
- /* connect(this, SIGNAL(jointAnglesReported()), this, SLOT(updateJointAnglesTable()), Qt::QueuedConnection);
- connect(this, SIGNAL(jointVelocitiesReported()), this, SLOT(updateJointVelocitiesTable()), Qt::QueuedConnection);
- connect(this, SIGNAL(jointTorquesReported()), this, SLOT(updateJointTorquesTable()), Qt::QueuedConnection);
- connect(this, SIGNAL(jointControlModesReported()), this, SLOT(updateControlModesTable()), Qt::QueuedConnection);
- connect(this, SIGNAL(jointMotorTemperaturesReported()), this, SLOT(updateJointMotorTemperaturesTable()), Qt::QueuedConnection);
- */
+ /* connect(this, SIGNAL(jointAnglesReported()), this, SLOT(updateJointAnglesTable()), Qt::QueuedConnection);
+ connect(this, SIGNAL(jointVelocitiesReported()), this, SLOT(updateJointVelocitiesTable()), Qt::QueuedConnection);
+ connect(this, SIGNAL(jointTorquesReported()), this, SLOT(updateJointTorquesTable()), Qt::QueuedConnection);
+ connect(this, SIGNAL(jointControlModesReported()), this, SLOT(updateControlModesTable()), Qt::QueuedConnection);
+ connect(this, SIGNAL(jointMotorTemperaturesReported()), this, SLOT(updateJointMotorTemperaturesTable()), Qt::QueuedConnection);
+ */
connect(this, SIGNAL(jointAnglesReported()), this, SLOT(updateJointAnglesTable()), Qt::QueuedConnection);
connect(this, SIGNAL(jointVelocitiesReported()), this, SLOT(updateJointVelocitiesTable()), Qt::QueuedConnection);
connect(this, SIGNAL(jointTorquesReported()), this, SLOT(updateJointTorquesTable()), Qt::QueuedConnection);
@@ -326,7 +344,7 @@ void KinematicUnitWidgetController::connectSlots()
connect(this, SIGNAL(jointStatusesReported()), this, SLOT(updateJointStatusesTable()), Qt::QueuedConnection);
- connect(ui.tableJointList, SIGNAL(cellDoubleClicked(int,int)), this, SLOT(selectJointFromTableWidget(int,int)), Qt::QueuedConnection);
+ connect(ui.tableJointList, SIGNAL(cellDoubleClicked(int, int)), this, SLOT(selectJointFromTableWidget(int, int)), Qt::QueuedConnection);
}
void KinematicUnitWidgetController::initializeUi()
@@ -339,7 +357,7 @@ void KinematicUnitWidgetController::initializeUi()
void KinematicUnitWidgetController::kinematicUnitZeroPosition()
{
- if(!robotNodeSet)
+ if (!robotNodeSet)
{
return;
}
@@ -348,16 +366,18 @@ void KinematicUnitWidgetController::kinematicUnitZeroPosition()
std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
NameValueMap jointAngles;
NameControlModeMap jointModes;
- for (unsigned int i=0;i<rn.size();i++)
+
+ for (unsigned int i = 0; i < rn.size(); i++)
{
jointModes[rn[i]->getName()] = ePositionControl;
- jointAngles[rn[i]->getName()]=0.0f;
+ jointAngles[rn[i]->getName()] = 0.0f;
}
+
kinematicUnitInterfacePrx->switchControlMode(jointModes);
kinematicUnitInterfacePrx->setJointAngles(jointAngles);
//set slider to 0 if position control mode is used
- if(selectedControlMode == ePositionControl)
+ if (selectedControlMode == ePositionControl)
{
ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
}
@@ -375,6 +395,7 @@ void KinematicUnitWidgetController::setControlModePosition()
{
NameControlModeMap jointModes;
selectedControlMode = ePositionControl;
+
if (currentNode)
{
jointModes[currentNode->getName()] = ePositionControl;
@@ -382,19 +403,25 @@ void KinematicUnitWidgetController::setControlModePosition()
if (kinematicUnitInterfacePrx)
+ {
kinematicUnitInterfacePrx->switchControlMode(jointModes);
+ }
float lo = currentNode->getJointLimitLo() * 180.0 / M_PI;
float hi = currentNode->getJointLimitHi() * 180.0 / M_PI;
- if (hi-lo <= 0.0f)
+
+ if (hi - lo <= 0.0f)
+ {
return;
+ }
+
float pos = currentNode->getJointValue() * 180.0 / M_PI;
ARMARX_INFO << "setting position control for current Node Name: " << currentNode->getName() << " with current value: " << pos;
- // ARMARX_INFO << "current joint position of " << currentNode->getName() << ": " << pos;
+ // ARMARX_INFO << "current joint position of " << currentNode->getName() << ": " << pos;
ui.horizontalSliderKinematicUnitPos->setMaximum(hi);
ui.horizontalSliderKinematicUnitPos->setMinimum(lo);
- // float ticks = (float)(ui.horizontalSliderKinematicUnitPos->maximum() - ui.horizontalSliderKinematicUnitPos->minimum()+1);
- // float posT = (pos-lo) / (hi-lo) * ticks + (float)ui.horizontalSliderKinematicUnitPos->minimum();
+ // float ticks = (float)(ui.horizontalSliderKinematicUnitPos->maximum() - ui.horizontalSliderKinematicUnitPos->minimum()+1);
+ // float posT = (pos-lo) / (hi-lo) * ticks + (float)ui.horizontalSliderKinematicUnitPos->minimum();
ui.horizontalSliderKinematicUnitPos->setSliderPosition((int)(pos));
}
@@ -403,6 +430,7 @@ void KinematicUnitWidgetController::setControlModePosition()
void KinematicUnitWidgetController::setControlModeVelocity()
{
NameControlModeMap jointModes;
+
if (currentNode)
{
jointModes[currentNode->getName()] = eVelocityControl;
@@ -412,16 +440,20 @@ void KinematicUnitWidgetController::setControlModeVelocity()
ui.horizontalSliderKinematicUnitPos->setMinimum(-90);
if (kinematicUnitInterfacePrx)
+ {
kinematicUnitInterfacePrx->switchControlMode(jointModes);
+ }
selectedControlMode = eVelocityControl;
}
+
resetSliderInVelocityControl();
}
void KinematicUnitWidgetController::setControlModeTorque()
{
NameControlModeMap jointModes;
+
if (currentNode)
{
jointModes[currentNode->getName()] = eTorqueControl;
@@ -430,8 +462,11 @@ void KinematicUnitWidgetController::setControlModeTorque()
ARMARX_INFO << "setting torque control for current Node Name: " << currentNode->getName() << flush;
if (kinematicUnitInterfacePrx)
+ {
kinematicUnitInterfacePrx->switchControlMode(jointModes);
+ }
}
+
selectedControlMode = eTorqueControl;
}
@@ -440,13 +475,17 @@ VirtualRobot::RobotPtr KinematicUnitWidgetController::loadRobotFile(std::string
VirtualRobot::RobotPtr robot;
if (verbose)
+ {
ARMARX_INFO << "Loading KinematicUnit " << kinematicUnitName << " from " << kinematicUnitFile << " ..." << flush;
+ }
- if (!ArmarXDataPath::getAbsolutePath(fileName,fileName))
+ if (!ArmarXDataPath::getAbsolutePath(fileName, fileName))
{
- ARMARX_INFO << "Could not find Robot XML file with name " << fileName << flush;
+ ARMARX_INFO << "Could not find Robot XML file with name " << fileName << flush;
}
+
robot = RobotIO::loadRobot(fileName);
+
if (!robot)
{
ARMARX_INFO << "Could not find Robot XML file with name " << fileName << "(" << kinematicUnitName << ")" << flush;
@@ -487,6 +526,7 @@ VirtualRobot::RobotNodeSetPtr KinematicUnitWidgetController::getRobotNodeSet(Vir
}
}
+
return nodeSetPtr;
}
@@ -498,17 +538,20 @@ bool KinematicUnitWidgetController::initGUIComboBox(VirtualRobot::RobotNodeSetPt
if (robotNodeSet)
{
std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
- for (unsigned int i=0;i<rn.size();i++)
+
+ for (unsigned int i = 0; i < rn.size(); i++)
{
// ARMARX_INFO << "adding item to joint combo box" << rn[i]->getName() << flush;
QString name(rn[i]->getName().c_str());
ui.nodeListComboBox->addItem(name);
}
+
selectJoint(0);
ui.nodeListComboBox->setCurrentIndex(0);
return true;
}
+
return false;
}
@@ -556,22 +599,22 @@ bool KinematicUnitWidgetController::initGUIJointListTable(VirtualRobot::RobotNod
// fill in joint names
- for (unsigned int i=0;i<rn.size();i++)
+ for (unsigned int i = 0; i < rn.size(); i++)
{
// ARMARX_INFO << "adding item to joint table" << rn[i]->getName() << flush;
QString name(rn[i]->getName().c_str());
- QTableWidgetItem *newItem = new QTableWidgetItem(name);
+ QTableWidgetItem* newItem = new QTableWidgetItem(name);
ui.tableJointList->setItem(i, eTabelColumnName, newItem);
}
// init missing table fields with default values
- for (unsigned int i=0;i<rn.size();i++)
+ for (unsigned int i = 0; i < rn.size(); i++)
{
- for (unsigned int j=1;j<numberOfColumns;j++)
+ for (unsigned int j = 1; j < numberOfColumns; j++)
{
QString state = "--";
- QTableWidgetItem *newItem = new QTableWidgetItem(state);
+ QTableWidgetItem* newItem = new QTableWidgetItem(state);
ui.tableJointList->setItem(i, j, newItem);
}
}
@@ -579,13 +622,14 @@ bool KinematicUnitWidgetController::initGUIJointListTable(VirtualRobot::RobotNod
//hide columns Operation, Error, Enabled and Emergency Stop
//they will be shown when changes occur
// TODO: for some reason the columns are not hidden
- ui.tableJointList->setColumnHidden(eTabelColumnOperation,true);
- ui.tableJointList->setColumnHidden(eTabelColumnError ,true);
- ui.tableJointList->setColumnHidden(eTabelColumnEnabled ,true);
- ui.tableJointList->setColumnHidden(eTabelColumnEmergencyStop ,true);
+ ui.tableJointList->setColumnHidden(eTabelColumnOperation, true);
+ ui.tableJointList->setColumnHidden(eTabelColumnError , true);
+ ui.tableJointList->setColumnHidden(eTabelColumnEnabled , true);
+ ui.tableJointList->setColumnHidden(eTabelColumnEmergencyStop , true);
return true;
}
+
return false;
}
@@ -593,19 +637,23 @@ bool KinematicUnitWidgetController::initGUIJointListTable(VirtualRobot::RobotNod
void KinematicUnitWidgetController::selectJoint(int i)
{
boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
- if (!robotNodeSet || i<0 || i>=(int)robotNodeSet->getSize())
+
+ if (!robotNodeSet || i < 0 || i >= (int)robotNodeSet->getSize())
+ {
return;
+ }
currentNode = robotNodeSet->getAllRobotNodes()[i];
+
/*
//ui.lcdNumberKinematicUnitJointValue->display(pos*180.0f/(float)M_PI);
ui.lcdNumberKinematicUnitJointValue->display(posT);
*/
- if(selectedControlMode == ePositionControl)
+ if (selectedControlMode == ePositionControl)
{
setControlModePosition();
}
- else if(selectedControlMode == eVelocityControl)
+ else if (selectedControlMode == eVelocityControl)
{
ui.horizontalSliderKinematicUnitPos->setMaximum(60);
ui.horizontalSliderKinematicUnitPos->setMinimum(-60);
@@ -613,7 +661,7 @@ void KinematicUnitWidgetController::selectJoint(int i)
setControlModeVelocity();
ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
}
- else if(selectedControlMode == eTorqueControl)
+ else if (selectedControlMode == eTorqueControl)
{
ui.horizontalSliderKinematicUnitPos->setMaximum(60);
ui.horizontalSliderKinematicUnitPos->setMinimum(-60);
@@ -625,26 +673,29 @@ void KinematicUnitWidgetController::selectJoint(int i)
void KinematicUnitWidgetController::selectJointFromTableWidget(int row, int column)
{
- if(column == eTabelColumnName)
+ if (column == eTabelColumnName)
{
ui.nodeListComboBox->setCurrentIndex(row);
-// selectJoint(row);
+ // selectJoint(row);
}
}
void KinematicUnitWidgetController::sliderValueChanged(int pos)
{
boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+
if (!currentNode)
+ {
return;
+ }
-// float ticks = static_cast<float>(ui.horizontalSliderKinematicUnitPos->maximum() - ui.horizontalSliderKinematicUnitPos->minimum()+1);
+ // float ticks = static_cast<float>(ui.horizontalSliderKinematicUnitPos->maximum() - ui.horizontalSliderKinematicUnitPos->minimum()+1);
float value = static_cast<float>(ui.horizontalSliderKinematicUnitPos->value());
-// float perc = (value - static_cast<float>(ui.horizontalSliderKinematicUnitPos->minimum())) / ticks;
+ // float perc = (value - static_cast<float>(ui.horizontalSliderKinematicUnitPos->minimum())) / ticks;
-// NameControlModeMap::const_iterator it;
-// it = reportedJointControlModes.find(currentNode->getName());
-// const ControlMode currentControlMode = it->second;
+ // NameControlModeMap::const_iterator it;
+ // it = reportedJointControlModes.find(currentNode->getName());
+ // const ControlMode currentControlMode = it->second;
const ControlMode currentControlMode = selectedControlMode;
if (currentControlMode == ePositionControl)
@@ -656,8 +707,9 @@ void KinematicUnitWidgetController::sliderValueChanged(int pos)
NameValueMap jointAngles;
-// jointAngles[currentNode->getName()] = (perc - 0.5) * 2 * M_PI;
+ // jointAngles[currentNode->getName()] = (perc - 0.5) * 2 * M_PI;
jointAngles[currentNode->getName()] = value / 180.0 * M_PI;
+
if (kinematicUnitInterfacePrx)
{
kinematicUnitInterfacePrx->setJointAngles(jointAngles);
@@ -668,16 +720,18 @@ void KinematicUnitWidgetController::sliderValueChanged(int pos)
{
NameValueMap jointVelocities;
jointVelocities[currentNode->getName()] = value / 180.0 * M_PI;;
+
if (kinematicUnitInterfacePrx)
{
kinematicUnitInterfacePrx->setJointVelocities(jointVelocities);
updateModel();
}
}
- else if(currentControlMode == eTorqueControl)
+ else if (currentControlMode == eTorqueControl)
{
NameValueMap jointTorques;
- jointTorques[currentNode->getName()] = value/10.0f;
+ jointTorques[currentNode->getName()] = value / 10.0f;
+
if (kinematicUnitInterfacePrx)
{
kinematicUnitInterfacePrx->setJointTorques(jointTorques);
@@ -694,19 +748,24 @@ void KinematicUnitWidgetController::sliderValueChanged(int pos)
void KinematicUnitWidgetController::updateControlModesTable()
{
- if(!getWidget() || !robotNodeSet)
+ if (!getWidget() || !robotNodeSet)
+ {
return;
+ }
boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
+
for (unsigned int i = 0; i < rn.size(); i++)
{
NameControlModeMap::const_iterator it;
it = reportedJointControlModes.find(rn[i]->getName());
QString state;
- if (it==reportedJointControlModes.end())
+ if (it == reportedJointControlModes.end())
+ {
state = "unknown";
+ }
else
{
ControlMode currentMode = it->second;
@@ -754,18 +813,21 @@ void KinematicUnitWidgetController::updateControlModesTable()
}
}
- QTableWidgetItem *newItem = new QTableWidgetItem(state);
+ QTableWidgetItem* newItem = new QTableWidgetItem(state);
ui.tableJointList->setItem(i, eTabelColumnControlMode, newItem);
}
}
void KinematicUnitWidgetController::updateJointStatusesTable()
{
- if(!getWidget() || !robotNodeSet)
+ if (!getWidget() || !robotNodeSet)
+ {
return;
+ }
boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
+
for (unsigned int i = 0; i < rn.size(); i++)
{
NameStatusMap::const_iterator it;
@@ -773,32 +835,33 @@ void KinematicUnitWidgetController::updateJointStatusesTable()
JointStatus currentStatus = it->second;
QString state = translateStatus(currentStatus.operation);
- QTableWidgetItem *newItem = new QTableWidgetItem(state);
+ QTableWidgetItem* newItem = new QTableWidgetItem(state);
ui.tableJointList->setItem(i, eTabelColumnOperation, newItem);
state = translateStatus(currentStatus.error);
newItem = new QTableWidgetItem(state);
ui.tableJointList->setItem(i, eTabelColumnError, newItem);
- state = currentStatus.enabled? "X" : "-";
+ state = currentStatus.enabled ? "X" : "-";
newItem = new QTableWidgetItem(state);
ui.tableJointList->setItem(i, eTabelColumnEnabled, newItem);
- state = currentStatus.emergencyStop? "X" : "-";
+ state = currentStatus.emergencyStop ? "X" : "-";
newItem = new QTableWidgetItem(state);
ui.tableJointList->setItem(i, eTabelColumnEmergencyStop, newItem);
}
+
//show columns
- ui.tableJointList->setColumnHidden(eTabelColumnOperation,false);
- ui.tableJointList->setColumnHidden(eTabelColumnError,false);
- ui.tableJointList->setColumnHidden(eTabelColumnEnabled,false);
- ui.tableJointList->setColumnHidden(eTabelColumnEmergencyStop,false);
+ ui.tableJointList->setColumnHidden(eTabelColumnOperation, false);
+ ui.tableJointList->setColumnHidden(eTabelColumnError, false);
+ ui.tableJointList->setColumnHidden(eTabelColumnEnabled, false);
+ ui.tableJointList->setColumnHidden(eTabelColumnEmergencyStop, false);
}
QString KinematicUnitWidgetController::translateStatus(OperationStatus status)
{
- switch(status)
+ switch (status)
{
case eOffline:
return "Offline";
@@ -816,7 +879,7 @@ QString KinematicUnitWidgetController::translateStatus(OperationStatus status)
QString KinematicUnitWidgetController::translateStatus(ErrorStatus status)
{
- switch(status)
+ switch (status)
{
case eOk:
return "Ok";
@@ -834,7 +897,7 @@ QString KinematicUnitWidgetController::translateStatus(ErrorStatus status)
void KinematicUnitWidgetController::updateJointAnglesTable()
{
- if(!robotNodeSet)
+ if (!robotNodeSet)
{
return;
}
@@ -850,12 +913,14 @@ void KinematicUnitWidgetController::updateJointAnglesTable()
NameValueMap::const_iterator it;
RobotNodePtr node = rn[i];
it = reportedJointAngles.find(node->getName());
- if(it == reportedJointAngles.end())
+
+ if (it == reportedJointAngles.end())
{
continue;
}
+
const float currentValue = it->second;
- dirty_squaresum += currentValue*currentValue;
+ dirty_squaresum += currentValue * currentValue;
QModelIndex index = ui.tableJointList->model()->index(i, eTabelColumnAngleProgressbar);
@@ -869,33 +934,41 @@ void KinematicUnitWidgetController::updateJointAnglesTable()
{
updateModel();
dirty_squaresum_old[0] = dirty_squaresum;
-// ARMARX_INFO << "update model" << flush;
+ // ARMARX_INFO << "update model" << flush;
}
}
-void KinematicUnitWidgetController::updateJointVelocitiesTable(){
- if(!getWidget() || !robotNodeSet)
+void KinematicUnitWidgetController::updateJointVelocitiesTable()
+{
+ if (!getWidget() || !robotNodeSet)
+ {
return;
+ }
float dirty_squaresum = 0;
boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
QTableWidgetItem* newItem;
+
for (unsigned int i = 0; i < rn.size(); i++)
{
NameValueMap::const_iterator it;
it = reportedJointVelocities.find(rn[i]->getName());
- if(it == reportedJointVelocities.end())
+
+ if (it == reportedJointVelocities.end())
{
continue;
}
+
const float currentValue = it->second;
- dirty_squaresum += currentValue*currentValue;
+ dirty_squaresum += currentValue * currentValue;
const QString Text = QString::number(cutJitter(currentValue));
newItem = new QTableWidgetItem(Text);
ui.tableJointList->setItem(i, eTabelColumnVelocity, newItem);
}
- if ( (fabs(dirty_squaresum_old[1] - dirty_squaresum)) > 0.0000005) {
+
+ if ((fabs(dirty_squaresum_old[1] - dirty_squaresum)) > 0.0000005)
+ {
updateModel();
dirty_squaresum_old[1] = dirty_squaresum;
}
@@ -903,8 +976,10 @@ void KinematicUnitWidgetController::updateJointVelocitiesTable(){
void KinematicUnitWidgetController::updateJointTorquesTable()
{
- if(!getWidget() || !robotNodeSet)
+ if (!getWidget() || !robotNodeSet)
+ {
return;
+ }
boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
@@ -914,7 +989,8 @@ void KinematicUnitWidgetController::updateJointTorquesTable()
for (unsigned int i = 0; i < rn.size(); i++)
{
it = reportedJointTorques.find(rn[i]->getName());
- if(it == reportedJointTorques.end())
+
+ if (it == reportedJointTorques.end())
{
continue;
}
@@ -927,8 +1003,10 @@ void KinematicUnitWidgetController::updateJointTorquesTable()
void KinematicUnitWidgetController::updateJointCurrentsTable()
{
- if(!getWidget() || !robotNodeSet)
+ if (!getWidget() || !robotNodeSet)
+ {
return;
+ }
boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
@@ -938,7 +1016,8 @@ void KinematicUnitWidgetController::updateJointCurrentsTable()
for (unsigned int i = 0; i < rn.size(); i++)
{
it = reportedJointCurrents.find(rn[i]->getName());
- if(it == reportedJointCurrents.end())
+
+ if (it == reportedJointCurrents.end())
{
continue;
}
@@ -951,8 +1030,10 @@ void KinematicUnitWidgetController::updateJointCurrentsTable()
void KinematicUnitWidgetController::updateJointMotorTemperaturesTable()
{
- if(!getWidget() || !robotNodeSet)
+ if (!getWidget() || !robotNodeSet)
+ {
return;
+ }
boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
@@ -962,7 +1043,8 @@ void KinematicUnitWidgetController::updateJointMotorTemperaturesTable()
for (unsigned int i = 0; i < rn.size(); i++)
{
it = reportedJointMotorTemperatures.find(rn[i]->getName());
- if(it == reportedJointMotorTemperatures.end())
+
+ if (it == reportedJointMotorTemperatures.end())
{
continue;
}
@@ -975,8 +1057,10 @@ void KinematicUnitWidgetController::updateJointMotorTemperaturesTable()
void KinematicUnitWidgetController::reportJointAngles(const NameValueMap& jointAngles, bool aValueChanged, const Ice::Current& c)
{
- if(!aValueChanged && reportedJointAngles.size() > 0)
+ if (!aValueChanged && reportedJointAngles.size() > 0)
+ {
return;
+ }
boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
reportedJointAngles = jointAngles;
@@ -985,8 +1069,10 @@ void KinematicUnitWidgetController::reportJointAngles(const NameValueMap& jointA
void KinematicUnitWidgetController::reportJointVelocities(const NameValueMap& jointVelocities, bool aValueChanged, const Ice::Current& c)
{
- if(!aValueChanged && reportedJointVelocities.size() > 0)
+ if (!aValueChanged && reportedJointVelocities.size() > 0)
+ {
return;
+ }
boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
reportedJointVelocities = jointVelocities;
@@ -995,8 +1081,10 @@ void KinematicUnitWidgetController::reportJointVelocities(const NameValueMap& jo
void KinematicUnitWidgetController::reportJointTorques(const NameValueMap& jointTorques, bool aValueChanged, const Ice::Current& c)
{
- if(!aValueChanged && reportedJointTorques.size() > 0)
+ if (!aValueChanged && reportedJointTorques.size() > 0)
+ {
return;
+ }
boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
reportedJointTorques = jointTorques;
@@ -1004,19 +1092,20 @@ void KinematicUnitWidgetController::reportJointTorques(const NameValueMap& joint
emit jointTorquesReported();
}
-void KinematicUnitWidgetController::reportJointAccelerations(const NameValueMap &jointAccelerations, bool aValueChanged, const Ice::Current &c)
+void KinematicUnitWidgetController::reportJointAccelerations(const NameValueMap& jointAccelerations, bool aValueChanged, const Ice::Current& c)
{
}
void KinematicUnitWidgetController::reportControlModeChanged(const NameControlModeMap& jointModes, bool aValueChanged, const Ice::Current& c)
{
-// if(!aValueChanged && reportedJointControlModes.size() > 0)
-// return;
+ // if(!aValueChanged && reportedJointControlModes.size() > 0)
+ // return;
boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
- for(auto&e : jointModes)
+
+ for (auto& e : jointModes)
{
-// ARMARX_INFO << "Setting jointMode of joint " << e.first << " to " << e.second;
+ // ARMARX_INFO << "Setting jointMode of joint " << e.first << " to " << e.second;
reportedJointControlModes[e.first] = e.second;
}
@@ -1026,8 +1115,10 @@ void KinematicUnitWidgetController::reportControlModeChanged(const NameControlMo
void KinematicUnitWidgetController::reportJointCurrents(const NameValueMap& jointCurrents, bool aValueChanged, const Ice::Current& c)
{
- if(!aValueChanged && reportedJointCurrents.size() > 0)
+ if (!aValueChanged && reportedJointCurrents.size() > 0)
+ {
return;
+ }
boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
reportedJointCurrents = jointCurrents;
@@ -1037,8 +1128,10 @@ void KinematicUnitWidgetController::reportJointCurrents(const NameValueMap& join
void KinematicUnitWidgetController::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, bool aValueChanged, const Ice::Current& c)
{
- if(!aValueChanged && reportedJointMotorTemperatures.size() > 0)
+ if (!aValueChanged && reportedJointMotorTemperatures.size() > 0)
+ {
return;
+ }
boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
reportedJointMotorTemperatures = jointMotorTemperatures;
@@ -1046,10 +1139,12 @@ void KinematicUnitWidgetController::reportJointMotorTemperatures(const NameValue
emit jointMotorTemperaturesReported();
}
-void KinematicUnitWidgetController::reportJointStatuses(const NameStatusMap &jointStatuses, bool aValueChanged, const Ice::Current &)
+void KinematicUnitWidgetController::reportJointStatuses(const NameStatusMap& jointStatuses, bool aValueChanged, const Ice::Current&)
{
- if(!aValueChanged && reportedJointStatuses.size() > 0)
+ if (!aValueChanged && reportedJointStatuses.size() > 0)
+ {
return;
+ }
boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
reportedJointStatuses = jointStatuses;
@@ -1060,12 +1155,12 @@ void KinematicUnitWidgetController::reportJointStatuses(const NameStatusMap &joi
void KinematicUnitWidgetController::updateModel()
{
- if(!robotNodeSet)
+ if (!robotNodeSet)
{
return;
}
-// ARMARX_INFO << "updateModel()" << flush;
+ // ARMARX_INFO << "updateModel()" << flush;
boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
std::vector< RobotNodePtr > rn2 = robotNodeSet->getAllRobotNodes();
@@ -1077,12 +1172,14 @@ void KinematicUnitWidgetController::updateModel()
VirtualRobot::RobotNodePtr node = robot->getRobotNode(rn2[i]->getName());
NameValueMap::const_iterator it;
it = reportedJointAngles.find(node->getName());
- if(it != reportedJointAngles.end())
+
+ if (it != reportedJointAngles.end())
{
usedNodes.push_back(node);
jv.push_back(it->second);
}
}
+
robot->setJointValues(usedNodes, jv);
}
@@ -1091,33 +1188,39 @@ void KinematicUnitWidgetController::setMutex3D(boost::shared_ptr<boost::recursiv
//ARMARX_IMPORTANT << "KinematicUnitWidgetController controller " << getInstanceName() << ": set mutex " << mutex3D.get();
this->mutex3D = mutex3D;
+
if (debugDrawer)
+ {
debugDrawer->setMutex(mutex3D);
+ }
}
float KinematicUnitWidgetController::cutJitter(float value)
{
- return (abs(value)<static_cast<float>(ui.jitterThresholdSpinBox->value()))?0:value;
+ return (abs(value) < static_cast<float>(ui.jitterThresholdSpinBox->value())) ? 0 : value;
}
-void RangeValueDelegate::paint(QPainter *painter, const QStyleOptionViewItem &option, const QModelIndex &index) const
+void RangeValueDelegate::paint(QPainter* painter, const QStyleOptionViewItem& option, const QModelIndex& index) const
{
- if (index.column() == KinematicUnitWidgetController::eTabelColumnAngleProgressbar) {
+ if (index.column() == KinematicUnitWidgetController::eTabelColumnAngleProgressbar)
+ {
float jointValue = index.data(KinematicUnitWidgetController::eJointAngleRole).toFloat();
- float loDeg = index.data(KinematicUnitWidgetController::eJointLoRole).toFloat()*180/M_PI;
- float hiDeg = index.data(KinematicUnitWidgetController::eJointHiRole).toFloat()*180/M_PI;
- if(hiDeg-loDeg <= 0)
+ float loDeg = index.data(KinematicUnitWidgetController::eJointLoRole).toFloat() * 180 / M_PI;
+ float hiDeg = index.data(KinematicUnitWidgetController::eJointHiRole).toFloat() * 180 / M_PI;
+
+ if (hiDeg - loDeg <= 0)
{
QStyledItemDelegate::paint(painter, option, index);
return;
}
+
QStyleOptionProgressBarV2 progressBarOption;
progressBarOption.rect = option.rect;
progressBarOption.minimum = loDeg;
progressBarOption.maximum = hiDeg;
- progressBarOption.progress = jointValue*180/M_PI;
+ progressBarOption.progress = jointValue * 180 / M_PI;
progressBarOption.text = QString::number(jointValue);
progressBarOption.textVisible = true;
QPalette pal;
@@ -1126,26 +1229,29 @@ void RangeValueDelegate::paint(QPainter *painter, const QStyleOptionViewItem &op
QApplication::style()->drawControl(QStyle::CE_ProgressBar,
&progressBarOption, painter);
-// QProgressBar renderer;
-// float progressPercentage = (jointValue*180.0f/M_PI-loDeg)/(hiDeg - loDeg);
-// ARMARX_INFO_S << VAROUT(progressPercentage);
-// // Customize style using style-sheet..
-// QColor color((int)(255*progressPercentage), ((int)(255*(1-progressPercentage))), 0);
-// QString style = renderer.styleSheet();
-// style += "QProgressBar::chunk { background-color: " + color.name() + "}";
-// ARMARX_INFO_S << VAROUT(style);
-// renderer.resize(option.rect.size());
-// renderer.setMinimum(loDeg);
-// renderer.setMaximum(hiDeg);
-// renderer.setValue(jointValue*180.0f);
-
-// renderer.setStyleSheet(style);
-// painter->save();
-// painter->translate(option.rect.topLeft());
-// renderer.render(painter);
-// painter->restore();
- } else
+ // QProgressBar renderer;
+ // float progressPercentage = (jointValue*180.0f/M_PI-loDeg)/(hiDeg - loDeg);
+ // ARMARX_INFO_S << VAROUT(progressPercentage);
+ // // Customize style using style-sheet..
+ // QColor color((int)(255*progressPercentage), ((int)(255*(1-progressPercentage))), 0);
+ // QString style = renderer.styleSheet();
+ // style += "QProgressBar::chunk { background-color: " + color.name() + "}";
+ // ARMARX_INFO_S << VAROUT(style);
+ // renderer.resize(option.rect.size());
+ // renderer.setMinimum(loDeg);
+ // renderer.setMaximum(hiDeg);
+ // renderer.setValue(jointValue*180.0f);
+
+ // renderer.setStyleSheet(style);
+ // painter->save();
+ // painter->translate(option.rect.topLeft());
+ // renderer.render(painter);
+ // painter->restore();
+ }
+ else
+ {
QStyledItemDelegate::paint(painter, option, index);
+ }
}
Q_EXPORT_PLUGIN2(robotapi_gui_KinematicUnitGuiPlugin, KinematicUnitGuiPlugin)
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
index 2b93b13f8e78ffd05920750c85bee3548027872b..07a887ab5b5a0b13a57d65dcf6280d695224b273 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
@@ -67,7 +67,7 @@ namespace armarx
\see KinematicUnitWidget
*/
class KinematicUnitGuiPlugin :
- public ArmarXGuiPlugin
+ public ArmarXGuiPlugin
{
public:
KinematicUnitGuiPlugin();
@@ -79,9 +79,9 @@ namespace armarx
};
class RangeValueDelegate :
- public QStyledItemDelegate
+ public QStyledItemDelegate
{
- void paint(QPainter *painter, const QStyleOptionViewItem &option, const QModelIndex &index) const;
+ void paint(QPainter* painter, const QStyleOptionViewItem& option, const QModelIndex& index) const;
};
/*!
@@ -91,23 +91,23 @@ namespace armarx
\image html KinematicUnitGUI.png
When you add the widget to the Gui, you need to specify the following parameters:
-
+
Parameter Name | Example Value | Required? | Description
:---------------- | :-------------: | :-------------- |:--------------------
Robot model filepath | $ArmarX_Core/Armar3/data/Armar3/robotmodel/ArmarIII.xml | Yes | The robot model to use. This needs to be the same model the kinematic unit is using.
Robot nodeset name | Robot | Yes | ?
- Kinematic unit name - Proxy | Armar3KinematicUnit | Yes | The kinematic unit you wish to observe/control.
+ Kinematic unit name - Proxy | Armar3KinematicUnit | Yes | The kinematic unit you wish to observe/control.
RobotState Topic Name | RobotState | ? | ?
-
- This widget allows you to both observe and control a kinematic unit. All joints are listed in
- the table in the center of the widget. The 3D viewer shows the current state of the robot.
+
+ This widget allows you to both observe and control a kinematic unit. All joints are listed in
+ the table in the center of the widget. The 3D viewer shows the current state of the robot.
On the top you can select the joint you wish to control and the control mode. You can control
a joint with the slider below.
*/
class KinematicUnitWidgetController :
- public ArmarXComponentWidgetController,
- public KinematicUnitListener
+ public ArmarXComponentWidgetController,
+ public KinematicUnitListener
{
Q_OBJECT
public:
@@ -115,7 +115,7 @@ namespace armarx
* @brief Holds the column index for the joint tabel.
* Used to avoid magic numbers.
*/
- enum JointTabelColumnIndex: int
+ enum JointTabelColumnIndex : int
{
eTabelColumnName = 0,
eTabelColumnControlMode,
@@ -130,7 +130,8 @@ namespace armarx
eTabelColumnEmergencyStop
};
- enum Roles {
+ enum Roles
+ {
eJointAngleRole = Qt::UserRole + 1,
eJointHiRole,
eJointLoRole
@@ -145,12 +146,18 @@ namespace armarx
virtual void onExitComponent();
// inherited of ArmarXWidget
- virtual QString getWidgetName() const { return "RobotControl.KinematicUnitGUI"; }
- virtual QIcon getWidgetIcon() const { return QIcon("://icons/kinematic_icon.svg"); }
+ virtual QString getWidgetName() const
+ {
+ return "RobotControl.KinematicUnitGUI";
+ }
+ virtual QIcon getWidgetIcon() const
+ {
+ return QIcon("://icons/kinematic_icon.svg");
+ }
QPointer<QDialog> getConfigDialog(QWidget* parent = 0);
- virtual void loadSettings(QSettings * settings);
- virtual void saveSettings(QSettings * settings);
+ virtual void loadSettings(QSettings* settings);
+ virtual void saveSettings(QSettings* settings);
void configured();
SoNode* getScene();
@@ -242,7 +249,7 @@ namespace armarx
void reportJointAccelerations(const NameValueMap& jointAccelerations, bool aValueChanged, const Ice::Current& c);
void reportJointCurrents(const NameValueMap& jointCurrents, bool aValueChanged, const Ice::Current& c);
void reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, bool aValueChanged, const Ice::Current& c);
- void reportJointStatuses(const NameStatusMap& jointStatuses, bool aValueChanged, const Ice::Current &);
+ void reportJointStatuses(const NameStatusMap& jointStatuses, bool aValueChanged, const Ice::Current&);
void updateModel();
diff --git a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitConfigDialog.cpp b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitConfigDialog.cpp
index daf1249859979b0ab98ca78f7dc8ba036b4e940f..8683057dd13c94900addaf91f665bb8e6428d5d1 100644
--- a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitConfigDialog.cpp
+++ b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitConfigDialog.cpp
@@ -26,7 +26,7 @@
#include <IceUtil/UUID.h>
-armarx::PlatformUnitConfigDialog::PlatformUnitConfigDialog(QWidget *parent) :
+armarx::PlatformUnitConfigDialog::PlatformUnitConfigDialog(QWidget* parent) :
QDialog(parent),
ui(new Ui::PlatformUnitConfigDialog),
uuid(IceUtil::generateUUID())
@@ -34,7 +34,7 @@ armarx::PlatformUnitConfigDialog::PlatformUnitConfigDialog(QWidget *parent) :
ui->setupUi(this);
finder = new IceProxyFinder<PlatformUnitInterfacePrx>(this);
finder->setSearchMask("*Unit");
- ui->gridLayout->addWidget(finder,0,1);
+ ui->gridLayout->addWidget(finder, 0, 1);
}
armarx::PlatformUnitConfigDialog::~PlatformUnitConfigDialog()
diff --git a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitConfigDialog.h b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitConfigDialog.h
index 1cb33441f443875c817e96a36ea0e188918a2a33..d735579e18c76c4132483da93455758dc3d20e3e 100644
--- a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitConfigDialog.h
+++ b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitConfigDialog.h
@@ -28,7 +28,8 @@
#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/IceProxyFinder.h>
#include <RobotAPI/interface/units/PlatformUnitInterface.h>
-namespace Ui {
+namespace Ui
+{
class PlatformUnitConfigDialog;
}
@@ -36,18 +37,18 @@ namespace armarx
{
class PlatformUnitConfigDialog :
public QDialog,
- virtual public ManagedIceObject
+ virtual public ManagedIceObject
{
Q_OBJECT
public:
- explicit PlatformUnitConfigDialog(QWidget *parent = 0);
+ explicit PlatformUnitConfigDialog(QWidget* parent = 0);
~PlatformUnitConfigDialog();
IceProxyFinder<PlatformUnitInterfacePrx>* finder;
private:
- Ui::PlatformUnitConfigDialog *ui;
+ Ui::PlatformUnitConfigDialog* ui;
std::string uuid;
friend class PlatformUnitWidget;
diff --git a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
index c63619f2eb098525e61a20744777b9190c423ec3..00c597a2be1467b87c331c9059726c50c5a0bb81 100644
--- a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
@@ -25,11 +25,11 @@ PlatformUnitGuiPlugin::PlatformUnitGuiPlugin()
PlatformUnitWidget::PlatformUnitWidget() :
platformUnitProxyName("PlatformUnit"), // overwritten in loadSettings() anyway?
platformName("Platform"),
- speedCtrl{nullptr},
- rotaCtrl{nullptr},
- ctrlEvaluationTimer{},
- platformRotation{0},
- platformMoves{false}
+ speedCtrl {nullptr},
+ rotaCtrl {nullptr},
+ ctrlEvaluationTimer {},
+ platformRotation {0},
+platformMoves {false}
{
// init gui
ui.setupUi(getWidget());
@@ -42,8 +42,8 @@ PlatformUnitWidget::PlatformUnitWidget() :
rotaCtrl = rotat.get();
rotaCtrl->setSteps(0);
//add joystick controls
- ui.gridLayout_2->addWidget(rotat.release(),2,0,1,2);
- ui.gridLayout_3->addWidget(speed.release(),2,0,1,2);
+ ui.gridLayout_2->addWidget(rotat.release(), 2, 0, 1, 2);
+ ui.gridLayout_3->addWidget(speed.release(), 2, 0, 1, 2);
ctrlEvaluationTimer.setSingleShot(false);
}
@@ -75,28 +75,29 @@ void PlatformUnitWidget::onExitComponent()
QPointer<QDialog> PlatformUnitWidget::getConfigDialog(QWidget* parent)
{
- if(!dialog)
+ if (!dialog)
{
dialog = new PlatformUnitConfigDialog(parent);
}
+
dialog->ui->editPlatformName->setText(QString::fromStdString(platformName));
return qobject_cast<PlatformUnitConfigDialog*>(dialog);
}
- void PlatformUnitWidget::configured()
- {
- platformUnitProxyName = dialog->finder->getSelectedProxyName().toStdString();
- platformName = dialog->ui->editPlatformName->text().toStdString();
- }
+void PlatformUnitWidget::configured()
+{
+ platformUnitProxyName = dialog->finder->getSelectedProxyName().toStdString();
+ platformName = dialog->ui->editPlatformName->text().toStdString();
+}
-void PlatformUnitWidget::loadSettings(QSettings *settings)
+void PlatformUnitWidget::loadSettings(QSettings* settings)
{
platformUnitProxyName = settings->value("platformUnitProxyName", QString::fromStdString(platformUnitProxyName)).toString().toStdString();
platformName = settings->value("platformName", QString::fromStdString(platformName)).toString().toStdString();
}
-void PlatformUnitWidget::saveSettings(QSettings *settings)
+void PlatformUnitWidget::saveSettings(QSettings* settings)
{
settings->setValue("platformUnitProxyName", QString::fromStdString(platformUnitProxyName));
settings->setValue("platformName", QString::fromStdString(platformName));
@@ -107,10 +108,10 @@ void PlatformUnitWidget::connectSlots()
{
connect(ui.buttonMoveToPosition, SIGNAL(clicked()), this, SLOT(moveTo()));
connect(&ctrlEvaluationTimer, SIGNAL(timeout()), this, SLOT(controlTimerTick()));
- connect(speedCtrl,SIGNAL(pressed()), this, SLOT(startControlTimer()));
- connect(rotaCtrl,SIGNAL(pressed()), this, SLOT(startControlTimer()));
- connect(speedCtrl,SIGNAL(released()), this, SLOT(stopPlatform()));
- connect(speedCtrl,SIGNAL(released()), this, SLOT(stopControlTimer()));
+ connect(speedCtrl, SIGNAL(pressed()), this, SLOT(startControlTimer()));
+ connect(rotaCtrl, SIGNAL(pressed()), this, SLOT(startControlTimer()));
+ connect(speedCtrl, SIGNAL(released()), this, SLOT(stopPlatform()));
+ connect(speedCtrl, SIGNAL(released()), this, SLOT(stopControlTimer()));
connect(rotaCtrl, SIGNAL(released()), this, SLOT(stopPlatform()));
connect(rotaCtrl, SIGNAL(released()), this, SLOT(stopControlTimer()));
connect(ui.buttonStopPlatform, SIGNAL(pressed()), this, SLOT(stopPlatform()));
@@ -187,7 +188,7 @@ void PlatformUnitWidget::controlTimerTick()
{
float translationFactor = ui.maxTranslationSpeed->value();
float rotationFactor = ui.maxRotationSpeed->value() * -1;
- float rotationVel = rotaCtrl->getRotation()/M_PI_2 * rotationFactor;
+ float rotationVel = rotaCtrl->getRotation() / M_PI_2 * rotationFactor;
ARMARX_INFO << deactivateSpam(0.5) << speedCtrl->getPosition().x()*translationFactor << ", " << speedCtrl->getPosition().y()*translationFactor;
ARMARX_INFO << deactivateSpam(0.5) << "Rotation Speed: " << (rotationVel);
platformUnitProxy->move(speedCtrl->getPosition().x()*translationFactor, -1 * speedCtrl->getPosition().y()*translationFactor, rotationVel);
diff --git a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h
index 162846e8efa06f2cb8f89a89f0c77a57a34be556..b7a0985614fc2b87b96d3fb2544034ec1a366c54 100644
--- a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h
@@ -50,7 +50,7 @@ namespace armarx
\see PlatformUnitWidget
*/
class PlatformUnitGuiPlugin :
- public ArmarXGuiPlugin
+ public ArmarXGuiPlugin
{
public:
PlatformUnitGuiPlugin();
@@ -63,7 +63,7 @@ namespace armarx
/*!
\page RobotAPI-GuiPlugins-PlatformUnitPlugin PlatformUnitPlugin
\brief With this widget the PlatformUnit can be controlled.
-
+
\image html PlatformUnitGUI.png "The plugin's ui." width=300px
-# The current position and rotation, fields to enter a new target and a button to set the platform in motion.
-# A joystick like control widget to move the platform. The platform does not rotate to move in a direction. Up moves the platform forward.
@@ -71,15 +71,15 @@ namespace armarx
-# Be careful to set a maximum velocity before using the joysticks.
When you add the widget to the Gui, you need to specify the following parameters:
-
+
Parameter Name | Example Value | Required? | Description
:---------------- | :-------------: | :-------------- |:--------------------
PlatformUnit - Proxy | PlatformUnit | Yes | The name of the platform unit.
Platform | Platform | Yes | The name of the platform.
*/
class PlatformUnitWidget :
- public ArmarXComponentWidgetController,
- public PlatformUnitListener
+ public ArmarXComponentWidgetController,
+ public PlatformUnitListener
{
Q_OBJECT
public:
@@ -99,12 +99,18 @@ namespace armarx
void reportPlatformVelocity(::Ice::Float currentPlatformVelocityX, ::Ice::Float currentPlatformVelocityY, ::Ice::Float currentPlatformVelocityRotation, const Ice::Current& c = ::Ice::Current());
// inherited of ArmarXWidget
- virtual QString getWidgetName() const { return "RobotControl.PlatformUnitGUI"; }
- virtual QIcon getWidgetIcon() const { return QIcon("://icons/retro_joystick2.svg"); }
+ virtual QString getWidgetName() const
+ {
+ return "RobotControl.PlatformUnitGUI";
+ }
+ virtual QIcon getWidgetIcon() const
+ {
+ return QIcon("://icons/retro_joystick2.svg");
+ }
QPointer<QDialog> getConfigDialog(QWidget* parent = 0);
- virtual void loadSettings(QSettings * settings);
- virtual void saveSettings(QSettings * settings);
+ virtual void loadSettings(QSettings* settings);
+ virtual void saveSettings(QSettings* settings);
void configured();
public slots:
@@ -129,9 +135,9 @@ namespace armarx
*/
void controlTimerTick();
- /**
- \brief Stops the platform
- */
+ /**
+ \brief Stops the platform
+ */
void stopPlatform();
private:
diff --git a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerConfigDialog.cpp b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerConfigDialog.cpp
index e95f108befca5e85440a4cb1676e62dd80c0c4b0..13e71860078b7bba0f5dff17c9af0b5551f61c07 100644
--- a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerConfigDialog.cpp
+++ b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerConfigDialog.cpp
@@ -36,7 +36,7 @@
using namespace armarx;
-RobotViewerConfigDialog::RobotViewerConfigDialog(QWidget *parent) :
+RobotViewerConfigDialog::RobotViewerConfigDialog(QWidget* parent) :
QDialog(parent),
ui(new Ui::RobotViewerConfigDialog),
uuid(IceUtil::generateUUID())
@@ -44,10 +44,10 @@ RobotViewerConfigDialog::RobotViewerConfigDialog(QWidget *parent) :
ui->setupUi(this);
connect(this->ui->buttonBox, SIGNAL(accepted()), this, SLOT(verifyConfig()));
- ui->buttonBox->button( QDialogButtonBox::Ok )->setDefault(true);
+ ui->buttonBox->button(QDialogButtonBox::Ok)->setDefault(true);
proxyFinder = new IceProxyFinder<RobotStateComponentInterfacePrx>(this);
proxyFinder->setSearchMask("RobotState*");
- ui->gridLayout->addWidget(proxyFinder, 2,1,1,2);
+ ui->gridLayout->addWidget(proxyFinder, 2, 1, 1, 2);
}
RobotViewerConfigDialog::~RobotViewerConfigDialog()
@@ -75,12 +75,14 @@ void RobotViewerConfigDialog::onExitComponent()
void RobotViewerConfigDialog::verifyConfig()
{
- if(proxyFinder->getSelectedProxyName().trimmed().length() == 0)
+ if (proxyFinder->getSelectedProxyName().trimmed().length() == 0)
{
QMessageBox::critical(this, "Invalid Configuration", "The proxy name must not be empty");
}
else
+ {
this->accept();
+ }
}
diff --git a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerConfigDialog.h b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerConfigDialog.h
index 1a65513600e37e89508e0a720c966efe822d7e28..b1e011efe331c2a50f9d4b41f877e106ec6b14e8 100644
--- a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerConfigDialog.h
+++ b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerConfigDialog.h
@@ -31,11 +31,13 @@
#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/IceProxyFinder.h>
-namespace Ui {
+namespace Ui
+{
class RobotViewerConfigDialog;
}
-namespace armarx{
+namespace armarx
+{
class RobotViewerConfigDialog :
public QDialog,
virtual public ManagedIceObject
@@ -43,11 +45,14 @@ namespace armarx{
Q_OBJECT
public:
- explicit RobotViewerConfigDialog(QWidget *parent = 0);
+ explicit RobotViewerConfigDialog(QWidget* parent = 0);
~RobotViewerConfigDialog();
// inherited from ManagedIceObject
- std::string getDefaultName() const { return "RobotViewerConfigDialog" + uuid;}
+ std::string getDefaultName() const
+ {
+ return "RobotViewerConfigDialog" + uuid;
+ }
void onInitComponent();
void onConnectComponent();
void onExitComponent();
@@ -60,7 +65,7 @@ namespace armarx{
private:
IceProxyFinderBase* proxyFinder;
- Ui::RobotViewerConfigDialog *ui;
+ Ui::RobotViewerConfigDialog* ui;
std::string uuid;
friend class RobotViewerWidgetController;
};
diff --git a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp
index 70690d5f525c3ddc8ad3c349180a500d605c6dcf..838783ebd9aca7d7c665fdae629ed9052ddc92c4 100644
--- a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp
@@ -81,12 +81,17 @@ void RobotViewerWidgetController::onInitComponent()
{
std::string debugDrawerComponentName = "RobotViewerGUIDebugDrawer_" + getName();
ARMARX_INFO << "Creating component " << debugDrawerComponentName;
- debugDrawer = Component::create<DebugDrawerComponent>(properties,debugDrawerComponentName);
+ debugDrawer = Component::create<DebugDrawerComponent>(properties, debugDrawerComponentName);
+
if (mutex3D)
{
debugDrawer->setMutex(mutex3D);
- } else
+ }
+ else
+ {
ARMARX_ERROR << " No 3d mutex available...";
+ }
+
ArmarXManagerPtr m = getArmarXManager();
m->addObject(debugDrawer);
@@ -99,6 +104,7 @@ void RobotViewerWidgetController::onInitComponent()
rootVisu->addChild(debugLayerVisu);
}
}
+
showRoot(true);
}
@@ -107,22 +113,30 @@ void RobotViewerWidgetController::onConnectComponent()
robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>(robotStateComponentName);
if (robotVisu)
+ {
robotVisu->removeAllChildren();
+ }
+
robot.reset();
- std::string rfile;
- StringList includePaths;
+ std::string rfile;
+ StringList includePaths;
// get robot filename
- try {
+ try
+ {
StringList packages = robotStateComponentPrx->getArmarXPackages();
packages.push_back(Application::GetProjectName());
ARMARX_VERBOSE << "ArmarX packages " << packages;
- for(const std::string &projectName : packages)
+
+ for (const std::string& projectName : packages)
{
- if(projectName.empty())
+ if (projectName.empty())
+ {
continue;
+ }
+
CMakePackageFinder project(projectName);
StringList projectIncludePaths;
auto pathsString = project.getDataDir();
@@ -135,28 +149,36 @@ void RobotViewerWidgetController::onConnectComponent()
includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
}
+
rfile = robotStateComponentPrx->getRobotFilename();
ARMARX_VERBOSE << "Relative robot file " << rfile;
ArmarXDataPath::getAbsolutePath(rfile, rfile, includePaths);
ARMARX_VERBOSE << "Absolute robot file " << rfile;
- } catch (...)
- {
- ARMARX_ERROR << "Unable to retrieve robot filename";
- }
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "Unable to retrieve robot filename";
+ }
- try {
+ try
+ {
ARMARX_INFO << "Loading robot from file " << rfile;
robot = loadRobotFile(rfile);
- } catch (...)
+ }
+ catch (...)
{
ARMARX_ERROR << "Failed to init robot";
}
- if(!robot)
+ if (!robot)
{
getObjectScheduler()->terminate();
- if(getWidget()->parentWidget())
+
+ if (getWidget()->parentWidget())
+ {
getWidget()->parentWidget()->close();
+ }
+
std::cout << "returning" << std::endl;
return;
}
@@ -179,12 +201,14 @@ void RobotViewerWidgetController::onDisconnectComponent()
{
ARMARX_INFO << "Disconnecting component";
+
// stop update timer
if (timerSensor)
{
SoSensorManager* sensor_mgr = SoDB::getSensorManager();
sensor_mgr->removeTimerSensor(timerSensor);
}
+
ARMARX_INFO << "Disconnecting component: timer stopped";
robotStateComponentPrx = NULL;
@@ -194,8 +218,9 @@ void RobotViewerWidgetController::onDisconnectComponent()
if (robotVisu)
{
ARMARX_INFO << "Disconnecting component: removing visu";
- robotVisu->removeAllChildren();
+ robotVisu->removeAllChildren();
}
+
robot.reset();
}
ARMARX_INFO << "Disconnecting component: finished";
@@ -231,37 +256,38 @@ void RobotViewerWidgetController::onExitComponent()
}
}
-/*
- if (debugDrawer && debugDrawer->getObjectScheduler())
- {
- ARMARX_INFO << "Removing DebugDrawer component...";
- debugDrawer->getObjectScheduler()->terminate();
- ARMARX_INFO << "Removing DebugDrawer component...done";
- }
-*/
+ /*
+ if (debugDrawer && debugDrawer->getObjectScheduler())
+ {
+ ARMARX_INFO << "Removing DebugDrawer component...";
+ debugDrawer->getObjectScheduler()->terminate();
+ ARMARX_INFO << "Removing DebugDrawer component...done";
+ }
+ */
}
QPointer<QDialog> RobotViewerWidgetController::getConfigDialog(QWidget* parent)
{
- if(!dialog)
+ if (!dialog)
{
dialog = new RobotViewerConfigDialog(parent);
dialog->setName(dialog->getDefaultName());
}
+
return qobject_cast<RobotViewerConfigDialog*>(dialog);
}
- void RobotViewerWidgetController::configured()
- {
- ARMARX_VERBOSE << "RobotViewerWidget::configured()";
- robotStateComponentName = dialog->proxyFinder->getSelectedProxyName().trimmed().toStdString();
- }
+void RobotViewerWidgetController::configured()
+{
+ ARMARX_VERBOSE << "RobotViewerWidget::configured()";
+ robotStateComponentName = dialog->proxyFinder->getSelectedProxyName().trimmed().toStdString();
+}
-void RobotViewerWidgetController::loadSettings(QSettings *settings)
+void RobotViewerWidgetController::loadSettings(QSettings* settings)
{
robotStateComponentName = settings->value("RobotStateComponent", QString::fromStdString(ROBOTSTATE_NAME_DEFAULT)).toString().toStdString();
bool showRob = settings->value("showRobot", QVariant(true)).toBool();
@@ -271,7 +297,7 @@ void RobotViewerWidgetController::loadSettings(QSettings *settings)
ui.radioButtonCol->setChecked(!fullMod);
}
-void RobotViewerWidgetController::saveSettings(QSettings *settings)
+void RobotViewerWidgetController::saveSettings(QSettings* settings)
{
settings->setValue("RobotStateComponent", QString::fromStdString(robotStateComponentName));
settings->setValue("showRobot", ui.cbRobot->isChecked());
@@ -281,18 +307,28 @@ void RobotViewerWidgetController::saveSettings(QSettings *settings)
void RobotViewerWidgetController::setRobotVisu(bool colModel)
{
robotVisu->removeAllChildren();
+
if (!robot)
+ {
return;
+ }
+
boost::recursive_mutex::scoped_lock lock(*mutex3D);
VirtualRobot::SceneObject::VisualizationType v = VirtualRobot::SceneObject::Full;
+
if (colModel)
- v = VirtualRobot::SceneObject::Collision;
+ {
+ v = VirtualRobot::SceneObject::Collision;
+ }
+
CoinVisualizationPtr robotViewerVisualization = robot->getVisualization<CoinVisualization>(v);
+
if (robotViewerVisualization)
{
robotVisu->addChild(robotViewerVisualization->getCoinVisualization());
- } else
+ }
+ else
{
ARMARX_WARNING << "no robot visu available...";
}
@@ -302,7 +338,7 @@ void RobotViewerWidgetController::showVisuLayers(bool show)
{
if (debugDrawer)
{
- if(show)
+ if (show)
{
debugDrawer->enableAllLayers();
}
@@ -316,14 +352,17 @@ void RobotViewerWidgetController::showVisuLayers(bool show)
void RobotViewerWidgetController::showRoot(bool show)
{
if (!debugDrawer)
+ {
return;
+ }
std::string poseName("root");
- if(show)
+
+ if (show)
{
Eigen::Matrix4f gp = Eigen::Matrix4f::Identity();
PosePtr gpP(new Pose(gp));
- debugDrawer->setPoseDebugLayerVisu(poseName,gpP);
+ debugDrawer->setPoseDebugLayerVisu(poseName, gpP);
}
else
{
@@ -334,19 +373,22 @@ void RobotViewerWidgetController::showRoot(bool show)
void RobotViewerWidgetController::showRobot(bool show)
{
if (!robotVisu)
+ {
return;
+ }
+
boost::recursive_mutex::scoped_lock lock(*mutex3D);
- if(show && rootVisu->findChild(robotVisu)<0)
+ if (show && rootVisu->findChild(robotVisu) < 0)
{
rootVisu->addChild(robotVisu);
}
- else if (!show && rootVisu->findChild(robotVisu)>=0)
+ else if (!show && rootVisu->findChild(robotVisu) >= 0)
{
rootVisu->removeChild(robotVisu);
}
}
-SoNode *RobotViewerWidgetController::getScene()
+SoNode* RobotViewerWidgetController::getScene()
{
return rootVisu;
}
@@ -354,8 +396,12 @@ SoNode *RobotViewerWidgetController::getScene()
void RobotViewerWidgetController::timerCB(void* data, SoSensor* sensor)
{
RobotViewerWidgetController* controller = static_cast<RobotViewerWidgetController*>(data);
+
if (!controller)
+ {
return;
+ }
+
controller->updateRobotVisu();
}
@@ -376,11 +422,13 @@ VirtualRobot::RobotPtr RobotViewerWidgetController::loadRobotFile(std::string fi
{
VirtualRobot::RobotPtr robot;
- if (!ArmarXDataPath::getAbsolutePath(fileName,fileName))
+ if (!ArmarXDataPath::getAbsolutePath(fileName, fileName))
{
- ARMARX_INFO << "Could not find Robot XML file with name " << fileName;
+ ARMARX_INFO << "Could not find Robot XML file with name " << fileName;
}
+
robot = RobotIO::loadRobot(fileName);
+
if (!robot)
{
ARMARX_INFO << "Could not find Robot XML file with name " << fileName;
@@ -393,8 +441,11 @@ VirtualRobot::RobotPtr RobotViewerWidgetController::loadRobotFile(std::string fi
void RobotViewerWidgetController::colModel(bool c)
{
bool colModel = false;
+
if (ui.radioButtonCol->isChecked())
+ {
colModel = true;
+ }
setRobotVisu(colModel);
}
@@ -405,12 +456,15 @@ void RobotViewerWidgetController::updateRobotVisu()
boost::recursive_mutex::scoped_lock lock(*mutex3D);
if (!robotStateComponentPrx || !robot)
+ {
return;
+ }
try
{
- RemoteRobot::synchronizeLocalClone(robot,robotStateComponentPrx);
- } catch (...)
+ RemoteRobot::synchronizeLocalClone(robot, robotStateComponentPrx);
+ }
+ catch (...)
{
ARMARX_INFO << deactivateSpam(5) << "Robot synchronization failed";
return;
@@ -418,14 +472,14 @@ void RobotViewerWidgetController::updateRobotVisu()
Eigen::Matrix4f gp = robot->getGlobalPose();
QString roboInfo("Robot Pose (global): pos: ");
- roboInfo += QString::number(gp(0,3), 'f', 2);
+ roboInfo += QString::number(gp(0, 3), 'f', 2);
roboInfo += QString(", ");
- roboInfo += QString::number(gp(1,3), 'f', 2);
+ roboInfo += QString::number(gp(1, 3), 'f', 2);
roboInfo += QString(", ");
- roboInfo += QString::number(gp(2,3), 'f', 2);
+ roboInfo += QString::number(gp(2, 3), 'f', 2);
roboInfo += QString(", rot:");
Eigen::Vector3f rpy;
- VirtualRobot::MathTools::eigen4f2rpy(gp,rpy);
+ VirtualRobot::MathTools::eigen4f2rpy(gp, rpy);
roboInfo += QString::number(rpy(0), 'f', 2);
roboInfo += QString(", ");
roboInfo += QString::number(rpy(1), 'f', 2);
@@ -438,8 +492,11 @@ void RobotViewerWidgetController::setMutex3D(boost::shared_ptr<boost::recursive_
{
//ARMARX_IMPORTANT << "set mutex3d :" << mutex3D.get();
this->mutex3D = mutex3D;
+
if (debugDrawer)
+ {
debugDrawer->setMutex(mutex3D);
+ }
}
diff --git a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h
index d879a300af440cee7c9114dc8d9f5367f9517f33..bc2a8c015a2f1537fdc15d620a1f47ff3348f1d1 100644
--- a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h
@@ -61,7 +61,7 @@ namespace armarx
\see RobotViewerWidget
*/
class RobotViewerGuiPlugin :
- public ArmarXGuiPlugin
+ public ArmarXGuiPlugin
{
public:
RobotViewerGuiPlugin();
@@ -79,13 +79,13 @@ namespace armarx
Further, DebugDrawer methods are available.
\image html RobotViewerGUI.png
When you add the widget to the Gui, you need to specify the following parameters:
-
+
Parameter Name | Example Value | Required? | Description
:---------------- | :-------------: | :-------------- |:--------------------
Proxy | RobotStateComponent | Yes | The name of the robot state component.
*/
class RobotViewerWidgetController :
- public ArmarXComponentWidgetController
+ public ArmarXComponentWidgetController
{
Q_OBJECT
public:
@@ -105,8 +105,8 @@ namespace armarx
return "RobotControl.RobotViewerGUI";
}
QPointer<QDialog> getConfigDialog(QWidget* parent = 0);
- virtual void loadSettings(QSettings * settings);
- virtual void saveSettings(QSettings * settings);
+ virtual void loadSettings(QSettings* settings);
+ virtual void saveSettings(QSettings* settings);
void configured();
SoNode* getScene();
@@ -146,7 +146,7 @@ namespace armarx
SoSeparator* debugLayerVisu;
armarx::DebugDrawerComponentPtr debugDrawer;
- static void timerCB(void *data, SoSensor *sensor);
+ static void timerCB(void* data, SoSensor* sensor);
void setRobotVisu(bool colModel);
protected slots:
void showVisuLayers(bool show);
diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.cpp b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.cpp
index 8c461c407d58ca250e016c0a1b1faa4c2217a7cd..a0618adeb178f217391532c66e974e3076c868ba 100644
--- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.cpp
+++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.cpp
@@ -70,20 +70,20 @@ namespace armarx
// Setup Plotter
///////////////
// panning with the left mouse button
- ( void ) new QwtPlotPanner( ui.qwtPlot->canvas() );
+ (void) new QwtPlotPanner(ui.qwtPlot->canvas());
// zoom in/out with the wheel
- QwtPlotMagnifier * magnifier = new QwtPlotMagnifier( ui.qwtPlot->canvas() );
+ QwtPlotMagnifier* magnifier = new QwtPlotMagnifier(ui.qwtPlot->canvas());
magnifier->setAxisEnabled(QwtPlot::xBottom, false);
ui.qwtPlot->canvas()->setPaintAttribute(QwtPlotCanvas::BackingStore, false); //increases performance for incremental drawing
- QwtLegend *legend = new QwtLegend;
- legend->setItemMode( QwtLegend::CheckableItem );
- ui.qwtPlot->insertLegend( legend, QwtPlot::BottomLegend );
+ QwtLegend* legend = new QwtLegend;
+ legend->setItemMode(QwtLegend::CheckableItem);
+ ui.qwtPlot->insertLegend(legend, QwtPlot::BottomLegend);
- ui.qwtPlot->setAxisTitle( QwtPlot::xBottom, "Time (in sec)");
+ ui.qwtPlot->setAxisTitle(QwtPlot::xBottom, "Time (in sec)");
// ui.qwtPlot->setAutoReplot();
dialog = new ArmarXPlotterDialog(getWidget(), NULL);
@@ -112,7 +112,7 @@ namespace armarx
void ArmarXPlotter::onConnectComponent()
{
- ARMARX_VERBOSE<< "ArmarXPlotter started" << flush;
+ ARMARX_VERBOSE << "ArmarXPlotter started" << flush;
dialog->setIceManager(getIceManager());
connect(dialog, SIGNAL(accepted()), this, SLOT(configDone()));
@@ -121,28 +121,33 @@ namespace armarx
connect(ui.BTNPlotterStatus, SIGNAL(toggled(bool)), this, SLOT(plottingPaused(bool)));
connect(ui.BTNAutoScale, SIGNAL(toggled(bool)), this, SLOT(autoScale(bool)));
- connect( ui.qwtPlot, SIGNAL( legendChecked( QwtPlotItem *, bool ) ),
- SLOT( showCurve( QwtPlotItem *, bool ) ) );
+ connect(ui.qwtPlot, SIGNAL(legendChecked(QwtPlotItem*, bool)),
+ SLOT(showCurve(QwtPlotItem*, bool)));
connect(ui.btnLogToFile, SIGNAL(toggled(bool)), this, SLOT(toggleLogging(bool)));
- if(!QMetaObject::invokeMethod(this, "setupCurves"))
+ if (!QMetaObject::invokeMethod(this, "setupCurves"))
+ {
ARMARX_WARNING << "Failed to invoke enable";
+ }
}
void ArmarXPlotter::onExitComponent()
{
- if(pollingTask)
+ if (pollingTask)
+ {
pollingTask->stop();
+ }
}
- void ArmarXPlotter::onCloseWidget(QCloseEvent *event)
+ void ArmarXPlotter::onCloseWidget(QCloseEvent* event)
{
ARMARX_VERBOSE << "closing" << flush;
timer->stop();
- if(logstream.is_open())
+
+ if (logstream.is_open())
{
logstream.close();
}
@@ -154,8 +159,11 @@ namespace armarx
void ArmarXPlotter::configDialog()
{
- if(!dialog)
+ if (!dialog)
+ {
return;
+ }
+
dialog->ui.spinBoxUpdateInterval->setValue(updateInterval);
dialog->ui.spinBoxShownInterval->setValue(shownInterval);
dialog->ui.listWidget->clear();
@@ -188,7 +196,7 @@ namespace armarx
void ArmarXPlotter::toggleLogging(bool toggled)
{
- if(toggled)
+ if (toggled)
{
std::string filename = "datalog-" + IceUtil::Time::now().toDateTime() + ".csv";
@@ -197,7 +205,8 @@ namespace armarx
ARMARX_INFO << "Logging to " << filename;
logstream.open(filename);
logStartTime = IceUtil::Time::now();
- if(!logstream.is_open())
+
+ if (!logstream.is_open())
{
ARMARX_ERROR << "Could not open file for logging: " << filename;
ui.btnLogToFile->setChecked(false);
@@ -205,10 +214,12 @@ namespace armarx
else
{
logstream << "Timestamp";
- for(auto& channel: selectedChannels)
+
+ for (auto& channel : selectedChannels)
{
logstream << "," << channel.toStdString();
}
+
logstream << std::endl;
}
}
@@ -222,7 +233,8 @@ namespace armarx
{
ScopedLock lock(dataMutex);
auto newData = getData(selectedChannels, dataMap);
- if(ui.btnLogToFile->isChecked())
+
+ if (ui.btnLogToFile->isChecked())
{
logToFile(IceUtil::Time::now(), newData);
}
@@ -235,94 +247,131 @@ namespace armarx
ScopedLock lock(dataMutex);
QPointF p;
-// int size = shownInterval*1000/updateInterval;
+ // int size = shownInterval*1000/updateInterval;
GraphDataMap::iterator it = dataMap.begin();
IceUtil::Time curTime = IceUtil::Time::now();
- for(; it != dataMap.end(); ++it)
+ for (; it != dataMap.end(); ++it)
{
QVector<QPointF> pointList;
pointList.clear();
std::vector<TimeData>& dataVec = it->second;
-// int newSize = min(size,(int)dataVec.size());
+ // int newSize = min(size,(int)dataVec.size());
pointList.resize(dataVec.size());
+
try
{
int j = 0;
- for (int i = dataVec.size()-1; i >=0 ; --i) {
+
+ for (int i = dataVec.size() - 1; i >= 0 ; --i)
+ {
TimeData& data = dataVec[i];
IceUtil::Time age = (data.time - curTime);
- if(age.toMilliSecondsDouble() <= shownInterval*1000)
+
+ if (age.toMilliSecondsDouble() <= shownInterval * 1000)
{
VariantPtr var = VariantPtr::dynamicCast(data.data);
- if(var->getInitialized())
+
+ if (var->getInitialized())
{
- if((signed int)j < pointList.size())
+ if ((signed int)j < pointList.size())
{
- if(var->getType() == VariantType::Float)
+ if (var->getType() == VariantType::Float)
+ {
pointList[j].setY(var->getFloat());
- else if(var->getType() == VariantType::Double)
+ }
+ else if (var->getType() == VariantType::Double)
+ {
pointList[j].setY(var->getDouble());
- else if(var->getType() == VariantType::Int)
+ }
+ else if (var->getType() == VariantType::Int)
+ {
pointList[j].setY(var->getInt());
- else continue;
- pointList[j].setX(0.001f*age.toMilliSecondsDouble());
- }else
+ }
+ else
+ {
+ continue;
+ }
+
+ pointList[j].setX(0.001f * age.toMilliSecondsDouble());
+ }
+ else
{
- if(var->getType() == VariantType::Float)
+ if (var->getType() == VariantType::Float)
+ {
p.setY(var->getFloat());
- else if(var->getType() == VariantType::Double)
+ }
+ else if (var->getType() == VariantType::Double)
+ {
p.setY(var->getDouble());
- else if(var->getType() == VariantType::Int)
+ }
+ else if (var->getType() == VariantType::Int)
+ {
p.setY(var->getInt());
- else continue;
- p.setX(0.001*age.toMilliSecondsDouble());
+ }
+ else
+ {
+ continue;
+ }
+
+ p.setX(0.001 * age.toMilliSecondsDouble());
pointList.push_back(p);
}
+
j++;
}
else
{
- ARMARX_WARNING<< "uninitialized field: " << it->first;
+ ARMARX_WARNING << "uninitialized field: " << it->first;
}
}
else
- break; // data too old from now
+ {
+ break; // data too old from now
+ }
}
}
- catch(...)
+ catch (...)
{
handleExceptions();
}
QwtSeriesData<QPointF>* pointSeries = new QwtPointSeriesData(pointList);
- if(curves.find(it->first) != curves.end()){
- QwtPlotCurve *curve = curves[it->first];
+
+ if (curves.find(it->first) != curves.end())
+ {
+ QwtPlotCurve* curve = curves[it->first];
curve->setData(pointSeries);
}
}
- ui.qwtPlot->setAxisScale( QwtPlot::xBottom, shownInterval*-1, 0.f);
- if(ui.BTNAutoScale->isChecked())
+ ui.qwtPlot->setAxisScale(QwtPlot::xBottom, shownInterval * -1, 0.f);
+
+ if (ui.BTNAutoScale->isChecked())
+ {
ui.qwtPlot->setAxisAutoScale(QwtPlot::yLeft, true);
+ }
+
// ui.qwtPlot->setAxisScale( QwtPlot::yLeft, -1, 1);
ui.qwtPlot->replot();
}
- void ArmarXPlotter::showCurve( QwtPlotItem *item, bool on )
+ void ArmarXPlotter::showCurve(QwtPlotItem* item, bool on)
{
- item->setVisible( on );
+ item->setVisible(on);
- QwtLegendItem *legendItem =
- qobject_cast<QwtLegendItem *>( ui.qwtPlot->legend()->find( item ) );
+ QwtLegendItem* legendItem =
+ qobject_cast<QwtLegendItem*>(ui.qwtPlot->legend()->find(item));
- if ( legendItem )
- legendItem->setChecked( on );
+ if (legendItem)
+ {
+ legendItem->setChecked(on);
+ }
ui.qwtPlot->replot();
}
@@ -341,9 +390,13 @@ namespace armarx
{
ScopedLock lock(dataMutex);
__plottingPaused = toggled;
- if(pollingTask)
+
+ if (pollingTask)
+ {
pollingTask->stop();
- if(__plottingPaused)
+ }
+
+ if (__plottingPaused)
{
timer->stop();
}
@@ -357,7 +410,7 @@ namespace armarx
- void ArmarXPlotter::saveSettings(QSettings * settings)
+ void ArmarXPlotter::saveSettings(QSettings* settings)
{
ScopedLock lock(dataMutex);
settings->setValue("updateInterval", updateInterval);
@@ -367,7 +420,7 @@ namespace armarx
settings->setValue("autoScaleActive", ui.BTNAutoScale->isChecked());
}
- void ArmarXPlotter::loadSettings(QSettings * settings)
+ void ArmarXPlotter::loadSettings(QSettings* settings)
{
ScopedLock lock(dataMutex);
updateInterval = settings->value("updateInterval", 100).toInt();
@@ -379,12 +432,12 @@ namespace armarx
ARMARX_VERBOSE << "Settings loaded" << flush;
}
- QwtPlotCurve * ArmarXPlotter::createCurve(const QString& label)
+ QwtPlotCurve* ArmarXPlotter::createCurve(const QString& label)
{
QwtPlotCurve* curve = new QwtPlotCurve(label);
- curve->setPen( QColor( Qt::GlobalColor((curves.size()+7)%15) ) );
- curve->setStyle( QwtPlotCurve::Lines );
+ curve->setPen(QColor(Qt::GlobalColor((curves.size() + 7) % 15)));
+ curve->setStyle(QwtPlotCurve::Lines);
curve->attach(ui.qwtPlot);
showCurve(curve, true);
@@ -396,112 +449,138 @@ namespace armarx
{
{
ScopedLock lock(dataMutex);
- for(int i = 0; i < selectedChannels.size(); i++){
+
+ for (int i = 0; i < selectedChannels.size(); i++)
+ {
ARMARX_VERBOSE << "Channel: " << selectedChannels.at(i).toStdString() << flush;
- DataFieldIdentifierPtr identifier = new DataFieldIdentifier( selectedChannels.at(i).toStdString());
+ DataFieldIdentifierPtr identifier = new DataFieldIdentifier(selectedChannels.at(i).toStdString());
auto prx = getProxy<ObserverInterfacePrx>(identifier->observerName);
VariantPtr var = VariantPtr::dynamicCast(prx->getDataField(identifier));
- if(VariantType::IsBasicType(var->getType()))
+
+ if (VariantType::IsBasicType(var->getType()))
{
QwtPlotCurve* curve = createCurve(selectedChannels.at(i));
- curves[selectedChannels.at(i).toStdString()]=curve;
+ curves[selectedChannels.at(i).toStdString()] = curve;
}
else
{
auto id = identifier->getIdentifierStr();
ARMARX_IMPORTANT << id;
auto dict = JSONObject::ConvertToBasicVariantMap(json, var);
- for(auto e : dict)
+
+ for (auto e : dict)
{
VariantTypeId type = e.second->getType();
- if(type == VariantType::Double
- || type == VariantType::Float
- || type == VariantType::Int)
+
+ if (type == VariantType::Double
+ || type == VariantType::Float
+ || type == VariantType::Int)
{
std::string key = id + "." + e.first;
ARMARX_INFO << key << ": " << *VariantPtr::dynamicCast(e.second);
QwtPlotCurve* curve = createCurve(QString::fromStdString(key));
- curves[key]=curve;
+ curves[key] = curve;
}
}
}
}
+
ui.qwtPlot->replot();
}
timer->start(updateInterval);
- if(pollingTask)
+ if (pollingTask)
+ {
pollingTask->stop();
+ }
+
pollingTask = new PeriodicTask<ArmarXPlotter>(this, & ArmarXPlotter::pollingExec, pollingInterval, false, "DataPollingTask");
pollingTask->start();
}
- std::map<string, VariantPtr> ArmarXPlotter::getData(const QStringList &channels, GraphDataMap &dataMaptoAppend)
+ std::map<string, VariantPtr> ArmarXPlotter::getData(const QStringList& channels, GraphDataMap& dataMaptoAppend)
{
map< std::string, DataFieldIdentifierBaseList > channelsSplittedByObserver;
foreach(QString channel, channels)
{
DataFieldIdentifierPtr identifier = new DataFieldIdentifier(channel.toStdString());
channelsSplittedByObserver[identifier->getObserverName()].push_back(identifier);
-// ARMARX_INFO << identifier;
+ // ARMARX_INFO << identifier;
}
std::map<std::string, VariantPtr> newData;
// first clear to old entries
auto now = IceUtil::Time::now();
GraphDataMap::iterator itmap = dataMaptoAppend.begin();
- for(; itmap != dataMaptoAppend.end(); ++itmap)
+
+ for (; itmap != dataMaptoAppend.end(); ++itmap)
{
std::vector<TimeData>& dataVec = itmap->second;
int stepSize = dataVec.size() * 0.01;
int thresholdIndex = -1;
- for(unsigned int i=0; dataVec.size(); i+=stepSize)
+
+ for (unsigned int i = 0; dataVec.size(); i += stepSize)
{
// only delete if entries are older than 2*showninterval
// and delete then all entries that are older than showninterval.
// otherwise it would delete items on every call, which would be very slow
- if((now - dataVec[i].time).toMilliSecondsDouble() > shownInterval*2*1000
- || (thresholdIndex != -1 && (now - dataVec[i].time ).toMilliSecondsDouble() > shownInterval*1000)
- )
+ if ((now - dataVec[i].time).toMilliSecondsDouble() > shownInterval * 2 * 1000
+ || (thresholdIndex != -1 && (now - dataVec[i].time).toMilliSecondsDouble() > shownInterval * 1000)
+ )
{
thresholdIndex = i;
- }else
+ }
+ else
+ {
break;
+ }
}
- if(thresholdIndex!=-1)
+
+ if (thresholdIndex != -1)
{
- unsigned int offset = std::min((int)dataVec.size(),thresholdIndex);
-// ARMARX_IMPORTANT << "Erasing " << offset << " fields";
- if(offset>dataVec.size())
+ unsigned int offset = std::min((int)dataVec.size(), thresholdIndex);
+
+ // ARMARX_IMPORTANT << "Erasing " << offset << " fields";
+ if (offset > dataVec.size())
+ {
dataVec.clear();
+ }
else
- dataVec.erase(dataVec.begin(), dataVec.begin()+offset);
+ {
+ dataVec.erase(dataVec.begin(), dataVec.begin() + offset);
+ }
}
-// ARMARX_IMPORTANT << deactivateSpam(5) << "size: " << dataVec.size();
+
+ // ARMARX_IMPORTANT << deactivateSpam(5) << "size: " << dataVec.size();
}
+
// now get new data
IceUtil::Time time = IceUtil::Time::now();
map<string, DataFieldIdentifierBaseList >::iterator it = channelsSplittedByObserver.begin();
+
try
{
- for(; it != channelsSplittedByObserver.end(); ++it)
+ for (; it != channelsSplittedByObserver.end(); ++it)
{
std::string observerName = it->first;
- if(proxyMap.find(observerName) == proxyMap.end())
+ if (proxyMap.find(observerName) == proxyMap.end())
{
proxyMap[observerName] = getProxy<ObserverInterfacePrx>(observerName);
}
+
// QDateTime time(QDateTime::currentDateTime());
VariantBaseList variants = proxyMap[observerName]->getDataFields(it->second);
-// ARMARX_IMPORTANT << "data from observer: " << observerName;
- for(unsigned int i = 0; i < variants.size(); ++i)
+
+ // ARMARX_IMPORTANT << "data from observer: " << observerName;
+ for (unsigned int i = 0; i < variants.size(); ++i)
{
-// ARMARX_IMPORTANT << "Variant: " << VariantPtr::dynamicCast(variants[i]);
+ // ARMARX_IMPORTANT << "Variant: " << VariantPtr::dynamicCast(variants[i]);
VariantPtr var = VariantPtr::dynamicCast(variants[i]);
- if(VariantType::IsBasicType(var->getType()))
+
+ if (VariantType::IsBasicType(var->getType()))
{
dataMaptoAppend[DataFieldIdentifierPtr::dynamicCast(it->second[i])->getIdentifierStr()].push_back(TimeData(time, var));
newData[DataFieldIdentifierPtr::dynamicCast(it->second[i])->getIdentifierStr()] = var;
@@ -509,13 +588,14 @@ namespace armarx
else
{
auto id = DataFieldIdentifierPtr::dynamicCast(it->second[i])->getIdentifierStr();
-// ARMARX_IMPORTANT << id;
+ // ARMARX_IMPORTANT << id;
auto dict = JSONObject::ConvertToBasicVariantMap(json, var);
- for(const auto &e : dict)
+
+ for (const auto& e : dict)
{
std::string key = id + "." + e.first;
-// ARMARX_INFO << key << ": " << *VariantPtr::dynamicCast(e.second);
- dataMaptoAppend[key].push_back(TimeData(time,VariantPtr::dynamicCast(e.second)));
+ // ARMARX_INFO << key << ": " << *VariantPtr::dynamicCast(e.second);
+ dataMaptoAppend[key].push_back(TimeData(time, VariantPtr::dynamicCast(e.second)));
newData[key] = VariantPtr::dynamicCast(e.second);
}
}
@@ -524,16 +604,19 @@ namespace armarx
}
}
- catch(Ice::NotRegisteredException &e){
+ catch (Ice::NotRegisteredException& e)
+ {
ARMARX_WARNING << deactivateSpam(3) << "Caught Ice::NotRegisteredException: " << e.what();
}
- catch(exceptions::local::InvalidDataFieldException &e){
+ catch (exceptions::local::InvalidDataFieldException& e)
+ {
ARMARX_WARNING << deactivateSpam(5) << "Caught InvalidDataFieldException: " << e.what();
}
- catch(exceptions::local::InvalidChannelException &e){
+ catch (exceptions::local::InvalidChannelException& e)
+ {
ARMARX_WARNING << deactivateSpam(5) << "Caught InvalidChannelException: " << e.what();
}
- catch(...)
+ catch (...)
{
handleExceptions();
}
@@ -541,16 +624,18 @@ namespace armarx
return newData;
}
- void ArmarXPlotter::logToFile(const IceUtil::Time &time, const std::map<std::string, VariantPtr> &dataMaptoAppend)
+ void ArmarXPlotter::logToFile(const IceUtil::Time& time, const std::map<std::string, VariantPtr>& dataMaptoAppend)
{
- if(logstream.is_open() && dataMaptoAppend.size() > 0)
+ if (logstream.is_open() && dataMaptoAppend.size() > 0)
{
- logstream << (time-logStartTime).toMilliSecondsDouble();
- for(const auto& elem : dataMaptoAppend)
+ logstream << (time - logStartTime).toMilliSecondsDouble();
+
+ for (const auto& elem : dataMaptoAppend)
{
logstream << "," /*<< elem.first << ","*/ << elem.second->getOutputValueOnly();
}
+
logstream << std::endl;
}
}
diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.h b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.h
index 6481b595372fc4c36bc34ede6aec7d5e67474fa2..3a816fa47a186af11a18b4235966c204ba412671 100644
--- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.h
+++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.h
@@ -51,8 +51,9 @@ class QwtPlotCurve;
namespace armarx
{
- struct TimeData{
- TimeData(const IceUtil::Time &time, const VariantPtr &data): time(time), data(data){}
+ struct TimeData
+ {
+ TimeData(const IceUtil::Time& time, const VariantPtr& data): time(time), data(data) {}
IceUtil::Time time;
VariantPtr data;
@@ -70,17 +71,17 @@ namespace armarx
* \ingroup RobotAPI-ArmarXGuiPlugins ArmarXGuiPlugins
*/
class ARMARXCOMPONENT_IMPORT_EXPORT
- ArmarXPlotter:
- public ArmarXComponentWidgetController
+ ArmarXPlotter:
+ public ArmarXComponentWidgetController
{
Q_OBJECT
public:
- std::map<std::string, QwtPlotCurve *> curves;
+ std::map<std::string, QwtPlotCurve*> curves;
Ui::ArmarXPlotter ui;
QPointer<ArmarXPlotterDialog> dialog;
- QTimer * timer;
+ QTimer* timer;
ConditionHandlerInterfacePrx handler;
// Configuration Parameters
@@ -92,9 +93,12 @@ namespace armarx
explicit ArmarXPlotter();
~ArmarXPlotter();
//inherited from ArmarXWidget
- virtual QString getWidgetName() const { return "Observers.Plotter";}
- void loadSettings(QSettings * settings);
- void saveSettings(QSettings * settings);
+ virtual QString getWidgetName() const
+ {
+ return "Observers.Plotter";
+ }
+ void loadSettings(QSettings* settings);
+ void saveSettings(QSettings* settings);
//for AbstractFactoryMethod class
@@ -108,15 +112,15 @@ namespace armarx
/**
* emits the closeRequest signal
*/
- void onCloseWidget(QCloseEvent *event);
- QwtPlotCurve * createCurve(const QString &label);
+ void onCloseWidget(QCloseEvent* event);
+ QwtPlotCurve* createCurve(const QString& label);
signals:
public slots:
void ButtonAddSensorChannelClicked();
void configDialog();
void updateGraph();
- void showCurve(QwtPlotItem *item, bool on);
+ void showCurve(QwtPlotItem* item, bool on);
void autoScale(bool toggled);
void plottingPaused(bool toggled);
void configDone();
@@ -136,8 +140,8 @@ namespace armarx
JSONObjectPtr json;
- std::map<std::string, VariantPtr> getData(const QStringList &channels, GraphDataMap &dataMaptoAppend);
- void logToFile(const IceUtil::Time &time,const std::map<std::string, VariantPtr> &dataMaptoAppend);
+ std::map<std::string, VariantPtr> getData(const QStringList& channels, GraphDataMap& dataMaptoAppend);
+ void logToFile(const IceUtil::Time& time, const std::map<std::string, VariantPtr>& dataMaptoAppend);
Mutex dataMutex;
PeriodicTask<ArmarXPlotter>::pointer_type pollingTask;
diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotterDialog.cpp b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotterDialog.cpp
index fa7ff7e71bb79a1f398dff5711927b7cd292e92f..30a7d58446425fd5044e3b6835a68f90cbe05380 100644
--- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotterDialog.cpp
+++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotterDialog.cpp
@@ -33,7 +33,7 @@ using namespace armarx;
using namespace std;
-ArmarXPlotterDialog::ArmarXPlotterDialog(QWidget *parent, IceManagerPtr iceManager) :
+ArmarXPlotterDialog::ArmarXPlotterDialog(QWidget* parent, IceManagerPtr iceManager) :
QDialog(parent),
uuid(IceUtil::generateUUID()),
iceManager(iceManager)
@@ -53,7 +53,7 @@ ArmarXPlotterDialog::ArmarXPlotterDialog(QWidget *parent, IceManagerPtr iceManag
ArmarXPlotterDialog::~ArmarXPlotterDialog()
{
-// ARMARX_INFO << "~ArmarXPlotterDialog" ;
+ // ARMARX_INFO << "~ArmarXPlotterDialog" ;
}
void ArmarXPlotterDialog::setIceManager(IceManagerPtr iceManager)
@@ -89,32 +89,32 @@ void ArmarXPlotterDialog::setIceManager(IceManagerPtr iceManager)
//}
-void ArmarXPlotterDialog::onCloseWidget(QCloseEvent *event)
+void ArmarXPlotterDialog::onCloseWidget(QCloseEvent* event)
{
}
void ArmarXPlotterDialog::updateObservers()
{
- if(!model)
+ if (!model)
{
model = new ObserverItemModel(iceManager);
ui.treeViewObservers->setModel(model);
}
- model->updateObservers();
+ model->updateObservers();
-// if(!iceManager)
-// return;
-// ObserverList observerList = iceManager->getIceGridSession()->getRegisteredObjectNames<ObserverInterfacePrx>("*Observer");
-// ObserverList::iterator iter = observerList.begin();
+ // if(!iceManager)
+ // return;
+ // ObserverList observerList = iceManager->getIceGridSession()->getRegisteredObjectNames<ObserverInterfacePrx>("*Observer");
+ // ObserverList::iterator iter = observerList.begin();
-// while(iter != observerList.end())
-// {
-// model->updateModel(*iter, iceManager->getProxy<ObserverInterfacePrx>(*iter)->getAvailableChannels(), StringConditionCheckMap());
-// iter++;
-// }
+ // while(iter != observerList.end())
+ // {
+ // model->updateModel(*iter, iceManager->getProxy<ObserverInterfacePrx>(*iter)->getAvailableChannels(), StringConditionCheckMap());
+ // iter++;
+ // }
}
@@ -124,20 +124,23 @@ void ArmarXPlotterDialog::updateObservers()
void ArmarXPlotterDialog::ButtonAddSelectedChannelClicked()
{
- QItemSelectionModel* selectionModel = ui.treeViewObservers->selectionModel();
- for(int i = 0; i < selectionModel->selectedIndexes().size(); ++i)
- {
- QModelIndex index = selectionModel->selectedIndexes().at(i);
- treeView_doubleClick(index);
- }
- ui.treeViewObservers->clearSelection();
+ QItemSelectionModel* selectionModel = ui.treeViewObservers->selectionModel();
+
+ for (int i = 0; i < selectionModel->selectedIndexes().size(); ++i)
+ {
+ QModelIndex index = selectionModel->selectedIndexes().at(i);
+ treeView_doubleClick(index);
+ }
+
+ ui.treeViewObservers->clearSelection();
}
void ArmarXPlotterDialog::ButtonRemoveChannelClicked()
{
QList<QListWidgetItem*> selectedItems = ui.listWidget->selectedItems();
- for(int i=0; i < selectedItems.size(); ++i)
+
+ for (int i = 0; i < selectedItems.size(); ++i)
{
delete selectedItems.at(i);
}
@@ -145,15 +148,15 @@ void ArmarXPlotterDialog::ButtonRemoveChannelClicked()
void ArmarXPlotterDialog::ButtonRemoveChannelClicked(QModelIndex index)
{
- delete ui.listWidget->item(index.row());
+ delete ui.listWidget->item(index.row());
}
QStringList ArmarXPlotterDialog::getSelectedChannels()
{
QStringList result;
QList <QListWidgetItem*> items = ui.listWidget->findItems(QString("*"), Qt::MatchWrap | Qt::MatchWildcard);
- foreach(QListWidgetItem *item, items)
- result.append(item->text());
+ foreach(QListWidgetItem * item, items)
+ result.append(item->text());
return result;
}
@@ -162,27 +165,31 @@ void ArmarXPlotterDialog::treeView_selected(const QItemSelection& selected, cons
{
}
-void ArmarXPlotterDialog::treeView_doubleClick(const QModelIndex &index)
+void ArmarXPlotterDialog::treeView_doubleClick(const QModelIndex& index)
{
- QStandardItem* item = model->itemFromIndex(index);
- QVariant id = item->data(OBSERVER_ITEM_ID);
- switch(item->data(OBSERVER_ITEM_TYPE).toInt())
- {
+ QStandardItem* item = model->itemFromIndex(index);
+ QVariant id = item->data(OBSERVER_ITEM_ID);
+
+ switch (item->data(OBSERVER_ITEM_TYPE).toInt())
+ {
case eDataFieldItem:
- if(ui.listWidget->findItems(id.toString(),Qt::MatchExactly).size() == 0)
+ if (ui.listWidget->findItems(id.toString(), Qt::MatchExactly).size() == 0)
+ {
ui.listWidget->addItem(id.toString());
+ }
+
break;
- }
+ }
}
-void ArmarXPlotterDialog::destroyed(QObject *)
+void ArmarXPlotterDialog::destroyed(QObject*)
{
setParent(0);
}
-void ArmarXPlotterDialog::showEvent(QShowEvent *)
+void ArmarXPlotterDialog::showEvent(QShowEvent*)
{
updateObservers();
}
@@ -191,8 +198,9 @@ void ArmarXPlotterDialog::showEvent(QShowEvent *)
void armarx::ArmarXPlotterDialog::on_btnSelectLoggingDir_clicked()
{
QString newLoggingDir = QFileDialog::getExistingDirectory(this, "Select a directory to which data should be logged",
- ui.editLoggingDirectory->text());
- if(!newLoggingDir.isEmpty())
+ ui.editLoggingDirectory->text());
+
+ if (!newLoggingDir.isEmpty())
{
ui.editLoggingDirectory->setText(newLoggingDir);
}
diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotterDialog.h b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotterDialog.h
index ad2f39a481709e74a06f54203f7629b35af0b282..b912d58f87489648f888fe40ee6e94a099536571 100644
--- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotterDialog.h
+++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotterDialog.h
@@ -47,24 +47,25 @@ namespace armarx
{
class ArmarXPlotter;
class ArmarXPlotterDialog:
- public QDialog
+ public QDialog
{
Q_OBJECT
QStringList availableChannelsList;
- ObserverItemModel *model;
+ ObserverItemModel* model;
std::string uuid;
IceManagerPtr iceManager;
int mdiId;
public:
- ArmarXPlotterDialog(QWidget *parent, IceManagerPtr iceManager);
+ ArmarXPlotterDialog(QWidget* parent, IceManagerPtr iceManager);
~ArmarXPlotterDialog();
ConditionHandlerInterfacePrx handler;
- struct ChannelWidgetsEntry{
+ struct ChannelWidgetsEntry
+ {
QComboBox* comboBox;
- QLabel *label;
- QPushButton *deleteButton;
+ QLabel* label;
+ QPushButton* deleteButton;
};
void setIceManager(IceManagerPtr iceManager);
@@ -74,24 +75,24 @@ namespace armarx
QStringList getSelectedChannels();
-// // inherited from Component
-// std::string getDefaultName() const;
-// virtual void onInitComponent();
-// virtual void onConnectComponent();
-// void onExitComponent();
+ // // inherited from Component
+ // std::string getDefaultName() const;
+ // virtual void onInitComponent();
+ // virtual void onConnectComponent();
+ // void onExitComponent();
/**
* emits the closeRequest signal
*/
- void onCloseWidget(QCloseEvent *event);
+ void onCloseWidget(QCloseEvent* event);
public slots:
void ButtonAddSelectedChannelClicked();
void ButtonRemoveChannelClicked();
void ButtonRemoveChannelClicked(QModelIndex index);
void updateObservers();
void treeView_selected(const QItemSelection& selected, const QItemSelection& deselected);
- void treeView_doubleClick(const QModelIndex &index);
- void destroyed(QObject *);
- void showEvent(QShowEvent *);
+ void treeView_doubleClick(const QModelIndex& index);
+ void destroyed(QObject*);
+ void showEvent(QShowEvent*);
private slots:
void on_btnSelectLoggingDir_clicked();
};
diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.cpp b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.cpp
index 81e90b88e7358e58dcb65ceeee0d51077b56607b..80b861c28d248f67d375eb5b4e3860f354d660f5 100644
--- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.cpp
+++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.cpp
@@ -46,8 +46,8 @@ TCPMover::TCPMover() :
{
-// handData[selectedTCP][0] = handData[selectedTCP][1] = handData[selectedTCP][2] = 0.0f;
-// handData[selectedTCP][3] = handData[selectedTCP][4] = handData[selectedTCP][5] = 0.0f;
+ // handData[selectedTCP][0] = handData[selectedTCP][1] = handData[selectedTCP][2] = 0.0f;
+ // handData[selectedTCP][3] = handData[selectedTCP][4] = handData[selectedTCP][5] = 0.0f;
ui.setupUi(getWidget());
ui.gridLayout->setEnabled(false);
// SIGNALS AND SLOTS CONNECTIONS
@@ -79,20 +79,23 @@ TCPMover::~TCPMover()
-void armarx::TCPMover::loadSettings(QSettings *settings)
+void armarx::TCPMover::loadSettings(QSettings* settings)
{
tcpMoverUnitName = settings->value("TCPControlUnitName", "TCPControlUnit").toString().toStdString();
}
-void armarx::TCPMover::saveSettings(QSettings *settings)
+void armarx::TCPMover::saveSettings(QSettings* settings)
{
settings->setValue("TCPControlUnitName", tcpMoverUnitName.c_str());
}
-QPointer<QDialog> TCPMover::getConfigDialog(QWidget *parent)
+QPointer<QDialog> TCPMover::getConfigDialog(QWidget* parent)
{
- if(!configDialog)
+ if (!configDialog)
+ {
configDialog = new TCPMoverConfigDialog(parent);
+ }
+
return qobject_cast<TCPMoverConfigDialog*>(configDialog);
}
@@ -106,19 +109,19 @@ void TCPMover::configured()
void armarx::TCPMover::onInitComponent()
{
-// kinematicUnitFile = getProperty<std::string>("RobotFileName", KINEMATIC_UNIT_FILE_DEFAULT).getValueOrDefault();
-// kinematicUnitName = getProperty<std::string>("RobotNodeSetName",KINEMATIC_UNIT_NAME_DEFAULT).getValueOrDefault();
+ // kinematicUnitFile = getProperty<std::string>("RobotFileName", KINEMATIC_UNIT_FILE_DEFAULT).getValueOrDefault();
+ // kinematicUnitName = getProperty<std::string>("RobotNodeSetName",KINEMATIC_UNIT_NAME_DEFAULT).getValueOrDefault();
-// // Load Robot
-// ARMARX_VERBOSE << ": Loading KinematicUnit " << kinematicUnitName << " from '" << kinematicUnitFile << "' ..." << flush;
-// robot = RobotIO::loadRobot(kinematicUnitFile);
-// if (!robot)
-// {
-// ARMARX_ERROR << "Could not find Robot XML file with name '" << kinematicUnitFile << "'" << flush;
-// }
+ // // Load Robot
+ // ARMARX_VERBOSE << ": Loading KinematicUnit " << kinematicUnitName << " from '" << kinematicUnitFile << "' ..." << flush;
+ // robot = RobotIO::loadRobot(kinematicUnitFile);
+ // if (!robot)
+ // {
+ // ARMARX_ERROR << "Could not find Robot XML file with name '" << kinematicUnitFile << "'" << flush;
+ // }
-// usingProxy(kinematicUnitName);
+ // usingProxy(kinematicUnitName);
usingProxy(tcpMoverUnitName);
usingProxy("RobotStateComponent");
@@ -133,53 +136,59 @@ void armarx::TCPMover::onConnectComponent()
robotPrx = robotStateComponentPrx->getSynchronizedRobot();
NameList nodeSets = robotPrx->getRobotNodeSets();
QStringList nodeSetsQStr;
- for(unsigned int i=0; i < nodeSets.size(); i++)
+
+ for (unsigned int i = 0; i < nodeSets.size(); i++)
{
- if(!robotPrx->getRobotNodeSet(nodeSets.at(i))->tcpName.empty())
+ if (!robotPrx->getRobotNodeSet(nodeSets.at(i))->tcpName.empty())
{
tcpData[nodeSets.at(i)].resize(6, 0.f);
nodeSetsQStr << QString::fromStdString(nodeSets.at(i));
}
}
+
QString selected = ui.cbselectedTCP->currentText();
ui.cbselectedTCP->clear();
ui.cbselectedTCP->addItems(nodeSetsQStr);
int index = ui.cbselectedTCP->findText(selected);
- if(index != -1)
+
+ if (index != -1)
+ {
ui.cbselectedTCP->setCurrentIndex(index);
+ }
+
refFrame = robotStateComponentPrx->getSynchronizedRobot()->getRootNode()->getName();
-// kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
-
-// tcpNodeSetName = "LARM";
-// tcpNodeSet = robot->getRobotNodeSet(tcpNodeSetName);
-// NameControlModeMap modeMap;
-// for(unsigned int i = 0; i < tcpNodeSet->getSize(); i++)
-// {
-// modeMap[tcpNodeSet->getNode(i)->getName()] = eVelocityControl;
-// }
-// kinematicUnitPrx->switchControlMode(modeMap);
-// ik = DifferentialIKPtr(new DifferentialIK(tcpNodeSet));
+ // kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
+
+ // tcpNodeSetName = "LARM";
+ // tcpNodeSet = robot->getRobotNodeSet(tcpNodeSetName);
+ // NameControlModeMap modeMap;
+ // for(unsigned int i = 0; i < tcpNodeSet->getSize(); i++)
+ // {
+ // modeMap[tcpNodeSet->getNode(i)->getName()] = eVelocityControl;
+ // }
+ // kinematicUnitPrx->switchControlMode(modeMap);
+ // ik = DifferentialIKPtr(new DifferentialIK(tcpNodeSet));
ui.gridLayout->setEnabled(false);
}
void TCPMover::onExitComponent()
{
-// if(robotRequested)
-// kinematicUnitPrx->release();
+ // if(robotRequested)
+ // kinematicUnitPrx->release();
}
void TCPMover::moveUp()
{
-// moveRelative(-1,0,0);
+ // moveRelative(-1,0,0);
tcpData[selectedTCP][0]++;
execMove();
}
void TCPMover::moveDown()
{
-// moveRelative(1,0,0);
+ // moveRelative(1,0,0);
tcpData[selectedTCP][0]--;
execMove();
}
@@ -198,7 +207,7 @@ void TCPMover::moveZDown()
void TCPMover::moveLeft()
{
-// moveRelative(0,1,0);
+ // moveRelative(0,1,0);
tcpData[selectedTCP][1] ++;
execMove();
@@ -257,35 +266,35 @@ void TCPMover::stopMoving()
void TCPMover::reset()
{
- tcpData[selectedTCP][0] = tcpData[selectedTCP][1] = tcpData[selectedTCP][2] = 0.0f;
- tcpData[selectedTCP].at(3) = tcpData[selectedTCP].at(4) = tcpData[selectedTCP].at(5) = 0.0f;
- execMove();
- tcpMoverUnitPrx->release();
-// tcpMoverUnitPrx->resetArmToHomePosition(eLeftHand);
+ tcpData[selectedTCP][0] = tcpData[selectedTCP][1] = tcpData[selectedTCP][2] = 0.0f;
+ tcpData[selectedTCP].at(3) = tcpData[selectedTCP].at(4) = tcpData[selectedTCP].at(5) = 0.0f;
+ execMove();
+ tcpMoverUnitPrx->release();
+ // tcpMoverUnitPrx->resetArmToHomePosition(eLeftHand);
}
void TCPMover::robotControl(bool request)
{
tcpMoverUnitPrx->request();
-// try{
-// if(request)
-// kinematicUnitPrx->request();
-// else
-// kinematicUnitPrx->release();
-// }catch(ResourceUnavailableException &e){
-// ui.BtnRequestControl->setChecked(false);
-
-// }catch(ResourceNotOwnedException &e){
-// ui.BtnRequestControl->setChecked(true);
-// }
-// robotRequested = ui.BtnRequestControl->isChecked();
+ // try{
+ // if(request)
+ // kinematicUnitPrx->request();
+ // else
+ // kinematicUnitPrx->release();
+ // }catch(ResourceUnavailableException &e){
+ // ui.BtnRequestControl->setChecked(false);
+
+ // }catch(ResourceNotOwnedException &e){
+ // ui.BtnRequestControl->setChecked(true);
+ // }
+ // robotRequested = ui.BtnRequestControl->isChecked();
}
void TCPMover::selectHand(int index)
{
- selectedTCP = ui.cbselectedTCP->itemText(index).toStdString();
+ selectedTCP = ui.cbselectedTCP->itemText(index).toStdString();
}
void TCPMover::execMove()
@@ -304,55 +313,61 @@ void TCPMover::execMove()
const auto agentName = robotPrx->getName();
FramedDirectionPtr tcpVel = new FramedDirection(vec, refFrame, agentName);
Eigen::Vector3f vecOri;
- vecOri << tcpData[selectedTCP].at(3)/180.f*3.145f, tcpData[selectedTCP].at(4)/180.f*3.145f, tcpData[selectedTCP].at(5)/180.f*3.145f;
+ vecOri << tcpData[selectedTCP].at(3) / 180.f * 3.145f, tcpData[selectedTCP].at(4) / 180.f * 3.145f, tcpData[selectedTCP].at(5) / 180.f * 3.145f;
vecOri *= ui.sbFactor->value();
FramedDirectionPtr tcpOri = new FramedDirection(vecOri, refFrame, agentName);
- if(!ui.cbTranslation->isChecked() )
+ if (!ui.cbTranslation->isChecked())
+ {
tcpVel = NULL;
- if(!ui.cbOrientation->isChecked() )
+ }
+
+ if (!ui.cbOrientation->isChecked())
+ {
tcpOri = NULL;
- tcpMoverUnitPrx->begin_setTCPVelocity(selectedTCP,ui.edtTCPName->text().toStdString(), tcpVel,tcpOri);
+ }
+
+ tcpMoverUnitPrx->begin_setTCPVelocity(selectedTCP, ui.edtTCPName->text().toStdString(), tcpVel, tcpOri);
-// tcpMoverUnitPrx->setCartesianTCPVelocity(selectedTCP, handData[selectedTCP][0], handData[selectedTCP][1], handData[selectedTCP][2], ui.sbFactor->value());
-// tcpMoverUnitPrx->setTCPOrientation(selectedTCP, handData[selectedTCP].at(3)/180.f*3.145f, handData[selectedTCP].at(4)/180.f*3.145f, handData[selectedTCP].at(5)/180.f*3.145f);
+ // tcpMoverUnitPrx->setCartesianTCPVelocity(selectedTCP, handData[selectedTCP][0], handData[selectedTCP][1], handData[selectedTCP][2], ui.sbFactor->value());
+ // tcpMoverUnitPrx->setTCPOrientation(selectedTCP, handData[selectedTCP].at(3)/180.f*3.145f, handData[selectedTCP].at(4)/180.f*3.145f, handData[selectedTCP].at(5)/180.f*3.145f);
}
void TCPMover::moveRelative(float x, float y, float z)
{
-// RobotNodePtr tcpNode = tcpNodeSet->getTCP();
-// // calculate cartesian error
-// Eigen::Matrix4f newPosRelative;
-// newPosRelative
-// << 1, 0, 0, 10*ui.sbFactor->value(),
-// 0, 1, 0, 0,
-// 0, 0, 1, 0,
-// 0, 0, 0, 1;
-// Eigen::VectorXf errorCartVec(3);
-// errorCartVec << x*ui.sbFactor->value(),y*ui.sbFactor->value(),z*ui.sbFactor->value();
-//// errorCartVec.segment(0,3) = newPosRelative.block(0,3,3,1);
-
-
-
-
-// Eigen::MatrixXf Ji = ik->getPseudoInverseJacobianMatrix(tcpNode, IKSolver::Position);
-// // calculat joint error
-// Eigen::VectorXf errorJoint(tcpNodeSet->getSize());
-// errorJoint = Ji * errorCartVec;
-// std::vector<float> angles = tcpNodeSet->getJointValues();
-// std::vector<float> targetAngles(tcpNodeSet->getSize());
-// NameValueMap targetAnglesMap;
-
-// for(unsigned int i = 0; i < tcpNodeSet->getSize(); i++)
-// {
-//// float newAngle = angles[i] + errorJoint(i);
-// float newAngle = errorJoint(i);
-// targetAngles[i] = newAngle;
-// targetAnglesMap[tcpNodeSet->getNode(i)->getName()] = newAngle;
-// //ARMARX_VERBOSE << tcpNodeSet->getNode(i)->getName() << ": " << newAngle;
-// }
-// tcpNodeSet->setJointValues(targetAngles);
-// kinematicUnitPrx->setJointVelocities(targetAnglesMap);
+ // RobotNodePtr tcpNode = tcpNodeSet->getTCP();
+ // // calculate cartesian error
+ // Eigen::Matrix4f newPosRelative;
+ // newPosRelative
+ // << 1, 0, 0, 10*ui.sbFactor->value(),
+ // 0, 1, 0, 0,
+ // 0, 0, 1, 0,
+ // 0, 0, 0, 1;
+ // Eigen::VectorXf errorCartVec(3);
+ // errorCartVec << x*ui.sbFactor->value(),y*ui.sbFactor->value(),z*ui.sbFactor->value();
+ //// errorCartVec.segment(0,3) = newPosRelative.block(0,3,3,1);
+
+
+
+
+ // Eigen::MatrixXf Ji = ik->getPseudoInverseJacobianMatrix(tcpNode, IKSolver::Position);
+ // // calculat joint error
+ // Eigen::VectorXf errorJoint(tcpNodeSet->getSize());
+ // errorJoint = Ji * errorCartVec;
+ // std::vector<float> angles = tcpNodeSet->getJointValues();
+ // std::vector<float> targetAngles(tcpNodeSet->getSize());
+ // NameValueMap targetAnglesMap;
+
+ // for(unsigned int i = 0; i < tcpNodeSet->getSize(); i++)
+ // {
+ //// float newAngle = angles[i] + errorJoint(i);
+ // float newAngle = errorJoint(i);
+ // targetAngles[i] = newAngle;
+ // targetAnglesMap[tcpNodeSet->getNode(i)->getName()] = newAngle;
+ // //ARMARX_VERBOSE << tcpNodeSet->getNode(i)->getName() << ": " << newAngle;
+ // }
+ // tcpNodeSet->setJointValues(targetAngles);
+ // kinematicUnitPrx->setJointVelocities(targetAnglesMap);
}
diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.h b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.h
index 0de853b9f60c63ae2b35a3b0af26955276d03e51..7acdeea5db1393b4f6b7add72aa4a49e4faf7ddc 100644
--- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.h
+++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.h
@@ -48,7 +48,8 @@
class QDialogButtonBox;
-namespace armarx {
+namespace armarx
+{
/*class TCPMoverConfigDialog : public QDialog
{
@@ -72,11 +73,11 @@ namespace armarx {
/**
* \page RobotAPI-GuiPlugins-TCPMover TCPMoverPlugin
* @brief The TCPMover widget allows direct control of a TCP.
- *
+ *
* \image html TCPMoverGUI.png
*/
class TCPMover :
- public ArmarXComponentWidgetController
+ public ArmarXComponentWidgetController
{
Q_OBJECT
@@ -84,12 +85,21 @@ namespace armarx {
TCPMover();
~TCPMover();
//inherited from ArmarXMdiWidget
- void loadSettings(QSettings * settings);
- void saveSettings(QSettings * settings);
- virtual QString getWidgetName() const { return "RobotControl.TCPMover"; }
- virtual QIcon getWidgetIcon() const { return QIcon("://icons/games.ico"); }
- virtual QIcon getWidgetCategoryIcon() const { return QIcon("://icons/games.ico"); }
- QPointer<QDialog> getConfigDialog(QWidget *parent = 0);
+ void loadSettings(QSettings* settings);
+ void saveSettings(QSettings* settings);
+ virtual QString getWidgetName() const
+ {
+ return "RobotControl.TCPMover";
+ }
+ virtual QIcon getWidgetIcon() const
+ {
+ return QIcon("://icons/games.ico");
+ }
+ virtual QIcon getWidgetCategoryIcon() const
+ {
+ return QIcon("://icons/games.ico");
+ }
+ QPointer<QDialog> getConfigDialog(QWidget* parent = 0);
void configured();
// inherited from Component
@@ -124,8 +134,8 @@ namespace armarx {
std::map<std::string, std::vector<float> > tcpData;
std::string selectedTCP;
std::string refFrame;
-// float velocities[2][3];
-// float orientation[2][3];
+ // float velocities[2][3];
+ // float orientation[2][3];
NameValueMap velocityMap;
bool robotRequested;
VirtualRobot::RobotNodeSetPtr tcpNodeSet;
diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMoverConfigDialog.cpp b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMoverConfigDialog.cpp
index 99871491cc1b21191c917bc8847225a1bdc8f573..4ec8ad2b5b6d31bf2091cb013c5d1eadded6e607 100644
--- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMoverConfigDialog.cpp
+++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMoverConfigDialog.cpp
@@ -27,7 +27,7 @@
#include <RobotAPI/interface/units/TCPControlUnit.h>
-armarx::TCPMoverConfigDialog::TCPMoverConfigDialog(QWidget *parent) :
+armarx::TCPMoverConfigDialog::TCPMoverConfigDialog(QWidget* parent) :
QDialog(parent),
ui(new Ui::TCPMoverConfigDialog),
uuid(IceUtil::generateUUID())
diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMoverConfigDialog.h b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMoverConfigDialog.h
index 72546c54159272021579579cb35eee6b9f3ffe53..c468ac34b4f979311ec5c6cb11d37dc3e70f3b78 100644
--- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMoverConfigDialog.h
+++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMoverConfigDialog.h
@@ -29,7 +29,8 @@
#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/IceProxyFinder.h>
-namespace Ui {
+namespace Ui
+{
class TCPMoverConfigDialog;
}
@@ -42,17 +43,20 @@ namespace armarx
Q_OBJECT
public:
- explicit TCPMoverConfigDialog(QWidget *parent = 0);
+ explicit TCPMoverConfigDialog(QWidget* parent = 0);
~TCPMoverConfigDialog();
protected:
// ManagedIceObject interface
- std::string getDefaultName() const { return "TCPMoverConfigDialog" + uuid;}
+ std::string getDefaultName() const
+ {
+ return "TCPMoverConfigDialog" + uuid;
+ }
void onInitComponent();
void onConnectComponent();
private:
- Ui::TCPMoverConfigDialog *ui;
+ Ui::TCPMoverConfigDialog* ui;
IceProxyFinderBase* proxyFinder;
std::string uuid;
diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/SensorActorWidgetsPlugin.cpp b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/SensorActorWidgetsPlugin.cpp
index d40db01839d8afd42c3f292df35e643f09695378..1845914fcfe5ede4e11b7794aafa219754d02c72 100644
--- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/SensorActorWidgetsPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/SensorActorWidgetsPlugin.cpp
@@ -28,7 +28,8 @@
namespace armarx
{
- SensorActorPlugin::SensorActorPlugin(){
+ SensorActorPlugin::SensorActorPlugin()
+ {
addWidget<TCPMover>();
addWidget<ArmarXPlotter>();
}
diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/SensorActorWidgetsPlugin.h b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/SensorActorWidgetsPlugin.h
index 40301dc6a249f7dc8d9ef34390a2f9f8069a2876..08ed7b53946b5444e3f5c1ceaac8f940e9b88aa3 100644
--- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/SensorActorWidgetsPlugin.h
+++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/SensorActorWidgetsPlugin.h
@@ -37,7 +37,7 @@ namespace armarx
* @see TCPMover
*/
class ARMARXCOMPONENT_IMPORT_EXPORT SensorActorPlugin :
- public ArmarXGuiPlugin
+ public ArmarXGuiPlugin
{
public:
SensorActorPlugin();
diff --git a/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/Backup/RobotViewerConfigDialog.cpp b/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/Backup/RobotViewerConfigDialog.cpp
index decc984af837c3322f9cac272e23538953bfbd22..1b467821f4b5566dea6acd6bc0bc4b483aaf5105 100644
--- a/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/Backup/RobotViewerConfigDialog.cpp
+++ b/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/Backup/RobotViewerConfigDialog.cpp
@@ -36,7 +36,7 @@
using namespace armarx;
-RobotViewerConfigDialog::RobotViewerConfigDialog(QWidget *parent) :
+RobotViewerConfigDialog::RobotViewerConfigDialog(QWidget* parent) :
QDialog(parent),
ui(new Ui::RobotViewerConfigDialog),
uuid(IceUtil::generateUUID())
@@ -44,10 +44,10 @@ RobotViewerConfigDialog::RobotViewerConfigDialog(QWidget *parent) :
ui->setupUi(this);
connect(this->ui->buttonBox, SIGNAL(accepted()), this, SLOT(verifyConfig()));
- ui->buttonBox->button( QDialogButtonBox::Ok )->setDefault(true);
+ ui->buttonBox->button(QDialogButtonBox::Ok)->setDefault(true);
proxyFinder = new IceProxyFinder<RobotStateComponentInterfacePrx>(this);
proxyFinder->setSearchMask("RobotState*");
- ui->gridLayout->addWidget(proxyFinder, 2,1,1,2);
+ ui->gridLayout->addWidget(proxyFinder, 2, 1, 1, 2);
}
RobotViewerConfigDialog::~RobotViewerConfigDialog()
@@ -75,12 +75,14 @@ void RobotViewerConfigDialog::onExitComponent()
void RobotViewerConfigDialog::verifyConfig()
{
- if(proxyFinder->getSelectedProxyName().trimmed().length() == 0)
+ if (proxyFinder->getSelectedProxyName().trimmed().length() == 0)
{
QMessageBox::critical(this, "Invalid Configuration", "The proxy name must not be empty");
}
else
+ {
this->accept();
+ }
}
diff --git a/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/Backup/RobotViewerConfigDialog.h b/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/Backup/RobotViewerConfigDialog.h
index bbadff39da43ab1077bffeb14623f967cf913814..9c8237d106d6468d725b3304f59310e72ecc0aa1 100644
--- a/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/Backup/RobotViewerConfigDialog.h
+++ b/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/Backup/RobotViewerConfigDialog.h
@@ -31,11 +31,13 @@
#include <ArmarXCore/core/ManagedIceObject.h>
#include <Gui/ArmarXGuiLib/utility/IceProxyFinder.h>
-namespace Ui {
+namespace Ui
+{
class RobotViewerConfigDialog;
}
-namespace armarx{
+namespace armarx
+{
class RobotViewerConfigDialog :
public QDialog,
virtual public ManagedIceObject
@@ -43,11 +45,14 @@ namespace armarx{
Q_OBJECT
public:
- explicit RobotViewerConfigDialog(QWidget *parent = 0);
+ explicit RobotViewerConfigDialog(QWidget* parent = 0);
~RobotViewerConfigDialog();
// inherited from ManagedIceObject
- std::string getDefaultName() const { return "RobotViewerConfigDialog" + uuid;}
+ std::string getDefaultName() const
+ {
+ return "RobotViewerConfigDialog" + uuid;
+ }
void onInitComponent();
void onConnectComponent();
void onExitComponent();
@@ -60,7 +65,7 @@ namespace armarx{
private:
IceProxyFinderBase* proxyFinder;
- Ui::RobotViewerConfigDialog *ui;
+ Ui::RobotViewerConfigDialog* ui;
std::string uuid;
friend class RobotViewerWidgetController;
};
diff --git a/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/Backup/RobotViewerGuiPlugin.cpp b/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/Backup/RobotViewerGuiPlugin.cpp
index 2e1af6e06b7df15edb0636bf4e015be0d99d7711..77a3673d5536e4b0073aaf4b854b6fca86e213a4 100644
--- a/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/Backup/RobotViewerGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/Backup/RobotViewerGuiPlugin.cpp
@@ -83,12 +83,17 @@ void RobotViewerWidgetController::onInitComponent()
{
std::string debugDrawerComponentName = "RobotViewerGUIDebugDrawer_" + getName();
ARMARX_INFO << "Creating component " << debugDrawerComponentName;
- debugDrawer = Component::create<DebugDrawerComponent>(properties,debugDrawerComponentName);
+ debugDrawer = Component::create<DebugDrawerComponent>(properties, debugDrawerComponentName);
+
if (mutex3D)
{
debugDrawer->setMutex(mutex3D);
- } else
+ }
+ else
+ {
ARMARX_ERROR << " No 3d mutex available...";
+ }
+
ArmarXManagerPtr m = getArmarXManager();
m->addObject(debugDrawer);
@@ -101,6 +106,7 @@ void RobotViewerWidgetController::onInitComponent()
rootVisu->addChild(debugLayerVisu);
}
}
+
showRoot(true);
}
@@ -112,22 +118,30 @@ void RobotViewerWidgetController::onConnectComponent()
if (robotVisu)
+ {
robotVisu->removeAllChildren();
+ }
+
robot.reset();
std::string rfile;
StringList includePaths;
// get robot filename
- try {
+ try
+ {
StringList packages = robotStateComponentPrx->getArmarXPackages();
packages.push_back(Application::GetProjectName());
ARMARX_VERBOSE << "ArmarX packages " << packages;
- for(const std::string &projectName : packages)
+
+ for (const std::string& projectName : packages)
{
- if(projectName.empty())
+ if (projectName.empty())
+ {
continue;
+ }
+
CMakePackageFinder project(projectName);
StringList projectIncludePaths;
auto pathsString = project.getDataDir();
@@ -138,28 +152,36 @@ void RobotViewerWidgetController::onConnectComponent()
includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
}
+
rfile = robotStateComponentPrx->getRobotFilename();
ARMARX_VERBOSE << "Relative robot file " << rfile;
ArmarXDataPath::getAbsolutePath(rfile, rfile, includePaths);
ARMARX_VERBOSE << "Absolute robot file " << rfile;
- } catch (...)
+ }
+ catch (...)
{
ARMARX_ERROR << "Unable to retrieve robot filename";
}
- try {
+ try
+ {
ARMARX_INFO << "Loading robot from file " << rfile;
robot = loadRobotFile(rfile);
- } catch (...)
+ }
+ catch (...)
{
ARMARX_ERROR << "Failed to init robot";
}
- if(!robot)
+ if (!robot)
{
getObjectScheduler()->terminate();
- if(getWidget()->parentWidget())
+
+ if (getWidget()->parentWidget())
+ {
getWidget()->parentWidget()->close();
+ }
+
std::cout << "returning" << std::endl;
return;
}
@@ -168,10 +190,12 @@ void RobotViewerWidgetController::onConnectComponent()
boost::recursive_mutex::scoped_lock lock(*mutex3D);
CoinVisualizationPtr robotViewerVisualization = robot->getVisualization<CoinVisualization>();
+
if (robotViewerVisualization)
{
robotVisu->addChild(robotViewerVisualization->getCoinVisualization());
- } else
+ }
+ else
{
ARMARX_WARNING << "no robot visu available...";
}
@@ -191,12 +215,14 @@ void RobotViewerWidgetController::onDisconnectComponent()
{
ARMARX_INFO << "Disconnecting component";
+
// stop update timer
if (timerSensor)
{
SoSensorManager* sensor_mgr = SoDB::getSensorManager();
sensor_mgr->removeTimerSensor(timerSensor);
}
+
ARMARX_INFO << "Disconnecting component: timer stopped";
robotStateComponentPrx = NULL;
@@ -208,6 +234,7 @@ void RobotViewerWidgetController::onDisconnectComponent()
ARMARX_INFO << "Disconnecting component: removing visu";
robotVisu->removeAllChildren();
}
+
robot.reset();
}
ARMARX_INFO << "Disconnecting component: finished";
@@ -250,17 +277,18 @@ void RobotViewerWidgetController::onExitComponent()
debugDrawer->getObjectScheduler()->terminate();
ARMARX_INFO << "Removing DebugDrawer component...done";
}
-*/
+ */
}
QPointer<QDialog> RobotViewerWidgetController::getConfigDialog(QWidget* parent)
{
- if(!dialog)
+ if (!dialog)
{
dialog = new RobotViewerConfigDialog(parent);
dialog->setName(dialog->getDefaultName());
}
+
return qobject_cast<RobotViewerConfigDialog*>(dialog);
}
@@ -278,12 +306,12 @@ void RobotViewerWidgetController::configured()
}
-void RobotViewerWidgetController::loadSettings(QSettings *settings)
+void RobotViewerWidgetController::loadSettings(QSettings* settings)
{
robotStateComponentName = settings->value("RobotStateComponent", QString::fromStdString(ROBOTSTATE_NAME_DEFAULT)).toString().toStdString();
}
-void RobotViewerWidgetController::saveSettings(QSettings *settings)
+void RobotViewerWidgetController::saveSettings(QSettings* settings)
{
settings->setValue("RobotStateComponent", QString::fromStdString(robotStateComponentName));
}
@@ -293,7 +321,7 @@ void RobotViewerWidgetController::showVisuLayers(bool show)
{
if (debugDrawer)
{
- if(show)
+ if (show)
{
debugDrawer->enableAllLayers();
}
@@ -307,14 +335,17 @@ void RobotViewerWidgetController::showVisuLayers(bool show)
void RobotViewerWidgetController::showRoot(bool show)
{
if (!debugDrawer)
+ {
return;
+ }
std::string poseName("root");
- if(show)
+
+ if (show)
{
Eigen::Matrix4f gp = Eigen::Matrix4f::Identity();
PosePtr gpP(new Pose(gp));
- debugDrawer->setPoseDebugLayerVisu(poseName,gpP);
+ debugDrawer->setPoseDebugLayerVisu(poseName, gpP);
}
else
{
@@ -325,19 +356,22 @@ void RobotViewerWidgetController::showRoot(bool show)
void RobotViewerWidgetController::showRobot(bool show)
{
if (!robotVisu)
+ {
return;
+ }
+
boost::recursive_mutex::scoped_lock lock(*mutex3D);
- if(show && rootVisu->findChild(robotVisu)<0)
+ if (show && rootVisu->findChild(robotVisu) < 0)
{
rootVisu->addChild(robotVisu);
}
- else if (!show && rootVisu->findChild(robotVisu)>=0)
+ else if (!show && rootVisu->findChild(robotVisu) >= 0)
{
rootVisu->removeChild(robotVisu);
}
}
-SoNode *RobotViewerWidgetController::getScene()
+SoNode* RobotViewerWidgetController::getScene()
{
return rootVisu;
}
@@ -345,8 +379,12 @@ SoNode *RobotViewerWidgetController::getScene()
void RobotViewerWidgetController::timerCB(void* data, SoSensor* sensor)
{
RobotViewerWidgetController* controller = static_cast<RobotViewerWidgetController*>(data);
+
if (!controller)
+ {
return;
+ }
+
controller->updateRobotVisu();
}
@@ -365,11 +403,13 @@ VirtualRobot::RobotPtr RobotViewerWidgetController::loadRobotFile(std::string fi
{
VirtualRobot::RobotPtr robot;
- if (!ArmarXDataPath::getAbsolutePath(fileName,fileName))
+ if (!ArmarXDataPath::getAbsolutePath(fileName, fileName))
{
ARMARX_INFO << "Could not find Robot XML file with name " << fileName;
}
+
robot = RobotIO::loadRobot(fileName);
+
if (!robot)
{
ARMARX_INFO << "Could not find Robot XML file with name " << fileName;
@@ -385,12 +425,15 @@ void RobotViewerWidgetController::updateRobotVisu()
boost::recursive_mutex::scoped_lock lock(*mutex3D);
if (!robotStateComponentPrx || !robot)
+ {
return;
+ }
try
{
- RemoteRobot::synchronizeLocalClone(robot,robotStateComponentPrx);
- } catch (...)
+ RemoteRobot::synchronizeLocalClone(robot, robotStateComponentPrx);
+ }
+ catch (...)
{
ARMARX_INFO << deactivateSpam(5) << "Robot synchronization failed";
return;
@@ -398,14 +441,14 @@ void RobotViewerWidgetController::updateRobotVisu()
Eigen::Matrix4f gp = robot->getGlobalPose();
QString roboInfo("Robot Pose (global): pos: ");
- roboInfo += QString::number(gp(0,3), 'f', 2);
+ roboInfo += QString::number(gp(0, 3), 'f', 2);
roboInfo += QString(", ");
- roboInfo += QString::number(gp(1,3), 'f', 2);
+ roboInfo += QString::number(gp(1, 3), 'f', 2);
roboInfo += QString(", ");
- roboInfo += QString::number(gp(2,3), 'f', 2);
+ roboInfo += QString::number(gp(2, 3), 'f', 2);
roboInfo += QString(", rot:");
Eigen::Vector3f rpy;
- VirtualRobot::MathTools::eigen4f2rpy(gp,rpy);
+ VirtualRobot::MathTools::eigen4f2rpy(gp, rpy);
roboInfo += QString::number(rpy(0), 'f', 2);
roboInfo += QString(", ");
roboInfo += QString::number(rpy(1), 'f', 2);
@@ -420,7 +463,7 @@ void RobotViewerWidgetController::solveIK()
::Ice::AsyncResultPtr asyncResult = robotIKPrx->begin_computeIKGlobalPose("LeftArm", new Pose(), eAll);
- while(!asyncResult->isCompleted())
+ while (!asyncResult->isCompleted())
{
qApp->processEvents();
}
@@ -435,8 +478,11 @@ void RobotViewerWidgetController::setMutex3D(boost::shared_ptr<boost::recursive_
{
//ARMARX_IMPORTANT << "set mutex3d :" << mutex3D.get();
this->mutex3D = mutex3D;
+
if (debugDrawer)
+ {
debugDrawer->setMutex(mutex3D);
+ }
}
diff --git a/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/Backup/RobotViewerGuiPlugin.h b/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/Backup/RobotViewerGuiPlugin.h
index 395727f4863cdd12303847a29d134808f6edca88..dc38933e61f3cce7055117c04719a4a6d1ab7d00 100644
--- a/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/Backup/RobotViewerGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/Backup/RobotViewerGuiPlugin.h
@@ -63,7 +63,7 @@ namespace armarx
\see RobotViewerWidget
*/
class RobotViewerGuiPlugin :
- public ArmarXGuiPlugin
+ public ArmarXGuiPlugin
{
public:
RobotViewerGuiPlugin();
@@ -80,7 +80,7 @@ namespace armarx
Further, DebugDrawer methods are available.
\image html RobotViewerGUI.png
When you add the widget to the Gui, you need to specify the following parameters:
-
+
Parameter Name | Example Value | Required? | Description
:---------------- | :-------------: | :-------------- |:--------------------
Proxy | RobotStateComponent | Yes | The name of the robot state component.
@@ -89,7 +89,7 @@ namespace armarx
\see RobotViewerGuiPlugin
*/
class RobotViewerWidgetController :
- public ArmarXComponentWidgetController
+ public ArmarXComponentWidgetController
{
Q_OBJECT
public:
@@ -109,8 +109,8 @@ namespace armarx
return "RobotControl.RobotIK";
}
QPointer<QDialog> getConfigDialog(QWidget* parent = 0);
- virtual void loadSettings(QSettings * settings);
- virtual void saveSettings(QSettings * settings);
+ virtual void loadSettings(QSettings* settings);
+ virtual void saveSettings(QSettings* settings);
void configured();
SoNode* getScene();
@@ -156,7 +156,7 @@ namespace armarx
SoSeparator* debugLayerVisu;
armarx::DebugDrawerComponentPtr debugDrawer;
- static void timerCB(void *data, SoSensor *sensor);
+ static void timerCB(void* data, SoSensor* sensor);
protected slots:
void showVisuLayers(bool show);
void showRoot(bool show);
diff --git a/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotIKConfigDialog.cpp b/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotIKConfigDialog.cpp
index 410129096ac85e86eb15edb7ed3f493c05d812fd..54b438f34feec6310d704fce6d5179e34bd5cb76 100644
--- a/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotIKConfigDialog.cpp
+++ b/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotIKConfigDialog.cpp
@@ -35,7 +35,7 @@
//Ice includes
#include <IceUtil/UUID.h>
-armarx::RobotIKConfigDialog::RobotIKConfigDialog(QWidget *parent) :
+armarx::RobotIKConfigDialog::RobotIKConfigDialog(QWidget* parent) :
QDialog(parent),
ui(new Ui::RobotIKConfigDialog),
uuid(IceUtil::generateUUID())
@@ -46,20 +46,20 @@ armarx::RobotIKConfigDialog::RobotIKConfigDialog(QWidget *parent) :
connect(this->ui->buttonBox, SIGNAL(accepted()), this, SLOT(verifyConfiguration()));
//Set OK button as default for convenience
- ui->buttonBox->button( QDialogButtonBox::Ok )->setDefault(true);
+ ui->buttonBox->button(QDialogButtonBox::Ok)->setDefault(true);
//Initialize proxyFinders for every component
robotStateComponentProxyFinder = new IceProxyFinder<RobotStateComponentInterfacePrx>(this);
robotStateComponentProxyFinder->setSearchMask("RobotState*");
- ui->gridLayout->addWidget(robotStateComponentProxyFinder, 1,1);
+ ui->gridLayout->addWidget(robotStateComponentProxyFinder, 1, 1);
kinematicUnitComponentProxyFinder = new IceProxyFinder<KinematicUnitInterfacePrx>(this);
kinematicUnitComponentProxyFinder->setSearchMask("Armar3Kinematic*");
- ui->gridLayout->addWidget(kinematicUnitComponentProxyFinder, 2,1);
+ ui->gridLayout->addWidget(kinematicUnitComponentProxyFinder, 2, 1);
robotIKComponentProxyFinder = new IceProxyFinder<RobotIKInterfacePrx>(this);
robotIKComponentProxyFinder->setSearchMask("RobotIK*");
- ui->gridLayout->addWidget(robotIKComponentProxyFinder, 3,1);
+ ui->gridLayout->addWidget(robotIKComponentProxyFinder, 3, 1);
}
armarx::RobotIKConfigDialog::~RobotIKConfigDialog()
@@ -86,14 +86,14 @@ void armarx::RobotIKConfigDialog::onExitComponent()
void armarx::RobotIKConfigDialog::verifyConfiguration()
{
- if(robotStateComponentProxyFinder->getSelectedProxyName().trimmed().length() == 0
- || kinematicUnitComponentProxyFinder->getSelectedProxyName().trimmed().length() == 0
- || robotIKComponentProxyFinder->getSelectedProxyName().trimmed().length() == 0)
+ if (robotStateComponentProxyFinder->getSelectedProxyName().trimmed().length() == 0
+ || kinematicUnitComponentProxyFinder->getSelectedProxyName().trimmed().length() == 0
+ || robotIKComponentProxyFinder->getSelectedProxyName().trimmed().length() == 0)
{
QMessageBox::critical(this, "Invalid Configuration", "The proxy name must not be empty");
}
else
{
- this->accept();
+ this->accept();
}
}
diff --git a/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotIKConfigDialog.h b/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotIKConfigDialog.h
index 42f06335c5fb950c019c7a7e159c95f18b7c0010..3d34f2f6ceace807b0b9c7b85349113209c33765 100644
--- a/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotIKConfigDialog.h
+++ b/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotIKConfigDialog.h
@@ -29,22 +29,27 @@
//Qt includes
#include <QDialog>
-namespace Ui {
+namespace Ui
+{
class RobotIKConfigDialog;
}
-namespace armarx {
+namespace armarx
+{
class RobotIKConfigDialog : public QDialog, virtual public ManagedIceObject
{
Q_OBJECT
friend class RobotIKWidgetController;
public:
- explicit RobotIKConfigDialog(QWidget *parent = 0);
- ~RobotIKConfigDialog();
+ explicit RobotIKConfigDialog(QWidget* parent = 0);
+ ~RobotIKConfigDialog();
// inherited from ManagedIceObject
- std::string getDefaultName() const { return "RobotIKConfigDialog" + uuid;}
+ std::string getDefaultName() const
+ {
+ return "RobotIKConfigDialog" + uuid;
+ }
void onInitComponent();
void onConnectComponent();
void onExitComponent();
@@ -57,7 +62,7 @@ namespace armarx {
IceProxyFinderBase* kinematicUnitComponentProxyFinder;
IceProxyFinderBase* robotIKComponentProxyFinder;
- Ui::RobotIKConfigDialog *ui;
+ Ui::RobotIKConfigDialog* ui;
std::string uuid;
};
}
diff --git a/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotIKGuiPlugin.cpp b/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotIKGuiPlugin.cpp
index 2f2d733030572b5e991021c0d5df28295b6a6be0..3e2badc6d74ac6c0b84bb804241a5c7b3711ac93 100644
--- a/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotIKGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotIKGuiPlugin.cpp
@@ -94,13 +94,16 @@ void armarx::RobotIKWidgetController::onConnectComponent()
//Load robot
robot = loadRobot(getIncludePaths());
- if(!robot)
+
+ if (!robot)
{
getObjectScheduler()->terminate();
- if(getWidget()->parentWidget())
+
+ if (getWidget()->parentWidget())
{
getWidget()->parentWidget()->close();
}
+
ARMARX_ERROR << "RobotIKPlugin: Unable to load robot file! Terminating." << std::endl;
return;
}
@@ -122,7 +125,9 @@ void armarx::RobotIKWidgetController::onConnectComponent()
//Get all kinematic chain descriptions and add them to the combo box
auto robotNodeSets = robot->getRobotNodeSets();
- for(VirtualRobot::RobotNodeSetPtr s : robotNodeSets) {
+
+ for (VirtualRobot::RobotNodeSetPtr s : robotNodeSets)
+ {
this->ui.comboBox->addItem(QString::fromStdString(s->getName()));
}
@@ -198,21 +203,22 @@ void armarx::RobotIKWidgetController::onExitComponent()
enableMainWidgetAsync(false);
}
-QPointer<QDialog> armarx::RobotIKWidgetController::getConfigDialog(QWidget *parent)
+QPointer<QDialog> armarx::RobotIKWidgetController::getConfigDialog(QWidget* parent)
{
- if(!dialog)
+ if (!dialog)
{
dialog = new RobotIKConfigDialog(parent);
}
+
return qobject_cast<RobotIKConfigDialog*>(dialog);
}
-void armarx::RobotIKWidgetController::loadSettings(QSettings *settings)
+void armarx::RobotIKWidgetController::loadSettings(QSettings* settings)
{
}
-void armarx::RobotIKWidgetController::saveSettings(QSettings *settings)
+void armarx::RobotIKWidgetController::saveSettings(QSettings* settings)
{
}
@@ -228,22 +234,25 @@ void armarx::RobotIKWidgetController::solveIK()
{
auto targetJointAngles = getIKSolution();
- if(targetJointAngles.isReachable) {
+ if (targetJointAngles.isReachable)
+ {
//Switch all control modes to ePositionControl
std::vector<VirtualRobot::RobotNodePtr> rn = robot->getRobotNodeSet(this->ui.comboBox->currentText().toStdString())->getAllRobotNodes();
NameControlModeMap jointModes;
NameValueMap jointAngles;
- for (unsigned int i=0;i<rn.size();i++)
+
+ for (unsigned int i = 0; i < rn.size(); i++)
{
jointModes[rn[i]->getName()] = ePositionControl;
- jointAngles[rn[i]->getName()]=0.0f;
+ jointAngles[rn[i]->getName()] = 0.0f;
}
+
kinematicUnitInterfacePrx->switchControlMode(jointModes);
kinematicUnitInterfacePrx->setJointAngles(targetJointAngles.jointAngles);
}
}
-void armarx::RobotIKWidgetController::kinematicChainChanged(const QString &arg1)
+void armarx::RobotIKWidgetController::kinematicChainChanged(const QString& arg1)
{
//An item has been selected, so we can allow the user to use the ui now
//The manipulator will be set to the position of the current tcp, so this pose
@@ -262,10 +271,13 @@ void armarx::RobotIKWidgetController::kinematicChainChanged(const QString &arg1)
this->ui.resetManip->setEnabled(true);
//Remove all current tcp and desired tcp pose visualization if present
- if(currentSeparator) {
+ if (currentSeparator)
+ {
this->ui.robotViewer->getRobotViewer()->getRootNode()->removeChild(currentSeparator);
}
- if(manipSeparator) {
+
+ if (manipSeparator)
+ {
this->ui.robotViewer->getRobotViewer()->getRootNode()->removeChild(manipSeparator);
}
@@ -330,10 +342,11 @@ void armarx::RobotIKWidgetController::kinematicChainChanged(const QString &arg1)
tcpManip->rotation.setValue(SbRotation(VirtualRobot::CoinVisualizationFactory::getSbMatrix(mat)));*/
}
-void armarx::RobotIKWidgetController::caertesianSelectionChanged(const QString &arg1)
+void armarx::RobotIKWidgetController::caertesianSelectionChanged(const QString& arg1)
{
//If there is a manip in the scene we pretend it just moved to update color etc.
- if(tcpManip) {
+ if (tcpManip)
+ {
manipFinishCallback(this, NULL);
}
}
@@ -341,7 +354,7 @@ void armarx::RobotIKWidgetController::caertesianSelectionChanged(const QString &
void armarx::RobotIKWidgetController::resetManip()
{
//Triggers reset of manipulator in kinematicChainChanged
- kinematicChainChanged(this->ui.comboBox->currentText());
+ kinematicChainChanged(this->ui.comboBox->currentText());
}
void armarx::RobotIKWidgetController::connectSlots()
@@ -356,13 +369,18 @@ armarx::StringList armarx::RobotIKWidgetController::getIncludePaths()
{
StringList includePaths;
- try {
+ try
+ {
StringList packages = robotStateComponentPrx->getArmarXPackages();
packages.push_back(Application::GetProjectName());
- for(const std::string &projectName : packages)
+
+ for (const std::string& projectName : packages)
{
- if(projectName.empty())
+ if (projectName.empty())
+ {
continue;
+ }
+
CMakePackageFinder project(projectName);
StringList projectIncludePaths;
auto pathsString = project.getDataDir();
@@ -372,7 +390,8 @@ armarx::StringList armarx::RobotIKWidgetController::getIncludePaths()
boost::token_compress_on);
includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
}
- } catch (...)
+ }
+ catch (...)
{
ARMARX_ERROR << "Unable to retrieve robot filename." << std::endl;
}
@@ -388,25 +407,28 @@ VirtualRobot::RobotPtr armarx::RobotIKWidgetController::loadRobot(StringList inc
ArmarXDataPath::getAbsolutePath(rfile, rfile, includePaths);
return VirtualRobot::RobotIO::loadRobot(rfile);
}
- catch(...)
+ catch (...)
{
ARMARX_ERROR << "Unable to load robot from file" << std::endl;
return VirtualRobot::RobotPtr();
}
}
-void armarx::RobotIKWidgetController::manipFinishCallback(void* data, SoDragger* dragger) {
+void armarx::RobotIKWidgetController::manipFinishCallback(void* data, SoDragger* dragger)
+{
RobotIKWidgetController* controller = static_cast<RobotIKWidgetController*>(data);
auto targetJointAngles = controller->getIKSolution();
- if(targetJointAngles.isReachable) {
+ if (targetJointAngles.isReachable)
+ {
//Green
controller->tcpManipColor->ambientColor.setValue(0, 1, 0);
controller->ui.reachableLabel->setText(QString::fromStdString("Pose reachable!"));
controller->ui.reachableLabel->setStyleSheet("QLabel { color : green; }");
}
- else {
+ else
+ {
//Red
controller->tcpManipColor->ambientColor.setValue(1, 0, 0);
controller->ui.reachableLabel->setText(QString::fromStdString("Pose unreachable!"));
@@ -417,16 +439,21 @@ void armarx::RobotIKWidgetController::manipFinishCallback(void* data, SoDragger*
controller->ui.errorValue->setText(QString::fromStdString("Calculated error: " + boost::lexical_cast<std::string>(targetJointAngles.error)));
}
-void armarx::RobotIKWidgetController::robotUpdateTimerCB(void *data, SoSensor *sensor)
+void armarx::RobotIKWidgetController::robotUpdateTimerCB(void* data, SoSensor* sensor)
{
RobotIKWidgetController* controller = static_cast<RobotIKWidgetController*>(data);
+
if (!controller || !controller->robotStateComponentPrx || !controller->robot)
+ {
return;
+ }
+
try
{
- armarx::RemoteRobot::synchronizeLocalClone(controller->robot,controller->robotStateComponentPrx);
+ armarx::RemoteRobot::synchronizeLocalClone(controller->robot, controller->robotStateComponentPrx);
- if(controller->currentSeparator) {
+ if (controller->currentSeparator)
+ {
Eigen::Matrix4f tcpMatrix = controller->robot->getRobotNodeSet(controller->ui.comboBox->currentText().toStdString())->getTCP()->getGlobalPose();
//Apply tcp position to indicator
tcpMatrix(0, 3) /= 1000;
@@ -435,20 +462,26 @@ void armarx::RobotIKWidgetController::robotUpdateTimerCB(void *data, SoSensor *s
controller->tcpCurrentTransform->setMatrix(VirtualRobot::CoinVisualizationFactory::getSbMatrix(tcpMatrix));
}
- if(controller->startUpCameraPositioningFlag) {
+ if (controller->startUpCameraPositioningFlag)
+ {
controller->ui.robotViewer->getRobotViewer()->cameraViewAll();
controller->startUpCameraPositioningFlag = false;
}
- } catch (...){};
+ }
+ catch (...) {};
}
-void armarx::RobotIKWidgetController::textFieldUpdateTimerCB(void *data, SoSensor *sensor)
+void armarx::RobotIKWidgetController::textFieldUpdateTimerCB(void* data, SoSensor* sensor)
{
RobotIKWidgetController* controller = static_cast<RobotIKWidgetController*>(data);
+
if (!controller)
+ {
return;
+ }
- if(controller->currentSeparator) {
+ if (controller->currentSeparator)
+ {
Eigen::Matrix4f tcpMatrix = controller->robot->getRobotNodeSet(controller->ui.comboBox->currentText().toStdString())->getTCP()->getGlobalPose();
//Set text label to tcp matrix
@@ -462,8 +495,8 @@ void armarx::RobotIKWidgetController::textFieldUpdateTimerCB(void *data, SoSenso
SoGetMatrixAction* action = new SoGetMatrixAction(SbViewportRegion());
SoSearchAction sa;
sa.setNode(controller->tcpManipSphere);
- sa.setSearchingAll( TRUE ); // Search all nodes
- SoBaseKit::setSearchingChildren( TRUE ); // Even inside nodekits
+ sa.setSearchingAll(TRUE); // Search all nodes
+ SoBaseKit::setSearchingChildren(TRUE); // Even inside nodekits
sa.apply(controller->ui.robotViewer->getRobotViewer()->getRootNode());
action->apply(sa.getPath());
@@ -471,25 +504,25 @@ void armarx::RobotIKWidgetController::textFieldUpdateTimerCB(void *data, SoSenso
SbMatrix matrix = action->getMatrix();
Eigen::Matrix4f mat = Eigen::Matrix4f::Identity();
- mat (0,0) = matrix[0][0];
- mat (0,1) = matrix[1][0];
- mat (0,2) = matrix[2][0];
- mat (0,3) = matrix[3][0] * 1000;
-
- mat (1,0) = matrix[0][1];
- mat (1,1) = matrix[1][1];
- mat (1,2) = matrix[2][1];
- mat (1,3) = matrix[3][1] * 1000;
-
- mat (2,0) = matrix[0][2];
- mat (2,1) = matrix[1][2];
- mat (2,2) = matrix[2][2];
- mat (2,3) = matrix[3][2] * 1000;
-
- mat (3,0) = matrix[0][3];
- mat (3,1) = matrix[1][3];
- mat (3,2) = matrix[2][3];
- mat (3,3) = matrix[3][3];
+ mat(0, 0) = matrix[0][0];
+ mat(0, 1) = matrix[1][0];
+ mat(0, 2) = matrix[2][0];
+ mat(0, 3) = matrix[3][0] * 1000;
+
+ mat(1, 0) = matrix[0][1];
+ mat(1, 1) = matrix[1][1];
+ mat(1, 2) = matrix[2][1];
+ mat(1, 3) = matrix[3][1] * 1000;
+
+ mat(2, 0) = matrix[0][2];
+ mat(2, 1) = matrix[1][2];
+ mat(2, 2) = matrix[2][2];
+ mat(2, 3) = matrix[3][2] * 1000;
+
+ mat(3, 0) = matrix[0][3];
+ mat(3, 1) = matrix[1][3];
+ mat(3, 2) = matrix[2][3];
+ mat(3, 3) = matrix[3][3];
//Set text label to manip matrix
std::stringstream buffer2;
@@ -506,8 +539,8 @@ armarx::ExtendedIKResult armarx::RobotIKWidgetController::getIKSolution()
SoGetMatrixAction* action = new SoGetMatrixAction(SbViewportRegion());
SoSearchAction sa;
sa.setNode(tcpManipSphere);
- sa.setSearchingAll( TRUE ); // Search all nodes
- SoBaseKit::setSearchingChildren( TRUE ); // Even inside nodekits
+ sa.setSearchingAll(TRUE); // Search all nodes
+ SoBaseKit::setSearchingChildren(TRUE); // Even inside nodekits
sa.apply(this->ui.robotViewer->getRobotViewer()->getRootNode());
action->apply(sa.getPath());
@@ -515,51 +548,58 @@ armarx::ExtendedIKResult armarx::RobotIKWidgetController::getIKSolution()
SbMatrix matrix = action->getMatrix();
Eigen::Matrix4f mat = Eigen::Matrix4f::Identity();
- mat (0,0) = matrix[0][0];
- mat (0,1) = matrix[1][0];
- mat (0,2) = matrix[2][0];
- mat (0,3) = matrix[3][0] * 1000;
-
- mat (1,0) = matrix[0][1];
- mat (1,1) = matrix[1][1];
- mat (1,2) = matrix[2][1];
- mat (1,3) = matrix[3][1] * 1000;
-
- mat (2,0) = matrix[0][2];
- mat (2,1) = matrix[1][2];
- mat (2,2) = matrix[2][2];
- mat (2,3) = matrix[3][2] * 1000;
-
- mat (3,0) = matrix[0][3];
- mat (3,1) = matrix[1][3];
- mat (3,2) = matrix[2][3];
- mat (3,3) = matrix[3][3];
-
-// mat = robot->getRootNode()->toLocalCoordinateSystem(mat);
- return(robotIKPrx->computeExtendedIKGlobalPose(this->ui.comboBox->currentText().toStdString(),new armarx::Pose(mat),
- convertOption(this->ui.cartesianselection->currentText().toStdString())));
+ mat(0, 0) = matrix[0][0];
+ mat(0, 1) = matrix[1][0];
+ mat(0, 2) = matrix[2][0];
+ mat(0, 3) = matrix[3][0] * 1000;
+
+ mat(1, 0) = matrix[0][1];
+ mat(1, 1) = matrix[1][1];
+ mat(1, 2) = matrix[2][1];
+ mat(1, 3) = matrix[3][1] * 1000;
+
+ mat(2, 0) = matrix[0][2];
+ mat(2, 1) = matrix[1][2];
+ mat(2, 2) = matrix[2][2];
+ mat(2, 3) = matrix[3][2] * 1000;
+
+ mat(3, 0) = matrix[0][3];
+ mat(3, 1) = matrix[1][3];
+ mat(3, 2) = matrix[2][3];
+ mat(3, 3) = matrix[3][3];
+
+ // mat = robot->getRootNode()->toLocalCoordinateSystem(mat);
+ return (robotIKPrx->computeExtendedIKGlobalPose(this->ui.comboBox->currentText().toStdString(), new armarx::Pose(mat),
+ convertOption(this->ui.cartesianselection->currentText().toStdString())));
}
armarx::CartesianSelection armarx::RobotIKWidgetController::convertOption(std::string option)
{
- if(option == "Orientation and Position") {
+ if (option == "Orientation and Position")
+ {
return eAll;
}
- else if(option == "Position") {
+ else if (option == "Position")
+ {
return ePosition;
}
- else if(option == "Orientation") {
+ else if (option == "Orientation")
+ {
return eOrientation;
}
- else if(option == "X position") {
+ else if (option == "X position")
+ {
return eX;
}
- else if(option == "Y position") {
+ else if (option == "Y position")
+ {
return eY;
}
- else if(option == "Z position") {
+ else if (option == "Z position")
+ {
return eZ;
}
+
return eAll;
}
diff --git a/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotIKGuiPlugin.h b/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotIKGuiPlugin.h
index 42a42436fbad8940af04ab767cafa789c4ea20a7..3dd135614b0630ccbcca55d254fc9da84c8e3291 100644
--- a/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotIKGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotIKGuiPlugin.h
@@ -54,88 +54,88 @@ namespace armarx
class RobotIKGuiPlugin : public ArmarXGuiPlugin
{
- public:
- RobotIKGuiPlugin();
+ public:
+ RobotIKGuiPlugin();
- QString getPluginName()
- {
- return "RobotIKGuiPlugin";
- }
+ QString getPluginName()
+ {
+ return "RobotIKGuiPlugin";
+ }
};
class RobotIKWidgetController : public ArmarXComponentWidgetController
{
Q_OBJECT
- public:
- RobotIKWidgetController();
- virtual ~RobotIKWidgetController() {}
+ public:
+ RobotIKWidgetController();
+ virtual ~RobotIKWidgetController() {}
- // inherited from Component
- virtual void onInitComponent();
- virtual void onConnectComponent();
- virtual void onDisconnectComponent();
- virtual void onExitComponent();
+ // inherited from Component
+ virtual void onInitComponent();
+ virtual void onConnectComponent();
+ virtual void onDisconnectComponent();
+ virtual void onExitComponent();
- // inherited of ArmarXWidget
- virtual QString getWidgetName() const
- {
- return "RobotControl.RobotIK";
- }
- QPointer<QDialog> getConfigDialog(QWidget* parent = 0);
- virtual void loadSettings(QSettings * settings);
- virtual void saveSettings(QSettings * settings);
- void configured();
+ // inherited of ArmarXWidget
+ virtual QString getWidgetName() const
+ {
+ return "RobotControl.RobotIK";
+ }
+ QPointer<QDialog> getConfigDialog(QWidget* parent = 0);
+ virtual void loadSettings(QSettings* settings);
+ virtual void saveSettings(QSettings* settings);
+ void configured();
- public slots:
- void solveIK();
- void kinematicChainChanged(const QString &arg1);
- void caertesianSelectionChanged(const QString &arg1);
- void resetManip();
+ public slots:
+ void solveIK();
+ void kinematicChainChanged(const QString& arg1);
+ void caertesianSelectionChanged(const QString& arg1);
+ void resetManip();
- protected:
- RobotStateComponentInterfacePrx robotStateComponentPrx;
- KinematicUnitInterfacePrx kinematicUnitInterfacePrx;
- RobotIKInterfacePrx robotIKPrx;
+ protected:
+ RobotStateComponentInterfacePrx robotStateComponentPrx;
+ KinematicUnitInterfacePrx kinematicUnitInterfacePrx;
+ RobotIKInterfacePrx robotIKPrx;
- Ui::RobotIKGuiPlugin ui;
+ Ui::RobotIKGuiPlugin ui;
- private:
- void connectSlots();
- std::string robotStateComponentName;
- std::string kinematicUnitComponentName;
- std::string robotIKComponentName;
+ private:
+ void connectSlots();
+ std::string robotStateComponentName;
+ std::string kinematicUnitComponentName;
+ std::string robotIKComponentName;
- QPointer<RobotIKConfigDialog> dialog;
+ QPointer<RobotIKConfigDialog> dialog;
- VirtualRobot::RobotPtr robot;
- StringList getIncludePaths();
- VirtualRobot::RobotPtr loadRobot(StringList includePaths);
+ VirtualRobot::RobotPtr robot;
+ StringList getIncludePaths();
+ VirtualRobot::RobotPtr loadRobot(StringList includePaths);
- SoSeparator* manipSeparator;
- SoSeparator* currentSeparator;
+ SoSeparator* manipSeparator;
+ SoSeparator* currentSeparator;
- SoTransformerManip* tcpManip;
- static void manipFinishCallback(void *data, SoDragger* manip);
- SoTransform* tcpManipTransform;
- SoMaterial* tcpManipColor;
- SoSphere* tcpManipSphere;
+ SoTransformerManip* tcpManip;
+ static void manipFinishCallback(void* data, SoDragger* manip);
+ SoTransform* tcpManipTransform;
+ SoMaterial* tcpManipColor;
+ SoSphere* tcpManipSphere;
- SoTransform* tcpCurrentTransform;
- SoMaterial* tcpCurrentColor;
- SoSphere* tcpCurrentSphere;
+ SoTransform* tcpCurrentTransform;
+ SoMaterial* tcpCurrentColor;
+ SoSphere* tcpCurrentSphere;
- SoTimerSensor* robotUpdateSensor;
- static void robotUpdateTimerCB(void *data, SoSensor *sensor);
+ SoTimerSensor* robotUpdateSensor;
+ static void robotUpdateTimerCB(void* data, SoSensor* sensor);
- SoTimerSensor* textFieldUpdateSensor;
- static void textFieldUpdateTimerCB(void *data, SoSensor *sensor);
+ SoTimerSensor* textFieldUpdateSensor;
+ static void textFieldUpdateTimerCB(void* data, SoSensor* sensor);
- ExtendedIKResult getIKSolution();
+ ExtendedIKResult getIKSolution();
- CartesianSelection convertOption(std::string option);
+ CartesianSelection convertOption(std::string option);
- bool startUpCameraPositioningFlag;
+ bool startUpCameraPositioningFlag;
};
typedef boost::shared_ptr<RobotIKWidgetController> RobotIKGuiPluginPtr;
diff --git a/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotViewer.cpp b/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotViewer.cpp
index 80f5806649d575bd3d2d26abe16d97ae78acafc8..51827017e3fffe8b6bd2d4016893c395e42c3d54 100644
--- a/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotViewer.cpp
+++ b/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotViewer.cpp
@@ -17,11 +17,11 @@ armarx::RobotViewer::RobotViewer(QWidget* widget) : SoQtExaminerViewer(widget),
this->setHeadlight(true);
this->setViewing(false);
this->setDecoration(false);
- #ifdef WIN32
- #ifndef _DEBUG
+#ifdef WIN32
+#ifndef _DEBUG
this->setAntialiasing(true, 4);
- #endif
- #endif
+#endif
+#endif
this->setTransparencyType(SoGLRenderAction::SORTED_OBJECT_BLEND);
this->setFeedbackVisibility(true);
@@ -177,7 +177,7 @@ armarx::RobotViewer::~RobotViewer()
sceneRootNode->unref();
}
-SoSeparator *armarx::RobotViewer::getRootNode()
+SoSeparator* armarx::RobotViewer::getRootNode()
{
return this->contentRootNode;
}
diff --git a/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotViewerWidget.cpp b/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotViewerWidget.cpp
index 859263a87551708221b3fd479ed3d7df8525e157..dd925f98f4a0807895ab539909826f1806d43854 100644
--- a/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotViewerWidget.cpp
+++ b/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotViewerWidget.cpp
@@ -2,7 +2,7 @@
#include <QGridLayout>
-armarx::RobotViewerWidget::RobotViewerWidget(QWidget *parent) : QWidget(parent)
+armarx::RobotViewerWidget::RobotViewerWidget(QWidget* parent) : QWidget(parent)
{
this->setContentsMargins(1, 1, 1, 1);
diff --git a/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotViewerWidget.h b/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotViewerWidget.h
index 81379c8e7d8f633699a932edce961792a60d2840..ccb8d99796aff9e4fb0d060244361d55b7f59152 100644
--- a/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotViewerWidget.h
+++ b/source/RobotAPI/gui-plugins/moved_to_robotcomponents_RobotIKPlugin/RobotViewerWidget.h
@@ -17,28 +17,28 @@ namespace armarx
{
Q_OBJECT
- public:
- /**
- * Constructor.
- * Constructs a robot viewer widget.
- * Expects a controller::ControllerPtr.
- *
- * @param control shared pointer to controller::controller
- * @param parent parent widget
- *
- */
- explicit RobotViewerWidget(QWidget* parent = 0);
-
- /**
- * Destructor.
- *
- */
- ~RobotViewerWidget();
-
- RobotViewerPtr getRobotViewer();
-
- private:
- RobotViewerPtr viewer;
+ public:
+ /**
+ * Constructor.
+ * Constructs a robot viewer widget.
+ * Expects a controller::ControllerPtr.
+ *
+ * @param control shared pointer to controller::controller
+ * @param parent parent widget
+ *
+ */
+ explicit RobotViewerWidget(QWidget* parent = 0);
+
+ /**
+ * Destructor.
+ *
+ */
+ ~RobotViewerWidget();
+
+ RobotViewerPtr getRobotViewer();
+
+ private:
+ RobotViewerPtr viewer;
};
}
diff --git a/source/RobotAPI/libraries/core/FramedPose.cpp b/source/RobotAPI/libraries/core/FramedPose.cpp
index 1f972824fa2c71ce7f87b1e97357c2bf4cd00725..de30cd40ab4a64109dd778e54447236f3ebf1137 100644
--- a/source/RobotAPI/libraries/core/FramedPose.cpp
+++ b/source/RobotAPI/libraries/core/FramedPose.cpp
@@ -23,7 +23,7 @@ namespace armarx
{
}
- FramedDirection::FramedDirection(const FramedDirection &source) :
+ FramedDirection::FramedDirection(const FramedDirection& source) :
IceUtil::Shared(source),
Vector3Base(source),
FramedDirectionBase(source),
@@ -31,7 +31,7 @@ namespace armarx
{
}
- FramedDirection::FramedDirection(const Eigen::Vector3f &vec, const string &frame, const string &agent) :
+ FramedDirection::FramedDirection(const Eigen::Vector3f& vec, const string& frame, const string& agent) :
Vector3(vec)
{
this->frame = frame;
@@ -39,7 +39,7 @@ namespace armarx
}
- FramedDirection::FramedDirection(Ice::Float x, ::Ice::Float y, ::Ice::Float z, const std::string &frame) :
+ FramedDirection::FramedDirection(Ice::Float x, ::Ice::Float y, ::Ice::Float z, const std::string& frame) :
Vector3(x, y, z)
{
this->frame = frame;
@@ -50,18 +50,23 @@ namespace armarx
return frame;
}
- FramedDirectionPtr FramedDirection::ChangeFrame(const VirtualRobot::RobotPtr robot, const FramedDirection &framedVec, const string &newFrame)
+ FramedDirectionPtr FramedDirection::ChangeFrame(const VirtualRobot::RobotPtr robot, const FramedDirection& framedVec, const string& newFrame)
{
- if(framedVec.getFrame() == newFrame)
+ if (framedVec.getFrame() == newFrame)
+ {
return FramedDirectionPtr::dynamicCast(framedVec.clone());
- if(!robot->hasRobotNode(newFrame))
+ }
+
+ if (!robot->hasRobotNode(newFrame))
+ {
throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << robot->getName();
+ }
Eigen::Matrix4f rotToNewFrame = __GetRotationBetweenFrames(framedVec.frame, newFrame, robot);
Eigen::Vector3f vecOldFrame = framedVec.Vector3::toEigen();
- Eigen::Vector3f newVec = rotToNewFrame.block(0,0,3,3).inverse() * vecOldFrame;
+ Eigen::Vector3f newVec = rotToNewFrame.block(0, 0, 3, 3).inverse() * vecOldFrame;
FramedDirectionPtr result = new FramedDirection();
result->x = newVec(0);
@@ -71,22 +76,29 @@ namespace armarx
return result;
}
- void FramedDirection::changeFrame(const VirtualRobot::RobotPtr robot, const string &newFrame)
+ void FramedDirection::changeFrame(const VirtualRobot::RobotPtr robot, const string& newFrame)
{
- if(getFrame() == newFrame)
+ if (getFrame() == newFrame)
+ {
return;
- if(newFrame == GlobalFrame)
+ }
+
+ if (newFrame == GlobalFrame)
{
changeToGlobal(robot);
return;
}
- if(!robot->hasRobotNode(newFrame))
+
+ if (!robot->hasRobotNode(newFrame))
+ {
throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << robot->getName();
+ }
+
Eigen::Matrix4f rotToNewFrame = __GetRotationBetweenFrames(frame, newFrame, robot);
Eigen::Vector3f vecOldFrame = Vector3::toEigen();
- Eigen::Vector3f newVec = rotToNewFrame.block(0,0,3,3).inverse() * vecOldFrame;
+ Eigen::Vector3f newVec = rotToNewFrame.block(0, 0, 3, 3).inverse() * vecOldFrame;
x = newVec(0);
@@ -95,18 +107,21 @@ namespace armarx
frame = newFrame;
}
- void FramedDirection::changeToGlobal(const SharedRobotInterfacePrx &referenceRobot)
+ void FramedDirection::changeToGlobal(const SharedRobotInterfacePrx& referenceRobot)
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
changeToGlobal(sharedRobot);
}
- void FramedDirection::changeToGlobal(const VirtualRobot::RobotPtr &referenceRobot)
+ void FramedDirection::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
{
- if(frame == GlobalFrame)
+ if (frame == GlobalFrame)
+ {
return;
+ }
+
changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
- Eigen::Vector3f pos = referenceRobot->getRootNode()->getGlobalPose().block<3,3>(0,0) * toEigen();
+ Eigen::Vector3f pos = referenceRobot->getRootNode()->getGlobalPose().block<3, 3>(0, 0) * toEigen();
x = pos[0];
y = pos[1];
z = pos[2];
@@ -114,41 +129,47 @@ namespace armarx
agent = "";
}
- FramedDirectionPtr FramedDirection::toGlobal(const SharedRobotInterfacePrx &referenceRobot) const
+ FramedDirectionPtr FramedDirection::toGlobal(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toGlobal(sharedRobot);
}
- FramedDirectionPtr FramedDirection::toGlobal(const VirtualRobot::RobotPtr &referenceRobot) const
+ FramedDirectionPtr FramedDirection::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
{
FramedDirectionPtr newPos = FramedDirectionPtr::dynamicCast(this->clone());
newPos->changeToGlobal(referenceRobot);
return newPos;
}
- string FramedDirection::output(const Ice::Current &c) const
+ string FramedDirection::output(const Ice::Current& c) const
{
std::stringstream s;
- s << toEigen() << std::endl << "frame: " << getFrame() << (agent.empty()?"": (" agent: " + agent));
+ s << toEigen() << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
return s.str();
}
- Eigen::Matrix4f FramedDirection::__GetRotationBetweenFrames(const std::string &oldFrame, const std::string &newFrame, VirtualRobot::RobotPtr robotState)
+ Eigen::Matrix4f FramedDirection::__GetRotationBetweenFrames(const std::string& oldFrame, const std::string& newFrame, VirtualRobot::RobotPtr robotState)
{
Eigen::Matrix4f toNewFrame;
+
if (oldFrame.compare(GlobalFrame) == 0)
+ {
toNewFrame = robotState->getRobotNode(newFrame)->getGlobalPose();
+ }
else
+ {
toNewFrame = robotState->getRobotNode(oldFrame)->getTransformationTo(robotState->getRobotNode(newFrame));
- toNewFrame(0,3) = 0;
- toNewFrame(1,3) = 0;
- toNewFrame(2,3) = 0;
+ }
+
+ toNewFrame(0, 3) = 0;
+ toNewFrame(1, 3) = 0;
+ toNewFrame(2, 3) = 0;
return toNewFrame;
}
- int FramedDirection::readFromXML(const string &xmlData, const Ice::Current &c)
+ int FramedDirection::readFromXML(const string& xmlData, const Ice::Current& c)
{
int result = Vector3::readFromXML(xmlData);
@@ -159,17 +180,17 @@ namespace armarx
return result;
}
- string FramedDirection::writeAsXML(const Ice::Current &c)
+ string FramedDirection::writeAsXML(const Ice::Current& c)
{
using namespace boost::property_tree;
ptree pt = Variant::GetPropertyTree(Vector3::writeAsXML());
pt.add("frame", frame);
- #if BOOST_VERSION >= 105600
- boost::property_tree::xml_parser::xml_writer_settings<std::string> settings('\t', 1);
- #else
- boost::property_tree::xml_parser::xml_writer_settings<char> settings('\t', 1);
- #endif
+#if BOOST_VERSION >= 105600
+ boost::property_tree::xml_parser::xml_writer_settings<std::string> settings('\t', 1);
+#else
+ boost::property_tree::xml_parser::xml_writer_settings<char> settings('\t', 1);
+#endif
std::stringstream stream;
xml_parser::write_xml(stream, pt, settings);
@@ -177,7 +198,7 @@ namespace armarx
return stream.str();
}
- void FramedDirection::serialize(const ObjectSerializerBasePtr &serializer, const Ice::Current &) const
+ void FramedDirection::serialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&) const
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
Vector3::serialize(serializer);
@@ -186,13 +207,16 @@ namespace armarx
}
- void FramedDirection::deserialize(const ObjectSerializerBasePtr &serializer, const Ice::Current &)
+ void FramedDirection::deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
Vector3::deserialize(serializer);
frame = obj->getString("frame");
- if(obj->hasElement("agent"))
+
+ if (obj->hasElement("agent"))
+ {
agent = obj->getString("agent");
+ }
}
@@ -203,7 +227,7 @@ namespace armarx
frame = "";
}
- FramedPose::FramedPose(const FramedPose &pose) :
+ FramedPose::FramedPose(const FramedPose& pose) :
IceUtil::Shared(pose),
PoseBase(pose),
FramedPoseBase(pose),
@@ -212,21 +236,21 @@ namespace armarx
}
- FramedPose::FramedPose(const Eigen::Matrix3f& m, const Eigen::Vector3f& v, const std::string& s, const string &agent) :
+ FramedPose::FramedPose(const Eigen::Matrix3f& m, const Eigen::Vector3f& v, const std::string& s, const string& agent) :
Pose(m, v)
{
frame = s;
this->agent = agent;
}
- FramedPose::FramedPose(const Eigen::Matrix4f& m, const std::string& s, const string &agent) :
+ FramedPose::FramedPose(const Eigen::Matrix4f& m, const std::string& s, const string& agent) :
Pose(m)
{
frame = s;
this->agent = agent;
}
- FramedPose::FramedPose(const armarx::Vector3BasePtr pos, const armarx::QuaternionBasePtr ori, const std::string& frame, const string &agent) :
+ FramedPose::FramedPose(const armarx::Vector3BasePtr pos, const armarx::QuaternionBasePtr ori, const std::string& frame, const string& agent) :
Pose(pos, ori)
{
this->frame = frame;
@@ -238,34 +262,44 @@ namespace armarx
return frame;
}
- string FramedPose::output(const Ice::Current &c) const
+ string FramedPose::output(const Ice::Current& c) const
{
std::stringstream s;
- s << toEigen() << std::endl << "frame: " << getFrame() << (agent.empty()?"": (" agent: " + agent));
+ s << toEigen() << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
return s.str();
}
- void FramedPose::changeFrame(const SharedRobotInterfacePrx &referenceRobot, const string &newFrame)
+ void FramedPose::changeFrame(const SharedRobotInterfacePrx& referenceRobot, const string& newFrame)
{
- if(newFrame == frame)
+ if (newFrame == frame)
+ {
return;
+ }
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
changeFrame(sharedRobot, newFrame);
- }
+ }
- void FramedPose::changeFrame(const VirtualRobot::RobotPtr &referenceRobot, const string &newFrame)
+ void FramedPose::changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const string& newFrame)
{
- if(newFrame == frame)
+ if (newFrame == frame)
+ {
return;
- if(newFrame == GlobalFrame)
+ }
+
+ if (newFrame == GlobalFrame)
{
changeToGlobal(referenceRobot);
return;
}
+
Eigen::Matrix4f oldFrameTransformation = Eigen::Matrix4f::Identity();
- if(!referenceRobot->hasRobotNode(newFrame))
+
+ if (!referenceRobot->hasRobotNode(newFrame))
+ {
throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << referenceRobot->getName();
+ }
+
if (frame != GlobalFrame)
{
oldFrameTransformation = referenceRobot->getRobotNode(frame)->getPoseInRootFrame();
@@ -274,34 +308,38 @@ namespace armarx
{
oldFrameTransformation = referenceRobot->getRootNode()->getGlobalPose().inverse();
}
+
Eigen::Matrix4f newPose =
- (referenceRobot->getRobotNode(newFrame)->getPoseInRootFrame().inverse() * oldFrameTransformation) * toEigen();
+ (referenceRobot->getRobotNode(newFrame)->getPoseInRootFrame().inverse() * oldFrameTransformation) * toEigen();
orientation = new Quaternion(newPose);
- Eigen::Vector3f pos = newPose.block<3,1>(0,3);
+ Eigen::Vector3f pos = newPose.block<3, 1>(0, 3);
position = new Vector3(pos);
frame = newFrame;
init();
}
- void FramedPose::changeToGlobal(const SharedRobotInterfacePrx &referenceRobot)
+ void FramedPose::changeToGlobal(const SharedRobotInterfacePrx& referenceRobot)
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
changeToGlobal(sharedRobot);
}
- void FramedPose::changeToGlobal(const VirtualRobot::RobotPtr &referenceRobot)
+ void FramedPose::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
{
- if(frame == GlobalFrame)
+ if (frame == GlobalFrame)
+ {
return;
+ }
+
changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
Eigen::Matrix4f global = referenceRobot->getRootNode()->getGlobalPose();
const Eigen::Matrix4f pose = global * toEigen();
- position->x = pose(0,3);
- position->y = pose(1,3);
- position->z = pose(2,3);
- Eigen::Quaternionf q(pose.block<3,3>(0,0));
+ position->x = pose(0, 3);
+ position->y = pose(1, 3);
+ position->z = pose(2, 3);
+ Eigen::Quaternionf q(pose.block<3, 3>(0, 0));
orientation->qw = q.w();
orientation->qx = q.x();
orientation->qy = q.y();
@@ -310,20 +348,21 @@ namespace armarx
agent = "";
}
- FramedPosePtr FramedPose::toGlobal(const SharedRobotInterfacePrx &referenceRobot) const
+ FramedPosePtr FramedPose::toGlobal(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toGlobal(sharedRobot);
}
- FramedPosePtr FramedPose::toGlobal(const VirtualRobot::RobotPtr &referenceRobot) const
+ FramedPosePtr FramedPose::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
{
FramedPosePtr newPose = FramedPosePtr::dynamicCast(this->clone());
newPose->changeToGlobal(referenceRobot);
return newPose;
}
- int FramedPose::readFromXML(const string &xmlData, const Ice::Current &c){
+ int FramedPose::readFromXML(const string& xmlData, const Ice::Current& c)
+ {
boost::property_tree::ptree pt = Variant::GetPropertyTree(xmlData);
@@ -333,12 +372,12 @@ namespace armarx
return 1;
}
- string FramedPose::writeAsXML(const Ice::Current &)
+ string FramedPose::writeAsXML(const Ice::Current&)
{
using namespace boost::property_tree;
std::stringstream stream;
stream << Pose::writeAsXML()
- << "<frame>"<<frame<<"</frame>";
+ << "<frame>" << frame << "</frame>";
return stream.str();
}
@@ -369,8 +408,11 @@ namespace armarx
Pose::deserialize(obj);
frame = obj->getString("frame");
- if(obj->hasElement("agent"))
+
+ if (obj->hasElement("agent"))
+ {
agent = obj->getString("agent");
+ }
}
FramedPosition::FramedPosition() :
@@ -379,14 +421,14 @@ namespace armarx
frame = "";
}
- FramedPosition::FramedPosition(const Eigen::Vector3f &v, const std::string &s, const string &agent) :
+ FramedPosition::FramedPosition(const Eigen::Vector3f& v, const std::string& s, const string& agent) :
Vector3(v)
{
frame = s;
this->agent = agent;
}
- FramedPosition::FramedPosition(const Eigen::Matrix4f &m, const std::string &s, const string &agent) :
+ FramedPosition::FramedPosition(const Eigen::Matrix4f& m, const std::string& s, const string& agent) :
Vector3(m)
{
frame = s;
@@ -394,44 +436,55 @@ namespace armarx
}
// this doesnt work for unknown reasons
-// FramedPosition::FramedPosition(const Vector3BasePtr pos, const std::string &frame )
-// {
-// this->x = pos->x;
-// this->y = pos->y;
-// this->z = pos->z;
-// this->frame = frame;
-// }
+ // FramedPosition::FramedPosition(const Vector3BasePtr pos, const std::string &frame )
+ // {
+ // this->x = pos->x;
+ // this->y = pos->y;
+ // this->z = pos->z;
+ // this->frame = frame;
+ // }
std::string FramedPosition::getFrame() const
{
return this->frame;
}
- void FramedPosition::changeFrame(const SharedRobotInterfacePrx &referenceRobot, const string &newFrame)
+ void FramedPosition::changeFrame(const SharedRobotInterfacePrx& referenceRobot, const string& newFrame)
{
- if(newFrame == frame)
+ if (newFrame == frame)
+ {
return;
+ }
- if(!referenceRobot->hasRobotNode(newFrame))
+ if (!referenceRobot->hasRobotNode(newFrame))
+ {
throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << referenceRobot->getName();
+ }
+
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
changeFrame(sharedRobot, newFrame);
}
- void FramedPosition::changeFrame(const VirtualRobot::RobotPtr &referenceRobot, const string &newFrame)
+ void FramedPosition::changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const string& newFrame)
{
- if(newFrame == frame)
+ if (newFrame == frame)
+ {
return;
+ }
- if(newFrame == GlobalFrame)
+ if (newFrame == GlobalFrame)
{
changeToGlobal(referenceRobot);
return;
}
Eigen::Matrix4f oldFrameTransformation = Eigen::Matrix4f::Identity();
- if(!referenceRobot->hasRobotNode(newFrame))
+
+ if (!referenceRobot->hasRobotNode(newFrame))
+ {
throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << referenceRobot->getName();
+ }
+
if (frame != GlobalFrame)
{
oldFrameTransformation = referenceRobot->getRobotNode(frame)->getPoseInRootFrame();
@@ -440,31 +493,35 @@ namespace armarx
{
oldFrameTransformation = referenceRobot->getRootNode()->getGlobalPose().inverse();
}
+
Eigen::Matrix4f oldPose = Eigen::Matrix4f::Identity();
- oldPose.block<3,1>(0,3) = toEigen();
+ oldPose.block<3, 1>(0, 3) = toEigen();
Eigen::Matrix4f newPose =
- (referenceRobot->getRobotNode(newFrame)->getPoseInRootFrame().inverse() * oldFrameTransformation) * oldPose;
+ (referenceRobot->getRobotNode(newFrame)->getPoseInRootFrame().inverse() * oldFrameTransformation) * oldPose;
- Eigen::Vector3f pos = newPose.block<3,1>(0,3);
+ Eigen::Vector3f pos = newPose.block<3, 1>(0, 3);
x = pos[0];
y = pos[1];
z = pos[2];
frame = newFrame;
}
- void FramedPosition::changeToGlobal(const SharedRobotInterfacePrx &referenceRobot)
+ void FramedPosition::changeToGlobal(const SharedRobotInterfacePrx& referenceRobot)
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
changeToGlobal(sharedRobot);
}
- void FramedPosition::changeToGlobal(const VirtualRobot::RobotPtr &referenceRobot)
+ void FramedPosition::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
{
- if(frame == GlobalFrame)
+ if (frame == GlobalFrame)
+ {
return;
+ }
+
changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
- Eigen::Vector3f pos = referenceRobot->getRootNode()->getGlobalPose().block<3,3>(0,0) * toEigen()
- + referenceRobot->getRootNode()->getGlobalPose().block<3,1>(0,3);
+ Eigen::Vector3f pos = referenceRobot->getRootNode()->getGlobalPose().block<3, 3>(0, 0) * toEigen()
+ + referenceRobot->getRootNode()->getGlobalPose().block<3, 1>(0, 3);
x = pos[0];
y = pos[1];
z = pos[2];
@@ -472,27 +529,28 @@ namespace armarx
agent = "";
}
- FramedPositionPtr FramedPosition::toGlobal(const SharedRobotInterfacePrx &referenceRobot) const
+ FramedPositionPtr FramedPosition::toGlobal(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toGlobal(sharedRobot);
}
- FramedPositionPtr FramedPosition::toGlobal(const VirtualRobot::RobotPtr &referenceRobot) const
+ FramedPositionPtr FramedPosition::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
{
FramedPositionPtr newPos = FramedPositionPtr::dynamicCast(this->clone());
newPos->changeToGlobal(referenceRobot);
return newPos;
}
- string FramedPosition::output(const Ice::Current &c) const
+ string FramedPosition::output(const Ice::Current& c) const
{
std::stringstream s;
- s << toEigen() << std::endl << "frame: " << getFrame() << (agent.empty()?"": (" agent: " + agent));
+ s << toEigen() << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
return s.str();
}
- int FramedPosition::readFromXML(const string &xmlData, const Ice::Current &c){
+ int FramedPosition::readFromXML(const string& xmlData, const Ice::Current& c)
+ {
using namespace boost::property_tree;
ptree pt;
std::stringstream stream;
@@ -506,13 +564,13 @@ namespace armarx
return 1;
}
- string FramedPosition::writeAsXML(const Ice::Current &)
+ string FramedPosition::writeAsXML(const Ice::Current&)
{
using namespace boost::property_tree;
std::stringstream stream;
stream << Vector3::writeAsXML() <<
- "<frame>"<<frame<<"</frame>"<<
- "<agent>"<<agent<<"</agent>";
+ "<frame>" << frame << "</frame>" <<
+ "<agent>" << agent << "</agent>";
return stream.str();
}
@@ -531,8 +589,11 @@ namespace armarx
Vector3::deserialize(obj);
frame = obj->getString("frame");
- if(obj->hasElement("agent"))
+
+ if (obj->hasElement("agent"))
+ {
agent = obj->getString("agent");
+ }
}
@@ -542,14 +603,14 @@ namespace armarx
frame = "";
}
- FramedOrientation::FramedOrientation(const Eigen::Matrix3f &m, const std::string &s, const string &agent) :
+ FramedOrientation::FramedOrientation(const Eigen::Matrix3f& m, const std::string& s, const string& agent) :
Quaternion(m)
{
frame = s;
this->agent = agent;
}
- FramedOrientation::FramedOrientation(const Eigen::Matrix4f &m, const std::string &s, const std::string &agent) :
+ FramedOrientation::FramedOrientation(const Eigen::Matrix4f& m, const std::string& s, const std::string& agent) :
Quaternion(m)
{
frame = s;
@@ -557,48 +618,57 @@ namespace armarx
}
// this doesnt work for an unknown reason
-// FramedOrientation::FramedOrientation(const QuaternionBasePtr ori, const std::string &frame )
-// {
-// Matrix3f rot;
-// rot = Quaternionf(ori->qw, ori->qx, ori->qy, ori->qz);
-// FramedOrientation(rot, frame);
-// }
+ // FramedOrientation::FramedOrientation(const QuaternionBasePtr ori, const std::string &frame )
+ // {
+ // Matrix3f rot;
+ // rot = Quaternionf(ori->qw, ori->qx, ori->qy, ori->qz);
+ // FramedOrientation(rot, frame);
+ // }
std::string FramedOrientation::getFrame() const
{
return this->frame;
}
- string FramedOrientation::output(const Ice::Current &c) const
+ string FramedOrientation::output(const Ice::Current& c) const
{
std::stringstream s;
- s << toEigen() << std::endl << "frame: " << getFrame() << (agent.empty()?"": (" agent: " + agent));
+ s << toEigen() << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
return s.str();
}
- void FramedOrientation::changeFrame(const SharedRobotInterfacePrx &referenceRobot, const string &newFrame)
+ void FramedOrientation::changeFrame(const SharedRobotInterfacePrx& referenceRobot, const string& newFrame)
{
- if(newFrame == frame)
+ if (newFrame == frame)
+ {
return;
+ }
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
changeFrame(sharedRobot, newFrame);
}
- void FramedOrientation::changeFrame(const VirtualRobot::RobotPtr &referenceRobot, const string &newFrame)
+ void FramedOrientation::changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const string& newFrame)
{
- if(newFrame == frame)
+ if (newFrame == frame)
+ {
return;
- if(newFrame == GlobalFrame)
+ }
+
+ if (newFrame == GlobalFrame)
{
changeToGlobal(referenceRobot);
return;
}
Eigen::Matrix4f oldFrameTransformation = Eigen::Matrix4f::Identity();
- if(!referenceRobot->hasRobotNode(newFrame))
+
+ if (!referenceRobot->hasRobotNode(newFrame))
+ {
throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << referenceRobot->getName();
+ }
+
if (frame != GlobalFrame)
{
oldFrameTransformation = referenceRobot->getRobotNode(frame)->getPoseInRootFrame();
@@ -609,12 +679,12 @@ namespace armarx
}
Eigen::Matrix4f oldPose = Eigen::Matrix4f::Identity();
- oldPose.block<3,3>(0,0) = toEigen();
+ oldPose.block<3, 3>(0, 0) = toEigen();
Eigen::Matrix4f newPose =
- (referenceRobot->getRobotNode(newFrame)->getPoseInRootFrame().inverse() * oldFrameTransformation) * oldPose;
+ (referenceRobot->getRobotNode(newFrame)->getPoseInRootFrame().inverse() * oldFrameTransformation) * oldPose;
- Eigen::Quaternionf quat(newPose.block<3,3>(0,0));
+ Eigen::Quaternionf quat(newPose.block<3, 3>(0, 0));
qw = quat.w();
qx = quat.x();
qy = quat.y();
@@ -622,18 +692,21 @@ namespace armarx
frame = newFrame;
}
- void FramedOrientation::changeToGlobal(const SharedRobotInterfacePrx &referenceRobot)
+ void FramedOrientation::changeToGlobal(const SharedRobotInterfacePrx& referenceRobot)
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
changeToGlobal(sharedRobot);
}
- void FramedOrientation::changeToGlobal(const VirtualRobot::RobotPtr &referenceRobot)
+ void FramedOrientation::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
{
- if(frame == GlobalFrame)
+ if (frame == GlobalFrame)
+ {
return;
+ }
+
changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
- Eigen::Matrix3f rot = referenceRobot->getRootNode()->getGlobalPose().block<3,3>(0,0) * toEigen();
+ Eigen::Matrix3f rot = referenceRobot->getRootNode()->getGlobalPose().block<3, 3>(0, 0) * toEigen();
Eigen::Quaternionf quat(rot);
qw = quat.w();
qx = quat.x();
@@ -643,20 +716,20 @@ namespace armarx
agent = "";
}
- FramedOrientationPtr FramedOrientation::toGlobal(const SharedRobotInterfacePrx &referenceRobot) const
+ FramedOrientationPtr FramedOrientation::toGlobal(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toGlobal(sharedRobot);
}
- FramedOrientationPtr FramedOrientation::toGlobal(const VirtualRobot::RobotPtr &referenceRobot) const
+ FramedOrientationPtr FramedOrientation::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
{
FramedOrientationPtr newPos = FramedOrientationPtr::dynamicCast(this->clone());
newPos->changeToGlobal(referenceRobot);
return newPos;
}
- int FramedOrientation::readFromXML(const string &xmlData, const Ice::Current &c)
+ int FramedOrientation::readFromXML(const string& xmlData, const Ice::Current& c)
{
using namespace boost::property_tree;
ptree pt;
@@ -671,13 +744,13 @@ namespace armarx
return 1;
}
- string FramedOrientation::writeAsXML(const Ice::Current &)
+ string FramedOrientation::writeAsXML(const Ice::Current&)
{
using namespace boost::property_tree;
std::stringstream stream;
stream << Quaternion::writeAsXML() <<
- "<frame>" << frame << "</frame>" <<
- "<agent>" << agent << "</agent>";
+ "<frame>" << frame << "</frame>" <<
+ "<agent>" << agent << "</agent>";
return stream.str();
}
@@ -696,22 +769,26 @@ namespace armarx
Quaternion::deserialize(obj);
frame = obj->getString("frame");
- if(obj->hasElement("agent"))
+
+ if (obj->hasElement("agent"))
+ {
agent = obj->getString("agent");
+ }
}
- VirtualRobot::LinkedCoordinate FramedPose::createLinkedCoordinate(const VirtualRobot::RobotPtr &virtualRobot, const FramedPositionPtr& position, const FramedOrientationPtr& orientation)
+ VirtualRobot::LinkedCoordinate FramedPose::createLinkedCoordinate(const VirtualRobot::RobotPtr& virtualRobot, const FramedPositionPtr& position, const FramedOrientationPtr& orientation)
{
VirtualRobot::LinkedCoordinate c(virtualRobot);
std::string frame;
+
if (position)
{
frame = position->getFrame();
if (orientation)
{
- if(!frame.empty() && frame != orientation->getFrame())
+ if (!frame.empty() && frame != orientation->getFrame())
{
throw armarx::UserException("Error: frames mismatch");
}
@@ -720,17 +797,26 @@ namespace armarx
else
{
if (!orientation)
+ {
armarx::UserException("createLinkedCoordinate: orientation and position are both NULL");
+ }
else
+ {
frame = orientation->getFrame();
+ }
}
Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
- if(orientation)
- pose.block<3,3>(0,0) = orientation->toEigen();
- if(position)
- pose.block<3,1>(0,3) = position->toEigen();
+ if (orientation)
+ {
+ pose.block<3, 3>(0, 0) = orientation->toEigen();
+ }
+
+ if (position)
+ {
+ pose.block<3, 1>(0, 3) = position->toEigen();
+ }
c.set(frame, pose);
diff --git a/source/RobotAPI/libraries/core/FramedPose.h b/source/RobotAPI/libraries/core/FramedPose.h
index 1289ad63c8991f453999247f90dc41032ecfee47..b247d8aaa8534175a05f089bfc9a55bdfaf6e443 100644
--- a/source/RobotAPI/libraries/core/FramedPose.h
+++ b/source/RobotAPI/libraries/core/FramedPose.h
@@ -76,18 +76,18 @@ namespace armarx
typedef IceInternal::Handle<FramedDirection> FramedDirectionPtr;
class FramedDirection :
- virtual public FramedDirectionBase,
- virtual public Vector3
+ virtual public FramedDirectionBase,
+ virtual public Vector3
{
public:
FramedDirection();
FramedDirection(const FramedDirection& source);
- FramedDirection(const Eigen::Vector3f & vec, const std::string &frame, const std::string& agent );
- FramedDirection(Ice::Float x, ::Ice::Float y, ::Ice::Float z, const std::string &frame);
+ FramedDirection(const Eigen::Vector3f& vec, const std::string& frame, const std::string& agent);
+ FramedDirection(Ice::Float x, ::Ice::Float y, ::Ice::Float z, const std::string& frame);
std::string getFrame() const;
- static FramedDirectionPtr ChangeFrame(const VirtualRobot::RobotPtr robot, const FramedDirection& framedVec, const std::string &newFrame);
- void changeFrame(const VirtualRobot::RobotPtr robot, const std::string &newFrame);
+ static FramedDirectionPtr ChangeFrame(const VirtualRobot::RobotPtr robot, const FramedDirection& framedVec, const std::string& newFrame);
+ void changeFrame(const VirtualRobot::RobotPtr robot, const std::string& newFrame);
void changeToGlobal(const SharedRobotInterfacePrx& referenceRobot);
void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
@@ -127,10 +127,10 @@ namespace armarx
* @return ErrorCode, 1 on Success
* @deprecated
*/
- int readFromXML(const std::string &xmlData, const Ice::Current &c = ::Ice::Current());
- std::string writeAsXML(const Ice::Current &c = ::Ice::Current());
+ int readFromXML(const std::string& xmlData, const Ice::Current& c = ::Ice::Current());
+ std::string writeAsXML(const Ice::Current& c = ::Ice::Current());
- friend std::ostream &operator<<(std::ostream &stream, const FramedDirection& rhs)
+ friend std::ostream& operator<<(std::ostream& stream, const FramedDirection& rhs)
{
stream << "FramedDirection: " << std::endl << rhs.output() << std::endl;
return stream;
@@ -141,7 +141,7 @@ namespace armarx
virtual void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current());
private:
- static Eigen::Matrix4f __GetRotationBetweenFrames(const std::string &oldFrame, const std::string &newFrame, VirtualRobot::RobotPtr robotState);
+ static Eigen::Matrix4f __GetRotationBetweenFrames(const std::string& oldFrame, const std::string& newFrame, VirtualRobot::RobotPtr robotState);
};
class FramedPosition;
@@ -154,18 +154,18 @@ namespace armarx
* @brief The FramedPosition class
*/
class FramedPosition :
- virtual public FramedPositionBase,
- virtual public Vector3
+ virtual public FramedPositionBase,
+ virtual public Vector3
{
public:
- FramedPosition( );
- FramedPosition(const Eigen::Vector3f &, const std::string &frame, const std::string &agent);
- FramedPosition(const Eigen::Matrix4f &, const std::string &frame, const std::string &agent);
+ FramedPosition();
+ FramedPosition(const Eigen::Vector3f&, const std::string& frame, const std::string& agent);
+ FramedPosition(const Eigen::Matrix4f&, const std::string& frame, const std::string& agent);
//FramedPosition(const Vector3BasePtr pos, const std::string &frame ); // this doesnt work for unknown reasons
std::string getFrame() const;
- void changeFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string &newFrame);
- void changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string &newFrame);
+ void changeFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame);
+ void changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame);
void changeToGlobal(const SharedRobotInterfacePrx& referenceRobot);
void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
FramedPositionPtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
@@ -205,10 +205,10 @@ namespace armarx
* @return ErrorCode, 1 on Success
* @deprecated
*/
- int readFromXML(const std::string &xmlData, const Ice::Current &c = ::Ice::Current());
- std::string writeAsXML(const Ice::Current &c = ::Ice::Current());
+ int readFromXML(const std::string& xmlData, const Ice::Current& c = ::Ice::Current());
+ std::string writeAsXML(const Ice::Current& c = ::Ice::Current());
- friend std::ostream &operator<<(std::ostream &stream, const FramedPosition& rhs)
+ friend std::ostream& operator<<(std::ostream& stream, const FramedPosition& rhs)
{
stream << "FramedPosition: " << std::endl << rhs.output() << std::endl;
return stream;
@@ -229,13 +229,13 @@ namespace armarx
* @brief The FramedOrientation class
*/
class FramedOrientation :
- virtual public FramedOrientationBase,
- virtual public Quaternion
+ virtual public FramedOrientationBase,
+ virtual public Quaternion
{
public:
FramedOrientation();
- FramedOrientation(const Eigen::Matrix4f &, const std::string &frame, const std::string &agent );
- FramedOrientation(const Eigen::Matrix3f &, const std::string &frame, const std::string &agent );
+ FramedOrientation(const Eigen::Matrix4f&, const std::string& frame, const std::string& agent);
+ FramedOrientation(const Eigen::Matrix3f&, const std::string& frame, const std::string& agent);
// this doesnt work for an unknown reason
//FramedOrientation(const QuaternionBasePtr ori, const std::string &frame );
std::string getFrame()const ;
@@ -259,8 +259,8 @@ namespace armarx
return true;
}
- void changeFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string &newFrame);
- void changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string &newFrame);
+ void changeFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame);
+ void changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame);
void changeToGlobal(const SharedRobotInterfacePrx& referenceRobot);
void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
FramedOrientationPtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
@@ -281,10 +281,10 @@ namespace armarx
* @return ErrorCode, 1 on Success
* @deprecated
*/
- int readFromXML(const std::string &xmlData, const Ice::Current &c = ::Ice::Current());
- std::string writeAsXML(const Ice::Current &c = ::Ice::Current());
+ int readFromXML(const std::string& xmlData, const Ice::Current& c = ::Ice::Current());
+ std::string writeAsXML(const Ice::Current& c = ::Ice::Current());
- friend std::ostream &operator<<(std::ostream &stream, const FramedOrientation& rhs)
+ friend std::ostream& operator<<(std::ostream& stream, const FramedOrientation& rhs)
{
stream << "FramedOrientation: " << std::endl << rhs.output() << std::endl;
return stream;
@@ -307,8 +307,8 @@ namespace armarx
* @brief The FramedPose class
*/
class FramedPose :
- virtual public FramedPoseBase,
- virtual public Pose
+ virtual public FramedPoseBase,
+ virtual public Pose
{
public:
FramedPose();
@@ -342,8 +342,8 @@ namespace armarx
return true;
}
- void changeFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string &newFrame);
- void changeFrame(const VirtualRobot::RobotPtr &referenceRobot, const std::string &newFrame);
+ void changeFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame);
+ void changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame);
void changeToGlobal(const SharedRobotInterfacePrx& referenceRobot);
void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
FramedPosePtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
@@ -365,10 +365,10 @@ namespace armarx
* @return ErrorCode, 1 on Success
* @deprecated
*/
- int readFromXML(const std::string &xmlData, const Ice::Current &c = ::Ice::Current());
- std::string writeAsXML(const Ice::Current &c = ::Ice::Current());
+ int readFromXML(const std::string& xmlData, const Ice::Current& c = ::Ice::Current());
+ std::string writeAsXML(const Ice::Current& c = ::Ice::Current());
- friend std::ostream &operator<<(std::ostream &stream, const FramedPose& rhs)
+ friend std::ostream& operator<<(std::ostream& stream, const FramedPose& rhs)
{
stream << "FramedPose: " << std::endl << rhs.output() << std::endl;
return stream;
@@ -376,7 +376,7 @@ namespace armarx
FramedPositionPtr getPosition() const;
FramedOrientationPtr getOrientation() const;
- static VirtualRobot::LinkedCoordinate createLinkedCoordinate(const VirtualRobot::RobotPtr &virtualRobot, const FramedPositionPtr& position, const FramedOrientationPtr& orientation);
+ static VirtualRobot::LinkedCoordinate createLinkedCoordinate(const VirtualRobot::RobotPtr& virtualRobot, const FramedPositionPtr& position, const FramedOrientationPtr& orientation);
public:
virtual void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const;
diff --git a/source/RobotAPI/libraries/core/LinkedPose.cpp b/source/RobotAPI/libraries/core/LinkedPose.cpp
index 6ab37a69b4552a18a45a92b06956ea22d72fad3e..ea4ebcb674a323ed0c2dd1083608ec9f1d0e462f 100644
--- a/source/RobotAPI/libraries/core/LinkedPose.cpp
+++ b/source/RobotAPI/libraries/core/LinkedPose.cpp
@@ -16,7 +16,8 @@
#include <Ice/ObjectAdapter.h>
-namespace armarx {
+namespace armarx
+{
LinkedPose::LinkedPose() :
Pose(),
@@ -25,7 +26,7 @@ namespace armarx {
this->referenceRobot = NULL;
}
- LinkedPose::LinkedPose(const LinkedPose &other) :
+ LinkedPose::LinkedPose(const LinkedPose& other) :
IceUtil::Shared(other),
PoseBase(other),
FramedPoseBase(other),
@@ -33,14 +34,14 @@ namespace armarx {
Pose(other),
FramedPose(other)
{
- if(referenceRobot)
+ if (referenceRobot)
{
//ARMARX_WARNING_S << "Calling referenceRobot->ref() in cctor of LinkedPose";
referenceRobot->ref();
}
}
- LinkedPose::LinkedPose(const FramedPose &other, const SharedRobotInterfacePrx &referenceRobot) :
+ LinkedPose::LinkedPose(const FramedPose& other, const SharedRobotInterfacePrx& referenceRobot) :
IceUtil::Shared(other),
PoseBase(other),
FramedPoseBase(other),
@@ -49,7 +50,8 @@ namespace armarx {
{
ARMARX_CHECK_EXPRESSION_W_HINT(referenceRobot, "The robot proxy must not be zero");
this->referenceRobot = referenceRobot;
- if(referenceRobot)
+
+ if (referenceRobot)
{
//ARMARX_WARNING_S << "Calling referenceRobot->ref() in cctor of LinkedPose";
referenceRobot->ref();
@@ -63,7 +65,7 @@ namespace armarx {
{
ARMARX_CHECK_EXPRESSION_W_HINT(referenceRobot, "The robot proxy must not be zero");
referenceRobot->ref();
- this->referenceRobot = referenceRobot;
+ this->referenceRobot = referenceRobot;
}
LinkedPose::LinkedPose(const Eigen::Matrix4f& m, const std::string& s, const SharedRobotInterfacePrx& referenceRobot) :
@@ -77,13 +79,14 @@ namespace armarx {
LinkedPose::~LinkedPose()
{
- try{
- if(referenceRobot)
+ try
+ {
+ if (referenceRobot)
{
referenceRobot->unref();
}
}
- catch(...)
+ catch (...)
{
handleExceptions();
}
@@ -98,8 +101,8 @@ namespace armarx {
Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
- pose.block<3,3>(0,0) = QuaternionPtr::dynamicCast(orientation)->toEigen();
- pose.block<3,1>(0,3) = Vector3Ptr::dynamicCast(position)->toEigen();
+ pose.block<3, 3>(0, 0) = QuaternionPtr::dynamicCast(orientation)->toEigen();
+ pose.block<3, 1>(0, 3) = Vector3Ptr::dynamicCast(position)->toEigen();
c.set(frame, pose);
@@ -111,29 +114,29 @@ namespace armarx {
return this->clone();
}
- VariantDataClassPtr LinkedPose::clone(const Ice::Current &c) const
+ VariantDataClassPtr LinkedPose::clone(const Ice::Current& c) const
{
return new LinkedPose(*this);
}
- std::string LinkedPose::output(const Ice::Current &c) const
+ std::string LinkedPose::output(const Ice::Current& c) const
{
std::stringstream s;
s << FramedPose::output() << std::endl << "reference robot: " << referenceRobot->ice_toString();
return s.str();
}
- VariantTypeId LinkedPose::getType(const Ice::Current &c) const
+ VariantTypeId LinkedPose::getType(const Ice::Current& c) const
{
return VariantType::LinkedPose;
}
- bool LinkedPose::validate(const Ice::Current &c)
+ bool LinkedPose::validate(const Ice::Current& c)
{
return true;
}
- void LinkedPose::changeFrame(const std::string &newFrame, const Ice::Current &c)
+ void LinkedPose::changeFrame(const std::string& newFrame, const Ice::Current& c)
{
FramedPose::changeFrame(referenceRobot, newFrame);
}
@@ -151,7 +154,8 @@ namespace armarx {
}
- int LinkedPose::readFromXML(const std::string &xmlData, const Ice::Current &c){
+ int LinkedPose::readFromXML(const std::string& xmlData, const Ice::Current& c)
+ {
using namespace boost::property_tree;
ptree pt;
std::stringstream stream;
@@ -163,7 +167,8 @@ namespace armarx {
std::string remoteRobotId = pt.get<std::string>("referenceRobot");
referenceRobot = SharedRobotInterfacePrx::uncheckedCast(c.adapter->getCommunicator()->stringToProxy(remoteRobotId));
- if(!referenceRobot)
+
+ if (!referenceRobot)
{
ARMARX_ERROR_S << "ReferenceRobot for LinkedPose not registered: " << remoteRobotId << flush;
return 0;
@@ -177,7 +182,7 @@ namespace armarx {
using namespace boost::property_tree;
std::stringstream stream;
stream << FramedPose::writeAsXML()
- << "<referenceRobot>" << "Not serializable" <<"</referenceRobot>";
+ << "<referenceRobot>" << "Not serializable" << "</referenceRobot>";
return stream.str();
}
@@ -197,7 +202,8 @@ namespace armarx {
std::string remoteRobotId = obj->getString("referenceRobot");
referenceRobot = SharedRobotInterfacePrx::uncheckedCast(c.adapter->getCommunicator()->stringToProxy(remoteRobotId));
- if(!referenceRobot)
+
+ if (!referenceRobot)
{
ARMARX_ERROR_S << "ReferenceRobot for LinkedPose not registered: " << remoteRobotId << flush;
}
@@ -205,11 +211,12 @@ namespace armarx {
void LinkedPose::ice_postUnmarshal()
{
- if(referenceRobot)
+ if (referenceRobot)
{
//ARMARX_WARNING_S << "Calling referenceRobot->ref() in __read(IceInternal::BasicStream *__is, bool __rid) of LinkedPose";
referenceRobot->ref();
}
+
FramedPose::ice_postUnmarshal();
}
@@ -218,7 +225,7 @@ namespace armarx {
{
}
- LinkedDirection::LinkedDirection(const LinkedDirection &source) :
+ LinkedDirection::LinkedDirection(const LinkedDirection& source) :
IceUtil::Shared(source),
Vector3Base(source),
FramedDirectionBase(source),
@@ -227,14 +234,15 @@ namespace armarx {
FramedDirection(source)
{
referenceRobot = source.referenceRobot;
- if(referenceRobot)
+
+ if (referenceRobot)
{
//ARMARX_WARNING_S << "Calling referenceRobot->ref() in cctor of LinkedPose";
referenceRobot->ref();
}
}
- LinkedDirection::LinkedDirection(const Eigen::Vector3f &v, const std::string &frame, const SharedRobotInterfacePrx &referenceRobot) :
+ LinkedDirection::LinkedDirection(const Eigen::Vector3f& v, const std::string& frame, const SharedRobotInterfacePrx& referenceRobot) :
FramedDirection(v, frame, referenceRobot->getName())
{
referenceRobot->ref();
@@ -243,22 +251,25 @@ namespace armarx {
LinkedDirection::~LinkedDirection()
{
- try{
- if(referenceRobot)
+ try
+ {
+ if (referenceRobot)
{
referenceRobot->unref();
}
}
- catch(...)
+ catch (...)
{
handleExceptions();
}
}
- void LinkedDirection::changeFrame(const std::string &newFrame, const Ice::Current &c)
+ void LinkedDirection::changeFrame(const std::string& newFrame, const Ice::Current& c)
{
- if(newFrame == frame)
+ if (newFrame == frame)
+ {
return;
+ }
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
@@ -269,35 +280,36 @@ namespace armarx {
frame = frVec->frame;
}
- int LinkedDirection::readFromXML(const std::string &xmlData, const Ice::Current &c)
+ int LinkedDirection::readFromXML(const std::string& xmlData, const Ice::Current& c)
{
throw LocalException("LinkedDirection cannot be serialized! Serialize FramedDirection");
}
- std::string LinkedDirection::writeAsXML(const Ice::Current &c)
+ std::string LinkedDirection::writeAsXML(const Ice::Current& c)
{
throw LocalException("LinkedDirection cannot be deserialized! Serialize FramedDirection");
}
- void LinkedDirection::serialize(const ObjectSerializerBasePtr &serializer, const Ice::Current &) const
+ void LinkedDirection::serialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&) const
{
throw LocalException("LinkedDirection cannot be serialized! Serialize FramedDirection");
}
- void LinkedDirection::deserialize(const ObjectSerializerBasePtr &serializer, const Ice::Current &)
+ void LinkedDirection::deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
{
throw LocalException("LinkedDirection cannot be deserialized! Deserialize FramedDirection");
}
void LinkedDirection::ice_postUnmarshal()
{
- if(referenceRobot)
+ if (referenceRobot)
{
-// ARMARX_WARNING_S << "Calling referenceRobot->ref() in __read(IceInternal::BasicStream *__is, bool __rid) of LinkedPose";
+ // ARMARX_WARNING_S << "Calling referenceRobot->ref() in __read(IceInternal::BasicStream *__is, bool __rid) of LinkedPose";
referenceRobot->ref();
}
+
FramedDirection::ice_postUnmarshal();
}
diff --git a/source/RobotAPI/libraries/core/LinkedPose.h b/source/RobotAPI/libraries/core/LinkedPose.h
index 2e06709049816e98978ac6bca9ede4d7ea814e8c..0ccf2b21043908ad6969065f4a53823adec6741a 100644
--- a/source/RobotAPI/libraries/core/LinkedPose.h
+++ b/source/RobotAPI/libraries/core/LinkedPose.h
@@ -59,13 +59,13 @@ namespace armarx
* @brief The LinkedPose class
*/
class LinkedPose :
- virtual public LinkedPoseBase,
- virtual public FramedPose
+ virtual public LinkedPoseBase,
+ virtual public FramedPose
{
public:
LinkedPose();
- LinkedPose(const LinkedPose &other);
- LinkedPose(const FramedPose &other, const SharedRobotInterfacePrx& referenceRobot);
+ LinkedPose(const LinkedPose& other);
+ LinkedPose(const FramedPose& other, const SharedRobotInterfacePrx& referenceRobot);
LinkedPose(const Eigen::Matrix3f& m, const Eigen::Vector3f& v, const std::string& frame, const SharedRobotInterfacePrx& referenceRobot);
LinkedPose(const Eigen::Matrix4f& m, const std::string& frame, const SharedRobotInterfacePrx& referenceRobot);
@@ -103,10 +103,10 @@ namespace armarx
* @param xmlData Strinconst SharedRobotInterfacePrx& referenceRobotg with xml-data. NOT a file path!
* @return ErrorCode, 1 on Success
*/
- int readFromXML(const std::string &xmlData, const Ice::Current &c = ::Ice::Current());
- std::string writeAsXML(const Ice::Current &c = ::Ice::Current());
+ int readFromXML(const std::string& xmlData, const Ice::Current& c = ::Ice::Current());
+ std::string writeAsXML(const Ice::Current& c = ::Ice::Current());
- friend std::ostream &operator<<(std::ostream &stream, const LinkedPose& rhs)
+ friend std::ostream& operator<<(std::ostream& stream, const LinkedPose& rhs)
{
stream << "LinkedPose: " << std::endl << rhs.output() << std::endl;
return stream;
@@ -129,8 +129,8 @@ namespace armarx
* @brief The LinkedDirection class
*/
class LinkedDirection :
- virtual public LinkedDirectionBase,
- virtual public FramedDirection
+ virtual public LinkedDirectionBase,
+ virtual public FramedDirection
{
public:
LinkedDirection();
@@ -139,7 +139,7 @@ namespace armarx
virtual ~LinkedDirection();
- void changeFrame(const std::string &newFrame, const Ice::Current &c = Ice::Current());
+ void changeFrame(const std::string& newFrame, const Ice::Current& c = Ice::Current());
// inherited from VariantDataClass
Ice::ObjectPtr ice_clone() const
@@ -170,10 +170,10 @@ namespace armarx
}
- int readFromXML(const std::string &xmlData, const Ice::Current &c = ::Ice::Current());
- std::string writeAsXML(const Ice::Current &c = ::Ice::Current());
+ int readFromXML(const std::string& xmlData, const Ice::Current& c = ::Ice::Current());
+ std::string writeAsXML(const Ice::Current& c = ::Ice::Current());
- friend std::ostream &operator<<(std::ostream &stream, const LinkedDirection& rhs)
+ friend std::ostream& operator<<(std::ostream& stream, const LinkedDirection& rhs)
{
stream << "LinkedDirection: " << std::endl << rhs.output() << std::endl;
return stream;
diff --git a/source/RobotAPI/libraries/core/PIDController.cpp b/source/RobotAPI/libraries/core/PIDController.cpp
index 0c91c9ca68e27b1ed4798f83002fe08ad41efdc8..efaf566195c99e83ea0c25d5629218d4c2a32a74 100644
--- a/source/RobotAPI/libraries/core/PIDController.cpp
+++ b/source/RobotAPI/libraries/core/PIDController.cpp
@@ -53,12 +53,14 @@ void PIDController::update(double measuredValue, double targetValue)
{
ScopedRecursiveLock lock(mutex);
IceUtil::Time now = IceUtil::Time::now();
- if(firstRun)
+
+ if (firstRun)
{
lastUpdateTime = IceUtil::Time::now();
}
+
double dt = (now - lastUpdateTime).toSecondsDouble();
- update(dt,measuredValue,targetValue);
+ update(dt, measuredValue, targetValue);
lastUpdateTime = now;
}
@@ -69,17 +71,22 @@ void PIDController::update(double deltaSec, double measuredValue, double targetV
target = targetValue;
double error = target - processValue;
+
//double dt = (now - lastUpdateTime).toSecondsDouble();
-// ARMARX_INFO << deactivateSpam() << VAROUT(dt);
- if(!firstRun)
+ // ARMARX_INFO << deactivateSpam() << VAROUT(dt);
+ if (!firstRun)
{
integral += error * deltaSec;
- if(deltaSec > 0.0)
+
+ if (deltaSec > 0.0)
+ {
derivative = (error - previousError) / deltaSec;
+ }
}
+
firstRun = false;
controlValue = Kp * error + Ki * integral + Kd * derivative;
- ARMARX_DEBUG << deactivateSpam(0.5) << " error: " << error << " cV: " << (controlValue) << " i: " << (Ki*integral) << " d: " << (Kd * derivative) << " dt: " << deltaSec;
+ ARMARX_DEBUG << deactivateSpam(0.5) << " error: " << error << " cV: " << (controlValue) << " i: " << (Ki * integral) << " d: " << (Kd * derivative) << " dt: " << deltaSec;
previousError = error;
lastUpdateTime += IceUtil::Time::seconds(deltaSec);
@@ -111,38 +118,51 @@ void MultiDimPIDController::update(const double deltaSec, const Eigen::VectorXf&
target = targetValue;
double error = (target - processValue).norm();
+
//double dt = (now - lastUpdateTime).toSecondsDouble();
-// ARMARX_INFO << deactivateSpam() << VAROUT(dt);
- if(!firstRun)
+ // ARMARX_INFO << deactivateSpam() << VAROUT(dt);
+ if (!firstRun)
{
integral += error * deltaSec;
- if(deltaSec > 0.0)
+
+ if (deltaSec > 0.0)
+ {
derivative = (error - previousError) / deltaSec;
+ }
}
+
firstRun = false;
Eigen::VectorXf direction = targetValue; // copy size
- if((target - processValue).norm() > 0)
+
+ if ((target - processValue).norm() > 0)
+ {
direction = (target - processValue).normalized();
+ }
else
+ {
direction.setZero();
+ }
+
controlValue = direction * (Kp * error + Ki * integral + Kd * derivative);
- ARMARX_DEBUG << deactivateSpam(0.5) << " error: " << error << " cV: " << (controlValue) << " i: " << (Ki*integral) << " d: " << (Kd * derivative) << " dt: " << deltaSec;
+ ARMARX_DEBUG << deactivateSpam(0.5) << " error: " << error << " cV: " << (controlValue) << " i: " << (Ki * integral) << " d: " << (Kd * derivative) << " dt: " << deltaSec;
previousError = error;
lastUpdateTime += IceUtil::Time::seconds(deltaSec);
}
-void MultiDimPIDController::update(const Eigen::VectorXf &measuredValue, const Eigen::VectorXf &targetValue)
+void MultiDimPIDController::update(const Eigen::VectorXf& measuredValue, const Eigen::VectorXf& targetValue)
{
ScopedRecursiveLock lock(mutex);
IceUtil::Time now = IceUtil::Time::now();
- if(firstRun)
+
+ if (firstRun)
{
lastUpdateTime = IceUtil::Time::now();
}
+
double dt = (now - lastUpdateTime).toSecondsDouble();
- update(dt,measuredValue,targetValue);
+ update(dt, measuredValue, targetValue);
lastUpdateTime = now;
}
@@ -158,7 +178,7 @@ void MultiDimPIDController::reset()
previousError = 0;
integral = 0;
lastUpdateTime = IceUtil::Time::now();
-// controlValue.setZero();
-// processValue.setZero();
-// target.setZero();
+ // controlValue.setZero();
+ // processValue.setZero();
+ // target.setZero();
}
diff --git a/source/RobotAPI/libraries/core/PIDController.h b/source/RobotAPI/libraries/core/PIDController.h
index 8aa5712af5c91a6255da69db93dfe2479ba3d3fb..624a4709536db9cde49a501209f7b39024c8caa1 100644
--- a/source/RobotAPI/libraries/core/PIDController.h
+++ b/source/RobotAPI/libraries/core/PIDController.h
@@ -30,7 +30,7 @@ namespace armarx
{
class PIDController :
- public Logging
+ public Logging
{
public:
PIDController(float Kp, float Ki, float Kd);
@@ -39,7 +39,7 @@ namespace armarx
double getControlValue() const;
void reset();
-// protected:
+ // protected:
float Kp, Ki, Kd;
double integral;
double derivative;
@@ -54,16 +54,16 @@ namespace armarx
typedef boost::shared_ptr<PIDController> PIDControllerPtr;
class MultiDimPIDController :
- public Logging
+ public Logging
{
public:
MultiDimPIDController(float Kp, float Ki, float Kd);
- void update(const double deltaSec, const Eigen::VectorXf &measuredValue, const Eigen::VectorXf &targetValue);
- void update(const Eigen::VectorXf &measuredValue, const Eigen::VectorXf &targetValue);
- const Eigen::VectorXf &getControlValue() const;
+ void update(const double deltaSec, const Eigen::VectorXf& measuredValue, const Eigen::VectorXf& targetValue);
+ void update(const Eigen::VectorXf& measuredValue, const Eigen::VectorXf& targetValue);
+ const Eigen::VectorXf& getControlValue() const;
void reset();
-// protected:
+ // protected:
float Kp, Ki, Kd;
double integral;
double derivative;
diff --git a/source/RobotAPI/libraries/core/Pose.cpp b/source/RobotAPI/libraries/core/Pose.cpp
index cc4ba42ef370383cea0a30867de28ff9f3c6a7e0..5ee4cbed2688553a928cd4191b84286935df9d3d 100644
--- a/source/RobotAPI/libraries/core/Pose.cpp
+++ b/source/RobotAPI/libraries/core/Pose.cpp
@@ -23,7 +23,7 @@ namespace armarx
y = 0;
}
- Vector2::Vector2(const Vector2f & v)
+ Vector2::Vector2(const Vector2f& v)
{
x = v(0);
y = v(1);
@@ -38,7 +38,7 @@ namespace armarx
Vector2f Vector2::toEigen() const
{
Vector2f v;
- v << this->x,this->y;
+ v << this->x, this->y;
return v;
}
@@ -48,14 +48,14 @@ namespace armarx
y = vec[1];
}
- string Vector2::output(const Ice::Current &c) const
+ string Vector2::output(const Ice::Current& c) const
{
std::stringstream s;
s << toEigen();
return s.str();
}
- int Vector2::readFromXML(const string &xmlData, const Ice::Current &c)
+ int Vector2::readFromXML(const string& xmlData, const Ice::Current& c)
{
boost::property_tree::ptree pt = Variant::GetPropertyTree(xmlData);
@@ -64,7 +64,7 @@ namespace armarx
return 1;
}
- string Vector2::writeAsXML(const Ice::Current &)
+ string Vector2::writeAsXML(const Ice::Current&)
{
using namespace boost::property_tree;
ptree pt;
@@ -99,18 +99,18 @@ namespace armarx
z = 0;
}
- Vector3::Vector3(const Vector3f & v)
+ Vector3::Vector3(const Vector3f& v)
{
x = v(0);
y = v(1);
z = v(2);
}
- Vector3::Vector3(const Matrix4f & m)
+ Vector3::Vector3(const Matrix4f& m)
{
- x = m(0,3);
- y = m(1,3);
- z = m(2,3);
+ x = m(0, 3);
+ y = m(1, 3);
+ z = m(2, 3);
}
Vector3::Vector3(::Ice::Float x, ::Ice::Float y, ::Ice::Float z)
@@ -123,7 +123,7 @@ namespace armarx
Vector3f Vector3::toEigen() const
{
Vector3f v;
- v << this->x,this->y,this->z;
+ v << this->x, this->y, this->z;
return v;
}
@@ -134,14 +134,14 @@ namespace armarx
z = vec[2];
}
- string Vector3::output(const Ice::Current &c) const
+ string Vector3::output(const Ice::Current& c) const
{
std::stringstream s;
s << toEigen();
return s.str();
}
- int Vector3::readFromXML(const string &xmlData, const Ice::Current &c)
+ int Vector3::readFromXML(const string& xmlData, const Ice::Current& c)
{
boost::property_tree::ptree pt = Variant::GetPropertyTree(xmlData);
@@ -151,7 +151,7 @@ namespace armarx
return 1;
}
- string Vector3::writeAsXML(const Ice::Current &)
+ string Vector3::writeAsXML(const Ice::Current&)
{
using namespace boost::property_tree;
ptree pt;
@@ -187,18 +187,18 @@ namespace armarx
{
}
- Quaternion::Quaternion(const Matrix4f &m4)
+ Quaternion::Quaternion(const Matrix4f& m4)
{
- Matrix3f m3=m4.block<3,3>(0,0);
+ Matrix3f m3 = m4.block<3, 3>(0, 0);
this->init(m3);
}
- Quaternion::Quaternion(const Matrix3f &m3)
+ Quaternion::Quaternion(const Matrix3f& m3)
{
this->init(m3);
}
- Quaternion::Quaternion(const Eigen::Quaternionf &q)
+ Quaternion::Quaternion(const Eigen::Quaternionf& q)
{
this->init(q);
}
@@ -215,10 +215,10 @@ namespace armarx
{
Matrix3f rot;
rot = Quaternionf(
- this->qw,
- this->qx,
- this->qy,
- this->qz);
+ this->qw,
+ this->qx,
+ this->qy,
+ this->qz);
return rot;
}
@@ -227,52 +227,54 @@ namespace armarx
return Quaternionf(this->qw, this->qx, this->qy, this->qz);
}
- void Quaternion::init(const Matrix3f &m3)
+ void Quaternion::init(const Matrix3f& m3)
{
Quaternionf q;
q = m3;
init(q);
}
- void Quaternion::init(const Eigen::Quaternionf &q)
+ void Quaternion::init(const Eigen::Quaternionf& q)
{
- this->qw=q.w();
- this->qx=q.x();
- this->qy=q.y();
- this->qz=q.z();
+ this->qw = q.w();
+ this->qx = q.x();
+ this->qy = q.y();
+ this->qz = q.z();
}
- Matrix3f Quaternion::slerp(float alpha, const Eigen::Matrix3f &m)
+ Matrix3f Quaternion::slerp(float alpha, const Eigen::Matrix3f& m)
{
- return Quaternion::slerp(alpha,this->toEigen(),m);
+ return Quaternion::slerp(alpha, this->toEigen(), m);
}
- Matrix3f Quaternion::slerp(float alpha, const Eigen::Matrix3f &m1, const Eigen::Matrix3f &m2)
+ Matrix3f Quaternion::slerp(float alpha, const Eigen::Matrix3f& m1, const Eigen::Matrix3f& m2)
{
Matrix3f result;
- Quaternionf q1,q2;
+ Quaternionf q1, q2;
q1 = m1;
- q2=m2;
- result= q1.slerp(alpha,q2);
+ q2 = m2;
+ result = q1.slerp(alpha, q2);
return result;
}
- string Quaternion::output(const Ice::Current &c) const
+ string Quaternion::output(const Ice::Current& c) const
{
std::stringstream s;
s << toEigen();
return s.str();
}
- int Quaternion::readFromXML(const string &xmlData, const Ice::Current &c)
+ int Quaternion::readFromXML(const string& xmlData, const Ice::Current& c)
{
boost::property_tree::ptree pt = Variant::GetPropertyTree(xmlData);
using namespace Eigen;
Quaternionf q;
- if(pt.get_optional<float>("angle"))
- {// AxisAngle-Notation
+
+ if (pt.get_optional<float>("angle"))
+ {
+ // AxisAngle-Notation
float angle = pt.get<float>("angle");
Vector3f axis;
@@ -296,7 +298,7 @@ namespace armarx
return 1;
}
- string Quaternion::writeAsXML(const Ice::Current &)
+ string Quaternion::writeAsXML(const Ice::Current&)
{
using namespace boost::property_tree;
ptree pt;
@@ -330,7 +332,7 @@ namespace armarx
qw = obj->getFloat("qw");
}
- Pose::Pose(const Eigen::Matrix3f &m, const Eigen::Vector3f &v)
+ Pose::Pose(const Eigen::Matrix3f& m, const Eigen::Vector3f& v)
{
this->orientation = new Quaternion(m);
this->position = new Vector3(v);
@@ -351,7 +353,7 @@ namespace armarx
init();
}
- Pose::Pose(const Pose &source) :
+ Pose::Pose(const Pose& source) :
IceUtil::Shared(source),
PoseBase(source)
{
@@ -360,14 +362,14 @@ namespace armarx
init();
}
- Pose::Pose(const Matrix4f &m)
+ Pose::Pose(const Matrix4f& m)
{
this->orientation = new Quaternion(m);
this->position = new Vector3(m);
init();
}
- void Pose::operator=(const Matrix4f &matrix)
+ void Pose::operator=(const Matrix4f& matrix)
{
this->orientation = new Quaternion(matrix);
this->position = new Vector3(matrix);
@@ -379,19 +381,19 @@ namespace armarx
Matrix4f m = Matrix4f::Identity();
ARMARX_CHECK_EXPRESSION(c_orientation);
ARMARX_CHECK_EXPRESSION(c_position);
- m.block<3,3>(0,0) = c_orientation->toEigen();
- m.block<3,1>(0,3) = c_position->toEigen();
+ m.block<3, 3>(0, 0) = c_orientation->toEigen();
+ m.block<3, 1>(0, 3) = c_position->toEigen();
return m;
}
- string Pose::output(const Ice::Current &c) const
+ string Pose::output(const Ice::Current& c) const
{
std::stringstream s;
s << toEigen();
return s.str();
}
- int Pose::readFromXML(const string &xmlData, const Ice::Current &c)
+ int Pose::readFromXML(const string& xmlData, const Ice::Current& c)
{
position->readFromXML(xmlData);
orientation->readFromXML(xmlData);
@@ -399,13 +401,13 @@ namespace armarx
return 1;
}
- string Pose::writeAsXML(const Ice::Current &)
+ string Pose::writeAsXML(const Ice::Current&)
{
using namespace boost::property_tree;
std::stringstream stream;
stream <<
- position->writeAsXML() <<
- orientation->writeAsXML();
+ position->writeAsXML() <<
+ orientation->writeAsXML();
return stream.str();
}
diff --git a/source/RobotAPI/libraries/core/Pose.h b/source/RobotAPI/libraries/core/Pose.h
index 5e67090edab805928ad5c9c2a12bc9e0419efca0..b5566e1d13eaedd204965369a7423348cfaf7af4 100644
--- a/source/RobotAPI/libraries/core/Pose.h
+++ b/source/RobotAPI/libraries/core/Pose.h
@@ -53,7 +53,7 @@ namespace armarx
{
public:
Vector2();
- Vector2(const Eigen::Vector2f &);
+ Vector2(const Eigen::Vector2f&);
Vector2(::Ice::Float x, ::Ice::Float y);
void operator=(const Eigen::Vector2f& ves);
@@ -90,10 +90,10 @@ namespace armarx
* @param xmlData String with xml-data. NOT a file path!
* @return ErrorCode, 1 on Success
*/
- int readFromXML(const std::string &xmlData, const Ice::Current &c = ::Ice::Current());
- std::string writeAsXML(const Ice::Current &c = ::Ice::Current());
+ int readFromXML(const std::string& xmlData, const Ice::Current& c = ::Ice::Current());
+ std::string writeAsXML(const Ice::Current& c = ::Ice::Current());
- friend std::ostream &operator<<(std::ostream &stream, const Vector2& rhs)
+ friend std::ostream& operator<<(std::ostream& stream, const Vector2& rhs)
{
stream << "Vector2: " << std::endl << rhs.output() << std::endl;
return stream;
@@ -114,12 +114,12 @@ namespace armarx
* @brief The Vector3 class
*/
class Vector3 :
- virtual public Vector3Base
+ virtual public Vector3Base
{
public:
Vector3();
- Vector3(const Eigen::Vector3f &);
- Vector3(const Eigen::Matrix4f &);
+ Vector3(const Eigen::Vector3f&);
+ Vector3(const Eigen::Matrix4f&);
Vector3(::Ice::Float x, ::Ice::Float y, ::Ice::Float z);
void operator=(const Eigen::Vector3f& vec);
@@ -157,10 +157,10 @@ namespace armarx
* @param xmlData String with xml-data. NOT a file path!
* @return ErrorCode, 1 on Success
*/
- int readFromXML(const std::string &xmlData, const Ice::Current &c = ::Ice::Current());
- std::string writeAsXML(const Ice::Current &c = ::Ice::Current());
+ int readFromXML(const std::string& xmlData, const Ice::Current& c = ::Ice::Current());
+ std::string writeAsXML(const Ice::Current& c = ::Ice::Current());
- friend std::ostream &operator<<(std::ostream &stream, const Vector3& rhs)
+ friend std::ostream& operator<<(std::ostream& stream, const Vector3& rhs)
{
stream << "Vector3: " << std::endl << rhs.output() << std::endl;
return stream;
@@ -181,20 +181,20 @@ namespace armarx
* @brief The Quaternion class
*/
class Quaternion :
- virtual public QuaternionBase
+ virtual public QuaternionBase
{
public:
Quaternion();
- Quaternion(const Eigen::Matrix4f &);
- Quaternion(const Eigen::Matrix3f &);
- Quaternion(const Eigen::Quaternionf &);
+ Quaternion(const Eigen::Matrix4f&);
+ Quaternion(const Eigen::Matrix3f&);
+ Quaternion(const Eigen::Quaternionf&);
Quaternion(::Ice::Float qw, ::Ice::Float qx, ::Ice::Float qy, ::Ice::Float qz);
Eigen::Matrix3f toEigen() const;
Eigen::Quaternionf toEigenQuaternion() const;
- Eigen::Matrix3f slerp(float, const Eigen::Matrix3f &);
+ Eigen::Matrix3f slerp(float, const Eigen::Matrix3f&);
- static Eigen::Matrix3f slerp(float, const Eigen::Matrix3f &, const Eigen::Matrix3f &);
+ static Eigen::Matrix3f slerp(float, const Eigen::Matrix3f&, const Eigen::Matrix3f&);
// inherited from VariantDataClass
Ice::ObjectPtr ice_clone() const
@@ -238,10 +238,10 @@ namespace armarx
* @param c
* @return ErrorCode, 1 on Success
*/
- int readFromXML(const std::string &xmlData, const Ice::Current &c = ::Ice::Current());
- std::string writeAsXML(const Ice::Current &c = ::Ice::Current());
+ int readFromXML(const std::string& xmlData, const Ice::Current& c = ::Ice::Current());
+ std::string writeAsXML(const Ice::Current& c = ::Ice::Current());
- friend std::ostream &operator<<(std::ostream &stream, const Quaternion& rhs)
+ friend std::ostream& operator<<(std::ostream& stream, const Quaternion& rhs)
{
stream << "Quaternion: " << std::endl << rhs.output() << std::endl;
return stream;
@@ -252,8 +252,8 @@ namespace armarx
virtual void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current());
private:
- void init(const Eigen::Matrix3f &);
- void init(const Eigen::Quaternionf &);
+ void init(const Eigen::Matrix3f&);
+ void init(const Eigen::Quaternionf&);
};
typedef IceInternal::Handle<Quaternion> QuaternionPtr;
@@ -266,16 +266,16 @@ namespace armarx
* @brief The Pose class
*/
class Pose :
- virtual public PoseBase
+ virtual public PoseBase
{
public:
Pose();
- Pose(const Pose&source);
- Pose(const Eigen::Matrix4f &);
- Pose(const Eigen::Matrix3f &, const Eigen::Vector3f &);
+ Pose(const Pose& source);
+ Pose(const Eigen::Matrix4f&);
+ Pose(const Eigen::Matrix3f&, const Eigen::Vector3f&);
Pose(const armarx::Vector3BasePtr pos, const armarx::QuaternionBasePtr ori);
- void operator=(const Eigen::Matrix4f &matrix);
+ void operator=(const Eigen::Matrix4f& matrix);
virtual Eigen::Matrix4f toEigen() const;
// inherited from VariantDataClass
@@ -316,10 +316,10 @@ namespace armarx
* @return ErrorCode, 1 on Success
* @see Quaternion::readFromXml() for AxisAngle-Notation
*/
- int readFromXML(const std::string &xmlData, const Ice::Current &c = ::Ice::Current());
- std::string writeAsXML(const Ice::Current &c = ::Ice::Current());
+ int readFromXML(const std::string& xmlData, const Ice::Current& c = ::Ice::Current());
+ std::string writeAsXML(const Ice::Current& c = ::Ice::Current());
- friend std::ostream &operator<<(std::ostream &stream, const Pose& rhs)
+ friend std::ostream& operator<<(std::ostream& stream, const Pose& rhs)
{
stream << "Pose: " << std::endl << rhs.output() << std::endl;
return stream;
diff --git a/source/RobotAPI/libraries/core/RobotStatechartContext.cpp b/source/RobotAPI/libraries/core/RobotStatechartContext.cpp
index 0e8f4e28fc72e0a164e39d6d8cec4850a08b68c5..7713be5cda5ee55e4f0a6153efe54d3f66116950 100644
--- a/source/RobotAPI/libraries/core/RobotStatechartContext.cpp
+++ b/source/RobotAPI/libraries/core/RobotStatechartContext.cpp
@@ -35,7 +35,7 @@ namespace armarx
// ****************************************************************
void RobotStatechartContext::onInitStatechartContext()
{
-// StatechartContext::onInitStatechart();
+ // StatechartContext::onInitStatechart();
ARMARX_LOG << eINFO << "Init RobotStatechartContext" << flush;
kinematicUnitObserverName = getProperty<std::string>("KinematicUnitObserverName").getValue();
@@ -44,12 +44,13 @@ namespace armarx
usingProxy("RobotStateComponent");
usingProxy(kinematicUnitObserverName);
- if(!getProperty<std::string>("HandUnits").getValue().empty())
+ if (!getProperty<std::string>("HandUnits").getValue().empty())
{
std::string handUnitsProp = getProperty<std::string>("HandUnits").getValue();
std::vector<std::string> handUnitList;
boost::split(handUnitList, handUnitsProp, boost::is_any_of(","));
- for(size_t i = 0; i < handUnitList.size(); i++)
+
+ for (size_t i = 0; i < handUnitList.size(); i++)
{
boost::algorithm::trim(handUnitList.at(i));
usingProxy(handUnitList.at(i));
@@ -59,14 +60,17 @@ namespace armarx
// headIKUnit
headIKUnitName = getProperty<std::string>("HeadIKUnitName").getValue();
headIKKinematicChainName = getProperty<std::string>("HeadIKKinematicChainName").getValue();
+
if (!headIKUnitName.empty())
+ {
usingProxy(headIKUnitName);
+ }
}
void RobotStatechartContext::onConnectStatechartContext()
{
-// StatechartContext::onConnectStatechart();
+ // StatechartContext::onConnectStatechart();
ARMARX_LOG << eINFO << "Starting RobotStatechartContext" << flush;
// retrieve proxies
@@ -85,12 +89,13 @@ namespace armarx
}
- if( !getProperty<std::string>("HandUnits").getValue().empty())
+ if (!getProperty<std::string>("HandUnits").getValue().empty())
{
std::string handUnitsProp = getProperty<std::string>("HandUnits").getValue();
std::vector<std::string> handUnitList;
boost::split(handUnitList, handUnitsProp, boost::is_any_of(","));
- for(size_t i = 0; i < handUnitList.size(); i++)
+
+ for (size_t i = 0; i < handUnitList.size(); i++)
{
boost::algorithm::trim(handUnitList.at(i));
HandUnitInterfacePrx handPrx = getProxy<HandUnitInterfacePrx>(handUnitList.at(i));
@@ -98,6 +103,7 @@ namespace armarx
ARMARX_LOG << eINFO << "Fetched handUnit proxy " << handUnitList.at(i) << ": " << handPrx << flush;
}
}
+
// initialize remote robot
remoteRobot.reset(new RemoteRobot(robotStateComponent->getSynchronizedRobot()));
ARMARX_LOG << eINFO << "Created remote robot" << flush;
@@ -109,27 +115,30 @@ namespace armarx
getConfigIdentifier()));
}
- HandUnitInterfacePrx RobotStatechartContext::getHandUnit(const std::string &handUnitName)
+ HandUnitInterfacePrx RobotStatechartContext::getHandUnit(const std::string& handUnitName)
{
- if (handUnits.find(handUnitName)!=handUnits.end())
+ if (handUnits.find(handUnitName) != handUnits.end())
{
ARMARX_LOG << eINFO << "Found proxy of hand unit with name " << handUnitName << flush;
return handUnits[handUnitName];
}
+
ARMARX_LOG << eINFO << "Do not know proxy of hand unit with name " << handUnitName << flush;
std::map<std::string, HandUnitInterfacePrx>::iterator it = handUnits.begin();
- while (it!=handUnits.end())
+
+ while (it != handUnits.end())
{
ARMARX_LOG << eINFO << "************ Known hand units: " << it->first << ":" << it->second << flush;
it++;
}
+
return HandUnitInterfacePrx();
}
- /* const VirtualRobot::RobotPtr armarx::Armar4Context::getRobot()
- {
- return remoteRobot;
- }*/
+ /* const VirtualRobot::RobotPtr armarx::Armar4Context::getRobot()
+ {
+ return remoteRobot;
+ }*/
}
diff --git a/source/RobotAPI/libraries/core/RobotStatechartContext.h b/source/RobotAPI/libraries/core/RobotStatechartContext.h
index 8490e966383a11d857b8cd0ee773dc447dae1539..81cbdf2529b1d6ee3c5ef7e3c5acb0240c855c38 100644
--- a/source/RobotAPI/libraries/core/RobotStatechartContext.h
+++ b/source/RobotAPI/libraries/core/RobotStatechartContext.h
@@ -67,13 +67,16 @@ namespace armarx
{
public:
// inherited from Component
- virtual std::string getDefaultName() { return "RobotStatechartContext"; }
+ virtual std::string getDefaultName()
+ {
+ return "RobotStatechartContext";
+ }
virtual void onInitStatechartContext();
virtual void onConnectStatechartContext();
// todo:read access should only be allowed via const getters ?!
//const VirtualRobot::RobotPtr getRobot();
- //private:
+ //private:
/**
@@ -81,9 +84,12 @@ namespace armarx
*/
virtual PropertyDefinitionsPtr createPropertyDefinitions();
- std::string getKinematicUnitObserverName() { return kinematicUnitObserverName; }
+ std::string getKinematicUnitObserverName()
+ {
+ return kinematicUnitObserverName;
+ }
- HandUnitInterfacePrx getHandUnit(const std::string &handUnitName);
+ HandUnitInterfacePrx getHandUnit(const std::string& handUnitName);
//! Prx for the RobotState
RobotStateComponentInterfacePrx robotStateComponent;
@@ -99,7 +105,7 @@ namespace armarx
private:
std::string kinematicUnitObserverName;
std::string headIKUnitName;
- };
+ };
}
#endif
diff --git a/source/RobotAPI/libraries/core/checks/ConditionCheckEqualsPose.h b/source/RobotAPI/libraries/core/checks/ConditionCheckEqualsPose.h
index b0d16eac912e862c4a81253ce2689aba64d4166d..30e6279ff01253fb967b8b8749f1f5a9141257ad 100644
--- a/source/RobotAPI/libraries/core/checks/ConditionCheckEqualsPose.h
+++ b/source/RobotAPI/libraries/core/checks/ConditionCheckEqualsPose.h
@@ -15,20 +15,20 @@ namespace armarx
{
setNumberParameters(1);
addSupportedType(VariantType::FramedPosition, createParameterTypeList(1, VariantType::FramedPosition);
- addSupportedType(VariantType::FramedOrientation, createParameterTypeList(1, VariantType::FramedOrientation);
- addSupportedType(VariantType::Vector3, createParameterTypeList(1, VariantType::Vector3);
- addSupportedType(VariantType::Quaternion, createParameterTypeList(1, VariantType::Quaternion);
- addSupportedType(VariantType::Pose, createParameterTypeList(1, VariantType::Pose);
+ addSupportedType(VariantType::FramedOrientation, createParameterTypeList(1, VariantType::FramedOrientation);
+ addSupportedType(VariantType::Vector3, createParameterTypeList(1, VariantType::Vector3);
+ addSupportedType(VariantType::Quaternion, createParameterTypeList(1, VariantType::Quaternion);
+ addSupportedType(VariantType::Pose, createParameterTypeList(1, VariantType::Pose);
}
- ConditionCheck* clone()
+ ConditionCheck* clone()
{
return new ConditionCheckEqualsPose(*this);
}
- bool evaluate(const StringVariantMap &dataFields)
+ bool evaluate(const StringVariantMap& dataFields)
{
- if(dataFields.size() != 1)
+ if (dataFields.size() != 1)
{
printf("Size of dataFields: %d\n", (int)dataFields.size());
throw InvalidConditionException("Wrong number of datafields for condition equals ");
@@ -37,36 +37,36 @@ namespace armarx
Variant& value = dataFields.begin()->second;
VariantTypeId type = value.getType();
- if(type == VariantType::Vector3)
- return (sqrt(((value.x-getParameter(0).getClass<Vector3>().x)*(value.x-getParameter(0).getClass<Vector3>().x))+
- ((value.y-getParameter(0).getClass<Vector3>().y)*(value.y-getParameter(0).getClass<Vector3>().y))+
- ((value.z-getParameter(0).getClass<Vector3>().x)*(value.x-getParameter(0).getClass<Vector3>().z))) == 0);
+ if (type == VariantType::Vector3)
+ return (sqrt(((value.x - getParameter(0).getClass<Vector3>().x) * (value.x - getParameter(0).getClass<Vector3>().x)) +
+ ((value.y - getParameter(0).getClass<Vector3>().y) * (value.y - getParameter(0).getClass<Vector3>().y)) +
+ ((value.z - getParameter(0).getClass<Vector3>().x) * (value.x - getParameter(0).getClass<Vector3>().z))) == 0);
- if(type == VariantType::FramedPosition)
- return (sqrt(((value.x-getParameter(0).getClass<FramedPosition>().x)*(value.x-getParameter(0).getClass<FramedPosition>().x))+
- ((value.y-getParameter(0).getClass<FramedPosition>().y)*(value.y-getParameter(0).getClass<FramedPosition>().y))+
- ((value.z-getParameter(0).getClass<FramedPosition>().x)*(value.x-getParameter(0).getClass<FramedPosition>().z))) == 0);
+ if (type == VariantType::FramedPosition)
+ return (sqrt(((value.x - getParameter(0).getClass<FramedPosition>().x) * (value.x - getParameter(0).getClass<FramedPosition>().x)) +
+ ((value.y - getParameter(0).getClass<FramedPosition>().y) * (value.y - getParameter(0).getClass<FramedPosition>().y)) +
+ ((value.z - getParameter(0).getClass<FramedPosition>().x) * (value.x - getParameter(0).getClass<FramedPosition>().z))) == 0);
- if(type == VariantType::Quaternion)
- return (sqrt(((value.qw-getParameter(0).getClass<Quaternion>().qw)*(value.qw-getParameter(0).getClass<Quaternion>().qw))+
- ((value.qx-getParameter(0).getClass<Quaternion>().qx)*(value.qx-getParameter(0).getClass<Quaternion>().qx))+
- ((value.qy-getParameter(0).getClass<Quaternion>().qy)*(value.qy-getParameter(0).getClass<Quaternion>().qy))+
- ((value.qz-getParameter(0).getClass<Quaternion>().qx)*(value.qx-getParameter(0).getClass<Quaternion>().qz))) == 0);
+ if (type == VariantType::Quaternion)
+ return (sqrt(((value.qw - getParameter(0).getClass<Quaternion>().qw) * (value.qw - getParameter(0).getClass<Quaternion>().qw)) +
+ ((value.qx - getParameter(0).getClass<Quaternion>().qx) * (value.qx - getParameter(0).getClass<Quaternion>().qx)) +
+ ((value.qy - getParameter(0).getClass<Quaternion>().qy) * (value.qy - getParameter(0).getClass<Quaternion>().qy)) +
+ ((value.qz - getParameter(0).getClass<Quaternion>().qx) * (value.qx - getParameter(0).getClass<Quaternion>().qz))) == 0);
- if(type == VariantType::FramedOrientation)
- return (sqrt(((value.qw-getParameter(0).getClass<FramedOrientation>().qw)*(value.qw-getParameter(0).getClass<FramedOrientation>().qw))+
- ((value.qx-getParameter(0).getClass<FramedOrientation>().qx)*(value.qx-getParameter(0).getClass<FramedOrientation>().qx))+
- ((value.qy-getParameter(0).getClass<FramedOrientation>().qy)*(value.qy-getParameter(0).getClass<FramedOrientation>().qy))+
- ((value.qz-getParameter(0).getClass<FramedOrientation>().qx)*(value.qx-getParameter(0).getClass<FramedOrientation>().qz))) == 0);
+ if (type == VariantType::FramedOrientation)
+ return (sqrt(((value.qw - getParameter(0).getClass<FramedOrientation>().qw) * (value.qw - getParameter(0).getClass<FramedOrientation>().qw)) +
+ ((value.qx - getParameter(0).getClass<FramedOrientation>().qx) * (value.qx - getParameter(0).getClass<FramedOrientation>().qx)) +
+ ((value.qy - getParameter(0).getClass<FramedOrientation>().qy) * (value.qy - getParameter(0).getClass<FramedOrientation>().qy)) +
+ ((value.qz - getParameter(0).getClass<FramedOrientation>().qx) * (value.qx - getParameter(0).getClass<FramedOrientation>().qz))) == 0);
- if(type == VariantType::Pose)
- return (sqrt(((value.x-getParameter(0).getClass<Pose>().x)*(value.x-getParameter(0).getClass<Pose>().x))+
- ((value.y-getParameter(0).getClass<Pose>().y)*(value.y-getParameter(0).getClass<Pose>().y))+
- ((value.z-getParameter(0).getClass<Pose>().x)*(value.x-getParameter(0).getClass<Pose>().z)))+
- sqrt(((value.qw-getParameter(0).getClass<Pose>().qw)*(value.qw-getParameter(0).getClass<Pose>().qw))+
- ((value.qx-getParameter(0).getClass<Pose>().qx)*(value.qx-getParameter(0).getClass<Pose>().qx))+
- ((value.qy-getParameter(0).getClass<Pose>().qy)*(value.qy-getParameter(0).getClass<Pose>().qy))+
- ((value.qz-getParameter(0).getClass<Pose>().qx)*(value.qx-getParameter(0).getClass<Pose>().qz))) == 0);
+ if (type == VariantType::Pose)
+ return (sqrt(((value.x - getParameter(0).getClass<Pose>().x) * (value.x - getParameter(0).getClass<Pose>().x)) +
+ ((value.y - getParameter(0).getClass<Pose>().y) * (value.y - getParameter(0).getClass<Pose>().y)) +
+ ((value.z - getParameter(0).getClass<Pose>().x) * (value.x - getParameter(0).getClass<Pose>().z))) +
+ sqrt(((value.qw - getParameter(0).getClass<Pose>().qw) * (value.qw - getParameter(0).getClass<Pose>().qw)) +
+ ((value.qx - getParameter(0).getClass<Pose>().qx) * (value.qx - getParameter(0).getClass<Pose>().qx)) +
+ ((value.qy - getParameter(0).getClass<Pose>().qy) * (value.qy - getParameter(0).getClass<Pose>().qy)) +
+ ((value.qz - getParameter(0).getClass<Pose>().qx) * (value.qx - getParameter(0).getClass<Pose>().qz))) == 0);
return false;
}
diff --git a/source/RobotAPI/libraries/core/checks/ConditionCheckEqualsPoseWithTolerance.h b/source/RobotAPI/libraries/core/checks/ConditionCheckEqualsPoseWithTolerance.h
index 5c92f1d956b377c9a2f3e2487bc8f3d06c22a375..a31803d6bc3a045cca19510c41030cd5fe941baf 100644
--- a/source/RobotAPI/libraries/core/checks/ConditionCheckEqualsPoseWithTolerance.h
+++ b/source/RobotAPI/libraries/core/checks/ConditionCheckEqualsPoseWithTolerance.h
@@ -26,9 +26,9 @@ namespace armarx
return new ConditionCheckApproxPose(*this);
}
- bool evaluate(const StringVariantMap &dataFields)
+ bool evaluate(const StringVariantMap& dataFields)
{
- if(dataFields.size() != 1)
+ if (dataFields.size() != 1)
{
printf("Size of dataFields: %d\n", (int)dataFields.size());
throw InvalidConditionException("Wrong number of datafields for condition equals ");
@@ -37,63 +37,63 @@ namespace armarx
const Variant& value = dataFields.begin()->second;
VariantTypeId type = value.getType();
- if(type == VariantType::Vector3)
+ if (type == VariantType::Vector3)
{
- const Vector3Ptr & typedValue = value.getClass<Vector3>();
- const Vector3Ptr & param = getParameter(0).getClass<Vector3>();
- return (sqrt(((typedValue->x-param->x)*(typedValue->x-param->x))+
- ((typedValue->y-param->y)*(typedValue->y-param->y))+
- ((typedValue->z-param->x)*(typedValue->x-param->z))) < getParameter(1).getFloat());
+ const Vector3Ptr& typedValue = value.getClass<Vector3>();
+ const Vector3Ptr& param = getParameter(0).getClass<Vector3>();
+ return (sqrt(((typedValue->x - param->x) * (typedValue->x - param->x)) +
+ ((typedValue->y - param->y) * (typedValue->y - param->y)) +
+ ((typedValue->z - param->x) * (typedValue->x - param->z))) < getParameter(1).getFloat());
}
- if(type == VariantType::Quaternion)
+ if (type == VariantType::Quaternion)
{
- const QuaternionPtr & typedValue = value.getClass<Quaternion>();
- const QuaternionPtr & param = getParameter(0).getClass<Quaternion>();
- Eigen::Matrix3f diffRot = typedValue->toEigen()*param->toEigen().transpose();
+ const QuaternionPtr& typedValue = value.getClass<Quaternion>();
+ const QuaternionPtr& param = getParameter(0).getClass<Quaternion>();
+ Eigen::Matrix3f diffRot = typedValue->toEigen() * param->toEigen().transpose();
Eigen::AngleAxisf aa(diffRot);
return fabs(aa.angle()) < getParameter(1).getFloat();
}
- if(type == VariantType::FramedPosition)
+ if (type == VariantType::FramedPosition)
{
- const FramedPositionPtr & typedValue = value.getClass<FramedPosition>();
- const FramedPositionPtr & param = getParameter(0).getClass<FramedPosition>();
+ const FramedPositionPtr& typedValue = value.getClass<FramedPosition>();
+ const FramedPositionPtr& param = getParameter(0).getClass<FramedPosition>();
return param->getFrame() == typedValue->getFrame()
- && (sqrt(((typedValue->x-param->x)*(typedValue->x-param->x))+
- ((typedValue->y-param->y)*(typedValue->y-param->y))+
- ((typedValue->z-param->x)*(typedValue->x-param->z))) < getParameter(1).getFloat());
+ && (sqrt(((typedValue->x - param->x) * (typedValue->x - param->x)) +
+ ((typedValue->y - param->y) * (typedValue->y - param->y)) +
+ ((typedValue->z - param->x) * (typedValue->x - param->z))) < getParameter(1).getFloat());
}
- if(type == VariantType::FramedOrientation)
+ if (type == VariantType::FramedOrientation)
{
- const FramedOrientationPtr & typedValue = value.getClass<FramedOrientation>();
- const FramedOrientationPtr & param = getParameter(0).getClass<FramedOrientation>();
- Eigen::Matrix3f diffRot = typedValue->toEigen()*param->toEigen().transpose();
+ const FramedOrientationPtr& typedValue = value.getClass<FramedOrientation>();
+ const FramedOrientationPtr& param = getParameter(0).getClass<FramedOrientation>();
+ Eigen::Matrix3f diffRot = typedValue->toEigen() * param->toEigen().transpose();
Eigen::AngleAxisf aa(diffRot);
return fabs(aa.angle()) < getParameter(1).getFloat();
}
- if(type == VariantType::FramedPose)
+ if (type == VariantType::FramedPose)
{
- const PosePtr & typedValue = value.getClass<FramedPose>();
- const PosePtr & param = getParameter(0).getClass<FramedPose>();
- bool positionOk = (sqrt(((typedValue->position->x-param->position->x)*(typedValue->position->x-param->position->x))+
- ((typedValue->position->y-param->position->y)*(typedValue->position->y-param->position->y))+
- ((typedValue->position->z-param->position->x)*(typedValue->position->x-param->position->z)))
- < getParameter(1).getFloat());
-
- Eigen::Matrix3f diffRot = typedValue->toEigen().block<3,3>(0,0)*param->toEigen().block<3,3>(0,0).transpose();
+ const PosePtr& typedValue = value.getClass<FramedPose>();
+ const PosePtr& param = getParameter(0).getClass<FramedPose>();
+ bool positionOk = (sqrt(((typedValue->position->x - param->position->x) * (typedValue->position->x - param->position->x)) +
+ ((typedValue->position->y - param->position->y) * (typedValue->position->y - param->position->y)) +
+ ((typedValue->position->z - param->position->x) * (typedValue->position->x - param->position->z)))
+ < getParameter(1).getFloat());
+
+ Eigen::Matrix3f diffRot = typedValue->toEigen().block<3, 3>(0, 0) * param->toEigen().block<3, 3>(0, 0).transpose();
Eigen::AngleAxisf aa(diffRot);
bool orientationOk =
- fabs(aa.angle()) < getParameter(2).getFloat();
+ fabs(aa.angle()) < getParameter(2).getFloat();
return positionOk && orientationOk;
}
- return false;
+ return false;
}
};
}
diff --git a/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.cpp b/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.cpp
index dccad8f961ce255f0704c382be2dcc97881cdef5..fc9bb8f412a3af45de340a28bfb5b01b035c8d61 100644
--- a/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.cpp
+++ b/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.cpp
@@ -1,7 +1,8 @@
#include "ConditionCheckMagnitudeChecks.h"
-namespace armarx {
-
+namespace armarx
+{
+
ConditionCheckMagnitudeLarger::ConditionCheckMagnitudeLarger()
{
setNumberParameters(1);
@@ -16,10 +17,10 @@ namespace armarx {
return new ConditionCheckMagnitudeLarger(*this);
}
- bool ConditionCheckMagnitudeLarger::evaluate(const StringVariantMap &dataFields)
+ bool ConditionCheckMagnitudeLarger::evaluate(const StringVariantMap& dataFields)
{
- if(dataFields.size() != 1)
+ if (dataFields.size() != 1)
{
printf("Size of dataFields: %d\n", (int)dataFields.size());
throw InvalidConditionException("Wrong number of datafields for condition equals ");
@@ -28,19 +29,20 @@ namespace armarx {
const Variant& value = dataFields.begin()->second;
VariantTypeId type = value.getType();
- if(type == VariantType::Vector3)
+ if (type == VariantType::Vector3)
{
Eigen::Vector3f vec = value.getClass<Vector3>()->toEigen();
return (vec).norm() > getParameter(0).getFloat();
}
- if(type == VariantType::FramedDirection)
+ if (type == VariantType::FramedDirection)
{
FramedDirectionPtr fV1 = value.getClass<FramedDirection>();
Eigen::Vector3f vec = value.getClass<FramedDirection>()->toEigen();
return (vec).norm() > getParameter(0).getFloat();
}
- if(type == VariantType::LinkedDirection)
+
+ if (type == VariantType::LinkedDirection)
{
LinkedDirectionPtr lV1 = value.getClass<LinkedDirection>();
@@ -66,10 +68,10 @@ namespace armarx {
return new ConditionCheckMagnitudeSmaller(*this);
}
- bool ConditionCheckMagnitudeSmaller::evaluate(const StringVariantMap &dataFields)
+ bool ConditionCheckMagnitudeSmaller::evaluate(const StringVariantMap& dataFields)
{
- if(dataFields.size() != 1)
+ if (dataFields.size() != 1)
{
ARMARX_WARNING_S << "Size of dataFields: %d\n" << dataFields.size();
throw InvalidConditionException("Wrong number of datafields for condition equals ");
@@ -78,21 +80,22 @@ namespace armarx {
const Variant& value = dataFields.begin()->second;
VariantTypeId type = value.getType();
- if(type == VariantType::Vector3)
+ if (type == VariantType::Vector3)
{
Eigen::Vector3f vec = value.getClass<Vector3>()->toEigen();
return (vec).norm() < getParameter(0).getFloat();
}
- if(type == VariantType::FramedDirection)
+ if (type == VariantType::FramedDirection)
{
-// FramedDirectionPtr fV1 = value.getClass<FramedDirection>();
+ // FramedDirectionPtr fV1 = value.getClass<FramedDirection>();
Eigen::Vector3f vec = value.getClass<FramedDirection>()->toEigen();
return (vec).norm() < getParameter(0).getFloat();
}
- if(type == VariantType::LinkedDirection)
+
+ if (type == VariantType::LinkedDirection)
{
-// LinkedDirectionPtr lV1 = value.getClass<LinkedDirection>();
+ // LinkedDirectionPtr lV1 = value.getClass<LinkedDirection>();
Eigen::Vector3f vec = value.getClass<LinkedDirection>()->toEigen();
return (vec).norm() < getParameter(0).getFloat();
}
@@ -114,9 +117,9 @@ namespace armarx {
return new ConditionCheckMagnitudeInRange(*this);
}
- bool ConditionCheckMagnitudeInRange::evaluate(const StringVariantMap &dataFields)
+ bool ConditionCheckMagnitudeInRange::evaluate(const StringVariantMap& dataFields)
{
- if(dataFields.size() != 1)
+ if (dataFields.size() != 1)
{
ARMARX_WARNING_S << "Size of dataFields: " << dataFields.size();
throw InvalidConditionException("Wrong number of datafields for condition InRange ");
@@ -125,22 +128,22 @@ namespace armarx {
const Variant& value = dataFields.begin()->second;
VariantTypeId type = value.getType();
- if(type == VariantType::Vector3)
+ if (type == VariantType::Vector3)
{
Eigen::Vector3f vec = value.getClass<Vector3>()->toEigen();
return ((vec).norm() > getParameter(0).getFloat()) && ((vec).norm() < getParameter(1).getFloat());
}
- if(type == VariantType::FramedDirection)
+ if (type == VariantType::FramedDirection)
{
-// FramedDirectionPtr fV1 = value.getClass<FramedDirection>();
+ // FramedDirectionPtr fV1 = value.getClass<FramedDirection>();
Eigen::Vector3f vec = value.getClass<FramedDirection>()->toEigen();
return ((vec).norm() > getParameter(0).getFloat()) && ((vec).norm() < getParameter(1).getFloat());
}
- if(type == VariantType::LinkedDirection)
+ if (type == VariantType::LinkedDirection)
{
-// LinkedDirectionPtr lV1 = value.getClass<LinkedDirection>();
+ // LinkedDirectionPtr lV1 = value.getClass<LinkedDirection>();
Eigen::Vector3f vec = value.getClass<LinkedDirection>()->toEigen();
return ((vec).norm() > getParameter(0).getFloat()) && ((vec).norm() < getParameter(1).getFloat());
}
diff --git a/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h b/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h
index ad6fdfea9cfe518962b48835893c0ad165c50f91..2c91a36fc0755fd5a8446586be472fb461dde936 100644
--- a/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h
+++ b/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h
@@ -6,15 +6,16 @@
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/LinkedPose.h>
-namespace armarx {
-
+namespace armarx
+{
+
class ConditionCheckMagnitudeLarger : public ConditionCheck, Logging
{
public:
ConditionCheckMagnitudeLarger();
- ConditionCheck *clone();
- bool evaluate(const StringVariantMap &dataFields);
+ ConditionCheck* clone();
+ bool evaluate(const StringVariantMap& dataFields);
};
class ConditionCheckMagnitudeSmaller : public ConditionCheck, Logging
@@ -22,8 +23,8 @@ namespace armarx {
public:
ConditionCheckMagnitudeSmaller();
- ConditionCheck *clone();
- bool evaluate(const StringVariantMap &dataFields);
+ ConditionCheck* clone();
+ bool evaluate(const StringVariantMap& dataFields);
};
class ConditionCheckMagnitudeInRange : public ConditionCheck, Logging
@@ -31,8 +32,8 @@ namespace armarx {
public:
ConditionCheckMagnitudeInRange();
- ConditionCheck *clone();
- bool evaluate(const StringVariantMap &dataFields);
+ ConditionCheck* clone();
+ bool evaluate(const StringVariantMap& dataFields);
};
}
diff --git a/source/RobotAPI/libraries/core/math/MathUtils.h b/source/RobotAPI/libraries/core/math/MathUtils.h
index 6b56ad0faee1cea310c39e949beda627bc1b1afb..22b5ea0e22d3c2ef0cecc94c8aa62b4f221aec49 100644
--- a/source/RobotAPI/libraries/core/math/MathUtils.h
+++ b/source/RobotAPI/libraries/core/math/MathUtils.h
@@ -46,7 +46,7 @@ namespace armarx
{
return value < min ? min : (value > max ? max : value);
}
- static Eigen::Vector3f LimitMinMax(const Eigen::Vector3f &min, const Eigen::Vector3f &max, const Eigen::Vector3f &value)
+ static Eigen::Vector3f LimitMinMax(const Eigen::Vector3f& min, const Eigen::Vector3f& max, const Eigen::Vector3f& value)
{
return Eigen::Vector3f(LimitMinMax(min(0), max(0), value(0)), LimitMinMax(min(1), max(1), value(1)), LimitMinMax(min(2), max(2), value(2)));
}
@@ -63,37 +63,43 @@ namespace armarx
{
return value >= min && value <= max;
}
- static bool CheckMinMax(const Eigen::Vector3f &min, const Eigen::Vector3f &max, const Eigen::Vector3f &value)
+ static bool CheckMinMax(const Eigen::Vector3f& min, const Eigen::Vector3f& max, const Eigen::Vector3f& value)
{
return CheckMinMax(min(0), max(0), value(0)) && CheckMinMax(min(1), max(1), value(1)) && CheckMinMax(min(2), max(2), value(2));
}
- static std::vector<float> VectorSubtract(const std::vector<float> &v1, const std::vector<float> &v2)
+ static std::vector<float> VectorSubtract(const std::vector<float>& v1, const std::vector<float>& v2)
{
std::vector<float> result;
- for(size_t i = 0; i < v1.size() && i < v2.size(); i++)
+
+ for (size_t i = 0; i < v1.size() && i < v2.size(); i++)
{
result.push_back(v1.at(i) - v2.at(i));
}
+
return result;
}
- static std::vector<float> VectorAbsDiff(const std::vector<float> &v1, const std::vector<float> &v2)
+ static std::vector<float> VectorAbsDiff(const std::vector<float>& v1, const std::vector<float>& v2)
{
std::vector<float> result;
- for(size_t i = 0; i < v1.size() && i < v2.size(); i++)
+
+ for (size_t i = 0; i < v1.size() && i < v2.size(); i++)
{
result.push_back(std::fabs(v1.at(i) - v2.at(i)));
}
+
return result;
}
- static float VectorMax(const std::vector<float> &vec)
+ static float VectorMax(const std::vector<float>& vec)
{
float max = vec.at(0);
- for(size_t i = 1; i < vec.size(); i++)
+
+ for (size_t i = 1; i < vec.size(); i++)
{
max = std::max(max, vec.at(i));
}
+
return max;
}
diff --git a/source/RobotAPI/libraries/core/math/MatrixHelpers.h b/source/RobotAPI/libraries/core/math/MatrixHelpers.h
index 7597fcd9a33881d2ed46743951cdcf22b9aafd1d..859be8dd2923c67ba10d0a80d80cc9f28e3b9733 100644
--- a/source/RobotAPI/libraries/core/math/MatrixHelpers.h
+++ b/source/RobotAPI/libraries/core/math/MatrixHelpers.h
@@ -33,31 +33,35 @@ namespace armarx
class MatrixHelpers
{
public:
- static void SetRowToValue(Eigen::MatrixXf &matrix, int rowNr, float value)
+ static void SetRowToValue(Eigen::MatrixXf& matrix, int rowNr, float value)
{
- for(int i = 0; i < matrix.cols(); i++)
+ for (int i = 0; i < matrix.cols(); i++)
{
matrix(rowNr, i) = value;
}
}
- static Eigen::Vector3f CalculateCog3D(const Eigen::MatrixXf &points)
+ static Eigen::Vector3f CalculateCog3D(const Eigen::MatrixXf& points)
{
Eigen::Vector3f sum(0, 0, 0);
- for(int i = 0; i < points.cols(); i++)
+
+ for (int i = 0; i < points.cols(); i++)
{
- sum += points.block<3,1>(0,i);
+ sum += points.block<3, 1>(0, i);
}
+
return sum / points.cols();
}
- static Eigen::MatrixXf SubtractVectorFromAllColumns3D(const Eigen::MatrixXf &points, const Eigen::Vector3f &vec)
+ static Eigen::MatrixXf SubtractVectorFromAllColumns3D(const Eigen::MatrixXf& points, const Eigen::Vector3f& vec)
{
Eigen::MatrixXf matrix(3, points.cols());
- for(int i = 0; i < points.cols(); i++)
+
+ for (int i = 0; i < points.cols(); i++)
{
- matrix.block<3,1>(0,i) = points.block<3,1>(0,i) - vec;
+ matrix.block<3, 1>(0, i) = points.block<3, 1>(0, i) - vec;
}
+
return matrix;
}
diff --git a/source/RobotAPI/libraries/core/math/SVD.h b/source/RobotAPI/libraries/core/math/SVD.h
index c955e5309f11e6297c4cdfc15ed2c9e29c2fe8f3..91fc384b12eeb5caef8b2f0775fd397e188851fb 100644
--- a/source/RobotAPI/libraries/core/math/SVD.h
+++ b/source/RobotAPI/libraries/core/math/SVD.h
@@ -48,7 +48,7 @@ namespace armarx
Eigen::Vector3f getLeftSingularVector3D(int nr)
{
- return matrixU.block<3,1>(0,nr);
+ return matrixU.block<3, 1>(0, nr);
}
};
diff --git a/source/RobotAPI/libraries/core/math/SlidingWindowVectorMedian.h b/source/RobotAPI/libraries/core/math/SlidingWindowVectorMedian.h
index 4b5e2cb91cfc35ef5257bb4be25344bdd29a5e44..defc3a5780f5e77a73c8618c8d346a9b1d95a087 100644
--- a/source/RobotAPI/libraries/core/math/SlidingWindowVectorMedian.h
+++ b/source/RobotAPI/libraries/core/math/SlidingWindowVectorMedian.h
@@ -51,20 +51,20 @@ namespace armarx
SlidingWindowVectorMedian(size_t vectorSize, size_t windowSize)
: windowSize(windowSize),
vectorSize(vectorSize),
- data(vectorSize * windowSize, 0), // initialize all data to 0
+ data(vectorSize* windowSize, 0), // initialize all data to 0
currentIndex(0),
fullCycle(false)
{
}
- void addEntry(const std::vector<float> &entry)
+ void addEntry(const std::vector<float>& entry)
{
- if(entry.size() != vectorSize)
+ if (entry.size() != vectorSize)
{
throw LocalException("Vector of wrong size added. Execting: ") << vectorSize << "; Actual: " << entry.size();
}
- for(size_t i = 0; i < entry.size(); i++)
+ for (size_t i = 0; i < entry.size(); i++)
{
data.at(i + currentIndex * vectorSize) = entry.at(i);
}
@@ -76,16 +76,20 @@ namespace armarx
std::vector<float> getMedian()
{
std::vector<float> median;
- for(size_t i = 0; i < vectorSize; i++)
+
+ for (size_t i = 0; i < vectorSize; i++)
{
std::vector<float> samples;
- for(size_t n = 0; n < windowSize; n++)
+
+ for (size_t n = 0; n < windowSize; n++)
{
samples.push_back(data.at(i + n * vectorSize));
}
+
std::sort(samples.begin(), samples.end());
median.push_back(StatUtils::GetMedian(samples));
}
+
return median;
}
diff --git a/source/RobotAPI/libraries/core/math/StatUtils.h b/source/RobotAPI/libraries/core/math/StatUtils.h
index 092adb3d07b8622afc44eefb85dd97939ad6c9bb..709997af13f0c0a1167978c631379b8f5dc6b4d5 100644
--- a/source/RobotAPI/libraries/core/math/StatUtils.h
+++ b/source/RobotAPI/libraries/core/math/StatUtils.h
@@ -33,34 +33,39 @@ namespace armarx
class StatUtils
{
public:
- static float GetPercentile(const std::vector<float> &sortedData, float percentile)
+ static float GetPercentile(const std::vector<float>& sortedData, float percentile)
{
- if(sortedData.size() == 0)
+ if (sortedData.size() == 0)
{
throw LocalException("GetPercentile not possible for empty vector");
}
+
float indexf = (sortedData.size() - 1) * percentile;
indexf = std::max(0.f, std::min(sortedData.size() - 1.f, indexf));
int index = (int)indexf;
float f = indexf - index;
- if(index == (int)sortedData.size() - 1)
+
+ if (index == (int)sortedData.size() - 1)
{
return sortedData.at(sortedData.size() - 1);
}
+
return sortedData.at(index) * (1 - f) + sortedData.at(index + 1) * f;
}
- static std::vector<float> GetPercentiles(const std::vector<float> &sortedData, int percentiles)
+ static std::vector<float> GetPercentiles(const std::vector<float>& sortedData, int percentiles)
{
std::vector<float> result;
result.push_back(sortedData.at(0));
- for(int i = 1; i < percentiles; i++)
+
+ for (int i = 1; i < percentiles; i++)
{
result.push_back(GetPercentile(sortedData, 1.f / percentiles * i));
}
+
result.push_back(sortedData.at(sortedData.size() - 1));
return result;
}
- static float GetMedian(const std::vector<float> &sortedData)
+ static float GetMedian(const std::vector<float>& sortedData)
{
return GetPercentile(sortedData, 0.5f);
}
diff --git a/source/RobotAPI/libraries/core/math/Trigonometry.h b/source/RobotAPI/libraries/core/math/Trigonometry.h
index 6cc3ccc31d48f88e385672c749bdb191e042569c..e3abce2577e76102633b216ea6777bba47716ef5 100644
--- a/source/RobotAPI/libraries/core/math/Trigonometry.h
+++ b/source/RobotAPI/libraries/core/math/Trigonometry.h
@@ -49,7 +49,7 @@ namespace armarx
return rad / M_PI * 180.0d;
}
- static double GetAngleFromVectorXY(const Eigen::Vector3f &vector)
+ static double GetAngleFromVectorXY(const Eigen::Vector3f& vector)
{
return atan2(vector(1), vector(0));
}
diff --git a/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h b/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h
index 6068d564e64717017a38042c861b4223ebd89ff2..3fbb1ea0cbc3f92c6f113279ae83e11e61973c8a 100644
--- a/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h
+++ b/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h
@@ -34,8 +34,8 @@ namespace armarx
{
class MatrixMaxFilter :
- public MatrixMaxFilterBase,
- public DatafieldFilter
+ public MatrixMaxFilterBase,
+ public DatafieldFilter
{
public:
MatrixMaxFilter()
@@ -43,17 +43,18 @@ namespace armarx
this->windowFilterSize = 1;
}
- VariantBasePtr calculate(const Ice::Current &) const
+ VariantBasePtr calculate(const Ice::Current&) const
{
- if(dataHistory.size() == 0)
+ if (dataHistory.size() == 0)
{
return new Variant(new MatrixFloat(1, 1));
}
+
VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
return new Variant(matrix->toEigen().maxCoeff());
}
- ParameterTypeList getSupportedTypes(const Ice::Current &) const
+ ParameterTypeList getSupportedTypes(const Ice::Current&) const
{
ParameterTypeList result;
result.push_back(VariantType::MatrixFloat);
@@ -62,8 +63,8 @@ namespace armarx
};
class MatrixMinFilter :
- public MatrixMinFilterBase,
- public DatafieldFilter
+ public MatrixMinFilterBase,
+ public DatafieldFilter
{
public:
MatrixMinFilter()
@@ -71,17 +72,18 @@ namespace armarx
this->windowFilterSize = 1;
}
- VariantBasePtr calculate(const Ice::Current &) const
+ VariantBasePtr calculate(const Ice::Current&) const
{
- if(dataHistory.size() == 0)
+ if (dataHistory.size() == 0)
{
return new Variant(new MatrixFloat(1, 1));
}
+
VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
return new Variant(matrix->toEigen().minCoeff());
}
- ParameterTypeList getSupportedTypes(const Ice::Current &) const
+ ParameterTypeList getSupportedTypes(const Ice::Current&) const
{
ParameterTypeList result;
result.push_back(VariantType::MatrixFloat);
@@ -90,8 +92,8 @@ namespace armarx
};
class MatrixAvgFilter :
- public MatrixAvgFilterBase,
- public DatafieldFilter
+ public MatrixAvgFilterBase,
+ public DatafieldFilter
{
public:
MatrixAvgFilter()
@@ -99,17 +101,18 @@ namespace armarx
this->windowFilterSize = 1;
}
- VariantBasePtr calculate(const Ice::Current &) const
+ VariantBasePtr calculate(const Ice::Current&) const
{
- if(dataHistory.size() == 0)
+ if (dataHistory.size() == 0)
{
return new Variant(new MatrixFloat(1, 1));
}
+
VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
return new Variant(matrix->toEigen().mean());
}
- ParameterTypeList getSupportedTypes(const Ice::Current &) const
+ ParameterTypeList getSupportedTypes(const Ice::Current&) const
{
ParameterTypeList result;
result.push_back(VariantType::MatrixFloat);
@@ -118,8 +121,8 @@ namespace armarx
};
class MatrixPercentileFilter :
- public MatrixPercentileFilterBase,
- public DatafieldFilter
+ public MatrixPercentileFilterBase,
+ public DatafieldFilter
{
public:
MatrixPercentileFilter()
@@ -132,46 +135,50 @@ namespace armarx
this->windowFilterSize = 1;
}
- VariantBasePtr calculate(const Ice::Current &) const
+ VariantBasePtr calculate(const Ice::Current&) const
{
- if(dataHistory.size() == 0)
+ if (dataHistory.size() == 0)
{
return new Variant(new MatrixFloat(1, 1));
}
+
VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
std::vector<float> vector = matrix->toVector();
std::sort(vector.begin(), vector.end());
return new Variant(GetPercentile(vector, percentile));
}
- ParameterTypeList getSupportedTypes(const Ice::Current &) const
+ ParameterTypeList getSupportedTypes(const Ice::Current&) const
{
ParameterTypeList result;
result.push_back(VariantType::MatrixFloat);
return result;
}
- static float GetPercentile(const std::vector<float> &sortedData, float percentile)
+ static float GetPercentile(const std::vector<float>& sortedData, float percentile)
{
- if(sortedData.size() == 0)
+ if (sortedData.size() == 0)
{
throw LocalException("GetPercentile not possible for empty vector");
}
+
float indexf = (sortedData.size() - 1) * percentile;
indexf = std::max(0.f, std::min(sortedData.size() - 1.f, indexf));
int index = (int)indexf;
float f = indexf - index;
- if(index == (int)sortedData.size() - 1)
+
+ if (index == (int)sortedData.size() - 1)
{
return sortedData.at(sortedData.size() - 1);
}
+
return sortedData.at(index) * (1 - f) + sortedData.at(index + 1) * f;
}
};
class MatrixPercentilesFilter :
- public MatrixPercentilesFilterBase,
- public DatafieldFilter
+ public MatrixPercentilesFilterBase,
+ public DatafieldFilter
{
public:
MatrixPercentilesFilter()
@@ -185,27 +192,30 @@ namespace armarx
this->windowFilterSize = 1;
}
- VariantBasePtr calculate(const Ice::Current &) const
+ VariantBasePtr calculate(const Ice::Current&) const
{
- if(dataHistory.size() == 0)
+ if (dataHistory.size() == 0)
{
ARMARX_IMPORTANT_S << "no data";
return new Variant(new MatrixFloat(1, 1));
}
+
VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
std::vector<float> vector = matrix->toVector();
std::sort(vector.begin(), vector.end());
std::vector<float> result;
result.push_back(vector.at(0));
- for(int i = 1; i < percentiles; i++)
+
+ for (int i = 1; i < percentiles; i++)
{
result.push_back(MatrixPercentileFilter::GetPercentile(vector, 1.f / percentiles * i));
}
+
result.push_back(vector.at(vector.size() - 1));
return new Variant(new MatrixFloat(1, result.size(), result));
}
- ParameterTypeList getSupportedTypes(const Ice::Current &) const
+ ParameterTypeList getSupportedTypes(const Ice::Current&) const
{
ParameterTypeList result;
result.push_back(VariantType::MatrixFloat);
@@ -214,8 +224,8 @@ namespace armarx
};
class MatrixCumulativeFrequencyFilter :
- public MatrixCumulativeFrequencyFilterBase,
- public DatafieldFilter
+ public MatrixCumulativeFrequencyFilterBase,
+ public DatafieldFilter
{
public:
MatrixCumulativeFrequencyFilter()
@@ -229,40 +239,43 @@ namespace armarx
this->bins = bins;
this->windowFilterSize = 1;
}
- VariantBasePtr calculate(const Ice::Current &) const
+ VariantBasePtr calculate(const Ice::Current&) const
{
- if(dataHistory.size() == 0)
+ if (dataHistory.size() == 0)
{
return new Variant(new MatrixFloat(1, 1));
}
+
VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
std::vector<float> vector = matrix->toVector();
std::sort(vector.begin(), vector.end());
std::vector<int> result = Calculate(vector, min, max, bins);
std::vector<float> resultF;
- for(int v : result)
+
+ for (int v : result)
{
resultF.push_back(v);
}
return new Variant(new MatrixFloat(1, resultF.size(), resultF));
}
- ParameterTypeList getSupportedTypes(const Ice::Current &) const
+ ParameterTypeList getSupportedTypes(const Ice::Current&) const
{
ParameterTypeList result;
result.push_back(VariantType::MatrixFloat);
return result;
}
- static std::vector<int> Calculate(const std::vector<float> &sortedData, float min, float max, int bins)
+ static std::vector<int> Calculate(const std::vector<float>& sortedData, float min, float max, int bins)
{
std::vector<int> result;
float val = min;
int nr = 0;
int lastCount = 0;
- for(size_t i = 0; i < sortedData.size(); i++)
+
+ for (size_t i = 0; i < sortedData.size(); i++)
{
- if(sortedData.at(i) > val && nr < bins)
+ if (sortedData.at(i) > val && nr < bins)
{
result.push_back(i);
nr++;
@@ -270,10 +283,12 @@ namespace armarx
lastCount = i;
}
}
- while((int)result.size() < bins)
+
+ while ((int)result.size() < bins)
{
result.push_back(lastCount);
}
+
return result;
}
diff --git a/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h b/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h
index 224e7756590c6fa484137c57735e6ef8ee97b69f..076e77c1874b176472602c335a6e8942d6720aa9 100644
--- a/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h
+++ b/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h
@@ -28,8 +28,10 @@
#include <RobotAPI/libraries/core/FramedPose.h>
#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
-namespace armarx {
- namespace filters {
+namespace armarx
+{
+ namespace filters
+ {
/**
* @class OffsetFilter
@@ -40,8 +42,8 @@ namespace armarx {
* at a specific moment.
*/
class OffsetFilter :
- public ::armarx::OffsetFilterBase,
- public DatafieldFilter
+ public ::armarx::OffsetFilterBase,
+ public DatafieldFilter
{
public:
OffsetFilter()
@@ -53,35 +55,38 @@ namespace armarx {
// DatafieldFilterBase interface
public:
- VariantBasePtr calculate(const Ice::Current &c = Ice::Current()) const
+ VariantBasePtr calculate(const Ice::Current& c = Ice::Current()) const
{
VariantPtr newVariant;
- if(initialValue && dataHistory.size() > 0)
+
+ if (initialValue && dataHistory.size() > 0)
{
VariantTypeId type = dataHistory.begin()->second->getType();
VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
- if(currentValue->getType() != initialValue->getType())
+
+ if (currentValue->getType() != initialValue->getType())
{
ARMARX_ERROR_S << "Types in OffsetFilter are different: " << Variant::typeToString(currentValue->getType()) << " and " << Variant::typeToString(initialValue->getType());
return NULL;
}
- if(type == VariantType::Int)
+
+ if (type == VariantType::Int)
{
int newValue = dataHistory.rbegin()->second->getInt() - initialValue->getInt();
newVariant = new Variant(newValue);
}
- else if(type == VariantType::Float)
+ else if (type == VariantType::Float)
{
float newValue = dataHistory.rbegin()->second->getFloat() - initialValue->getFloat();
newVariant = new Variant(newValue);
}
- else if(type == VariantType::Double)
+ else if (type == VariantType::Double)
{
double newValue = dataHistory.rbegin()->second->getDouble() - initialValue->getDouble();
newVariant = new Variant(newValue);
}
- else if(type == VariantType::FramedDirection)
+ else if (type == VariantType::FramedDirection)
{
FramedDirectionPtr vec = FramedDirectionPtr::dynamicCast(currentValue->get<FramedDirection>());
FramedDirectionPtr intialVec = FramedDirectionPtr::dynamicCast(initialValue->get<FramedDirection>());
@@ -89,14 +94,14 @@ namespace armarx {
newVariant = new Variant(new FramedDirection(newValue, vec->frame, vec->agent));
}
- else if(type == VariantType::FramedPosition)
+ else if (type == VariantType::FramedPosition)
{
FramedPositionPtr pos = FramedPositionPtr::dynamicCast(currentValue->get<FramedPosition>());
FramedPositionPtr intialPos = FramedPositionPtr::dynamicCast(initialValue->get<FramedPosition>());
Eigen::Vector3f newValue = pos->toEigen() - intialPos->toEigen();
newVariant = new Variant(new FramedPosition(newValue, pos->frame, pos->agent));
}
- else if(type == VariantType::MatrixFloat)
+ else if (type == VariantType::MatrixFloat)
{
MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
MatrixFloatPtr initialMatrix = MatrixFloatPtr::dynamicCast(initialValue->get<MatrixFloat>());
@@ -104,10 +109,11 @@ namespace armarx {
newVariant = new Variant(new MatrixFloat(newMatrix));
}
}
+
return newVariant;
}
- ParameterTypeList getSupportedTypes(const Ice::Current &) const
+ ParameterTypeList getSupportedTypes(const Ice::Current&) const
{
ParameterTypeList result;
result.push_back(VariantType::Int);
@@ -124,13 +130,17 @@ namespace armarx {
// DatafieldFilterBase interface
public:
- void update(Ice::Long timestamp, const VariantBasePtr &value, const Ice::Current &c)
+ void update(Ice::Long timestamp, const VariantBasePtr& value, const Ice::Current& c)
{
DatafieldFilter::update(timestamp, value, c);
- if(firstRun)
+
+ if (firstRun)
{
- if( dataHistory.size() == 0 )
+ if (dataHistory.size() == 0)
+ {
return;
+ }
+
initialValue = VariantPtr::dynamicCast(dataHistory.begin()->second);
firstRun = false;
}
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h b/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
index 29c86acc1f0cb49db19929881809ff5c247f49ca..64e558129a5b284b3a8cd157b7cd5ac953e936ae 100644
--- a/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
@@ -5,8 +5,10 @@
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/interface/core/PoseBase.h>
-namespace armarx {
- namespace filters {
+namespace armarx
+{
+ namespace filters
+ {
/**
* @class PoseMedianFilter
@@ -15,8 +17,8 @@ namespace armarx {
* for a median for datafields of type float, int and double.
*/
class PoseMedianFilter :
- public ::armarx::PoseMedianFilterBase,
- public MedianFilter
+ public ::armarx::PoseMedianFilterBase,
+ public MedianFilter
{
public:
PoseMedianFilter(int windowSize = 11)
@@ -26,10 +28,12 @@ namespace armarx {
// DatafieldFilterBase interface
public:
- VariantBasePtr calculate(const Ice::Current &c) const
+ VariantBasePtr calculate(const Ice::Current& c) const
{
- if(dataHistory.size() == 0)
+ if (dataHistory.size() == 0)
+ {
return NULL;
+ }
VariantTypeId type = dataHistory.begin()->second->getType();
@@ -41,6 +45,7 @@ namespace armarx {
std::string frame = "";
std::string agent = "";
VariantPtr var = VariantPtr::dynamicCast(dataHistory.begin()->second);
+
if (type == VariantType::FramedDirection)
{
FramedDirectionPtr p = var->get<FramedDirection>();
@@ -54,15 +59,18 @@ namespace armarx {
agent = p->agent;
}
- for (int i = 0; i < 3; ++i) {
+ for (int i = 0; i < 3; ++i)
+ {
std::vector<double> values;
- for(auto v : dataHistory)
+
+ for (auto v : dataHistory)
{
VariantPtr v2 = VariantPtr::dynamicCast(v.second);
values.push_back(v2->get<Vector3>()->toEigen()[i]);
}
+
std::sort(values.begin(), values.end());
- vec[i] = values.at(values.size()/2);
+ vec[i] = values.at(values.size() / 2);
}
if (type == VariantType::Vector3)
@@ -87,16 +95,17 @@ namespace armarx {
return NULL;
}
}
- else if(type == VariantType::Double)
+ else if (type == VariantType::Double)
{
-// auto values = SortVariants<double>(dataHistory);
-// return new Variant(values.at(values.size()/2));
+ // auto values = SortVariants<double>(dataHistory);
+ // return new Variant(values.at(values.size()/2));
}
- else if(type == VariantType::Int)
+ else if (type == VariantType::Int)
{
-// auto values = SortVariants<int>(dataHistory);
-// return new Variant(values.at(values.size()/2));
+ // auto values = SortVariants<int>(dataHistory);
+ // return new Variant(values.at(values.size()/2));
}
+
return MedianFilter::calculate(c);
}
@@ -104,7 +113,7 @@ namespace armarx {
* @brief This filter supports: Vector3, FramedDirection, FramedPosition
* @return List of VariantTypes
*/
- ParameterTypeList getSupportedTypes(const Ice::Current &c) const
+ ParameterTypeList getSupportedTypes(const Ice::Current& c) const
{
ParameterTypeList result = MedianFilter::getSupportedTypes(c);
result.push_back(VariantType::Vector3);
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
index a9754e47255936831ae22d95229c2cea78b3a5d3..a9007094e5e4430de94625917a1c60cbff1c7d08 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
@@ -17,375 +17,413 @@ using namespace boost;
using namespace VirtualRobot;
using namespace Eigen;
-namespace armarx{
-
-boost::recursive_mutex RemoteRobot::m;
-
-RemoteRobot::RemoteRobot(SharedRobotInterfacePrx robot) :
- Robot(),
- _robot(robot)
+namespace armarx
{
- _robot->ref();
-}
-RemoteRobot::~RemoteRobot()
-{
- try
+ boost::recursive_mutex RemoteRobot::m;
+
+ RemoteRobot::RemoteRobot(SharedRobotInterfacePrx robot) :
+ Robot(),
+ _robot(robot)
{
- _robot->unref();
+ _robot->ref();
}
- catch(std::exception &e)
+
+ RemoteRobot::~RemoteRobot()
{
- ARMARX_DEBUG_S << "Unref of SharedRobot failed: " << e.what();
+ try
+ {
+ _robot->unref();
+ }
+ catch (std::exception& e)
+ {
+ ARMARX_DEBUG_S << "Unref of SharedRobot failed: " << e.what();
+ }
+ catch (...)
+ {
+ ARMARX_DEBUG_S << "Unref of SharedRobot failed: reason unknown";
+ }
+
}
- catch(...)
+
+ RobotNodePtr RemoteRobot::getRootNode()
{
- ARMARX_DEBUG_S << "Unref of SharedRobot failed: reason unknown";
- }
+ if (!_root)
+ {
+ _root = RemoteRobot::createRemoteRobotNode(_robot->getRootNode(), shared_from_this());
+ }
-}
+ return _root;
+ }
-RobotNodePtr RemoteRobot::getRootNode()
-{
- if (!_root)
- _root = RemoteRobot::createRemoteRobotNode(_robot->getRootNode(),shared_from_this());
- return _root;
-}
+ bool RemoteRobot::hasRobotNode(const string& robotNodeName)
+ {
+ if (_cachedNodes.find(name) == _cachedNodes.end())
+ {
+ return _robot->hasRobotNode(robotNodeName);
+ }
+ else
+ {
+ return true;
+ }
+ }
-bool RemoteRobot::hasRobotNode( const string &robotNodeName )
-{
- if (_cachedNodes.find(name)==_cachedNodes.end())
- return _robot->hasRobotNode(robotNodeName);
- else
- return true;
-}
+ bool RemoteRobot::hasRobotNode(RobotNodePtr robotNode)
+ {
+ return this->hasRobotNode(robotNode->getName());
+
+ /*
+ * This just does not work. because you cannot tell wheter RemoteRobotNode<RobotNodeRevolute> or another type is used
+ * perhaps you can infer the actual RobotNodeType somehow. Until now we just check for names which is
+ * much faster!
+ *
+ if ( (_cachedNodes.find(name)==_cachedNodes.end()) || _robot->hasRobotNode(robotNode->getName())) {
+ shared_ptr<RemoteRobotNode> remoteNode(dynamic_pointer_cast<RemoteRobotNode>(robotNode));
+ if (! remoteNode) return false;
+
+ SharedRobotNodeInterfacePrx sharedNode = remoteNode->getSharedNode();
+ SharedRobotNodeInterfacePrx otherSharedNode = dynamic_pointer_cast<RemoteRobotNode>(this->getRobotNode(robotNodeName))->getSharedNode();
+ if (sharedNode == otherSharedNode)
+ return true;
+ }
-bool RemoteRobot::hasRobotNode( RobotNodePtr robotNode )
-{
- return this->hasRobotNode(robotNode->getName());
-
- /*
- * This just does not work. because you cannot tell wheter RemoteRobotNode<RobotNodeRevolute> or another type is used
- * perhaps you can infer the actual RobotNodeType somehow. Until now we just check for names which is
- * much faster!
- *
- if ( (_cachedNodes.find(name)==_cachedNodes.end()) || _robot->hasRobotNode(robotNode->getName())) {
- shared_ptr<RemoteRobotNode> remoteNode(dynamic_pointer_cast<RemoteRobotNode>(robotNode));
- if (! remoteNode) return false;
-
- SharedRobotNodeInterfacePrx sharedNode = remoteNode->getSharedNode();
- SharedRobotNodeInterfacePrx otherSharedNode = dynamic_pointer_cast<RemoteRobotNode>(this->getRobotNode(robotNodeName))->getSharedNode();
- if (sharedNode == otherSharedNode)
- return true;
+ return false;
+ */
}
- return false;
- */
-}
+ RobotNodePtr RemoteRobot::getRobotNode(const string& robotNodeName)
+ {
+ DMES((format("Node: %1%") % robotNodeName));
-RobotNodePtr RemoteRobot::getRobotNode(const string &robotNodeName)
-{
- DMES((format("Node: %1%") % robotNodeName));
+ if (_cachedNodes.find(robotNodeName) == _cachedNodes.end())
+ {
+ DMES("No cache hit");
+ _cachedNodes[robotNodeName] = RemoteRobot::createRemoteRobotNode(_robot->getRobotNode(robotNodeName),
+ shared_from_this());
+ }
+ else
+ {
+ DMES("Cache hit");
+ }
- if (_cachedNodes.find(robotNodeName)==_cachedNodes.end())
- {
- DMES("No cache hit");
- _cachedNodes[robotNodeName]=RemoteRobot::createRemoteRobotNode(_robot->getRobotNode(robotNodeName),
- shared_from_this());
+ return _cachedNodes[robotNodeName];
}
- else
+
+ void RemoteRobot::getRobotNodes(vector< RobotNodePtr >& storeNodes, bool clearVector)
{
- DMES("Cache hit");
- }
- return _cachedNodes[robotNodeName];
-}
+ if (clearVector)
+ {
+ storeNodes.clear();
+ }
-void RemoteRobot::getRobotNodes(vector< RobotNodePtr > &storeNodes, bool clearVector)
-{
- if (clearVector) storeNodes.clear();
- NameList nodes = _robot->getRobotNodes();
- BOOST_FOREACH(string name, nodes){
- storeNodes.push_back(this->getRobotNode(name));
+ NameList nodes = _robot->getRobotNodes();
+ BOOST_FOREACH(string name, nodes)
+ {
+ storeNodes.push_back(this->getRobotNode(name));
+ }
}
-}
-bool RemoteRobot::hasRobotNodeSet( const string &name )
-{
- return _robot->hasRobotNodeSet(name);
-}
+ bool RemoteRobot::hasRobotNodeSet(const string& name)
+ {
+ return _robot->hasRobotNodeSet(name);
+ }
-RobotNodeSetPtr RemoteRobot::getRobotNodeSet(const string &nodeSetName)
-{
- vector<RobotNodePtr> storeNodes;
- RobotNodeSetInfoPtr info = _robot->getRobotNodeSet(nodeSetName);
- return RobotNodeSet::createRobotNodeSet(
- shared_from_this(),nodeSetName, info->names, info->rootName, info->tcpName);
-}
+ RobotNodeSetPtr RemoteRobot::getRobotNodeSet(const string& nodeSetName)
+ {
+ vector<RobotNodePtr> storeNodes;
+ RobotNodeSetInfoPtr info = _robot->getRobotNodeSet(nodeSetName);
+ return RobotNodeSet::createRobotNodeSet(
+ shared_from_this(), nodeSetName, info->names, info->rootName, info->tcpName);
+ }
-void RemoteRobot::getRobotNodeSets(vector<RobotNodeSetPtr> &storeNodeSet)
-{
- NameList sets= _robot->getRobotNodeSets();
+ void RemoteRobot::getRobotNodeSets(vector<RobotNodeSetPtr>& storeNodeSet)
+ {
+ NameList sets = _robot->getRobotNodeSets();
- BOOST_FOREACH(string name, sets){
- storeNodeSet.push_back(this->getRobotNodeSet(name));
+ BOOST_FOREACH(string name, sets)
+ {
+ storeNodeSet.push_back(this->getRobotNodeSet(name));
+ }
}
-}
-
-Matrix4f RemoteRobot::getGlobalPose() const
-{
- PosePtr p = PosePtr::dynamicCast(_robot->getGlobalPose());
- return p->toEigen(); // convert to eigen first
-}
-
-string RemoteRobot::getName()
-{
- return _robot->getName();
-}
-VirtualRobot::RobotNodePtr RemoteRobot::createLocalNode(SharedRobotNodeInterfacePrx remoteNode, std::vector<VirtualRobot::RobotNodePtr>& allNodes, std::map< VirtualRobot::RobotNodePtr, std::vector<std::string> > &childrenMap, RobotPtr robo)
-{
- boost::recursive_mutex::scoped_lock cloneLock(m);
- static int nonameCounter = 0;
- if (!remoteNode || !robo)
+ Matrix4f RemoteRobot::getGlobalPose() const
{
- ARMARX_ERROR_S << " NULL data " << endl;
- return VirtualRobot::RobotNodePtr();
+ PosePtr p = PosePtr::dynamicCast(_robot->getGlobalPose());
+ return p->toEigen(); // convert to eigen first
}
- VirtualRobot::RobotNodeFactoryPtr revoluteNodeFactory = VirtualRobot::RobotNodeFactory::fromName(VirtualRobot::RobotNodeRevoluteFactory::getName(), NULL);
- VirtualRobot::RobotNodeFactoryPtr fixedNodeFactory = VirtualRobot::RobotNodeFactory::fromName(VirtualRobot::RobotNodeFixedFactory::getName(), NULL);
- VirtualRobot::RobotNodeFactoryPtr prismaticNodeFactory = VirtualRobot::RobotNodeFactory::fromName(VirtualRobot::RobotNodePrismaticFactory::getName(), NULL);
-
- Eigen::Matrix4f idMatrix = Eigen::Matrix4f::Identity();
- Eigen::Vector3f idVec3 = Eigen::Vector3f::Zero();
- std::string name = remoteNode->getName();
- if (name.empty())
+
+ string RemoteRobot::getName()
{
- ARMARX_LOG_S << "Node without name!!!" << endl;
- std::stringstream ss;
- ss << "robot_node_" << nonameCounter;
- nonameCounter++;
- name = ss.str();
+ return _robot->getName();
}
- VirtualRobot::RobotNodePtr result;
- PosePtr lTbase = PosePtr::dynamicCast(remoteNode->getLocalTransformation());
- Eigen::Matrix4f localTransform = lTbase->toEigen();
-
- //float jv = remoteNode->getJointValue();
- float jvLo = remoteNode->getJointLimitLow();
- float jvHi = remoteNode->getJointLimitHigh();
- float jointOffset = 0;//remoteNode->getJointOffset();
-
- JointType jt = remoteNode->getType();
- switch (jt)
+ VirtualRobot::RobotNodePtr RemoteRobot::createLocalNode(SharedRobotNodeInterfacePrx remoteNode, std::vector<VirtualRobot::RobotNodePtr>& allNodes, std::map< VirtualRobot::RobotNodePtr, std::vector<std::string> >& childrenMap, RobotPtr robo)
{
- case ePrismatic:
+ boost::recursive_mutex::scoped_lock cloneLock(m);
+ static int nonameCounter = 0;
+
+ if (!remoteNode || !robo)
{
- Vector3Ptr axisBase = Vector3Ptr::dynamicCast(remoteNode->getJointTranslationDirection());
- Eigen::Vector3f axis = axisBase->toEigen();
- // convert axis to local coord system
- Eigen::Vector4f result4f = Eigen::Vector4f::Zero();
- result4f.segment(0,3) = axis;
- PosePtr gp = PosePtr::dynamicCast(remoteNode->getGlobalPose());
- result4f = gp->toEigen().inverse() * result4f;
- axis = result4f.block(0,0,3,1);
-
- result = prismaticNodeFactory->createRobotNode(robo, name, VirtualRobot::VisualizationNodePtr(), VirtualRobot::CollisionModelPtr(),
- jvLo, jvHi, jointOffset, idMatrix, idVec3, axis);
+ ARMARX_ERROR_S << " NULL data " << endl;
+ return VirtualRobot::RobotNodePtr();
}
- break;
- case eFixed:
- result = fixedNodeFactory->createRobotNode(robo, name, VirtualRobot::VisualizationNodePtr(), VirtualRobot::CollisionModelPtr(), 0,
- 0, 0, localTransform, idVec3, idVec3);
- break;
- case eRevolute:
+
+ VirtualRobot::RobotNodeFactoryPtr revoluteNodeFactory = VirtualRobot::RobotNodeFactory::fromName(VirtualRobot::RobotNodeRevoluteFactory::getName(), NULL);
+ VirtualRobot::RobotNodeFactoryPtr fixedNodeFactory = VirtualRobot::RobotNodeFactory::fromName(VirtualRobot::RobotNodeFixedFactory::getName(), NULL);
+ VirtualRobot::RobotNodeFactoryPtr prismaticNodeFactory = VirtualRobot::RobotNodeFactory::fromName(VirtualRobot::RobotNodePrismaticFactory::getName(), NULL);
+
+ Eigen::Matrix4f idMatrix = Eigen::Matrix4f::Identity();
+ Eigen::Vector3f idVec3 = Eigen::Vector3f::Zero();
+ std::string name = remoteNode->getName();
+
+ if (name.empty())
{
- Vector3Ptr axisBase = Vector3Ptr::dynamicCast(remoteNode->getJointRotationAxis());
- Eigen::Vector3f axis = axisBase->toEigen();
- // convert axis to local coord system
- Eigen::Vector4f result4f = Eigen::Vector4f::Zero();
- result4f.segment(0,3) = axis;
- PosePtr gp = PosePtr::dynamicCast(remoteNode->getGlobalPose());
- result4f = gp->toEigen().inverse() * result4f;
- axis = result4f.block(0,0,3,1);
-
- result = revoluteNodeFactory->createRobotNode(robo, name, VirtualRobot::VisualizationNodePtr(), VirtualRobot::CollisionModelPtr(),
- jvLo, jvHi, jointOffset, localTransform, axis, idVec3);
+ ARMARX_LOG_S << "Node without name!!!" << endl;
+ std::stringstream ss;
+ ss << "robot_node_" << nonameCounter;
+ nonameCounter++;
+ name = ss.str();
}
- break;
- default:
- ARMARX_ERROR_S << "JointType nyi..." << endl;
- return VirtualRobot::RobotNodePtr();
- break;
- }
- robo->registerRobotNode(result);
- allNodes.push_back(result);
+ VirtualRobot::RobotNodePtr result;
+ PosePtr lTbase = PosePtr::dynamicCast(remoteNode->getLocalTransformation());
+ Eigen::Matrix4f localTransform = lTbase->toEigen();
- // setup joint->nextNodes children
- std::vector<std::string> childrenBase = remoteNode->getChildren();
- std::vector<std::string> children;
+ //float jv = remoteNode->getJointValue();
+ float jvLo = remoteNode->getJointLimitLow();
+ float jvHi = remoteNode->getJointLimitHigh();
+ float jointOffset = 0;//remoteNode->getJointOffset();
- // check for RobotNodes (sensors do also register as children!)
- for (size_t i=0;i<childrenBase.size();i++)
- {
- if (_robot->hasRobotNode(childrenBase[i]))
+ JointType jt = remoteNode->getType();
+
+ switch (jt)
{
- SharedRobotNodeInterfacePrx rnRemote = _robot->getRobotNode(childrenBase[i]);
- VirtualRobot::RobotNodePtr localNode = createLocalNode(rnRemote, allNodes, childrenMap, robo);
- /* boost::shared_ptr< RemoteRobotNode<VirtualRobotNodeType> > rnRemote = boost::dynamic_pointer_cast<RemoteRobotNode>(rnRemoteBase);
- if (!rnRemote)
+ case ePrismatic:
{
- ARMARX_ERROR_S << "RemoteRobot does not know robot node with name " << children[i] << endl;
- continue;
- }*/
+ Vector3Ptr axisBase = Vector3Ptr::dynamicCast(remoteNode->getJointTranslationDirection());
+ Eigen::Vector3f axis = axisBase->toEigen();
+ // convert axis to local coord system
+ Eigen::Vector4f result4f = Eigen::Vector4f::Zero();
+ result4f.segment(0, 3) = axis;
+ PosePtr gp = PosePtr::dynamicCast(remoteNode->getGlobalPose());
+ result4f = gp->toEigen().inverse() * result4f;
+ axis = result4f.block(0, 0, 3, 1);
+
+ result = prismaticNodeFactory->createRobotNode(robo, name, VirtualRobot::VisualizationNodePtr(), VirtualRobot::CollisionModelPtr(),
+ jvLo, jvHi, jointOffset, idMatrix, idVec3, axis);
+ }
+ break;
+ case eFixed:
+ result = fixedNodeFactory->createRobotNode(robo, name, VirtualRobot::VisualizationNodePtr(), VirtualRobot::CollisionModelPtr(), 0,
+ 0, 0, localTransform, idVec3, idVec3);
+ break;
- if (!localNode)
+ case eRevolute:
{
- ARMARX_ERROR_S << "Could not create local node: " << children[i] << endl;
- continue;
+ Vector3Ptr axisBase = Vector3Ptr::dynamicCast(remoteNode->getJointRotationAxis());
+ Eigen::Vector3f axis = axisBase->toEigen();
+ // convert axis to local coord system
+ Eigen::Vector4f result4f = Eigen::Vector4f::Zero();
+ result4f.segment(0, 3) = axis;
+ PosePtr gp = PosePtr::dynamicCast(remoteNode->getGlobalPose());
+ result4f = gp->toEigen().inverse() * result4f;
+ axis = result4f.block(0, 0, 3, 1);
+
+ result = revoluteNodeFactory->createRobotNode(robo, name, VirtualRobot::VisualizationNodePtr(), VirtualRobot::CollisionModelPtr(),
+ jvLo, jvHi, jointOffset, localTransform, axis, idVec3);
}
- children.push_back(childrenBase[i]);
+ break;
+
+ default:
+ ARMARX_ERROR_S << "JointType nyi..." << endl;
+ return VirtualRobot::RobotNodePtr();
+ break;
}
- }
- childrenMap[result] = children;
- return result;
-}
-VirtualRobot::RobotPtr RemoteRobot::createLocalClone()
-{
- //ARMARX_IMPORTANT_S << "RemoteRobot local clone" << flush;
- boost::recursive_mutex::scoped_lock cloneLock(m);
- std::string robotType = getName();
- std::string robotName = getName();
- VirtualRobot::RobotPtr robo(new VirtualRobot::LocalRobot(robotName,robotType));
+ robo->registerRobotNode(result);
+ allNodes.push_back(result);
- //RobotNodePtr
- SharedRobotNodeInterfacePrx root = _robot->getRootNode();
- std::vector<VirtualRobot::RobotNodePtr> allNodes;
- std::map< VirtualRobot::RobotNodePtr, std::vector<std::string> > childrenMap;
+ // setup joint->nextNodes children
+ std::vector<std::string> childrenBase = remoteNode->getChildren();
+ std::vector<std::string> children;
- VirtualRobot::RobotNodePtr rootLocal = createLocalNode(root, allNodes, childrenMap, robo);
+ // check for RobotNodes (sensors do also register as children!)
+ for (size_t i = 0; i < childrenBase.size(); i++)
+ {
+ if (_robot->hasRobotNode(childrenBase[i]))
+ {
+ SharedRobotNodeInterfacePrx rnRemote = _robot->getRobotNode(childrenBase[i]);
+ VirtualRobot::RobotNodePtr localNode = createLocalNode(rnRemote, allNodes, childrenMap, robo);
+ /* boost::shared_ptr< RemoteRobotNode<VirtualRobotNodeType> > rnRemote = boost::dynamic_pointer_cast<RemoteRobotNode>(rnRemoteBase);
+ if (!rnRemote)
+ {
+ ARMARX_ERROR_S << "RemoteRobot does not know robot node with name " << children[i] << endl;
+ continue;
+ }*/
+
+
+ if (!localNode)
+ {
+ ARMARX_ERROR_S << "Could not create local node: " << children[i] << endl;
+ continue;
+ }
+
+ children.push_back(childrenBase[i]);
+ }
+ }
- bool res = VirtualRobot::RobotFactory::initializeRobot(robo, allNodes, childrenMap, rootLocal);
- if (!res)
- {
- ARMARX_ERROR_S << "Failed to initialize local robot..." << endl;
- return VirtualRobot::RobotPtr();
+ childrenMap[result] = children;
+ return result;
}
- // clone rns
- std::vector<std::string> rns = _robot->getRobotNodeSets();
- for (size_t i=0;i<rns.size();i++)
+ VirtualRobot::RobotPtr RemoteRobot::createLocalClone()
{
- RobotNodeSetInfoPtr rnsInfo = _robot->getRobotNodeSet(rns[i]);
- RobotNodeSet::createRobotNodeSet(robo,rnsInfo->name,rnsInfo->names,rnsInfo->rootName,rnsInfo->tcpName,true);
- }
- //ARMARX_IMPORTANT_S << "RemoteRobot local clone end" << flush;
- auto pose = PosePtr::dynamicCast(_robot->getGlobalPose());
- robo->setGlobalPose(pose->toEigen());
- return robo;
-}
+ //ARMARX_IMPORTANT_S << "RemoteRobot local clone" << flush;
+ boost::recursive_mutex::scoped_lock cloneLock(m);
+ std::string robotType = getName();
+ std::string robotName = getName();
+ VirtualRobot::RobotPtr robo(new VirtualRobot::LocalRobot(robotName, robotType));
-VirtualRobot::RobotPtr RemoteRobot::createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const string &filename)
-{
- return createLocalClone(robotStatePrx->getSynchronizedRobot(), filename);
-}
+ //RobotNodePtr
+ SharedRobotNodeInterfacePrx root = _robot->getRootNode();
-RobotPtr RemoteRobot::createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, const string &filename)
-{
- boost::recursive_mutex::scoped_lock cloneLock(m);
- ARMARX_VERBOSE_S << "Creating local clone of remote robot (filename:" << filename << ")" << endl;
- VirtualRobot::RobotPtr result;
+ std::vector<VirtualRobot::RobotNodePtr> allNodes;
+ std::map< VirtualRobot::RobotNodePtr, std::vector<std::string> > childrenMap;
+
+ VirtualRobot::RobotNodePtr rootLocal = createLocalNode(root, allNodes, childrenMap, robo);
+
+ bool res = VirtualRobot::RobotFactory::initializeRobot(robo, allNodes, childrenMap, rootLocal);
+
+ if (!res)
+ {
+ ARMARX_ERROR_S << "Failed to initialize local robot..." << endl;
+ return VirtualRobot::RobotPtr();
+ }
+
+ // clone rns
+ std::vector<std::string> rns = _robot->getRobotNodeSets();
+
+ for (size_t i = 0; i < rns.size(); i++)
+ {
+ RobotNodeSetInfoPtr rnsInfo = _robot->getRobotNodeSet(rns[i]);
+ RobotNodeSet::createRobotNodeSet(robo, rnsInfo->name, rnsInfo->names, rnsInfo->rootName, rnsInfo->tcpName, true);
+ }
+
+ //ARMARX_IMPORTANT_S << "RemoteRobot local clone end" << flush;
+ auto pose = PosePtr::dynamicCast(_robot->getGlobalPose());
+ robo->setGlobalPose(pose->toEigen());
+ return robo;
+ }
- if (!sharedRobotPrx)
+ VirtualRobot::RobotPtr RemoteRobot::createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const string& filename)
{
- ARMARX_ERROR_S << "NULL sharedRobotPrx. Aborting..." << endl;
- return result;
+ return createLocalClone(robotStatePrx->getSynchronizedRobot(), filename);
}
- if (filename.empty())
+ RobotPtr RemoteRobot::createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, const string& filename)
{
- RemoteRobotPtr rob(new RemoteRobot(sharedRobotPrx));
- result = rob->createLocalClone();
- if (!result)
+ boost::recursive_mutex::scoped_lock cloneLock(m);
+ ARMARX_VERBOSE_S << "Creating local clone of remote robot (filename:" << filename << ")" << endl;
+ VirtualRobot::RobotPtr result;
+
+ if (!sharedRobotPrx)
{
- ARMARX_ERROR_S << "Could not create local clone. Aborting..." << endl;
+ ARMARX_ERROR_S << "NULL sharedRobotPrx. Aborting..." << endl;
return result;
}
- } else
- {
- result = RobotIO::loadRobot(filename);
- if (!result)
+
+ if (filename.empty())
{
- ARMARX_ERROR_S << "Could not load robot file " << filename << ". Aborting..." << endl;
- return result;
+ RemoteRobotPtr rob(new RemoteRobot(sharedRobotPrx));
+ result = rob->createLocalClone();
+
+ if (!result)
+ {
+ ARMARX_ERROR_S << "Could not create local clone. Aborting..." << endl;
+ return result;
+ }
}
- }
- synchronizeLocalClone(result,sharedRobotPrx);
- return result;
-}
+ else
+ {
+ result = RobotIO::loadRobot(filename);
-bool RemoteRobot::synchronizeLocalClone(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx)
-{
- return synchronizeLocalClone(robot, robotStatePrx->getSynchronizedRobot());
-}
+ if (!result)
+ {
+ ARMARX_ERROR_S << "Could not load robot file " << filename << ". Aborting..." << endl;
+ return result;
+ }
+ }
-bool RemoteRobot::synchronizeLocalClone(VirtualRobot::RobotPtr robot, SharedRobotInterfacePrx sharedRobotPrx)
-{
- if (!sharedRobotPrx || !robot)
+ synchronizeLocalClone(result, sharedRobotPrx);
+ return result;
+ }
+
+ bool RemoteRobot::synchronizeLocalClone(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx)
{
- ARMARX_ERROR_S << "NULL data. Aborting..." << endl;
- return false;
+ return synchronizeLocalClone(robot, robotStatePrx->getSynchronizedRobot());
}
- RobotConfigPtr c(new RobotConfig(robot,"synchronizeLocalClone"));
- NameValueMap jv = sharedRobotPrx->getConfig();
- for ( NameValueMap::const_iterator it = jv.begin(); it!=jv.end(); it++ )
+
+ bool RemoteRobot::synchronizeLocalClone(VirtualRobot::RobotPtr robot, SharedRobotInterfacePrx sharedRobotPrx)
{
- // joint values
- const std::string& jointName = it->first;
- float jointAngle = it->second;
- if (!c->setConfig(jointName,jointAngle))
+ if (!sharedRobotPrx || !robot)
{
- ARMARX_WARNING_S << "Joint not known in local copy:" << jointName << ". Skipping..." << endl;
+ ARMARX_ERROR_S << "NULL data. Aborting..." << endl;
+ return false;
+ }
+
+ RobotConfigPtr c(new RobotConfig(robot, "synchronizeLocalClone"));
+ NameValueMap jv = sharedRobotPrx->getConfig();
+
+ for (NameValueMap::const_iterator it = jv.begin(); it != jv.end(); it++)
+ {
+ // joint values
+ const std::string& jointName = it->first;
+ float jointAngle = it->second;
+
+ if (!c->setConfig(jointName, jointAngle))
+ {
+ ARMARX_WARNING_S << "Joint not known in local copy:" << jointName << ". Skipping..." << endl;
+ }
}
+
+ robot->setConfig(c);
+ auto pose = PosePtr::dynamicCast(sharedRobotPrx->getGlobalPose());
+ robot->setGlobalPose(pose->toEigen());
+ return true;
}
- robot->setConfig(c);
- auto pose = PosePtr::dynamicCast(sharedRobotPrx->getGlobalPose());
- robot->setGlobalPose(pose->toEigen());
- return true;
-}
-// Private (unused methods)
+ // Private (unused methods)
-bool RemoteRobot::hasEndEffector(const string& endEffectorName)
-{
- return false;
-}
+ bool RemoteRobot::hasEndEffector(const string& endEffectorName)
+ {
+ return false;
+ }
-EndEffectorPtr RemoteRobot::getEndEffector(const string& endEffectorName)
-{
- return EndEffectorPtr();
-}
+ EndEffectorPtr RemoteRobot::getEndEffector(const string& endEffectorName)
+ {
+ return EndEffectorPtr();
+ }
-void RemoteRobot::getEndEffectors(vector<EndEffectorPtr> &storeEEF){}
-void RemoteRobot::setRootNode(RobotNodePtr node){}
-void RemoteRobot::registerRobotNode(RobotNodePtr node){}
-void RemoteRobot::deregisterRobotNode(RobotNodePtr node){}
-void RemoteRobot::registerRobotNodeSet(RobotNodeSetPtr nodeSet){}
-void RemoteRobot::deregisterRobotNodeSet(RobotNodeSetPtr nodeSet){}
-void RemoteRobot::registerEndEffector(EndEffectorPtr endEffector){}
+ void RemoteRobot::getEndEffectors(vector<EndEffectorPtr>& storeEEF) {}
+ void RemoteRobot::setRootNode(RobotNodePtr node) {}
+ void RemoteRobot::registerRobotNode(RobotNodePtr node) {}
+ void RemoteRobot::deregisterRobotNode(RobotNodePtr node) {}
+ void RemoteRobot::registerRobotNodeSet(RobotNodeSetPtr nodeSet) {}
+ void RemoteRobot::deregisterRobotNodeSet(RobotNodeSetPtr nodeSet) {}
+ void RemoteRobot::registerEndEffector(EndEffectorPtr endEffector) {}
-void RemoteRobot::setGlobalPose(const Eigen::Matrix4f &globalPose, bool applyValues)
-{
- if(_robot)
- _robot->setGlobalPose(new Pose(globalPose));
-}
+ void RemoteRobot::setGlobalPose(const Eigen::Matrix4f& globalPose, bool applyValues)
+ {
+ if (_robot)
+ {
+ _robot->setGlobalPose(new Pose(globalPose));
+ }
+ }
}
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
index 4d677edf3b3e9a79dcb9e8e918575d9ac5f61d46..64bca72366ea03d0c8f6399d93352f1433f63888 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
@@ -72,7 +72,7 @@ namespace armarx
*/
template<class VirtualRobotNodeType>
class RemoteRobotNode :
- public VirtualRobotNodeType
+ public VirtualRobotNodeType
{
friend struct RemoteRobotNodeInitializer<VirtualRobotNodeType>;
@@ -81,7 +81,7 @@ namespace armarx
_node(node)
{
_node->ref();
- this->name=_node->getName();
+ this->name = _node->getName();
this->robot = vr;
_node->getJointValueOffest();
setJointLimits(_node->getJointLimitLow(), _node->getJointLimitHigh());
@@ -102,9 +102,9 @@ namespace armarx
virtual Eigen::Matrix4f getGlobalPose() const;
virtual Eigen::Matrix4f getPoseInRootFrame() const;
virtual Eigen::Vector3f getPositionInRootFrame() const;
- virtual bool hasChildNode(const std::string &child, bool recursive = false) const;
+ virtual bool hasChildNode(const std::string& child, bool recursive = false) const;
- virtual std::vector<VirtualRobot::RobotNodePtr> getAllParents( VirtualRobot::RobotNodeSetPtr rns );
+ virtual std::vector<VirtualRobot::RobotNodePtr> getAllParents(VirtualRobot::RobotNodeSetPtr rns);
virtual VirtualRobot::RobotNodePtr getParent();
inline SharedRobotNodeInterfacePrx getSharedRobotNode()
{
@@ -117,20 +117,20 @@ namespace armarx
///////////////////////// SETUP ////////////////////////////////////
virtual void setJointLimits(float lo, float hi);
//virtual void setPostJointTransformation(const Eigen::Matrix4f &trafo);
- virtual void setLocalTransformation(const Eigen::Matrix4f &trafo);
+ virtual void setLocalTransformation(const Eigen::Matrix4f& trafo);
virtual std::string getParentName() const;
virtual std::vector< VirtualRobot::SceneObjectPtr> getChildren() const;
virtual void updateTransformationMatrices();
- virtual void updateTransformationMatrices(const Eigen::Matrix4f &globalPose);
+ virtual void updateTransformationMatrices(const Eigen::Matrix4f& globalPose);
virtual bool hasChildNode(const VirtualRobot::RobotNodePtr child, bool recursive = false) const;
virtual void addChildNode(VirtualRobot::RobotNodePtr child);
virtual bool initialize(VirtualRobot::RobotNodePtr parent, bool initializeChildren = false);
virtual void reset();
- virtual void setGlobalPose( const Eigen::Matrix4f &pose );
+ virtual void setGlobalPose(const Eigen::Matrix4f& pose);
virtual void setJointValue(float q, bool updateTransformations = true, bool clampToLimits = true);
SharedRobotNodeInterfacePrx _node;
@@ -143,24 +143,24 @@ namespace armarx
class RemoteRobot : public VirtualRobot::Robot
{
template<class T>
- friend void boost::checked_delete(T *);
+ friend void boost::checked_delete(T*);
protected:
~RemoteRobot();
public:
- RemoteRobot (SharedRobotInterfacePrx robot);
+ RemoteRobot(SharedRobotInterfacePrx robot);
virtual VirtualRobot::RobotNodePtr getRootNode();
- virtual bool hasRobotNode( const std::string &robotNodeName );
- virtual bool hasRobotNode( VirtualRobot::RobotNodePtr );
+ virtual bool hasRobotNode(const std::string& robotNodeName);
+ virtual bool hasRobotNode(VirtualRobot::RobotNodePtr);
- virtual VirtualRobot::RobotNodePtr getRobotNode(const std::string &robotNodeName);
- virtual void getRobotNodes(std::vector< VirtualRobot::RobotNodePtr > &storeNodes, bool clearVector=true);
+ virtual VirtualRobot::RobotNodePtr getRobotNode(const std::string& robotNodeName);
+ virtual void getRobotNodes(std::vector< VirtualRobot::RobotNodePtr >& storeNodes, bool clearVector = true);
- virtual bool hasRobotNodeSet( const std::string &name );
- virtual VirtualRobot::RobotNodeSetPtr getRobotNodeSet(const std::string &nodeSetName);
- virtual void getRobotNodeSets(std::vector<VirtualRobot::RobotNodeSetPtr> &storeNodeSet);
+ virtual bool hasRobotNodeSet(const std::string& name);
+ virtual VirtualRobot::RobotNodeSetPtr getRobotNodeSet(const std::string& nodeSetName);
+ virtual void getRobotNodeSets(std::vector<VirtualRobot::RobotNodeSetPtr>& storeNodeSet);
/**
*
@@ -185,9 +185,9 @@ namespace armarx
In order to get a fully featured robot model you can pass a filename to VirtualRobot::Robot,
but then you need to make sure that the loaded model is identical to the model of the remote robot (otherwise errors will occur).
*/
- static VirtualRobot::RobotPtr createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const std::string &filename=std::string());
+ static VirtualRobot::RobotPtr createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const std::string& filename = std::string());
- static VirtualRobot::RobotPtr createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, const std::string &filename=std::string());
+ static VirtualRobot::RobotPtr createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, const std::string& filename = std::string());
/*!
Use this method to synchronize (i.e. copy the joint values) from the remote robot to the local clone.
@@ -209,7 +209,7 @@ namespace armarx
/// Not implemented yet
virtual VirtualRobot::EndEffectorPtr getEndEffector(const std::string& endEffectorName);
/// Not implemented yet
- virtual void getEndEffectors(std::vector<VirtualRobot::EndEffectorPtr> &storeEEF);
+ virtual void getEndEffectors(std::vector<VirtualRobot::EndEffectorPtr>& storeEEF);
/// Not implemented yet
virtual void setRootNode(VirtualRobot::RobotNodePtr node);
@@ -228,9 +228,9 @@ namespace armarx
* @param globalPose new global pose
* @param applyValues No effect. Will be always applied.
*/
- virtual void setGlobalPose(const Eigen::Matrix4f &globalPose, bool applyValues = true);
+ virtual void setGlobalPose(const Eigen::Matrix4f& globalPose, bool applyValues = true);
- VirtualRobot::RobotNodePtr createLocalNode(SharedRobotNodeInterfacePrx remoteNode, std::vector<VirtualRobot::RobotNodePtr> &allNodes, std::map<VirtualRobot::RobotNodePtr, std::vector<std::string> > &childrenMap, VirtualRobot::RobotPtr robo);
+ VirtualRobot::RobotNodePtr createLocalNode(SharedRobotNodeInterfacePrx remoteNode, std::vector<VirtualRobot::RobotNodePtr>& allNodes, std::map<VirtualRobot::RobotNodePtr, std::vector<std::string> >& childrenMap, VirtualRobot::RobotPtr robo);
protected:
SharedRobotInterfacePrx _robot;
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp
index 700155030c8e50663d48f5d816f48ad3d73f7043..0e08d9dfe215b65a413f7e6303b7c8e044bfb348 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp
@@ -9,186 +9,213 @@
namespace armarx
{
-using namespace std;
-using namespace boost;
-using namespace VirtualRobot;
-using namespace Eigen;
+ using namespace std;
+ using namespace boost;
+ using namespace VirtualRobot;
+ using namespace Eigen;
-RobotNodePtr RemoteRobot::createRemoteRobotNode(SharedRobotNodeInterfacePrx node, RobotPtr robot)
-{
- switch (node->getType()) {
- case ePrismatic:
- return RobotNodePtr(new RemoteRobotNode<RobotNodePrismatic>(node,robot));
- case eRevolute:
- return RobotNodePtr(new RemoteRobotNode<RobotNodeRevolute>(node,robot));
- case eFixed:
- return RobotNodePtr(new RemoteRobotNode<RobotNodeFixed>(node,robot));
- default:
- break; // thow an exception
- }
- return RobotNodePtr();
-}
+ RobotNodePtr RemoteRobot::createRemoteRobotNode(SharedRobotNodeInterfacePrx node, RobotPtr robot)
+ {
+ switch (node->getType())
+ {
+ case ePrismatic:
+ return RobotNodePtr(new RemoteRobotNode<RobotNodePrismatic>(node, robot));
+ case eRevolute:
+ return RobotNodePtr(new RemoteRobotNode<RobotNodeRevolute>(node, robot));
-template<>
-void RemoteRobotNodeInitializer<VirtualRobot::RobotNodeRevolute>::initialize(RemoteRobotNode<VirtualRobot::RobotNodeRevolute>* remoteNode)
-{
- // set rotation axis
- remoteNode->jointRotationAxis = remoteNode->getGlobalPose().block<3,3>(0,0).inverse() * Vector3Ptr::dynamicCast(remoteNode->_node->getJointRotationAxis())->toEigen();
-}
+ case eFixed:
+ return RobotNodePtr(new RemoteRobotNode<RobotNodeFixed>(node, robot));
-template<>
-void RemoteRobotNodeInitializer<VirtualRobot::RobotNodePrismatic>::initialize(RemoteRobotNode<VirtualRobot::RobotNodePrismatic>* remoteNode)
-{
- // set translation direction
- remoteNode->jointTranslationDirection = remoteNode->getGlobalPose().block<3,3>(0,0).inverse() * Vector3Ptr::dynamicCast(remoteNode->_node->getJointTranslationDirection())->toEigen();
-}
+ default:
+ break; // thow an exception
+ }
-template<>
-void RemoteRobotNodeInitializer<VirtualRobot::RobotNodeFixed>::initialize(RemoteRobotNode<VirtualRobot::RobotNodeFixed>* remoteNode)
-{
- // nothing to do for fixed joints
-}
+ return RobotNodePtr();
+ }
-template<class RobotNodeType>
-RemoteRobotNode<RobotNodeType>::~RemoteRobotNode(){
- try
+ template<>
+ void RemoteRobotNodeInitializer<VirtualRobot::RobotNodeRevolute>::initialize(RemoteRobotNode<VirtualRobot::RobotNodeRevolute>* remoteNode)
{
- _node->unref();
+ // set rotation axis
+ remoteNode->jointRotationAxis = remoteNode->getGlobalPose().block<3, 3>(0, 0).inverse() * Vector3Ptr::dynamicCast(remoteNode->_node->getJointRotationAxis())->toEigen();
}
- catch(std::exception &e)
+
+ template<>
+ void RemoteRobotNodeInitializer<VirtualRobot::RobotNodePrismatic>::initialize(RemoteRobotNode<VirtualRobot::RobotNodePrismatic>* remoteNode)
{
- ARMARX_DEBUG_S << "Unref of SharedRobotNode failed: " << e.what();
+ // set translation direction
+ remoteNode->jointTranslationDirection = remoteNode->getGlobalPose().block<3, 3>(0, 0).inverse() * Vector3Ptr::dynamicCast(remoteNode->_node->getJointTranslationDirection())->toEigen();
}
- catch(...)
+
+ template<>
+ void RemoteRobotNodeInitializer<VirtualRobot::RobotNodeFixed>::initialize(RemoteRobotNode<VirtualRobot::RobotNodeFixed>* remoteNode)
{
- ARMARX_DEBUG_S << "Unref of SharedRobotNode failed: reason unknown";
+ // nothing to do for fixed joints
}
-}
-template<class RobotNodeType>
-float RemoteRobotNode<RobotNodeType>::getJointValue() const{
- return _node->getJointValue();
-}
-template<class RobotNodeType>
-float RemoteRobotNode<RobotNodeType>::getJointLimitHi() const
-{
- return _node->getJointLimitHigh();
-}
-template<class RobotNodeType>
-float RemoteRobotNode<RobotNodeType>::getJointLimitLo() const
-{
- return _node->getJointLimitLow();
-}
-/*
-template<class RobotNodeType>
-Eigen::Matrix4f RemoteRobotNode<RobotNodeType>::getPostJointTransformation(){
- return PosePtr::dynamicCast(_node->getPostJointTransformation())->toEigen();
-}*/
-
-template<class RobotNodeType>
-Eigen::Matrix4f RemoteRobotNode<RobotNodeType>::getLocalTransformation(){
- return PosePtr::dynamicCast(_node->getLocalTransformation())->toEigen();
-}
+ template<class RobotNodeType>
+ RemoteRobotNode<RobotNodeType>::~RemoteRobotNode()
+ {
+ try
+ {
+ _node->unref();
+ }
+ catch (std::exception& e)
+ {
+ ARMARX_DEBUG_S << "Unref of SharedRobotNode failed: " << e.what();
+ }
+ catch (...)
+ {
+ ARMARX_DEBUG_S << "Unref of SharedRobotNode failed: reason unknown";
+ }
+ }
-template<class RobotNodeType>
-Eigen::Matrix4f RemoteRobotNode<RobotNodeType>::getGlobalPose() const {
- return FramedPosePtr::dynamicCast(_node->getGlobalPose())->toEigen();
-}
-template<class RobotNodeType>
-Eigen::Matrix4f RemoteRobotNode<RobotNodeType>::getPoseInRootFrame() const
-{
- return FramedPosePtr::dynamicCast(_node->getPoseInRootFrame())->toEigen();
-}
+ template<class RobotNodeType>
+ float RemoteRobotNode<RobotNodeType>::getJointValue() const
+ {
+ return _node->getJointValue();
+ }
-template<class RobotNodeType>
-Eigen::Vector3f RemoteRobotNode<RobotNodeType>::getPositionInRootFrame() const
-{
- Vector3Ptr pos = Vector3Ptr::dynamicCast(_node->getPoseInRootFrame()->position);
- ARMARX_CHECK_EXPRESSION(pos);
- return pos->toEigen();
-}
+ template<class RobotNodeType>
+ float RemoteRobotNode<RobotNodeType>::getJointLimitHi() const
+ {
+ return _node->getJointLimitHigh();
+ }
+ template<class RobotNodeType>
+ float RemoteRobotNode<RobotNodeType>::getJointLimitLo() const
+ {
+ return _node->getJointLimitLow();
+ }
+ /*
+ template<class RobotNodeType>
+ Eigen::Matrix4f RemoteRobotNode<RobotNodeType>::getPostJointTransformation(){
+ return PosePtr::dynamicCast(_node->getPostJointTransformation())->toEigen();
+ }*/
+
+ template<class RobotNodeType>
+ Eigen::Matrix4f RemoteRobotNode<RobotNodeType>::getLocalTransformation()
+ {
+ return PosePtr::dynamicCast(_node->getLocalTransformation())->toEigen();
+ }
-template<class RobotNodeType>
-bool RemoteRobotNode<RobotNodeType>::hasChildNode(const RobotNodePtr child, bool recursive) const{
- return false;
-}
-template<class RobotNodeType>
-bool RemoteRobotNode<RobotNodeType>::hasChildNode(const std::string &child, bool recursive) const{
- return _node->hasChild(child,recursive);
-}
+ template<class RobotNodeType>
+ Eigen::Matrix4f RemoteRobotNode<RobotNodeType>::getGlobalPose() const
+ {
+ return FramedPosePtr::dynamicCast(_node->getGlobalPose())->toEigen();
+ }
+ template<class RobotNodeType>
+ Eigen::Matrix4f RemoteRobotNode<RobotNodeType>::getPoseInRootFrame() const
+ {
+ return FramedPosePtr::dynamicCast(_node->getPoseInRootFrame())->toEigen();
+ }
-template<class RobotNodeType>
-void RemoteRobotNode<RobotNodeType>::setJointLimits(float lo, float hi){
-}
+ template<class RobotNodeType>
+ Eigen::Vector3f RemoteRobotNode<RobotNodeType>::getPositionInRootFrame() const
+ {
+ Vector3Ptr pos = Vector3Ptr::dynamicCast(_node->getPoseInRootFrame()->position);
+ ARMARX_CHECK_EXPRESSION(pos);
+ return pos->toEigen();
+ }
-template<class RobotNodeType>
-vector<RobotNodePtr> RemoteRobotNode<RobotNodeType>::getAllParents( RobotNodeSetPtr rns ){
- NameList nodes = _node->getAllParents(rns->getName());
- vector<RobotNodePtr> result;
- RobotPtr robot = this->robot.lock();
+ template<class RobotNodeType>
+ bool RemoteRobotNode<RobotNodeType>::hasChildNode(const RobotNodePtr child, bool recursive) const
+ {
+ return false;
+ }
+ template<class RobotNodeType>
+ bool RemoteRobotNode<RobotNodeType>::hasChildNode(const std::string& child, bool recursive) const
+ {
+ return _node->hasChild(child, recursive);
+ }
- BOOST_FOREACH(string name, nodes){
- result.push_back(robot->getRobotNode(name));
- }
- return result;
-}
+ template<class RobotNodeType>
+ void RemoteRobotNode<RobotNodeType>::setJointLimits(float lo, float hi)
+ {
+ }
-template<class RobotNodeType>
-RobotNodePtr RemoteRobotNode<RobotNodeType>::getParent(){
- return this->robot.lock()->getRobotNode(_node->getParent());
-}
-/*
-template<class RobotNodeType>
-void RemoteRobotNode<RobotNodeType>::setPostJointTransformation(const Eigen::Matrix4f &trafo){
-}*/
-template<class RobotNodeType>
-void RemoteRobotNode<RobotNodeType>::setLocalTransformation(const Eigen::Matrix4f &trafo){
-}
-template<class RobotNodeType>
-std::vector<std::string> RemoteRobotNode<RobotNodeType>::getChildrenNames() const{
- return _node->getChildren();
-}
-template<class RobotNodeType>
-std::string RemoteRobotNode<RobotNodeType>::getParentName() const{
- return _node->getParent();
-}
-template<class RobotNodeType>
-std::vector<SceneObjectPtr> RemoteRobotNode<RobotNodeType>::getChildren() const{
- NameList nodes = _node->getChildren();
- vector<SceneObjectPtr> result;
- BOOST_FOREACH(string name, nodes){
- result.push_back(this->robot.lock()->getRobotNode(name));
- }
- return result;
-}
+ template<class RobotNodeType>
+ vector<RobotNodePtr> RemoteRobotNode<RobotNodeType>::getAllParents(RobotNodeSetPtr rns)
+ {
+ NameList nodes = _node->getAllParents(rns->getName());
+ vector<RobotNodePtr> result;
+ RobotPtr robot = this->robot.lock();
+
+ BOOST_FOREACH(string name, nodes)
+ {
+ result.push_back(robot->getRobotNode(name));
+ }
+ return result;
+ }
+
+ template<class RobotNodeType>
+ RobotNodePtr RemoteRobotNode<RobotNodeType>::getParent()
+ {
+ return this->robot.lock()->getRobotNode(_node->getParent());
+ }
+ /*
+ template<class RobotNodeType>
+ void RemoteRobotNode<RobotNodeType>::setPostJointTransformation(const Eigen::Matrix4f &trafo){
+ }*/
+ template<class RobotNodeType>
+ void RemoteRobotNode<RobotNodeType>::setLocalTransformation(const Eigen::Matrix4f& trafo)
+ {
+ }
+ template<class RobotNodeType>
+ std::vector<std::string> RemoteRobotNode<RobotNodeType>::getChildrenNames() const
+ {
+ return _node->getChildren();
+ }
+ template<class RobotNodeType>
+ std::string RemoteRobotNode<RobotNodeType>::getParentName() const
+ {
+ return _node->getParent();
+ }
+ template<class RobotNodeType>
+ std::vector<SceneObjectPtr> RemoteRobotNode<RobotNodeType>::getChildren() const
+ {
+ NameList nodes = _node->getChildren();
+ vector<SceneObjectPtr> result;
+ BOOST_FOREACH(string name, nodes)
+ {
+ result.push_back(this->robot.lock()->getRobotNode(name));
+ }
+ return result;
+ }
-template<class RobotNodeType>
-void RemoteRobotNode<RobotNodeType>::updateTransformationMatrices(){
-}
-template<class RobotNodeType>
-void RemoteRobotNode<RobotNodeType>::updateTransformationMatrices(const Eigen::Matrix4f &globalPose){
-}
+ template<class RobotNodeType>
+ void RemoteRobotNode<RobotNodeType>::updateTransformationMatrices()
+ {
+ }
+ template<class RobotNodeType>
+ void RemoteRobotNode<RobotNodeType>::updateTransformationMatrices(const Eigen::Matrix4f& globalPose)
+ {
+ }
-template<class RobotNodeType>
-void RemoteRobotNode<RobotNodeType>::addChildNode(RobotNodePtr child){
-}
-template<class RobotNodeType>
-bool RemoteRobotNode<RobotNodeType>::initialize(RobotNodePtr parent, bool initializeChildren){
- return false;
-}
-template<class RobotNodeType>
-void RemoteRobotNode<RobotNodeType>::reset(){
-}
-template<class RobotNodeType>
-void RemoteRobotNode<RobotNodeType>::setGlobalPose( const Eigen::Matrix4f &pose ){
-}
-template<class RobotNodeType>
-void RemoteRobotNode<RobotNodeType>::setJointValue(float q, bool updateTransformations, bool clampToLimits){
-}
+ template<class RobotNodeType>
+ void RemoteRobotNode<RobotNodeType>::addChildNode(RobotNodePtr child)
+ {
+ }
+ template<class RobotNodeType>
+ bool RemoteRobotNode<RobotNodeType>::initialize(RobotNodePtr parent, bool initializeChildren)
+ {
+ return false;
+ }
+ template<class RobotNodeType>
+ void RemoteRobotNode<RobotNodeType>::reset()
+ {
+ }
+ template<class RobotNodeType>
+ void RemoteRobotNode<RobotNodeType>::setGlobalPose(const Eigen::Matrix4f& pose)
+ {
+ }
+ template<class RobotNodeType>
+ void RemoteRobotNode<RobotNodeType>::setJointValue(float q, bool updateTransformations, bool clampToLimits)
+ {
+ }
}
diff --git a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
index 71470505ab4b0d2c05ed3e48a4206ad308055d08..36713d9ad74c379c9ee248f1aac33964fdb4e913 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
@@ -56,11 +56,11 @@ RobotStateObserver::RobotStateObserver()
void RobotStateObserver::onInitObserver()
{
- // register all checks
- offerConditionCheck("equals", new ConditionCheckEquals());
- offerConditionCheck("inrange", new ConditionCheckInRange());
- offerConditionCheck("larger", new ConditionCheckLarger());
- offerConditionCheck("smaller", new ConditionCheckSmaller());
+ // register all checks
+ offerConditionCheck("equals", new ConditionCheckEquals());
+ offerConditionCheck("inrange", new ConditionCheckInRange());
+ offerConditionCheck("larger", new ConditionCheckLarger());
+ offerConditionCheck("smaller", new ConditionCheckSmaller());
}
void RobotStateObserver::onConnectObserver()
@@ -80,52 +80,68 @@ void RobotStateObserver::onConnectObserver()
void RobotStateObserver::updatePoses()
{
- if(getState() < eManagedIceObjectStarting)
+ if (getState() < eManagedIceObjectStarting)
+ {
return;
- if(!robot)
+ }
+
+ if (!robot)
+ {
return;
+ }
+
ScopedRecursiveLock lock(dataMutex);
ReadLockPtr lock2 = robot->getReadLock();
FramedPoseBaseMap tcpPoses;
std::string rootFrame = robot->getRootNode()->getName();
+
//IceUtil::Time start = IceUtil::Time::now();
- for(unsigned int i=0; i < nodesToReport.size(); i++)
+ for (unsigned int i = 0; i < nodesToReport.size(); i++)
{
VirtualRobot::RobotNodePtr& node = nodesToReport.at(i);
- const std::string &tcpName = node->getName();
- const Eigen::Matrix4f ¤tPose = node->getPoseInRootFrame();
+ const std::string& tcpName = node->getName();
+ const Eigen::Matrix4f& currentPose = node->getPoseInRootFrame();
tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame, robot->getName());
}
+
udpatePoseDatafields(tcpPoses);
}
-void RobotStateObserver::udpatePoseDatafields(const FramedPoseBaseMap &poseMap)
+void RobotStateObserver::udpatePoseDatafields(const FramedPoseBaseMap& poseMap)
{
-// ARMARX_INFO << deactivateSpam() << "new tcp poses reported";
+ // ARMARX_INFO << deactivateSpam() << "new tcp poses reported";
FramedPoseBaseMap::const_iterator it = poseMap.begin();
- for(; it != poseMap.end(); it++)
+
+ for (; it != poseMap.end(); it++)
{
FramedPosePtr vec = FramedPosePtr::dynamicCast(it->second);
- const std::string &tcpName = it->first;
- if(!existsDataField(TCP_POSE_CHANNEL, tcpName))
+ const std::string& tcpName = it->first;
+
+ if (!existsDataField(TCP_POSE_CHANNEL, tcpName))
+ {
offerDataFieldWithDefault(TCP_POSE_CHANNEL, tcpName, Variant(it->second), "Pose of " + tcpName);
+ }
else
+ {
setDataField(TCP_POSE_CHANNEL, tcpName, Variant(it->second));
+ }
+
updateChannel(TCP_POSE_CHANNEL);
- if(!existsChannel(tcpName))
+
+ if (!existsChannel(tcpName))
{
offerChannel(tcpName, "pose components of " + tcpName);
- offerDataFieldWithDefault(tcpName,"x", Variant(vec->position->x), "X axis");
- offerDataFieldWithDefault(tcpName,"y", Variant(vec->position->y), "Y axis");
- offerDataFieldWithDefault(tcpName,"z", Variant(vec->position->z), "Z axis");
- offerDataFieldWithDefault(tcpName,"qx", Variant(vec->orientation->qx), "Quaternion part x");
- offerDataFieldWithDefault(tcpName,"qy", Variant(vec->orientation->qy), "Quaternion part y");
- offerDataFieldWithDefault(tcpName,"qz", Variant(vec->orientation->qz), "Quaternion part z");
- offerDataFieldWithDefault(tcpName,"qw", Variant(vec->orientation->qw), "Quaternion part w");
- offerDataFieldWithDefault(tcpName,"frame", Variant(vec->frame), "Reference Frame");
+ offerDataFieldWithDefault(tcpName, "x", Variant(vec->position->x), "X axis");
+ offerDataFieldWithDefault(tcpName, "y", Variant(vec->position->y), "Y axis");
+ offerDataFieldWithDefault(tcpName, "z", Variant(vec->position->z), "Z axis");
+ offerDataFieldWithDefault(tcpName, "qx", Variant(vec->orientation->qx), "Quaternion part x");
+ offerDataFieldWithDefault(tcpName, "qy", Variant(vec->orientation->qy), "Quaternion part y");
+ offerDataFieldWithDefault(tcpName, "qz", Variant(vec->orientation->qz), "Quaternion part z");
+ offerDataFieldWithDefault(tcpName, "qw", Variant(vec->orientation->qw), "Quaternion part w");
+ offerDataFieldWithDefault(tcpName, "frame", Variant(vec->frame), "Reference Frame");
}
else
{
@@ -140,111 +156,141 @@ void RobotStateObserver::udpatePoseDatafields(const FramedPoseBaseMap &poseMap)
newValues["frame"] = new Variant(vec->frame);
setDataFieldsFlatCopy(tcpName, newValues);
}
+
updateChannel(tcpName);
}
}
-DatafieldRefBasePtr RobotStateObserver::getPoseDatafield(const std::string &nodeName, const Ice::Current &) const
+DatafieldRefBasePtr RobotStateObserver::getPoseDatafield(const std::string& nodeName, const Ice::Current&) const
{
return getDataFieldRef(new DataFieldIdentifier(getName(), TCP_POSE_CHANNEL, nodeName));
}
-void RobotStateObserver::updateNodeVelocities(const NameValueMap &jointVel)
+void RobotStateObserver::updateNodeVelocities(const NameValueMap& jointVel)
{
- if(getState() < eManagedIceObjectStarting)
+ if (getState() < eManagedIceObjectStarting)
+ {
return;
+ }
+
ScopedRecursiveLock lock(dataMutex);
- if(!robot)
+
+ if (!robot)
+ {
return;
+ }
+
ReadLockPtr lock2 = robot->getReadLock();
- if(!velocityReportRobot)
+
+ if (!velocityReportRobot)
+ {
velocityReportRobot = robot->clone(robot->getName());
-// IceUtil::Time start = IceUtil::Time::now();
-// ARMARX_INFO << jointVel; FramedPoseBaseMap tcpPoses;
+ }
+
+ // IceUtil::Time start = IceUtil::Time::now();
+ // ARMARX_INFO << jointVel; FramedPoseBaseMap tcpPoses;
FramedDirectionMap tcpTranslationVelocities;
FramedDirectionMap tcpOrientationVelocities;
std::string rootFrame = robot->getRootNode()->getName();
NameValueMap tempJointAngles = robot->getConfig()->getRobotNodeJointValueMap();
FramedPoseBaseMap tcpPoses;
- for(unsigned int i=0; i < nodesToReport.size(); i++)
+
+ for (unsigned int i = 0; i < nodesToReport.size(); i++)
{
RobotNodePtr node = nodesToReport.at(i);
- const std::string &tcpName = node->getName();
- const Eigen::Matrix4f ¤tPose = node->getPoseInRootFrame();
+ const std::string& tcpName = node->getName();
+ const Eigen::Matrix4f& currentPose = node->getPoseInRootFrame();
tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame, robot->getName());
}
double tDelta = 0.001;
- for(NameValueMap::iterator it = tempJointAngles.begin(); it != tempJointAngles.end(); it++)
+
+ for (NameValueMap::iterator it = tempJointAngles.begin(); it != tempJointAngles.end(); it++)
{
NameValueMap::const_iterator itSrc = jointVel.find(it->first);
- if(itSrc != jointVel.end())
+
+ if (itSrc != jointVel.end())
+ {
it->second += itSrc->second * tDelta;
+ }
}
velocityReportRobot->setJointValues(tempJointAngles);
-// ARMARX_INFO << deactivateSpam(1) << "duration: " << (IceUtil::Time::now()-start).toMicroSecondsDouble();
-// start = IceUtil::Time::now();
+ // ARMARX_INFO << deactivateSpam(1) << "duration: " << (IceUtil::Time::now()-start).toMicroSecondsDouble();
+ // start = IceUtil::Time::now();
Eigen::Matrix4f mat;
Eigen::Vector3f rpy;
- for(unsigned int i=0; i < nodesToReport.size(); i++)
+
+ for (unsigned int i = 0; i < nodesToReport.size(); i++)
{
RobotNodePtr node = velocityReportRobot->getRobotNode(nodesToReport.at(i)->getName());
- const std::string &tcpName = node->getName();
- const Eigen::Matrix4f ¤tPose = node->getPoseInRootFrame();
+ const std::string& tcpName = node->getName();
+ const Eigen::Matrix4f& currentPose = node->getPoseInRootFrame();
FramedPosePtr lastPose = FramedPosePtr::dynamicCast(tcpPoses[tcpName]);
- tcpTranslationVelocities[tcpName] = new FramedDirection((currentPose.block(0,3,3,1) - lastPose->toEigen().block(0,3,3,1))/tDelta, rootFrame, robot->getName());
+ tcpTranslationVelocities[tcpName] = new FramedDirection((currentPose.block(0, 3, 3, 1) - lastPose->toEigen().block(0, 3, 3, 1)) / tDelta, rootFrame, robot->getName());
mat = currentPose * lastPose->toEigen().inverse();
VirtualRobot::MathTools::eigen4f2rpy(mat, rpy);
- tcpOrientationVelocities[tcpName] = new FramedDirection(rpy/tDelta, rootFrame, robot->getName());
+ tcpOrientationVelocities[tcpName] = new FramedDirection(rpy / tDelta, rootFrame, robot->getName());
}
-// ARMARX_INFO << deactivateSpam(1) << "duration2: " << (IceUtil::Time::now()-start).toMicroSecondsDouble();
-// ARMARX_INFO << deactivateSpam(5) << "TCP Pose Vel Calc took: " << ( IceUtil::Time::now() - start).toMilliSecondsDouble();
+
+ // ARMARX_INFO << deactivateSpam(1) << "duration2: " << (IceUtil::Time::now()-start).toMicroSecondsDouble();
+ // ARMARX_INFO << deactivateSpam(5) << "TCP Pose Vel Calc took: " << ( IceUtil::Time::now() - start).toMilliSecondsDouble();
updateVelocityDatafields(tcpTranslationVelocities, tcpOrientationVelocities);
}
-void RobotStateObserver::updateVelocityDatafields(const FramedDirectionMap &tcpTranslationVelocities, const FramedDirectionMap &tcpOrientationVelocities)
+void RobotStateObserver::updateVelocityDatafields(const FramedDirectionMap& tcpTranslationVelocities, const FramedDirectionMap& tcpOrientationVelocities)
{
FramedDirectionMap::const_iterator it = tcpTranslationVelocities.begin();
- for(; it != tcpTranslationVelocities.end(); it++)
+
+ for (; it != tcpTranslationVelocities.end(); it++)
{
FramedDirectionPtr vec = FramedDirectionPtr::dynamicCast(it->second);
FramedDirectionPtr vecOri;
FramedDirectionMap::const_iterator itOri = tcpOrientationVelocities.find(it->first);
- if(itOri != tcpOrientationVelocities.end())
+
+ if (itOri != tcpOrientationVelocities.end())
+ {
vecOri = FramedDirectionPtr::dynamicCast(itOri->second);
- const std::string &tcpName = it->first;
+ }
+
+ const std::string& tcpName = it->first;
ARMARX_CHECK_EXPRESSION(vec->frame == vecOri->frame);
- if(!existsDataField(TCP_TRANS_VELOCITIES_CHANNEL, tcpName))
+ if (!existsDataField(TCP_TRANS_VELOCITIES_CHANNEL, tcpName))
+ {
offerDataFieldWithDefault(TCP_TRANS_VELOCITIES_CHANNEL, tcpName, Variant(it->second), "Pose of " + tcpName);
+ }
else
+ {
setDataField(TCP_TRANS_VELOCITIES_CHANNEL, tcpName, Variant(it->second));
+ }
+
updateChannel(TCP_TRANS_VELOCITIES_CHANNEL);
- const std::string channelName = tcpName+"Velocities";
- if(!existsChannel(channelName))
+ const std::string channelName = tcpName + "Velocities";
+
+ if (!existsChannel(channelName))
{
offerChannel(channelName, "pose components of " + tcpName);
- offerDataFieldWithDefault(channelName,"x", Variant(vec->x), "X axis");
- offerDataFieldWithDefault(channelName,"y", Variant(vec->y), "Y axis");
- offerDataFieldWithDefault(channelName,"z", Variant(vec->z), "Z axis");
- offerDataFieldWithDefault(channelName,"roll", Variant(vecOri->x), "Roll");
- offerDataFieldWithDefault(channelName,"pitch", Variant(vecOri->y), "Pitch");
- offerDataFieldWithDefault(channelName,"yaw", Variant(vecOri->z), "Yaw");
- offerDataFieldWithDefault(channelName,"frame", Variant(vecOri->frame), "Reference Frame");
+ offerDataFieldWithDefault(channelName, "x", Variant(vec->x), "X axis");
+ offerDataFieldWithDefault(channelName, "y", Variant(vec->y), "Y axis");
+ offerDataFieldWithDefault(channelName, "z", Variant(vec->z), "Z axis");
+ offerDataFieldWithDefault(channelName, "roll", Variant(vecOri->x), "Roll");
+ offerDataFieldWithDefault(channelName, "pitch", Variant(vecOri->y), "Pitch");
+ offerDataFieldWithDefault(channelName, "yaw", Variant(vecOri->z), "Yaw");
+ offerDataFieldWithDefault(channelName, "frame", Variant(vecOri->frame), "Reference Frame");
}
else
{
@@ -258,6 +304,7 @@ void RobotStateObserver::updateVelocityDatafields(const FramedDirectionMap &tcpT
newValues["frame"] = new Variant(vec->frame);
setDataFieldsFlatCopy(channelName, newValues);
}
+
updateChannel(channelName);
}
@@ -279,15 +326,19 @@ void RobotStateObserver::setRobot(RobotPtr robot)
robotNodes = robot->getRobotNodeSets();
std::string nodesetsString = getProperty<std::string>("TCPsToReport").getValue();
- if(!nodesetsString.empty())
+
+ if (!nodesetsString.empty())
{
- if(nodesetsString=="*")
+ if (nodesetsString == "*")
{
auto nodesets = robot->getRobotNodeSets();
- for(RobotNodeSetPtr& set : nodesets)
+
+ for (RobotNodeSetPtr& set : nodesets)
{
- if(set->getTCP())
+ if (set->getTCP())
+ {
nodesToReport.push_back(set->getTCP());
+ }
}
}
else
@@ -297,15 +348,20 @@ void RobotStateObserver::setRobot(RobotPtr robot)
nodesetsString,
boost::is_any_of(","),
boost::token_compress_on);
- for(auto name : nodesetNames)
+
+ for (auto name : nodesetNames)
{
boost::trim(name);
auto node = robot->getRobotNode(name);
- if(node)
+ if (node)
+ {
nodesToReport.push_back(node);
+ }
else
+ {
ARMARX_ERROR << "Could not find node set with name: " << name;
+ }
}
}
}
diff --git a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h
index e114de0ca2691cbe7d9f4c5a4e01ff2cc0f88b66..2c4eae0066b81b1f61ca99cb9c2777254fb4a47d 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h
+++ b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h
@@ -49,10 +49,10 @@ namespace armarx
* RobotStatePropertyDefinition Property Definitions
*/
class RobotStateObserverPropertyDefinitions:
- public ComponentPropertyDefinitions
+ public ComponentPropertyDefinitions
{
public:
- RobotStateObserverPropertyDefinitions(std::string prefix):
+ RobotStateObserverPropertyDefinitions(std::string prefix):
ComponentPropertyDefinitions(prefix)
{
defineOptionalProperty<std::string>("TCPsToReport", "", "comma seperated list of nodesets' endeffectors, which poses and velocities that should be reported. * for all, empty for none");
@@ -61,48 +61,51 @@ namespace armarx
typedef ::std::map< ::std::string, ::armarx::FramedPoseBasePtr> FramedPoseBaseMap;
- /**
- * ArmarX RobotStateObserver.
- *
- * The RobotStateObserver allows to install conditions on all channel reported by the KinematicUnit.
- * These include joint angles, velocities, torques and motor temperatures
- *
- * The RobotStateObserver retrieves its configuration from a VirtualRobot robot model. Within the model, the joints
- * which are observer by the unit are define by a robotnodeset
- */
+ /**
+ * ArmarX RobotStateObserver.
+ *
+ * The RobotStateObserver allows to install conditions on all channel reported by the KinematicUnit.
+ * These include joint angles, velocities, torques and motor temperatures
+ *
+ * The RobotStateObserver retrieves its configuration from a VirtualRobot robot model. Within the model, the joints
+ * which are observer by the unit are define by a robotnodeset
+ */
class ARMARXCORE_IMPORT_EXPORT RobotStateObserver :
virtual public Observer,
virtual public RobotStateObserverInterface
{
public:
RobotStateObserver();
- // framework hooks
- void onInitObserver();
- void onConnectObserver();
+ // framework hooks
+ void onInitObserver();
+ void onConnectObserver();
virtual PropertyDefinitionsPtr createPropertyDefinitions();
void setRobot(VirtualRobot::RobotPtr robot);
- virtual std::string getDefaultName() const { return "RobotStateObserver"; }
+ virtual std::string getDefaultName() const
+ {
+ return "RobotStateObserver";
+ }
void updatePoses();
- void updateNodeVelocities(const NameValueMap &jointVel);
+ void updateNodeVelocities(const NameValueMap& jointVel);
protected:
- void updateVelocityDatafields(const FramedDirectionMap &tcpTranslationVelocities, const FramedDirectionMap &tcpOrientationVelocities);
+ void updateVelocityDatafields(const FramedDirectionMap& tcpTranslationVelocities, const FramedDirectionMap& tcpOrientationVelocities);
- void udpatePoseDatafields(const FramedPoseBaseMap &poseMap);
+ void udpatePoseDatafields(const FramedPoseBaseMap& poseMap);
private:
- std::string robotNodeSetName;
- RobotStateComponentInterfacePrx server;
+ std::string robotNodeSetName;
+ RobotStateComponentInterfacePrx server;
VirtualRobot::RobotPtr robot, velocityReportRobot;
std::vector<VirtualRobot::RobotNodePtr > nodesToReport;
RecursiveMutex dataMutex;
// RobotStateObserverInterface interface
public:
- DatafieldRefBasePtr getPoseDatafield(const std::string &nodeName, const Ice::Current &) const;
+ DatafieldRefBasePtr getPoseDatafield(const std::string& nodeName, const Ice::Current&) const;
};
typedef IceInternal::Handle<RobotStateObserver> RobotStateObserverPtr;
diff --git a/source/RobotAPI/libraries/core/remoterobot/test/ArmarPoseTest.cpp b/source/RobotAPI/libraries/core/remoterobot/test/ArmarPoseTest.cpp
index bdaf225f068cadc4ecc6edb6760056b252ec9cda..0c7f49a96a80e183ac3c9c84c3614e8b003b15a1 100644
--- a/source/RobotAPI/libraries/core/remoterobot/test/ArmarPoseTest.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/test/ArmarPoseTest.cpp
@@ -40,8 +40,9 @@ BOOST_AUTO_TEST_CASE(complexVariantToDict)
pos->z = 3;
pos->frame = "Base";
VariantPtr var = new Variant(pos);
- auto dict = json->ConvertToBasicVariantMap(json,var);
- for(auto e : dict)
+ auto dict = json->ConvertToBasicVariantMap(json, var);
+
+ for (auto e : dict)
{
ARMARX_INFO_S << e.first << ": " << *VariantPtr::dynamicCast(e.second);
}
diff --git a/source/RobotAPI/libraries/drivers/WeissHapticSensor/AbstractInterface.cpp b/source/RobotAPI/libraries/drivers/WeissHapticSensor/AbstractInterface.cpp
index b450fb668f81c4bc9bc73986211bb46d92ef2733..d043c16bb07af22398afd2a338be706ed3b557c0 100644
--- a/source/RobotAPI/libraries/drivers/WeissHapticSensor/AbstractInterface.cpp
+++ b/source/RobotAPI/libraries/drivers/WeissHapticSensor/AbstractInterface.cpp
@@ -19,11 +19,11 @@ AbstractInterface::~AbstractInterface()
{
}
-int AbstractInterface::read(unsigned char *buf, unsigned int len)
+int AbstractInterface::read(unsigned char* buf, unsigned int len)
{
int res = readInternal(buf, len);
- if(log != NULL)
+ if (log != NULL)
{
log->logRead(buf, res);
}
@@ -31,9 +31,9 @@ int AbstractInterface::read(unsigned char *buf, unsigned int len)
return res;
}
-int AbstractInterface::write(unsigned char *buf, unsigned int len)
+int AbstractInterface::write(unsigned char* buf, unsigned int len)
{
- if(log != NULL)
+ if (log != NULL)
{
log->logWrite(buf, len);
}
@@ -42,26 +42,27 @@ int AbstractInterface::write(unsigned char *buf, unsigned int len)
}
-Response AbstractInterface::submitCmd( unsigned char id, unsigned char *payload, unsigned int len, bool pending )
+Response AbstractInterface::submitCmd(unsigned char id, unsigned char* payload, unsigned int len, bool pending)
{
fireAndForgetCmd(id, payload, len, pending);
return receive(pending, id);
}
-void AbstractInterface::fireAndForgetCmd( unsigned char id, unsigned char *payload, unsigned int len, bool pending )
+void AbstractInterface::fireAndForgetCmd(unsigned char id, unsigned char* payload, unsigned int len, bool pending)
{
int res;
// Check if we're connected
- if ( !connected )
+ if (!connected)
{
throw TransmissionException("Interface not connected");
}
// Send command
- res = send( id, len, payload );
- if ( res < 0 )
+ res = send(id, len, payload);
+
+ if (res < 0)
{
throw TransmissionException("Message send failed");
}
@@ -74,7 +75,7 @@ void AbstractInterface::startLogging(std::string file)
void AbstractInterface::logText(std::string message)
{
- if(log != NULL)
+ if (log != NULL)
{
log->logText(message);
}
@@ -87,13 +88,14 @@ Response AbstractInterface::receiveWithoutChecks()
msg_t msg;
// Receive response data
- res = receive( &msg );
- if ( res < 0 )
+ res = receive(&msg);
+
+ if (res < 0)
{
throw TransmissionException("Message receive failed");
}
- status = (status_t) make_short( msg.data[0], msg.data[1] );
+ status = (status_t) make_short(msg.data[0], msg.data[1]);
return Response(res, msg.id, status, msg.data, msg.len);
}
@@ -108,14 +110,15 @@ Response AbstractInterface::receive(bool pending, unsigned char expectedId)
do
{
// Receive response data
- res = receive( &msg );
- if ( res < 0 )
+ res = receive(&msg);
+
+ if (res < 0)
{
throw TransmissionException("Message receive failed");
}
// Check response ID
- if ( msg.id != expectedId )
+ if (msg.id != expectedId)
{
//std::strstream strStream;
//strStream << "Response ID ()" << msg.id << ") does not match submitted command ID (" << id << ")";
@@ -123,37 +126,39 @@ Response AbstractInterface::receive(bool pending, unsigned char expectedId)
throw TransmissionException(str(boost::format("Response ID (%02X) does not match submitted command ID (%02X)") % (int)msg.id % (int)expectedId));
}
- if ( pending )
+ if (pending)
{
- if ( msg.len < 2 )
+ if (msg.len < 2)
{
throw TransmissionException("No status code received");
}
- status = (status_t) make_short( msg.data[0], msg.data[1] );
+ status = (status_t) make_short(msg.data[0], msg.data[1]);
}
}
- while( pending && status == E_CMD_PENDING );
+ while (pending && status == E_CMD_PENDING);
- status = (status_t) make_short( msg.data[0], msg.data[1] );
+ status = (status_t) make_short(msg.data[0], msg.data[1]);
return Response(res, msg.id, status, msg.data, msg.len);
}
-int AbstractInterface::receive( msg_t *msg )
+int AbstractInterface::receive(msg_t* msg)
{
int res;
- unsigned char header[3]; // 1 byte command, 2 bytes payload length
- unsigned short checksum = 0x50f5; // Checksum over preamble (0xaa 0xaa 0xaa)
+ unsigned char header[3]; // 1 byte command, 2 bytes payload length
+ unsigned short checksum = 0x50f5; // Checksum over preamble (0xaa 0xaa 0xaa)
unsigned int sync;
logText("read preamble");
// Syncing - necessary for compatibility with serial interface
sync = 0;
- while( sync != MSG_PREAMBLE_LEN )
+
+ while (sync != MSG_PREAMBLE_LEN)
{
- res = read( header, 1 );
- if ( res > 0 && header[0] == MSG_PREAMBLE_BYTE )
+ res = read(header, 1);
+
+ if (res > 0 && header[0] == MSG_PREAMBLE_BYTE)
{
sync++;
}
@@ -164,20 +169,21 @@ int AbstractInterface::receive( msg_t *msg )
}
// Read header
- res = read( header, 3 );
- if ( res < 3 )
+ res = read(header, 3);
+
+ if (res < 3)
{
throw TransmissionException(str(boost::format("Failed to receive header data (%d bytes read)") % res));
}
// Calculate checksum over header
- checksum = Checksum::Update_crc16( header, 3, checksum );
+ checksum = Checksum::Update_crc16(header, 3, checksum);
// Get message id of received
msg->id = header[0];
// Get payload size of received message
- msg->len = make_short( header[1], header[2] );
+ msg->len = make_short(header[1], header[2]);
// Allocate space for payload and checksum
//msg->data.resize( msg->len + 2u );
@@ -210,7 +216,8 @@ int AbstractInterface::receive( msg_t *msg )
// Read payload and checksum:
// payload: msg->len
// checksum: 2 byte
- int read = this->read( data, msg->len + 2 );
+ int read = this->read(data, msg->len + 2);
+
if (read != (int)msg->len + 2)
{
delete[] data;
@@ -227,8 +234,9 @@ int AbstractInterface::receive( msg_t *msg )
}*/
// Check checksum
- checksum = Checksum::Update_crc16( data, msg->len + 2, checksum );
- if ( checksum != 0 )
+ checksum = Checksum::Update_crc16(data, msg->len + 2, checksum);
+
+ if (checksum != 0)
{
delete[] data;
logText("CHECKSUM ERROR");
@@ -241,7 +249,7 @@ int AbstractInterface::receive( msg_t *msg )
return msg->len + 8;
}
-int AbstractInterface::send(unsigned char id, unsigned int len, unsigned char *data)
+int AbstractInterface::send(unsigned char id, unsigned int len, unsigned char* data)
{
unsigned char header[MSG_PREAMBLE_LEN + 3];
unsigned short crc;
@@ -250,29 +258,33 @@ int AbstractInterface::send(unsigned char id, unsigned int len, unsigned char *d
logText("write preamble");
// Preamble
- for ( i = 0; i < MSG_PREAMBLE_LEN; i++ ) header[i] = MSG_PREAMBLE_BYTE;
+ for (i = 0; i < MSG_PREAMBLE_LEN; i++)
+ {
+ header[i] = MSG_PREAMBLE_BYTE;
+ }
// Command ID
header[MSG_PREAMBLE_LEN] = id;
// Length
- header[MSG_PREAMBLE_LEN + 1] = lo( len );
- header[MSG_PREAMBLE_LEN + 2] = hi( len );
+ header[MSG_PREAMBLE_LEN + 1] = lo(len);
+ header[MSG_PREAMBLE_LEN + 2] = hi(len);
// Checksum
- crc = Checksum::Crc16( header, 6 );
- crc = Checksum::Update_crc16( data, len, crc );
+ crc = Checksum::Crc16(header, 6);
+ crc = Checksum::Update_crc16(data, len, crc);
+
+ unsigned char* buf = new unsigned char[ 6 + len + 2 ];
+ memcpy(buf, header, 6);
+ memcpy(buf + 6, data, len);
+ memcpy(buf + 6 + len, (unsigned char*) &crc, 2);
- unsigned char *buf = new unsigned char[ 6 + len + 2 ];
- memcpy( buf, header, 6 );
- memcpy( buf + 6, data, len );
- memcpy( buf + 6 + len, (unsigned char *) &crc, 2 );
+ res = write(buf, 6 + len + 2);
- res = write( buf, 6 + len + 2 );
- if ( res < 6 + (int)len + 2 )
+ if (res < 6 + (int)len + 2)
{
- delete[] buf;
+ delete[] buf;
close();
throw TransmissionException("Failed to submit message checksum");
//cerr << "Failed to submit message checksum" << endl;
@@ -288,6 +300,7 @@ int AbstractInterface::send(unsigned char id, unsigned int len, unsigned char *d
-std::ostream& operator<<(std::ostream &strm, const AbstractInterface &a) {
- return strm << a.toString();
+std::ostream& operator<<(std::ostream& strm, const AbstractInterface& a)
+{
+ return strm << a.toString();
}
diff --git a/source/RobotAPI/libraries/drivers/WeissHapticSensor/AbstractInterface.h b/source/RobotAPI/libraries/drivers/WeissHapticSensor/AbstractInterface.h
index 19e45a9cc1821573cd222cd4f1c53b2894c640af..b148061e0c0701d48d080bf3ba192c27fb33f0a3 100644
--- a/source/RobotAPI/libraries/drivers/WeissHapticSensor/AbstractInterface.h
+++ b/source/RobotAPI/libraries/drivers/WeissHapticSensor/AbstractInterface.h
@@ -7,18 +7,18 @@
#include <boost/shared_ptr.hpp>
-#define MSG_PREAMBLE_BYTE 0xaa
-#define MSG_PREAMBLE_LEN 3
+#define MSG_PREAMBLE_BYTE 0xaa
+#define MSG_PREAMBLE_LEN 3
// Combine bytes to different types
-#define make_short( lowbyte, highbyte ) ( (unsigned short)lowbyte | ( (unsigned short)highbyte << 8 ) )
-#define make_signed_short( lowbyte, highbyte ) ( (signed short) ( (unsigned short) lowbyte | ( (unsigned short) highbyte << 8 ) ) )
-#define make_int( lowbyte, mid1, mid2, highbyte ) ( (unsigned int) lowbyte | ( (unsigned int) mid1 << 8 ) | ( (unsigned int) mid2 << 16 ) | ( (unsigned int) highbyte << 24 ) )
-#define make_float( result, byteptr ) memcpy( &result, byteptr, sizeof( float ) )
+#define make_short( lowbyte, highbyte ) ( (unsigned short)lowbyte | ( (unsigned short)highbyte << 8 ) )
+#define make_signed_short( lowbyte, highbyte ) ( (signed short) ( (unsigned short) lowbyte | ( (unsigned short) highbyte << 8 ) ) )
+#define make_int( lowbyte, mid1, mid2, highbyte ) ( (unsigned int) lowbyte | ( (unsigned int) mid1 << 8 ) | ( (unsigned int) mid2 << 16 ) | ( (unsigned int) highbyte << 24 ) )
+#define make_float( result, byteptr ) memcpy( &result, byteptr, sizeof( float ) )
// Byte access
-#define hi( x ) (unsigned char) ( ((x) >> 8) & 0xff ) // Returns the upper byte of the passed short
-#define lo( x ) (unsigned char) ( (x) & 0xff ) // Returns the lower byte of the passed short
+#define hi( x ) (unsigned char) ( ((x) >> 8) & 0xff ) // Returns the upper byte of the passed short
+#define lo( x ) (unsigned char) ( (x) & 0xff ) // Returns the lower byte of the passed short
@@ -31,19 +31,22 @@ public:
virtual ~AbstractInterface();
virtual int open() = 0;
virtual void close() = 0;
- int read( unsigned char *buf, unsigned int len);
- int write( unsigned char *buf, unsigned int len);
+ int read(unsigned char* buf, unsigned int len);
+ int write(unsigned char* buf, unsigned int len);
- bool IsConnected() const { return connected; }
+ bool IsConnected() const
+ {
+ return connected;
+ }
virtual std::string toString() const = 0;
- int send(unsigned char id, unsigned int len, unsigned char *data);
- int receive( msg_t *msg );
- Response submitCmd( unsigned char id, unsigned char *payload, unsigned int len, bool pending );
+ int send(unsigned char id, unsigned int len, unsigned char* data);
+ int receive(msg_t* msg);
+ Response submitCmd(unsigned char id, unsigned char* payload, unsigned int len, bool pending);
Response receive(bool pending, unsigned char expectedId);
Response receiveWithoutChecks();
- void fireAndForgetCmd( unsigned char id, unsigned char *payload, unsigned int len, bool pending );
+ void fireAndForgetCmd(unsigned char id, unsigned char* payload, unsigned int len, bool pending);
void startLogging(std::string file);
void logText(std::string message);
@@ -51,15 +54,15 @@ public:
protected:
bool connected;
- virtual int readInternal( unsigned char *buf, unsigned int len) = 0;
- virtual int writeInternal( unsigned char *buf, unsigned int len) = 0;
+ virtual int readInternal(unsigned char* buf, unsigned int len) = 0;
+ virtual int writeInternal(unsigned char* buf, unsigned int len) = 0;
private:
friend std::ostream& operator<<(std::ostream&, const AbstractInterface&);
boost::shared_ptr<BinaryLogger> log;
};
-std::ostream& operator<<(std::ostream &strm, const AbstractInterface &a);
+std::ostream& operator<<(std::ostream& strm, const AbstractInterface& a);
#endif // ABSTRACTINTERFACE_H
diff --git a/source/RobotAPI/libraries/drivers/WeissHapticSensor/BinaryLogger.cpp b/source/RobotAPI/libraries/drivers/WeissHapticSensor/BinaryLogger.cpp
index 40c39409735c5b9fefd71983e83294bc19d6c9ca..9142b7349a5bcb8b8260d101d4e3baed9568d50f 100644
--- a/source/RobotAPI/libraries/drivers/WeissHapticSensor/BinaryLogger.cpp
+++ b/source/RobotAPI/libraries/drivers/WeissHapticSensor/BinaryLogger.cpp
@@ -11,24 +11,28 @@ BinaryLogger::~BinaryLogger()
log.close();
}
-void BinaryLogger::logRead(unsigned char *buf, unsigned int len)
+void BinaryLogger::logRead(unsigned char* buf, unsigned int len)
{
log << "READ";
- for(unsigned int i = 0; i < len; i++)
+
+ for (unsigned int i = 0; i < len; i++)
{
log << boost::format(" %02X") % (int)buf[i];
}
+
log << std::endl;
log.flush();
}
-void BinaryLogger::logWrite(unsigned char *buf, unsigned int len)
+void BinaryLogger::logWrite(unsigned char* buf, unsigned int len)
{
log << "WRITE";
- for(unsigned int i = 0; i < len; i++)
+
+ for (unsigned int i = 0; i < len; i++)
{
log << boost::format(" %02X") % (int)buf[i];
}
+
log << std::endl;
log.flush();
}
diff --git a/source/RobotAPI/libraries/drivers/WeissHapticSensor/BinaryLogger.h b/source/RobotAPI/libraries/drivers/WeissHapticSensor/BinaryLogger.h
index 535786e85c1b1f02e3fd12dc3fda398cdd239654..e48ce78dc925f224852fce4002aa8089a45af474 100644
--- a/source/RobotAPI/libraries/drivers/WeissHapticSensor/BinaryLogger.h
+++ b/source/RobotAPI/libraries/drivers/WeissHapticSensor/BinaryLogger.h
@@ -9,8 +9,8 @@ public:
BinaryLogger(std::string filename);
~BinaryLogger();
- void logRead(unsigned char *buf, unsigned int len);
- void logWrite(unsigned char *buf, unsigned int len);
+ void logRead(unsigned char* buf, unsigned int len);
+ void logWrite(unsigned char* buf, unsigned int len);
void logText(std::string message);
private:
diff --git a/source/RobotAPI/libraries/drivers/WeissHapticSensor/CalibrationHelper.cpp b/source/RobotAPI/libraries/drivers/WeissHapticSensor/CalibrationHelper.cpp
index 244edebedae87da6a36d37eff64a6bd955d0c682..0497f62321d65cb6b724e257ff3fd466fb15acc2 100644
--- a/source/RobotAPI/libraries/drivers/WeissHapticSensor/CalibrationHelper.cpp
+++ b/source/RobotAPI/libraries/drivers/WeissHapticSensor/CalibrationHelper.cpp
@@ -17,7 +17,7 @@ void CalibrationHelper::addNoiseSample(Eigen::MatrixXf data)
bool CalibrationHelper::addMaxValueSample(Eigen::MatrixXf data)
{
- if(data.maxCoeff() <= noiseThreshold)
+ if (data.maxCoeff() <= noiseThreshold)
{
this->maximumValues = this->maximumValues.cwiseMax(data);
return true;
@@ -50,15 +50,17 @@ int CalibrationHelper::getNoiseSampleCount()
Eigen::MatrixXf CalibrationHelper::getMatrixAverage(std::vector<Eigen::MatrixXf> samples)
{
- if(samples.size() == 0)
+ if (samples.size() == 0)
{
throw std::runtime_error("Average of zero samples not possible");
}
Eigen::MatrixXf sum = samples.at(0);
- for(std::vector<Eigen::MatrixXf>::iterator it = samples.begin() + 1; it != samples.end(); ++it)
+
+ for (std::vector<Eigen::MatrixXf>::iterator it = samples.begin() + 1; it != samples.end(); ++it)
{
sum += *it;
}
+
return sum / (float)samples.size();
}
diff --git a/source/RobotAPI/libraries/drivers/WeissHapticSensor/Checksum.cpp b/source/RobotAPI/libraries/drivers/WeissHapticSensor/Checksum.cpp
index 15b3cecfff3a5073d476ce5c544f6ad617696859..44ebd21ff0fa2cbfd03f22e32d4c5670349c4f3c 100644
--- a/source/RobotAPI/libraries/drivers/WeissHapticSensor/Checksum.cpp
+++ b/source/RobotAPI/libraries/drivers/WeissHapticSensor/Checksum.cpp
@@ -8,7 +8,8 @@
* corresponding to x^16 + x^12 + x^5 + 1
*/
-const unsigned short Checksum::CRC_TABLE_CCITT16[256] = {
+const unsigned short Checksum::CRC_TABLE_CCITT16[256] =
+{
0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7,
0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef,
0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6,
@@ -59,15 +60,16 @@ const unsigned short Checksum::CRC_TABLE_CCITT16[256] = {
* @return CRC16 checksum
*/
-unsigned short Checksum::Update_crc16( unsigned char *data, unsigned int size, unsigned short crc )
+unsigned short Checksum::Update_crc16(unsigned char* data, unsigned int size, unsigned short crc)
{
unsigned long c;
/* process each byte prior to checksum field */
- for ( c=0; c < size; c++ )
+ for (c = 0; c < size; c++)
{
- crc = CRC_TABLE_CCITT16[ ( crc ^ *( data ++ )) & 0x00FF ] ^ ( crc >> 8 );
+ crc = CRC_TABLE_CCITT16[(crc ^ * (data ++)) & 0x00FF ] ^ (crc >> 8);
}
+
return crc;
}
@@ -80,15 +82,15 @@ unsigned short Checksum::Update_crc16( unsigned char *data, unsigned int size, u
* Note: The checksum generated by this function is NOT according
* to CCITT standard!
*
- * @param *data Points to the byte array from which checksum should
- * be calculated
- * @param size Size of the byte array
+ * @param *data Points to the byte array from which checksum should
+ * be calculated
+ * @param size Size of the byte array
*
* @return CRC16 checksum
*/
-unsigned short Checksum::Crc16( unsigned char *data, unsigned int size )
+unsigned short Checksum::Crc16(unsigned char* data, unsigned int size)
{
- return( Update_crc16( data, size, 0xffff ) );
+ return (Update_crc16(data, size, 0xffff));
}
diff --git a/source/RobotAPI/libraries/drivers/WeissHapticSensor/Checksum.h b/source/RobotAPI/libraries/drivers/WeissHapticSensor/Checksum.h
index 7603e1b69f4a71856b801a716c2454e18024ffca..2561f727b247456d73eef711a053220f96b3ecf6 100644
--- a/source/RobotAPI/libraries/drivers/WeissHapticSensor/Checksum.h
+++ b/source/RobotAPI/libraries/drivers/WeissHapticSensor/Checksum.h
@@ -4,8 +4,8 @@
class Checksum
{
public:
- static unsigned short Crc16( unsigned char *data, unsigned int size );
- static unsigned short Update_crc16( unsigned char *data, unsigned int size, unsigned short crc );
+ static unsigned short Crc16(unsigned char* data, unsigned int size);
+ static unsigned short Update_crc16(unsigned char* data, unsigned int size, unsigned short crc);
private:
static const unsigned short CRC_TABLE_CCITT16[256];
diff --git a/source/RobotAPI/libraries/drivers/WeissHapticSensor/Response.h b/source/RobotAPI/libraries/drivers/WeissHapticSensor/Response.h
index b63cf1cd1018471e90ebac3643de3b73e9c5d16c..a6ea680edea82f600fa4e52349343ecb215869ba 100644
--- a/source/RobotAPI/libraries/drivers/WeissHapticSensor/Response.h
+++ b/source/RobotAPI/libraries/drivers/WeissHapticSensor/Response.h
@@ -9,19 +9,20 @@
#include <vector>
#include <ArmarXCore/core/logging/Logging.h>
-struct Response {
+struct Response
+{
public:
- Response(int res, unsigned char cmdId, status_t status, const std::vector<unsigned char> &data, unsigned int len)
+ Response(int res, unsigned char cmdId, status_t status, const std::vector<unsigned char>& data, unsigned int len)
: res(res), cmdId(cmdId), status(status), data(data), len(len) {}
unsigned int getUInt(int index)
{
- return (unsigned int)data[index] | ( (unsigned int)data[index + 1] << 8) | ( (unsigned int)data[index + 2] << 16) | ( (unsigned int)data[index + 3] << 24);
+ return (unsigned int)data[index] | ((unsigned int)data[index + 1] << 8) | ((unsigned int)data[index + 2] << 16) | ((unsigned int)data[index + 3] << 24);
}
unsigned short getShort(int index)
{
- return (unsigned short)data[index] | ( (unsigned short)data[index + 1] << 8);
+ return (unsigned short)data[index] | ((unsigned short)data[index + 1] << 8);
}
unsigned char getByte(int index)
{
@@ -30,7 +31,8 @@ public:
void ensureMinLength(int len)
{
- if ( res < len ) {
+ if (res < len)
+ {
//std::strstream strStream;
//strStream << "Response length is too short, should be = " << len << " (is " << res << ")";
//throw std::runtime_error(strStream.str());
@@ -40,18 +42,21 @@ public:
void ensureSuccess()
{
- if ( status != E_SUCCESS )
+ if (status != E_SUCCESS)
{
//std::strstream strStream;
//strStream << "Command not successful: " << status_to_str( status );
//throw std::runtime_error(strStream.str());
std::stringstream ss;
ss << " status != E_SUCCESS";
- for(int i=0; i<(int)len; i++){
+
+ for (int i = 0; i < (int)len; i++)
+ {
ss << boost::format("%02X ") % (int)data[i];
}
+
ARMARX_ERROR_S << ss.str();
- throw TransmissionException(str(boost::format("Command not successful: %1% (0x%2$02X)") % status_to_str( status ) % status));
+ throw TransmissionException(str(boost::format("Command not successful: %1% (0x%2$02X)") % status_to_str(status) % status));
}
}
@@ -61,42 +66,105 @@ public:
std::vector<unsigned char> data;
unsigned int len;
- static const char* status_to_str( status_t status )
+ static const char* status_to_str(status_t status)
{
- switch( status )
+ switch (status)
{
- case E_SUCCESS: return( "No error" );
- case E_NOT_AVAILABLE: return( "Service or data is not available" );
- case E_NO_SENSOR: return( "No sensor connected" );
- case E_NOT_INITIALIZED: return( "The device is not initialized" );
- case E_ALREADY_RUNNING: return( "Service is already running" );
- case E_FEATURE_NOT_SUPPORTED: return( "The requested feature is not supported" );
- case E_INCONSISTENT_DATA: return( "One or more dependent parameters mismatch" );
- case E_TIMEOUT: return( "Timeout error" );
- case E_READ_ERROR: return( "Error while reading from a device" );
- case E_WRITE_ERROR: return( "Error while writing to a device" );
- case E_INSUFFICIENT_RESOURCES: return( "No memory available" );
- case E_CHECKSUM_ERROR: return( "Checksum error" );
- case E_NO_PARAM_EXPECTED: return( "No parameters expected" );
- case E_NOT_ENOUGH_PARAMS: return( "Not enough parameters" );
- case E_CMD_UNKNOWN: return( "Unknown command" );
- case E_CMD_FORMAT_ERROR: return( "Command format error" );
- case E_ACCESS_DENIED: return( "Access denied" );
- case E_ALREADY_OPEN: return( "Interface already open" );
- case E_CMD_FAILED: return( "Command failed" );
- case E_CMD_ABORTED: return( "Command aborted" );
- case E_INVALID_HANDLE: return( "Invalid handle" );
- case E_NOT_FOUND: return( "Device not found" );
- case E_NOT_OPEN: return( "Device not open" );
- case E_IO_ERROR: return( "General I/O-Error" );
- case E_INVALID_PARAMETER: return( "Invalid parameter" );
- case E_INDEX_OUT_OF_BOUNDS: return( "Index out of bounds" );
- case E_CMD_PENDING: return( "Command is pending..." );
- case E_OVERRUN: return( "Data overrun" );
- case E_RANGE_ERROR: return( "Value out of range" );
- case E_AXIS_BLOCKED: return( "Axis is blocked" );
- case E_FILE_EXISTS: return( "File already exists" );
- default: return( "Internal error. Unknown error code." );
+ case E_SUCCESS:
+ return ("No error");
+
+ case E_NOT_AVAILABLE:
+ return ("Service or data is not available");
+
+ case E_NO_SENSOR:
+ return ("No sensor connected");
+
+ case E_NOT_INITIALIZED:
+ return ("The device is not initialized");
+
+ case E_ALREADY_RUNNING:
+ return ("Service is already running");
+
+ case E_FEATURE_NOT_SUPPORTED:
+ return ("The requested feature is not supported");
+
+ case E_INCONSISTENT_DATA:
+ return ("One or more dependent parameters mismatch");
+
+ case E_TIMEOUT:
+ return ("Timeout error");
+
+ case E_READ_ERROR:
+ return ("Error while reading from a device");
+
+ case E_WRITE_ERROR:
+ return ("Error while writing to a device");
+
+ case E_INSUFFICIENT_RESOURCES:
+ return ("No memory available");
+
+ case E_CHECKSUM_ERROR:
+ return ("Checksum error");
+
+ case E_NO_PARAM_EXPECTED:
+ return ("No parameters expected");
+
+ case E_NOT_ENOUGH_PARAMS:
+ return ("Not enough parameters");
+
+ case E_CMD_UNKNOWN:
+ return ("Unknown command");
+
+ case E_CMD_FORMAT_ERROR:
+ return ("Command format error");
+
+ case E_ACCESS_DENIED:
+ return ("Access denied");
+
+ case E_ALREADY_OPEN:
+ return ("Interface already open");
+
+ case E_CMD_FAILED:
+ return ("Command failed");
+
+ case E_CMD_ABORTED:
+ return ("Command aborted");
+
+ case E_INVALID_HANDLE:
+ return ("Invalid handle");
+
+ case E_NOT_FOUND:
+ return ("Device not found");
+
+ case E_NOT_OPEN:
+ return ("Device not open");
+
+ case E_IO_ERROR:
+ return ("General I/O-Error");
+
+ case E_INVALID_PARAMETER:
+ return ("Invalid parameter");
+
+ case E_INDEX_OUT_OF_BOUNDS:
+ return ("Index out of bounds");
+
+ case E_CMD_PENDING:
+ return ("Command is pending...");
+
+ case E_OVERRUN:
+ return ("Data overrun");
+
+ case E_RANGE_ERROR:
+ return ("Value out of range");
+
+ case E_AXIS_BLOCKED:
+ return ("Axis is blocked");
+
+ case E_FILE_EXISTS:
+ return ("File already exists");
+
+ default:
+ return ("Internal error. Unknown error code.");
}
}
};
diff --git a/source/RobotAPI/libraries/drivers/WeissHapticSensor/SerialInterface.cpp b/source/RobotAPI/libraries/drivers/WeissHapticSensor/SerialInterface.cpp
index 387d2f5236177c7f4583460325209259bad95993..bdf17ddceb516bbe65fa07311767601770876f5b 100644
--- a/source/RobotAPI/libraries/drivers/WeissHapticSensor/SerialInterface.cpp
+++ b/source/RobotAPI/libraries/drivers/WeissHapticSensor/SerialInterface.cpp
@@ -15,98 +15,127 @@
#include <boost/lexical_cast.hpp>
#include <boost/format.hpp>
-static inline tcflag_t __bitrate_to_flag( unsigned int bitrate )
+static inline tcflag_t __bitrate_to_flag(unsigned int bitrate)
{
- switch( bitrate )
+ switch (bitrate)
{
- case 1200: return B1200;
- case 2400: return B2400;
- case 4800: return B4800;
- case 9600: return B9600;
- case 19200: return B19200;
- case 38400: return B38400;
- case 57600: return B57600;
- case 115200: return B115200;
- case 230400: return B230400;
- case 460800: return B460800;
- default: return 0;
+ case 1200:
+ return B1200;
+
+ case 2400:
+ return B2400;
+
+ case 4800:
+ return B4800;
+
+ case 9600:
+ return B9600;
+
+ case 19200:
+ return B19200;
+
+ case 38400:
+ return B38400;
+
+ case 57600:
+ return B57600;
+
+ case 115200:
+ return B115200;
+
+ case 230400:
+ return B230400;
+
+ case 460800:
+ return B460800;
+
+ default:
+ return 0;
}
}
-SerialInterface::SerialInterface(const char *device, unsigned int bitrate)
+SerialInterface::SerialInterface(const char* device, unsigned int bitrate)
{
this->device = device;
this->bitrate = bitrate;
- this->fd = -1;
+ this->fd = -1;
}
-SerialInterface::~SerialInterface() {
+SerialInterface::~SerialInterface()
+{
}
-int SerialInterface::open() {
+int SerialInterface::open()
+{
// Convert bitrate to flag
tcflag_t bitrate = __bitrate_to_flag(this->bitrate);
- if ( bitrate == 0 )
+
+ if (bitrate == 0)
{
- fprintf( stderr, "Invalid bitrate '%d' for serial device\n", this->bitrate );
+ fprintf(stderr, "Invalid bitrate '%d' for serial device\n", this->bitrate);
return -1;
}
// Open serial device
- fd = ::open( device, O_RDWR | O_NOCTTY );
- if ( fd < 0 )
+ fd = ::open(device, O_RDWR | O_NOCTTY);
+
+ if (fd < 0)
{
- fprintf( stderr, "Failed to open serial device '%s' (errno: %s)\n", device, strerror(errno) );
+ fprintf(stderr, "Failed to open serial device '%s' (errno: %s)\n", device, strerror(errno));
return -1;
}
+
if (::ioctl(fd, TIOCEXCL))
{
- fprintf( stderr, "Failed to lock serial device '%s' (errno: %s)\n", device, strerror(errno) );
+ fprintf(stderr, "Failed to lock serial device '%s' (errno: %s)\n", device, strerror(errno));
return -1;
}
// Check if device is a terminal device
- if ( !isatty( fd ) )
+ if (!isatty(fd))
{
- fprintf( stderr, "Device '%s' is not a terminal device (errno: %s)!\n", device, strerror(errno) );
- ::close( fd );
+ fprintf(stderr, "Device '%s' is not a terminal device (errno: %s)!\n", device, strerror(errno));
+ ::close(fd);
return -1;
}
struct termios settings;
+
// Set input flags
settings.c_iflag = IGNBRK // Ignore BREAKS on Input
- | IGNPAR; // No Parity
- // ICRNL: map CR to NL (otherwise a CR input on the other computer will not terminate input)
+ | IGNPAR; // No Parity
+
+ // ICRNL: map CR to NL (otherwise a CR input on the other computer will not terminate input)
// Set output flags
- settings.c_oflag = 0; // Raw output
+ settings.c_oflag = 0; // Raw output
// Set controlflags
settings.c_cflag = bitrate
- | CS8 // 8 bits per byte
- | CSTOPB // Stop bit
- | CREAD // characters may be read
- | CLOCAL; // ignore modem state, local connection
+ | CS8 // 8 bits per byte
+ | CSTOPB // Stop bit
+ | CREAD // characters may be read
+ | CLOCAL; // ignore modem state, local connection
// Set local flags
- settings.c_lflag = 0; // Other option: ICANON = enable canonical input
+ settings.c_lflag = 0; // Other option: ICANON = enable canonical input
// Set maximum wait time on input - cf. Linux Serial Programming HowTo, non-canonical mode
// http://tldp.org/HOWTO/Serial-Programming-HOWTO/x115.html
- settings.c_cc[VTIME] = 10; // 0 means timer is not uses
+ settings.c_cc[VTIME] = 10; // 0 means timer is not uses
// Set minimum bytes to read
- settings.c_cc[VMIN] = 0; // 1 means wait until at least 1 character is received
+ settings.c_cc[VMIN] = 0; // 1 means wait until at least 1 character is received
// Now clean the modem line and activate the settings for the port
- tcflush( fd, TCIFLUSH );
- tcsetattr( fd, TCSANOW, &settings );
+ tcflush(fd, TCIFLUSH);
+
+ tcsetattr(fd, TCSANOW, &settings);
connected = true;
@@ -115,16 +144,21 @@ int SerialInterface::open() {
void SerialInterface::close()
{
- if(connected) ::close(fd);
+ if (connected)
+ {
+ ::close(fd);
+ }
+
connected = false;
}
-int SerialInterface::readInternal( unsigned char *buf, unsigned int len)
+int SerialInterface::readInternal(unsigned char* buf, unsigned int len)
{
int res;
- res = blockingReadAll( buf, len );
- if ( res < 0 )
+ res = blockingReadAll(buf, len);
+
+ if (res < 0)
{
std::cerr << "Failed to read from serial device" << std::endl;
}
@@ -133,38 +167,44 @@ int SerialInterface::readInternal( unsigned char *buf, unsigned int len)
}
-int SerialInterface::blockingReadAll(unsigned char *buf, unsigned int len)
+int SerialInterface::blockingReadAll(unsigned char* buf, unsigned int len)
{
int dataToRead = len;
+
while (1)
{
- int res = ::read( fd, buf, dataToRead );
+ int res = ::read(fd, buf, dataToRead);
+
if (res < 0)
{
return res;
}
+
dataToRead -= res;
buf += res;
+
if (dataToRead == 0)
{
return len;
}
+
if (dataToRead < 0)
{
throw new std::runtime_error("Internal error: dataToRead < 0");
}
+
usleep(1);
}
}
-int SerialInterface::writeInternal( unsigned char *buf, unsigned int len)
+int SerialInterface::writeInternal(unsigned char* buf, unsigned int len)
{
- return( ::write( fd, (void *) buf, len ) );
+ return (::write(fd, (void*) buf, len));
}
std::string SerialInterface::toString() const
{
return str(boost::format("SerialInterface(connected=%1%, device=%2%, bitrate=%3%, fd=%4%)")
- % connected % device % bitrate % fd);
+ % connected % device % bitrate % fd);
}
diff --git a/source/RobotAPI/libraries/drivers/WeissHapticSensor/SerialInterface.h b/source/RobotAPI/libraries/drivers/WeissHapticSensor/SerialInterface.h
index c64e501d085f0e752ec1ea8bd3f1e7bd018d3d01..33770d3b817c42b325e85b81e719c7bf68e6a005 100644
--- a/source/RobotAPI/libraries/drivers/WeissHapticSensor/SerialInterface.h
+++ b/source/RobotAPI/libraries/drivers/WeissHapticSensor/SerialInterface.h
@@ -5,9 +5,10 @@
#include <iostream>
-class SerialInterface : public AbstractInterface {
+class SerialInterface : public AbstractInterface
+{
public:
- SerialInterface(const char *device, const unsigned int bitrate);
+ SerialInterface(const char* device, const unsigned int bitrate);
virtual ~SerialInterface();
virtual int open();
@@ -16,15 +17,15 @@ public:
virtual std::string toString() const;
protected:
- virtual int readInternal( unsigned char *, unsigned int );
- virtual int writeInternal( unsigned char *, unsigned int );
+ virtual int readInternal(unsigned char*, unsigned int);
+ virtual int writeInternal(unsigned char*, unsigned int);
private:
- const char *device;
+ const char* device;
unsigned int bitrate;
int fd;
- int blockingReadAll(unsigned char *buf, unsigned int len);
+ int blockingReadAll(unsigned char* buf, unsigned int len);
};
#endif // SERIALINTERFACE_H
diff --git a/source/RobotAPI/libraries/drivers/WeissHapticSensor/TactileSensor.cpp b/source/RobotAPI/libraries/drivers/WeissHapticSensor/TactileSensor.cpp
index 639beb12b73aa2fba7b15b67e2b587804afad679..da246c4414726ac488c386e8266b4e4ab2997206 100644
--- a/source/RobotAPI/libraries/drivers/WeissHapticSensor/TactileSensor.cpp
+++ b/source/RobotAPI/libraries/drivers/WeissHapticSensor/TactileSensor.cpp
@@ -10,15 +10,16 @@ TactileSensor::TactileSensor(boost::shared_ptr<AbstractInterface> interface)
this->interface = interface;
}
-TactileSensor::~TactileSensor() {
+TactileSensor::~TactileSensor()
+{
// TODO Auto-generated destructor stub
}
-tac_matrix_info_t TactileSensor::getMatrixInformation( )
+tac_matrix_info_t TactileSensor::getMatrixInformation()
{
unsigned char payload[0];
- Response response = interface->submitCmd( 0x30, payload, sizeof(payload), false );
+ Response response = interface->submitCmd(0x30, payload, sizeof(payload), false);
response.ensureMinLength(12);
response.ensureSuccess();
@@ -34,22 +35,25 @@ tac_matrix_info_t TactileSensor::getMatrixInformation( )
return matrix_info;
}
-void TactileSensor::printMatrixInfo( tac_matrix_info_t *mi )
+void TactileSensor::printMatrixInfo(tac_matrix_info_t* mi)
{
printf("res_x = %d, res_y = %d, cell_width = %d, cell_height = %d, fullscale = %X\n",
- mi->res_x, mi->res_y, mi->cell_width, mi->cell_height, mi->fullscale);
+ mi->res_x, mi->res_y, mi->cell_width, mi->cell_height, mi->fullscale);
}
-void TactileSensor::printMatrix ( short *matrix, int width, int height )
+void TactileSensor::printMatrix(short* matrix, int width, int height)
{
int x, y;
+
for (y = 0; y < height; y++)
{
printf("%03X", matrix[y * width]);
+
for (x = 1; x < width; x++)
{
printf(", %03X", matrix[y * width + x]);
}
+
printf("\n");
}
}
@@ -58,17 +62,19 @@ FrameData TactileSensor::readSingleFrame()
{
unsigned char payload[1];
payload[0] = 0x00; // FLAGS = 0
- Response response = interface->submitCmd( 0x20, payload, sizeof(payload), false );
+ Response response = interface->submitCmd(0x20, payload, sizeof(payload), false);
return getFrameData(&response);
}
PeriodicFrameData TactileSensor::receicePeriodicFrame()
{
Response response = interface->receiveWithoutChecks();
- if(response.cmdId == 0x21)
+
+ if (response.cmdId == 0x21)
{
response = interface->receiveWithoutChecks();
}
- if(response.cmdId == 0x00)
+
+ if (response.cmdId == 0x00)
{
return getPeriodicFrameData(&response);
}
@@ -78,27 +84,29 @@ PeriodicFrameData TactileSensor::receicePeriodicFrame()
}
}
-PeriodicFrameData TactileSensor::getPeriodicFrameData(Response *response)
+PeriodicFrameData TactileSensor::getPeriodicFrameData(Response* response)
{
response->ensureMinLength(7);
- unsigned int timestamp = response->getUInt( 0 );
+ unsigned int timestamp = response->getUInt(0);
int offset = 5;
int count = (response->len - offset) / 2;
int i;
boost::shared_ptr<std::vector<short> > data;
data.reset(new std::vector<short>(count, 0));
+
//short* data = new short[ count ];
- for(i = 0; i < count; i++)
+ for (i = 0; i < count; i++)
{
- short value = response->getShort(i*2 + offset);
+ short value = response->getShort(i * 2 + offset);
(*data)[i] = value;
}
+
return PeriodicFrameData(data, count, timestamp);
}
-FrameData TactileSensor::getFrameData(Response *response)
+FrameData TactileSensor::getFrameData(Response* response)
{
response->ensureMinLength(7);
response->ensureSuccess();
@@ -109,82 +117,94 @@ FrameData TactileSensor::getFrameData(Response *response)
int i;
boost::shared_ptr<std::vector<short> > data;
data.reset(new std::vector<short>(count, 0));
- for(i = 0; i < count; i++)
+
+ for (i = 0; i < count; i++)
{
- short value = response->getShort(i*2 + offset);
+ short value = response->getShort(i * 2 + offset);
(*data)[i] = value;
}
+
return FrameData(data, count);
}
-void TactileSensor::startPeriodicFrameAcquisition( unsigned short delay_ms )
+void TactileSensor::startPeriodicFrameAcquisition(unsigned short delay_ms)
{
unsigned char payload[3];
payload[0] = 0x00; // FLAGS = 0
payload[1] = delay_ms & 0xFF;
payload[2] = (delay_ms >> 8) & 0xFF;
- interface->fireAndForgetCmd( 0x21, payload, sizeof(payload), false );
+ interface->fireAndForgetCmd(0x21, payload, sizeof(payload), false);
}
-void TactileSensor::stopPeriodicFrameAcquisition( void )
+void TactileSensor::stopPeriodicFrameAcquisition(void)
{
- while(1)
+ while (1)
{
unsigned char payload[0];
- interface->fireAndForgetCmd( 0x22, payload, sizeof(payload), false );
+ interface->fireAndForgetCmd(0x22, payload, sizeof(payload), false);
int waitCount = 10;
+
while (waitCount > 0)
{
Response response = interface->receiveWithoutChecks();
- if(response.cmdId == 0x22) {
+
+ if (response.cmdId == 0x22)
+ {
return;
}
else
{
cout << boost::format("stopPeriodicFrameAcquisition :: Discarding Response with ID 0x%02X") % (int)response.cmdId << endl;
}
+
waitCount--;
}
}
}
-void TactileSensor::tareSensorMatrix( unsigned char operation )
+void TactileSensor::tareSensorMatrix(unsigned char operation)
{
unsigned char payload[1];
payload[0] = operation; // OPERATION: 0 = un-tare the sensor matrix using the currently set threshold value, 1 = tare the sensor matrix
- Response response = interface->submitCmd( 0x23, payload, sizeof(payload), false );
+ Response response = interface->submitCmd(0x23, payload, sizeof(payload), false);
response.ensureMinLength(2);
response.ensureSuccess();
}
-void TactileSensor::setAquisitionWindow( unsigned char x1, unsigned char y1, unsigned char x2, unsigned char y2 )
+void TactileSensor::setAquisitionWindow(unsigned char x1, unsigned char y1, unsigned char x2, unsigned char y2)
{
unsigned char payload[4];
payload[0] = x1;
payload[1] = y1;
payload[2] = x2;
payload[3] = y2;
- Response response = interface->submitCmd( 0x31, payload, sizeof(payload), false );
+ Response response = interface->submitCmd(0x31, payload, sizeof(payload), false);
response.ensureMinLength(2);
response.ensureSuccess();
}
-int TactileSensor::setAdvanvedAcquisitionMask( char *mask ) { return 0; }
-int TactileSensor::getAcquisitionMask( char **mask, int *mask_len ) { return 0; }
-void TactileSensor::setThreshold( short threshold )
+int TactileSensor::setAdvanvedAcquisitionMask(char* mask)
+{
+ return 0;
+}
+int TactileSensor::getAcquisitionMask(char** mask, int* mask_len)
+{
+ return 0;
+}
+void TactileSensor::setThreshold(short threshold)
{
unsigned char payload[2];
payload[0] = threshold & 0xFF;
payload[1] = (threshold >> 8) & 0xFF;
- Response response = interface->submitCmd( 0x34, payload, sizeof(payload), false );
+ Response response = interface->submitCmd(0x34, payload, sizeof(payload), false);
response.ensureMinLength(2);
response.ensureSuccess();
}
unsigned short TactileSensor::getThreshold()
{
unsigned char payload[0];
- Response response = interface->submitCmd( 0x35, payload, sizeof(payload), false );
+ Response response = interface->submitCmd(0x35, payload, sizeof(payload), false);
response.ensureMinLength(2);
response.ensureSuccess();
return response.getShort(2);
}
-void TactileSensor::setFrontEndGain( unsigned char gain )
+void TactileSensor::setFrontEndGain(unsigned char gain)
{
/*
* Adjust the pressure sensitivity of a matrix by setting the gain of the Analog Front-End. The gain can
@@ -193,7 +213,7 @@ void TactileSensor::setFrontEndGain( unsigned char gain )
*/
unsigned char payload[1];
payload[0] = gain;
- Response response = interface->submitCmd( 0x36, payload, sizeof(payload), false );
+ Response response = interface->submitCmd(0x36, payload, sizeof(payload), false);
response.ensureMinLength(2);
response.ensureSuccess();
}
@@ -204,7 +224,7 @@ unsigned char TactileSensor::getFrontEndGain()
* 255, where 0 is the most insensitive (lowest gain) and 255 is the most sensitive (highest gain) setting.
*/
unsigned char payload[0];
- Response response = interface->submitCmd( 0x37, payload, sizeof(payload), false );
+ Response response = interface->submitCmd(0x37, payload, sizeof(payload), false);
response.ensureMinLength(3);
response.ensureSuccess();
unsigned char gain = response.getByte(2);
@@ -216,7 +236,7 @@ std::string TactileSensor::getSensorType()
* Return a string containing the sensor type.
*/
unsigned char payload[0];
- Response response = interface->submitCmd( 0x38, payload, sizeof(payload), false );
+ Response response = interface->submitCmd(0x38, payload, sizeof(payload), false);
response.ensureMinLength(2);
response.ensureSuccess();
std::string type = std::string((char*)response.data.data() + 2, response.len - 2);
@@ -225,7 +245,7 @@ std::string TactileSensor::getSensorType()
float TactileSensor::readDeviceTemperature()
{
unsigned char payload[0];
- Response response = interface->submitCmd( 0x46, payload, sizeof(payload), false );
+ Response response = interface->submitCmd(0x46, payload, sizeof(payload), false);
response.ensureMinLength(2);
short value = (short)response.getShort(2);
return value * 0.1f;
@@ -233,7 +253,7 @@ float TactileSensor::readDeviceTemperature()
tac_system_information_t TactileSensor::getSystemInformation()
{
unsigned char payload[0];
- Response response = interface->submitCmd( 0x50, payload, sizeof(payload), false );
+ Response response = interface->submitCmd(0x50, payload, sizeof(payload), false);
response.ensureMinLength(9);
response.ensureSuccess();
tac_system_information_t si;
@@ -250,45 +270,51 @@ void TactileSensor::printSystemInformation(tac_system_information_t si)
int v3 = (si.fw_version & 0x00F0) >> 4;
int v4 = (si.fw_version & 0x000F) >> 0;
cout << boost::format("System Type=%1%, Hardware Revision=%2%, Firmware Version=%3%.%4%.%5%.%6% (0x%7$04X), Serial Number=%8%")
- % (int)si.type % (int)si.hw_rev % v1 % v2 % v3 % v4 % si.fw_version % si.sn << endl;
+ % (int)si.type % (int)si.hw_rev % v1 % v2 % v3 % v4 % si.fw_version % si.sn << endl;
}
void TactileSensor::setDeviceTag(string tag)
{
- unsigned char *payload = (unsigned char*)tag.c_str();
- Response response = interface->submitCmd( 0x51, payload, tag.length(), false );
+ unsigned char* payload = (unsigned char*)tag.c_str();
+ Response response = interface->submitCmd(0x51, payload, tag.length(), false);
response.ensureMinLength(2);
response.ensureSuccess();
}
string TactileSensor::getDeviceTag()
{
unsigned char payload[0];
- Response response = interface->submitCmd( 0x52, payload, sizeof(payload), false );
+ Response response = interface->submitCmd(0x52, payload, sizeof(payload), false);
response.ensureMinLength(2);
response.ensureSuccess();
std::string tag = std::string((char*)response.data.data() + 2, response.len - 2);
return tag;
}
-bool TactileSensor::tryGetDeviceTag(string &tag)
+bool TactileSensor::tryGetDeviceTag(string& tag)
{
unsigned char payload[0];
- Response response = interface->submitCmd( 0x52, payload, sizeof(payload), false );
+ Response response = interface->submitCmd(0x52, payload, sizeof(payload), false);
response.ensureMinLength(2);
+
if (response.status == E_NOT_AVAILABLE)
{
return false;
}
+
response.ensureSuccess();
tag = std::string((char*)response.data.data() + 2, response.len - 2);
return true;
}
-int TactileSensor::loop( char *data, int data_len ) { return 0; }
+int TactileSensor::loop(char* data, int data_len)
+{
+ return 0;
+}
string TactileSensor::getInterfaceInfo()
{
return interface->toString();
}
-ostream& operator<<(ostream &strm, const TactileSensor &a) {
- return strm << a.interface;
+ostream& operator<<(ostream& strm, const TactileSensor& a)
+{
+ return strm << a.interface;
}
diff --git a/source/RobotAPI/libraries/drivers/WeissHapticSensor/TactileSensor.h b/source/RobotAPI/libraries/drivers/WeissHapticSensor/TactileSensor.h
index 7f36b86cc7fe1f0fa99f71ab2671d3b8f3dfe144..131d10d765f90dead6133a7e4d7a2a7e203bc7ee 100644
--- a/source/RobotAPI/libraries/drivers/WeissHapticSensor/TactileSensor.h
+++ b/source/RobotAPI/libraries/drivers/WeissHapticSensor/TactileSensor.h
@@ -5,21 +5,21 @@
using namespace std;
#include <boost/shared_ptr.hpp>
-#define extract_short( array, index ) ( (unsigned short)array[index] | ( (unsigned short)array[index + 1] << 8 ) )
-#define TAC_CHECK_RES( res, expected, resp ) { \
- if ( res < expected ) { \
- dbgPrint( "Response length is too short, should be = %d (is %d)\n", expected, res ); \
- if ( res > 0 ) free( resp ); \
- return -1; \
- } \
- status_t status = cmd_get_response_status( resp ); \
- if ( status != E_SUCCESS ) \
- { \
- dbgPrint( "Command not successful: %s\n", status_to_str( status ) ); \
- free( resp ); \
- return -1; \
- } \
- }
+#define extract_short( array, index ) ( (unsigned short)array[index] | ( (unsigned short)array[index + 1] << 8 ) )
+#define TAC_CHECK_RES( res, expected, resp ) { \
+ if ( res < expected ) { \
+ dbgPrint( "Response length is too short, should be = %d (is %d)\n", expected, res ); \
+ if ( res > 0 ) free( resp ); \
+ return -1; \
+ } \
+ status_t status = cmd_get_response_status( resp ); \
+ if ( status != E_SUCCESS ) \
+ { \
+ dbgPrint( "Command not successful: %s\n", status_to_str( status ) ); \
+ free( resp ); \
+ return -1; \
+ } \
+ }
typedef struct
{
@@ -58,26 +58,27 @@ public:
unsigned int timestamp;
};
-class TactileSensor {
+class TactileSensor
+{
public:
TactileSensor(boost::shared_ptr<AbstractInterface> interface);
virtual ~TactileSensor();
- tac_matrix_info_t getMatrixInformation( );
- static void printMatrixInfo( tac_matrix_info_t *mi );
+ tac_matrix_info_t getMatrixInformation();
+ static void printMatrixInfo(tac_matrix_info_t* mi);
FrameData readSingleFrame();
- static void printMatrix ( short *matrix, int width, int height );
+ static void printMatrix(short* matrix, int width, int height);
- void startPeriodicFrameAcquisition( unsigned short delay_ms );
- void stopPeriodicFrameAcquisition( void );
+ void startPeriodicFrameAcquisition(unsigned short delay_ms);
+ void stopPeriodicFrameAcquisition(void);
PeriodicFrameData receicePeriodicFrame();
- void tareSensorMatrix( unsigned char operation );
- void setAquisitionWindow( unsigned char x1, unsigned char y1, unsigned char x2, unsigned char y2 );
- int setAdvanvedAcquisitionMask( char *mask );
- int getAcquisitionMask( char **mask, int *mask_len );
- void setThreshold( short threshold );
+ void tareSensorMatrix(unsigned char operation);
+ void setAquisitionWindow(unsigned char x1, unsigned char y1, unsigned char x2, unsigned char y2);
+ int setAdvanvedAcquisitionMask(char* mask);
+ int getAcquisitionMask(char** mask, int* mask_len);
+ void setThreshold(short threshold);
unsigned short getThreshold();
- void setFrontEndGain( unsigned char gain );
+ void setFrontEndGain(unsigned char gain);
unsigned char getFrontEndGain();
string getSensorType();
float readDeviceTemperature();
@@ -85,20 +86,20 @@ public:
static void printSystemInformation(tac_system_information_t si);
void setDeviceTag(string tag);
string getDeviceTag();
- bool tryGetDeviceTag(string &tag);
- int loop( char *data, int data_len );
+ bool tryGetDeviceTag(string& tag);
+ int loop(char* data, int data_len);
string getInterfaceInfo();
private:
boost::shared_ptr<AbstractInterface> interface;
- FrameData getFrameData(Response *response);
- PeriodicFrameData getPeriodicFrameData(Response *response);
+ FrameData getFrameData(Response* response);
+ PeriodicFrameData getPeriodicFrameData(Response* response);
private:
friend std::ostream& operator<<(std::ostream&, const TactileSensor&);
};
-std::ostream& operator<<(std::ostream &strm, const TactileSensor &a);
+std::ostream& operator<<(std::ostream& strm, const TactileSensor& a);
#endif // TACTILESENSOR_H
diff --git a/source/RobotAPI/libraries/drivers/WeissHapticSensor/TransmissionException.h b/source/RobotAPI/libraries/drivers/WeissHapticSensor/TransmissionException.h
index 93710f7ec2476bb1be56dfafc5757ab8277f4051..c50a5f0931c55b4a6ffe3fa9a4f1cdfc495e49a9 100644
--- a/source/RobotAPI/libraries/drivers/WeissHapticSensor/TransmissionException.h
+++ b/source/RobotAPI/libraries/drivers/WeissHapticSensor/TransmissionException.h
@@ -3,17 +3,19 @@
#include <stdexcept>
-class TransmissionException : public std::runtime_error {
+class TransmissionException : public std::runtime_error
+{
public:
TransmissionException(std::string message)
- : runtime_error(message)
+ : runtime_error(message)
{ }
};
-class ChecksumErrorException : public TransmissionException {
+class ChecksumErrorException : public TransmissionException
+{
public:
ChecksumErrorException(std::string message)
- : TransmissionException(message)
+ : TransmissionException(message)
{ }
};
diff --git a/source/RobotAPI/libraries/drivers/WeissHapticSensor/Types.h b/source/RobotAPI/libraries/drivers/WeissHapticSensor/Types.h
index 1fec7ca5f2535af8cea1197b7e87ca326d7d317e..19275ae1226d6e996e1c389d4ea4e2152d253547 100644
--- a/source/RobotAPI/libraries/drivers/WeissHapticSensor/Types.h
+++ b/source/RobotAPI/libraries/drivers/WeissHapticSensor/Types.h
@@ -11,7 +11,8 @@ typedef struct
} msg_t;
-typedef enum {
+typedef enum
+{
E_SUCCESS = 0, // No error
E_NOT_AVAILABLE, // Device, service or data is not available
E_NO_SENSOR, // No sensor connected
@@ -33,19 +34,19 @@ typedef enum {
E_CMD_FAILED, // Command failed
E_CMD_ABORTED, // Command aborted
E_INVALID_HANDLE, // invalid handle
- E_NOT_FOUND, // device not found
- E_NOT_OPEN, // device not open
+ E_NOT_FOUND, // device not found
+ E_NOT_OPEN, // device not open
E_IO_ERROR, // I/O error
E_INVALID_PARAMETER, // invalid parameter
E_INDEX_OUT_OF_BOUNDS, // index out of bounds
E_CMD_PENDING, // Command was received correctly, but the execution needs more time. If the command was completely processed, another status message is returned indicating the command's result
- E_OVERRUN, // Data overrun
- E_RANGE_ERROR, // Range error
- E_AXIS_BLOCKED, // Axis is blocked
- E_FILE_EXISTS // File already exists
+ E_OVERRUN, // Data overrun
+ E_RANGE_ERROR, // Range error
+ E_AXIS_BLOCKED, // Axis is blocked
+ E_FILE_EXISTS // File already exists
} status_t;
-const char* status_to_str( status_t status );
+const char* status_to_str(status_t status);
#endif // TYPES_H
diff --git a/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensor.cpp b/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensor.cpp
index 32b44ac4749e7ad76dae442406d97fef4211889c..cb5d80f620055dd65932e3bb73b4f754fb613fb7 100644
--- a/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensor.cpp
+++ b/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensor.cpp
@@ -10,7 +10,7 @@ WeissHapticSensor::WeissHapticSensor(std::string device, int minimumReportInterv
{
sensorTask = new RunningTask<WeissHapticSensor>(this, &WeissHapticSensor::frameAcquisitionTaskLoop);
boost::smatch match;
- boost::regex_search( device, match, boost::regex("\\w+$") );
+ boost::regex_search(device, match, boost::regex("\\w+$"));
this->deviceFileName = match[0];
}
@@ -37,7 +37,8 @@ void WeissHapticSensor::connect()
//sensor->setDeviceTag("Test Tag");
string tag;
- if(sensor->tryGetDeviceTag(tag))
+
+ if (sensor->tryGetDeviceTag(tag))
{
ARMARX_INFO << "[" << device << "] Got Device Tag: " << tag;
this->tag = tag;
@@ -118,7 +119,7 @@ void WeissHapticSensor::frameAcquisitionTaskLoop()
math::SlidingWindowVectorMedian slidingMedian(mi.res_x * mi.res_y, 21); // inter sample dely ~= 3,7ms, 11 samples ~== 40ms delay
- while(!sensorTask->isStopped())
+ while (!sensorTask->isStopped())
{
//ARMARX_INFO << deactivateSpam(1) << this << ": receicePeriodicFrame";
@@ -130,14 +131,17 @@ void WeissHapticSensor::frameAcquisitionTaskLoop()
//cout << end - start << endl;
std::vector<float> sensorValues;
- for(int i = 0; i < mi.res_x * mi.res_y; i++)
+
+ for (int i = 0; i < mi.res_x * mi.res_y; i++)
{
sensorValues.push_back(data.data->at(i));
}
+
slidingMedian.addEntry(sensorValues);
MatrixFloatPtr matrix = new MatrixFloat(mi.res_y, mi.res_x);
std::vector<float> filteredSensorValues = slidingMedian.getMedian();
+
for (int y = 0; y < mi.res_y; y++)
{
for (int x = 0; x < mi.res_x; x++)
@@ -165,18 +169,19 @@ void WeissHapticSensor::frameAcquisitionTaskLoop()
writeMatrixToJs(matrix, nowTimestamp);
IceUtil::Time interval = now - lastFrameTime;
- if(interval.toMilliSeconds() >= minimumReportIntervalMs)
+
+ if (interval.toMilliSeconds() >= minimumReportIntervalMs)
{
listenerPrx->reportSensorValues(device, tag, matrix, nowTimestamp);
lastFrameTime = now;
}
}
- catch(ChecksumErrorException)
+ catch (ChecksumErrorException)
{
ARMARX_WARNING << "Caught ChecksumErrorException on " << device << ", skipping frame";
}
- if(setDeviceTagScheduled)
+ if (setDeviceTagScheduled)
{
boost::mutex::scoped_lock lock(mutex);
setDeviceTagScheduled = false;
@@ -191,6 +196,7 @@ void WeissHapticSensor::frameAcquisitionTaskLoop()
ARMARX_INFO << "[" << device << "] Starting periodic frame aquisition";
sensor->startPeriodicFrameAcquisition(0);
}
+
//usleep(3000);
//usleep(3000);
@@ -204,7 +210,7 @@ void WeissHapticSensor::frameAcquisitionTaskLoop()
void WeissHapticSensor::writeMatrixToJs(MatrixFloatPtr matrix, TimestampVariantPtr timestamp)
{
//std::cout << "writeMatrixToJs" << std::endl;
- if(jsWriter != NULL)
+ if (jsWriter != NULL)
{
jsWriter->writeLine(str(boost::format("%s.data.push([%i, %s]);") % deviceFileName % timestamp->getTimestamp() % matrix->toJsonRowMajor()));
}
diff --git a/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticUnit.cpp b/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticUnit.cpp
index 62aaf9c37b20abb683e730b8033d969551d9e83e..eadc5b159629e074545992d36097e64a672396a6 100644
--- a/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticUnit.cpp
+++ b/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticUnit.cpp
@@ -1,149 +1,156 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as
- * published by the Free Software Foundation; either version 2 of
- * the License, or (at your option) any later version.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package ArmarXCore::units
- * @author Peter Kaiser
- * @date 2014
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "WeissHapticUnit.h"
-
-#include <boost/regex.hpp>
-#include <boost/filesystem.hpp>
-
-
-using namespace armarx;
-
-void WeissHapticUnit::onInitHapticUnit()
-{
-
- std::vector<std::string> devices = getDevices();
-
- for(std::vector<std::string>::iterator it = devices.begin(); it != devices.end(); ++it)
- {
- WeissHapticSensorPtr sensor(new WeissHapticSensor(*it, 20)); // minimumReportIntervalMs = 20, limit to maximum 50 frames/s
- this->sensors.push_back(sensor);
- }
-
- std::cout << "Connect Interfaces" << std::endl;
-
- for(std::vector<boost::shared_ptr<WeissHapticSensor> >::iterator it = sensors.begin(); it != sensors.end(); ++it)
- {
- (*it)->connect();
- }
-
-}
-
-std::vector< std::string > WeissHapticUnit::getDevices()
-{
- const std::string target_path( "/dev/" );
- const boost::regex my_filter( "ttyACM[0-9]+" );
-
- std::vector< std::string > files;
-
- boost::filesystem::directory_iterator end_itr; // Default ctor yields past-the-end
- for( boost::filesystem::directory_iterator i( target_path ); i != end_itr; ++i )
- {
- // Skip if not a file
- //if( !boost::filesystem::is_( i->status() ) ) continue;
-
- boost::smatch what;
-
- // Skip if no match
- if( !boost::regex_match( i->path().filename().string(), what, my_filter ) ) continue;
-
-
- // File matches, store it
- files.push_back( "/dev/" + i->path().filename().string() );
- }
- std::sort(files.begin(), files.end());
-
- if(files.size() == 0)
- {
- ARMARX_WARNING << "No ACM-Interfaces found";
- }
- else
- {
- ARMARX_INFO << "Detected ACM-Interfaces: " << files.size();
- for(std::string file : files)
- {
- ARMARX_INFO << "Found device: " << file;
- }
- }
-
- return files;
-}
-
-void WeissHapticUnit::setDeviceTag(const string &deviceName, const string &tag, const Ice::Current &)
-{
- for(WeissHapticSensorPtr sensor : sensors)
- {
- if(sensor->getDeviceName() == deviceName)
- {
- ARMARX_IMPORTANT << "scheduling to set new device tag for " << deviceName << ": " << tag;
- sensor->scheduleSetDeviceTag(tag);
- return;
- }
- }
- ARMARX_WARNING << "device not found: " << deviceName;
-}
-
-void WeissHapticUnit::startLogging(const Ice::Current &)
-{
- // @@@ TODO NotImplemented
-}
-
-void WeissHapticUnit::stopLogging(const Ice::Current &)
-{
- // @@@ TODO NotImplemented
-}
-
-void WeissHapticUnit::onStartHapticUnit()
-{
-
- for(std::vector<boost::shared_ptr<WeissHapticSensor> >::iterator it = sensors.begin(); it != sensors.end(); ++it)
- {
- (*it)->setListenerPrx(hapticTopicPrx);
- (*it)->startSampling();
- }
-
-}
-
-void WeissHapticUnit::onExitHapticUnit()
-{
- for(std::vector<boost::shared_ptr<WeissHapticSensor> >::iterator it = sensors.begin(); it != sensors.end(); ++it)
- {
- (*it)->disconnect();
- }
-}
-
-/*void WeissHapticUnit::onConnectComponent()
-{
-
-}*/
-
-void WeissHapticUnit::onDisconnectComponent()
-{
-}
-
-
-PropertyDefinitionsPtr WeissHapticUnit::createPropertyDefinitions()
-{
- return PropertyDefinitionsPtr(new WeissHapticUnitPropertyDefinitions(getConfigIdentifier()));
-}
-
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarXCore::units
+ * @author Peter Kaiser
+ * @date 2014
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "WeissHapticUnit.h"
+
+#include <boost/regex.hpp>
+#include <boost/filesystem.hpp>
+
+
+using namespace armarx;
+
+void WeissHapticUnit::onInitHapticUnit()
+{
+
+ std::vector<std::string> devices = getDevices();
+
+ for (std::vector<std::string>::iterator it = devices.begin(); it != devices.end(); ++it)
+ {
+ WeissHapticSensorPtr sensor(new WeissHapticSensor(*it, 20)); // minimumReportIntervalMs = 20, limit to maximum 50 frames/s
+ this->sensors.push_back(sensor);
+ }
+
+ std::cout << "Connect Interfaces" << std::endl;
+
+ for (std::vector<boost::shared_ptr<WeissHapticSensor> >::iterator it = sensors.begin(); it != sensors.end(); ++it)
+ {
+ (*it)->connect();
+ }
+
+}
+
+std::vector< std::string > WeissHapticUnit::getDevices()
+{
+ const std::string target_path("/dev/");
+ const boost::regex my_filter("ttyACM[0-9]+");
+
+ std::vector< std::string > files;
+
+ boost::filesystem::directory_iterator end_itr; // Default ctor yields past-the-end
+
+ for (boost::filesystem::directory_iterator i(target_path); i != end_itr; ++i)
+ {
+ // Skip if not a file
+ //if( !boost::filesystem::is_( i->status() ) ) continue;
+
+ boost::smatch what;
+
+ // Skip if no match
+ if (!boost::regex_match(i->path().filename().string(), what, my_filter))
+ {
+ continue;
+ }
+
+
+ // File matches, store it
+ files.push_back("/dev/" + i->path().filename().string());
+ }
+
+ std::sort(files.begin(), files.end());
+
+ if (files.size() == 0)
+ {
+ ARMARX_WARNING << "No ACM-Interfaces found";
+ }
+ else
+ {
+ ARMARX_INFO << "Detected ACM-Interfaces: " << files.size();
+
+ for (std::string file : files)
+ {
+ ARMARX_INFO << "Found device: " << file;
+ }
+ }
+
+ return files;
+}
+
+void WeissHapticUnit::setDeviceTag(const string& deviceName, const string& tag, const Ice::Current&)
+{
+ for (WeissHapticSensorPtr sensor : sensors)
+ {
+ if (sensor->getDeviceName() == deviceName)
+ {
+ ARMARX_IMPORTANT << "scheduling to set new device tag for " << deviceName << ": " << tag;
+ sensor->scheduleSetDeviceTag(tag);
+ return;
+ }
+ }
+
+ ARMARX_WARNING << "device not found: " << deviceName;
+}
+
+void WeissHapticUnit::startLogging(const Ice::Current&)
+{
+ // @@@ TODO NotImplemented
+}
+
+void WeissHapticUnit::stopLogging(const Ice::Current&)
+{
+ // @@@ TODO NotImplemented
+}
+
+void WeissHapticUnit::onStartHapticUnit()
+{
+
+ for (std::vector<boost::shared_ptr<WeissHapticSensor> >::iterator it = sensors.begin(); it != sensors.end(); ++it)
+ {
+ (*it)->setListenerPrx(hapticTopicPrx);
+ (*it)->startSampling();
+ }
+
+}
+
+void WeissHapticUnit::onExitHapticUnit()
+{
+ for (std::vector<boost::shared_ptr<WeissHapticSensor> >::iterator it = sensors.begin(); it != sensors.end(); ++it)
+ {
+ (*it)->disconnect();
+ }
+}
+
+/*void WeissHapticUnit::onConnectComponent()
+{
+
+}*/
+
+void WeissHapticUnit::onDisconnectComponent()
+{
+}
+
+
+PropertyDefinitionsPtr WeissHapticUnit::createPropertyDefinitions()
+{
+ return PropertyDefinitionsPtr(new WeissHapticUnitPropertyDefinitions(getConfigIdentifier()));
+}
+
diff --git a/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticUnit.h b/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticUnit.h
index dd7e68483c39f58abfae64ee1b1188572084598b..0c9f6651044c70748ba0dc7f90efd6df15627122 100644
--- a/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticUnit.h
+++ b/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticUnit.h
@@ -1,85 +1,88 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as
- * published by the Free Software Foundation; either version 2 of
- * the License, or (at your option) any later version.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package Armar4::units
- * @author Simon Ottenhaus <simon dot ottenhaus at kit dot edu>
- * @date 2014
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#ifndef ARMAR4_FT_UNIT_ARMAR4_H
-#define ARMAR4_FT_UNIT_ARMAR4_H
-
-#include <RobotAPI/components/units/HapticUnit.h>
-#include <RobotAPI/interface/units/WeissHapticUnit.h>
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-
-#include <string>
-#include "WeissHapticSensor.h"
-
-namespace armarx
-{
- class WeissHapticUnitPropertyDefinitions : public HapticUnitPropertyDefinitions
- {
- public:
- WeissHapticUnitPropertyDefinitions(std::string prefix):
- HapticUnitPropertyDefinitions(prefix)
- {
- }
- };
-
- class WeissHapticUnit :
- virtual public WeissHapticUnitInterface,
- virtual public HapticUnit
- {
- public:
- virtual std::string getDefaultName() { return "WeissHapticUnit"; }
-
- virtual void onInitHapticUnit();
- virtual void onStartHapticUnit();
- virtual void onExitHapticUnit();
-
- //virtual void onConnectComponent();
- virtual void onDisconnectComponent();
-
- virtual PropertyDefinitionsPtr createPropertyDefinitions();
-
- protected:
- //void proceedSensorCategory(SensorCategoryDefinition<MatrixFloat> *category);
-
- //std::map<std::string, MatrixFloatPtr> currentValues;
-
-
-
- //HapticSensorProtocolMaster hapticProtocol;
-
- //bool remoteSystemReady;
-
- private:
- std::vector< std::string > getDevices();
-
- std::vector< boost::shared_ptr< WeissHapticSensor > > sensors;
-
- // WeissHapticUnitInterface interface
- public:
- void setDeviceTag(const string &deviceName, const string &tag, const Ice::Current &);
- void startLogging(const Ice::Current &);
- void stopLogging(const Ice::Current &);
- };
-}
-
-#endif
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package Armar4::units
+ * @author Simon Ottenhaus <simon dot ottenhaus at kit dot edu>
+ * @date 2014
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef ARMAR4_FT_UNIT_ARMAR4_H
+#define ARMAR4_FT_UNIT_ARMAR4_H
+
+#include <RobotAPI/components/units/HapticUnit.h>
+#include <RobotAPI/interface/units/WeissHapticUnit.h>
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+
+#include <string>
+#include "WeissHapticSensor.h"
+
+namespace armarx
+{
+ class WeissHapticUnitPropertyDefinitions : public HapticUnitPropertyDefinitions
+ {
+ public:
+ WeissHapticUnitPropertyDefinitions(std::string prefix):
+ HapticUnitPropertyDefinitions(prefix)
+ {
+ }
+ };
+
+ class WeissHapticUnit :
+ virtual public WeissHapticUnitInterface,
+ virtual public HapticUnit
+ {
+ public:
+ virtual std::string getDefaultName()
+ {
+ return "WeissHapticUnit";
+ }
+
+ virtual void onInitHapticUnit();
+ virtual void onStartHapticUnit();
+ virtual void onExitHapticUnit();
+
+ //virtual void onConnectComponent();
+ virtual void onDisconnectComponent();
+
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+ protected:
+ //void proceedSensorCategory(SensorCategoryDefinition<MatrixFloat> *category);
+
+ //std::map<std::string, MatrixFloatPtr> currentValues;
+
+
+
+ //HapticSensorProtocolMaster hapticProtocol;
+
+ //bool remoteSystemReady;
+
+ private:
+ std::vector< std::string > getDevices();
+
+ std::vector< boost::shared_ptr< WeissHapticSensor > > sensors;
+
+ // WeissHapticUnitInterface interface
+ public:
+ void setDeviceTag(const string& deviceName, const string& tag, const Ice::Current&);
+ void startLogging(const Ice::Current&);
+ void stopLogging(const Ice::Current&);
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IIMUEventDispatcher.cpp b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IIMUEventDispatcher.cpp
index 92deccbc7b25866cc117d01d996814634078e701..92c0657631ca1b8d193586d71e14bbc13405401d 100644
--- a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IIMUEventDispatcher.cpp
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IIMUEventDispatcher.cpp
@@ -3,7 +3,7 @@
*
* Created on: Mar 16, 2014
* Author: Dr.-Ing. David Israel González Aguirre
- * Mail: david.gonzalez@kit.edu
+ * Mail: david.gonzalez@kit.edu
*/
#include "IIMUEventDispatcher.h"
@@ -11,267 +11,289 @@
namespace IMU
{
- IIMUEventDispatcher::IIMUEventDispatcher(CIMUDevice* pIMUDevice) :
- m_DispatchingMode(eCoupled), m_MaximalPendingEvents(2), m_EventFlags(0XFFFF), m_pIMUDevice(pIMUDevice), m_LastStartTimeStamp(CTimeStamp::s_Zero), m_LastStopTimeStamp(CTimeStamp::s_Zero), m_LastCycleReferenceTimeStamp(CTimeStamp::s_Zero), m_LastFusedCycleReferenceTimeStamp(CTimeStamp::s_Zero), m_LastIntegratedStateReferenceTimeStamp(CTimeStamp::s_Zero), m_LastCustomEventReferenceTimeStamp(CTimeStamp::s_Zero)
- {
- pthread_mutex_init(&m_DispatchingModeMutex,NULL);
- pthread_mutex_init(&m_MaximalPendingEventsMutex,NULL);
- pthread_mutex_init(&m_EventFlagsMutex,NULL);
- pthread_mutex_init(&m_IMUDeviceMutex,NULL);
- pthread_mutex_init(&m_EventsQueueMutex,NULL);
- pthread_mutex_init(&m_LastStartTimeStampMutex,NULL);
- pthread_mutex_init(&m_LastStopTimeStampMutex,NULL);
- pthread_mutex_init(&m_LastCycleReferenceTimeStampMutex,NULL);
- pthread_mutex_init(&m_LastFusedCycleReferenceTimeStampMutex,NULL);
- pthread_mutex_init(&m_LastIntegratedStateReferenceTimeStampMutex,NULL);
- pthread_mutex_init(&m_LastCustomEventReferenceTimeStampMutex,NULL);
- }
-
- IIMUEventDispatcher::IIMUEventDispatcher() :
- m_DispatchingMode(eCoupled), m_MaximalPendingEvents(2), m_EventFlags(0XFFFF), m_pIMUDevice(NULL), m_EventsQueue(), m_LastStartTimeStamp(CTimeStamp::s_Zero), m_LastStopTimeStamp(CTimeStamp::s_Zero), m_LastCycleReferenceTimeStamp(CTimeStamp::s_Zero), m_LastFusedCycleReferenceTimeStamp(CTimeStamp::s_Zero), m_LastIntegratedStateReferenceTimeStamp(CTimeStamp::s_Zero), m_LastCustomEventReferenceTimeStamp(CTimeStamp::s_Zero)
- {
- pthread_mutex_init(&m_DispatchingModeMutex,NULL);
- pthread_mutex_init(&m_MaximalPendingEventsMutex,NULL);
- pthread_mutex_init(&m_EventFlagsMutex,NULL);
- pthread_mutex_init(&m_IMUDeviceMutex,NULL);
- pthread_mutex_init(&m_EventsQueueMutex,NULL);
- pthread_mutex_init(&m_LastStartTimeStampMutex,NULL);
- pthread_mutex_init(&m_LastStopTimeStampMutex,NULL);
- pthread_mutex_init(&m_LastCycleReferenceTimeStampMutex,NULL);
- pthread_mutex_init(&m_LastFusedCycleReferenceTimeStampMutex,NULL);
- pthread_mutex_init(&m_LastIntegratedStateReferenceTimeStampMutex,NULL);
- pthread_mutex_init(&m_LastCustomEventReferenceTimeStampMutex,NULL);
- }
-
- IIMUEventDispatcher::~IIMUEventDispatcher()
- {
- if (m_pIMUDevice)
- m_pIMUDevice->UnregisterEventDispatcher(this);
- }
-
- void IIMUEventDispatcher::SetIMU(CIMUDevice* pIMUDevice)
- {
- _MINIMAL___LOCK(m_IMUDeviceMutex)
- m_pIMUDevice = pIMUDevice;
- _MINIMAL_UNLOCK(m_IMUDeviceMutex)
- }
-
- uint32_t IIMUEventDispatcher::GetEventFlags()
- {
- _MINIMAL___LOCK(m_EventFlagsMutex)
- const uint32_t EventFlagsCurrentState = m_EventFlags;
- _MINIMAL_UNLOCK(m_EventFlagsMutex)
- return EventFlagsCurrentState;
- }
-
- void IIMUEventDispatcher::SetDispatchingMode(const DispatchingMode Mode)
- {
- _MINIMAL___LOCK(m_DispatchingModeMutex)
- m_DispatchingMode = Mode;
- _MINIMAL_UNLOCK(m_DispatchingModeMutex)
- }
-
- IIMUEventDispatcher::DispatchingMode IIMUEventDispatcher::GetDispatchingMode()
- {
- _MINIMAL___LOCK(m_DispatchingModeMutex)
- const DispatchingMode DispatchingModeCurrentState = m_DispatchingMode;
- _MINIMAL_UNLOCK(m_DispatchingModeMutex)
- return DispatchingModeCurrentState;
- }
-
- void IIMUEventDispatcher::SetMaximalPendingEvents(const uint32_t MaximalPendingEvents)
- {
- if ((MaximalPendingEvents > 1) && (MaximalPendingEvents != m_MaximalPendingEvents))
- {
- _MINIMAL___LOCK(m_MaximalPendingEventsMutex)
- m_MaximalPendingEvents = MaximalPendingEvents;
- _MINIMAL_UNLOCK(m_MaximalPendingEventsMutex)
- if (m_DispatchingMode == eDecoupled)
- PurgeEvents();
- }
- }
-
- uint32_t IIMUEventDispatcher::GetMaximalPendingEvents()
- {
- _MINIMAL___LOCK(m_MaximalPendingEventsMutex)
- const uint32_t MaximalPendingEventsCurrentState = m_MaximalPendingEvents;
- _MINIMAL_UNLOCK(m_MaximalPendingEventsMutex)
- return MaximalPendingEventsCurrentState;
- }
-
- void IIMUEventDispatcher::SetEventHandling(const CIMUEvent::EventType Type, const bool Enabled)
- {
- _MINIMAL___LOCK(m_EventFlagsMutex)
- m_EventFlags = Enabled ? (m_EventFlags | Type) : (m_EventFlags & (~Type));
- _MINIMAL_UNLOCK(m_EventFlagsMutex)
- }
-
- uint32_t IIMUEventDispatcher::GetEventHandlingFlags()
- {
- _MINIMAL___LOCK(m_EventFlagsMutex);
- const uint32_t EventHandlingFlagsCurrentState = m_EventFlags;
- _MINIMAL_UNLOCK(m_EventFlagsMutex);
- return EventHandlingFlagsCurrentState;
- }
-
- void IIMUEventDispatcher::ReceiveEvent(const CIMUEvent& Event)
- {
- _MINIMAL___LOCK(m_EventFlagsMutex)
- const bool HandelEvent = Event.GetEventType() & m_EventFlags;
- _MINIMAL_UNLOCK(m_EventFlagsMutex)
-
- if (HandelEvent)
- {
- if (m_DispatchingMode == eDecoupled)
- {
- _MINIMAL___LOCK(m_EventsQueueMutex)
- if (m_EventsQueue.size() == m_MaximalPendingEvents)
- m_EventsQueue.pop_front();
- m_EventsQueue.push_back(Event);
- _MINIMAL_UNLOCK(m_EventsQueueMutex)
-
- switch(Event.GetEventType())
- {
- case CIMUEvent::eOnIMUCycle:
- _MINIMAL___LOCK(m_LastCycleReferenceTimeStampMutex)
- gettimeofday(&m_LastCycleReferenceTimeStamp,NULL);
- _MINIMAL_UNLOCK(m_LastCycleReferenceTimeStampMutex)
- return;
- case CIMUEvent::eOnIMUFusedCycle:
- _MINIMAL___LOCK(m_LastFusedCycleReferenceTimeStampMutex)
- gettimeofday(&m_LastFusedCycleReferenceTimeStamp,NULL);
- _MINIMAL_UNLOCK(m_LastFusedCycleReferenceTimeStampMutex)
- return;
- case CIMUEvent::eOnIMUIntegratedState:
- _MINIMAL___LOCK(m_LastIntegratedStateReferenceTimeStampMutex)
- gettimeofday(&m_LastIntegratedStateReferenceTimeStamp,NULL);
- _MINIMAL_UNLOCK(m_LastIntegratedStateReferenceTimeStampMutex)
- return;
- case CIMUEvent::eOnIMUCustomEvent:
- _MINIMAL___LOCK(m_LastCustomEventReferenceTimeStampMutex)
- gettimeofday(&m_LastCustomEventReferenceTimeStamp,NULL);
- _MINIMAL_UNLOCK(m_LastCustomEventReferenceTimeStampMutex)
- return;
- case CIMUEvent::eOnIMUStart:
- _MINIMAL___LOCK(m_LastStartTimeStampMutex)
- gettimeofday(&m_LastStartTimeStamp,NULL);
- _MINIMAL_UNLOCK(m_LastStartTimeStampMutex)
- return;
- case CIMUEvent::eOnIMUStop:
- _MINIMAL___LOCK(m_LastStopTimeStampMutex)
- gettimeofday(&m_LastStopTimeStamp,NULL);
- _MINIMAL_UNLOCK(m_LastStopTimeStampMutex)
- return;
- }
- }
- else
- OnIMUEvent(Event);
- }
- }
-
- bool IIMUEventDispatcher::ProcessPendingEvent()
- {
- _MINIMAL___LOCK(m_EventsQueueMutex)
- if (m_EventsQueue.size())
- {
- OnIMUEvent(m_EventsQueue.front());
- m_EventsQueue.pop_front();
- _MINIMAL_UNLOCK(m_EventsQueueMutex)
- return true;
- }
- else
- {
- _MINIMAL_UNLOCK(m_EventsQueueMutex)
- return false;
- }
- }
-
- void IIMUEventDispatcher::SetReferenceTimeStamps(const timeval& Reference)
- {
- _MINIMAL___LOCK(m_LastStartTimeStampMutex)
- m_LastStartTimeStamp = Reference;
- _MINIMAL_UNLOCK(m_LastStartTimeStampMutex)
-
- _MINIMAL___LOCK(m_LastStopTimeStampMutex)
- m_LastStopTimeStamp = Reference;
- _MINIMAL_UNLOCK(m_LastStopTimeStampMutex)
-
- _MINIMAL___LOCK(m_LastCycleReferenceTimeStampMutex)
- m_LastCycleReferenceTimeStamp = Reference;
- _MINIMAL_UNLOCK(m_LastCycleReferenceTimeStampMutex)
-
- _MINIMAL___LOCK(m_LastFusedCycleReferenceTimeStampMutex)
- m_LastFusedCycleReferenceTimeStamp = Reference;
- _MINIMAL_UNLOCK(m_LastFusedCycleReferenceTimeStampMutex)
-
- _MINIMAL___LOCK(m_LastIntegratedStateReferenceTimeStampMutex)
- m_LastIntegratedStateReferenceTimeStamp = Reference;
- _MINIMAL_UNLOCK(m_LastIntegratedStateReferenceTimeStampMutex)
-
- _MINIMAL___LOCK(m_LastCustomEventReferenceTimeStampMutex)
- m_LastCustomEventReferenceTimeStamp = Reference;
- _MINIMAL_UNLOCK(m_LastCustomEventReferenceTimeStampMutex)
- }
-
- timeval IIMUEventDispatcher::GetLastStartTimeStamp()
- {
- _MINIMAL___LOCK(m_LastStartTimeStampMutex)
- timeval TimeStampCurrentState = m_LastStartTimeStamp;
- _MINIMAL_UNLOCK(m_LastStartTimeStampMutex)
- return TimeStampCurrentState;
- }
-
- timeval IIMUEventDispatcher::GetLastStopTimeStamp()
- {
- _MINIMAL___LOCK(m_LastStopTimeStampMutex)
- timeval TimeStampCurrentState = m_LastStopTimeStamp;
- _MINIMAL_UNLOCK(m_LastStopTimeStampMutex)
- return TimeStampCurrentState;
- }
-
- timeval IIMUEventDispatcher::GetLastCycleReferenceTimeStamp()
- {
- _MINIMAL___LOCK(m_LastCycleReferenceTimeStampMutex)
- timeval TimeStampCurrentState = m_LastCycleReferenceTimeStamp;
- _MINIMAL_UNLOCK(m_LastCycleReferenceTimeStampMutex)
- return TimeStampCurrentState;
- }
-
- timeval IIMUEventDispatcher::GetLastFusedCycleReferenceTimeStamp()
- {
- _MINIMAL___LOCK(m_LastFusedCycleReferenceTimeStampMutex)
- timeval TimeStampCurrentState = m_LastFusedCycleReferenceTimeStamp;
- _MINIMAL_UNLOCK(m_LastFusedCycleReferenceTimeStampMutex)
- return TimeStampCurrentState;
- }
-
- timeval IIMUEventDispatcher::GetLastIntegratedStateReferenceTimeStamp()
- {
- _MINIMAL___LOCK(m_LastIntegratedStateReferenceTimeStampMutex)
- timeval TimeStampCurrentState = m_LastIntegratedStateReferenceTimeStamp;
- _MINIMAL_UNLOCK(m_LastIntegratedStateReferenceTimeStampMutex)
- return TimeStampCurrentState;
- }
-
- timeval IIMUEventDispatcher::GetLastCustomEventReferenceTimeStamp()
- {
- _MINIMAL___LOCK(m_LastCustomEventReferenceTimeStampMutex)
- timeval TimeStampCurrentState = m_LastCustomEventReferenceTimeStamp;
- _MINIMAL_UNLOCK(m_LastCustomEventReferenceTimeStampMutex)
- return TimeStampCurrentState;
- }
-
- void IIMUEventDispatcher::PurgeEvents()
- {
- _MINIMAL___LOCK(m_EventsQueueMutex)
- if (m_EventsQueue.size() >= m_MaximalPendingEvents)
- {
- const uint32_t TotalEventsToRemove = (uint32_t(m_EventsQueue.size()) - m_MaximalPendingEvents) + 1;
- for(uint32_t i = 0 ; i < TotalEventsToRemove ; ++i)
- m_EventsQueue.pop_front();
- }
- _MINIMAL_UNLOCK(m_EventsQueueMutex)
- }
+ IIMUEventDispatcher::IIMUEventDispatcher(CIMUDevice* pIMUDevice) :
+ m_DispatchingMode(eCoupled), m_MaximalPendingEvents(2), m_EventFlags(0XFFFF), m_pIMUDevice(pIMUDevice), m_LastStartTimeStamp(CTimeStamp::s_Zero), m_LastStopTimeStamp(CTimeStamp::s_Zero), m_LastCycleReferenceTimeStamp(CTimeStamp::s_Zero), m_LastFusedCycleReferenceTimeStamp(CTimeStamp::s_Zero), m_LastIntegratedStateReferenceTimeStamp(CTimeStamp::s_Zero), m_LastCustomEventReferenceTimeStamp(CTimeStamp::s_Zero)
+ {
+ pthread_mutex_init(&m_DispatchingModeMutex, NULL);
+ pthread_mutex_init(&m_MaximalPendingEventsMutex, NULL);
+ pthread_mutex_init(&m_EventFlagsMutex, NULL);
+ pthread_mutex_init(&m_IMUDeviceMutex, NULL);
+ pthread_mutex_init(&m_EventsQueueMutex, NULL);
+ pthread_mutex_init(&m_LastStartTimeStampMutex, NULL);
+ pthread_mutex_init(&m_LastStopTimeStampMutex, NULL);
+ pthread_mutex_init(&m_LastCycleReferenceTimeStampMutex, NULL);
+ pthread_mutex_init(&m_LastFusedCycleReferenceTimeStampMutex, NULL);
+ pthread_mutex_init(&m_LastIntegratedStateReferenceTimeStampMutex, NULL);
+ pthread_mutex_init(&m_LastCustomEventReferenceTimeStampMutex, NULL);
+ }
+
+ IIMUEventDispatcher::IIMUEventDispatcher() :
+ m_DispatchingMode(eCoupled), m_MaximalPendingEvents(2), m_EventFlags(0XFFFF), m_pIMUDevice(NULL), m_EventsQueue(), m_LastStartTimeStamp(CTimeStamp::s_Zero), m_LastStopTimeStamp(CTimeStamp::s_Zero), m_LastCycleReferenceTimeStamp(CTimeStamp::s_Zero), m_LastFusedCycleReferenceTimeStamp(CTimeStamp::s_Zero), m_LastIntegratedStateReferenceTimeStamp(CTimeStamp::s_Zero), m_LastCustomEventReferenceTimeStamp(CTimeStamp::s_Zero)
+ {
+ pthread_mutex_init(&m_DispatchingModeMutex, NULL);
+ pthread_mutex_init(&m_MaximalPendingEventsMutex, NULL);
+ pthread_mutex_init(&m_EventFlagsMutex, NULL);
+ pthread_mutex_init(&m_IMUDeviceMutex, NULL);
+ pthread_mutex_init(&m_EventsQueueMutex, NULL);
+ pthread_mutex_init(&m_LastStartTimeStampMutex, NULL);
+ pthread_mutex_init(&m_LastStopTimeStampMutex, NULL);
+ pthread_mutex_init(&m_LastCycleReferenceTimeStampMutex, NULL);
+ pthread_mutex_init(&m_LastFusedCycleReferenceTimeStampMutex, NULL);
+ pthread_mutex_init(&m_LastIntegratedStateReferenceTimeStampMutex, NULL);
+ pthread_mutex_init(&m_LastCustomEventReferenceTimeStampMutex, NULL);
+ }
+
+ IIMUEventDispatcher::~IIMUEventDispatcher()
+ {
+ if (m_pIMUDevice)
+ {
+ m_pIMUDevice->UnregisterEventDispatcher(this);
+ }
+ }
+
+ void IIMUEventDispatcher::SetIMU(CIMUDevice* pIMUDevice)
+ {
+ _MINIMAL___LOCK(m_IMUDeviceMutex)
+ m_pIMUDevice = pIMUDevice;
+ _MINIMAL_UNLOCK(m_IMUDeviceMutex)
+ }
+
+ uint32_t IIMUEventDispatcher::GetEventFlags()
+ {
+ _MINIMAL___LOCK(m_EventFlagsMutex)
+ const uint32_t EventFlagsCurrentState = m_EventFlags;
+ _MINIMAL_UNLOCK(m_EventFlagsMutex)
+ return EventFlagsCurrentState;
+ }
+
+ void IIMUEventDispatcher::SetDispatchingMode(const DispatchingMode Mode)
+ {
+ _MINIMAL___LOCK(m_DispatchingModeMutex)
+ m_DispatchingMode = Mode;
+ _MINIMAL_UNLOCK(m_DispatchingModeMutex)
+ }
+
+ IIMUEventDispatcher::DispatchingMode IIMUEventDispatcher::GetDispatchingMode()
+ {
+ _MINIMAL___LOCK(m_DispatchingModeMutex)
+ const DispatchingMode DispatchingModeCurrentState = m_DispatchingMode;
+ _MINIMAL_UNLOCK(m_DispatchingModeMutex)
+ return DispatchingModeCurrentState;
+ }
+
+ void IIMUEventDispatcher::SetMaximalPendingEvents(const uint32_t MaximalPendingEvents)
+ {
+ if ((MaximalPendingEvents > 1) && (MaximalPendingEvents != m_MaximalPendingEvents))
+ {
+ _MINIMAL___LOCK(m_MaximalPendingEventsMutex)
+ m_MaximalPendingEvents = MaximalPendingEvents;
+ _MINIMAL_UNLOCK(m_MaximalPendingEventsMutex)
+
+ if (m_DispatchingMode == eDecoupled)
+ {
+ PurgeEvents();
+ }
+ }
+ }
+
+ uint32_t IIMUEventDispatcher::GetMaximalPendingEvents()
+ {
+ _MINIMAL___LOCK(m_MaximalPendingEventsMutex)
+ const uint32_t MaximalPendingEventsCurrentState = m_MaximalPendingEvents;
+ _MINIMAL_UNLOCK(m_MaximalPendingEventsMutex)
+ return MaximalPendingEventsCurrentState;
+ }
+
+ void IIMUEventDispatcher::SetEventHandling(const CIMUEvent::EventType Type, const bool Enabled)
+ {
+ _MINIMAL___LOCK(m_EventFlagsMutex)
+ m_EventFlags = Enabled ? (m_EventFlags | Type) : (m_EventFlags & (~Type));
+ _MINIMAL_UNLOCK(m_EventFlagsMutex)
+ }
+
+ uint32_t IIMUEventDispatcher::GetEventHandlingFlags()
+ {
+ _MINIMAL___LOCK(m_EventFlagsMutex);
+ const uint32_t EventHandlingFlagsCurrentState = m_EventFlags;
+ _MINIMAL_UNLOCK(m_EventFlagsMutex);
+ return EventHandlingFlagsCurrentState;
+ }
+
+ void IIMUEventDispatcher::ReceiveEvent(const CIMUEvent& Event)
+ {
+ _MINIMAL___LOCK(m_EventFlagsMutex)
+ const bool HandelEvent = Event.GetEventType() & m_EventFlags;
+ _MINIMAL_UNLOCK(m_EventFlagsMutex)
+
+ if (HandelEvent)
+ {
+ if (m_DispatchingMode == eDecoupled)
+ {
+ _MINIMAL___LOCK(m_EventsQueueMutex)
+
+ if (m_EventsQueue.size() == m_MaximalPendingEvents)
+ {
+ m_EventsQueue.pop_front();
+ }
+
+ m_EventsQueue.push_back(Event);
+ _MINIMAL_UNLOCK(m_EventsQueueMutex)
+
+ switch (Event.GetEventType())
+ {
+ case CIMUEvent::eOnIMUCycle:
+ _MINIMAL___LOCK(m_LastCycleReferenceTimeStampMutex)
+ gettimeofday(&m_LastCycleReferenceTimeStamp, NULL);
+ _MINIMAL_UNLOCK(m_LastCycleReferenceTimeStampMutex)
+ return;
+
+ case CIMUEvent::eOnIMUFusedCycle:
+ _MINIMAL___LOCK(m_LastFusedCycleReferenceTimeStampMutex)
+ gettimeofday(&m_LastFusedCycleReferenceTimeStamp, NULL);
+ _MINIMAL_UNLOCK(m_LastFusedCycleReferenceTimeStampMutex)
+ return;
+
+ case CIMUEvent::eOnIMUIntegratedState:
+ _MINIMAL___LOCK(m_LastIntegratedStateReferenceTimeStampMutex)
+ gettimeofday(&m_LastIntegratedStateReferenceTimeStamp, NULL);
+ _MINIMAL_UNLOCK(m_LastIntegratedStateReferenceTimeStampMutex)
+ return;
+
+ case CIMUEvent::eOnIMUCustomEvent:
+ _MINIMAL___LOCK(m_LastCustomEventReferenceTimeStampMutex)
+ gettimeofday(&m_LastCustomEventReferenceTimeStamp, NULL);
+ _MINIMAL_UNLOCK(m_LastCustomEventReferenceTimeStampMutex)
+ return;
+
+ case CIMUEvent::eOnIMUStart:
+ _MINIMAL___LOCK(m_LastStartTimeStampMutex)
+ gettimeofday(&m_LastStartTimeStamp, NULL);
+ _MINIMAL_UNLOCK(m_LastStartTimeStampMutex)
+ return;
+
+ case CIMUEvent::eOnIMUStop:
+ _MINIMAL___LOCK(m_LastStopTimeStampMutex)
+ gettimeofday(&m_LastStopTimeStamp, NULL);
+ _MINIMAL_UNLOCK(m_LastStopTimeStampMutex)
+ return;
+ }
+ }
+ else
+ {
+ OnIMUEvent(Event);
+ }
+ }
+ }
+
+ bool IIMUEventDispatcher::ProcessPendingEvent()
+ {
+ _MINIMAL___LOCK(m_EventsQueueMutex)
+
+ if (m_EventsQueue.size())
+ {
+ OnIMUEvent(m_EventsQueue.front());
+ m_EventsQueue.pop_front();
+ _MINIMAL_UNLOCK(m_EventsQueueMutex)
+ return true;
+ }
+ else
+ {
+ _MINIMAL_UNLOCK(m_EventsQueueMutex)
+ return false;
+ }
+ }
+
+ void IIMUEventDispatcher::SetReferenceTimeStamps(const timeval& Reference)
+ {
+ _MINIMAL___LOCK(m_LastStartTimeStampMutex)
+ m_LastStartTimeStamp = Reference;
+ _MINIMAL_UNLOCK(m_LastStartTimeStampMutex)
+
+ _MINIMAL___LOCK(m_LastStopTimeStampMutex)
+ m_LastStopTimeStamp = Reference;
+ _MINIMAL_UNLOCK(m_LastStopTimeStampMutex)
+
+ _MINIMAL___LOCK(m_LastCycleReferenceTimeStampMutex)
+ m_LastCycleReferenceTimeStamp = Reference;
+ _MINIMAL_UNLOCK(m_LastCycleReferenceTimeStampMutex)
+
+ _MINIMAL___LOCK(m_LastFusedCycleReferenceTimeStampMutex)
+ m_LastFusedCycleReferenceTimeStamp = Reference;
+ _MINIMAL_UNLOCK(m_LastFusedCycleReferenceTimeStampMutex)
+
+ _MINIMAL___LOCK(m_LastIntegratedStateReferenceTimeStampMutex)
+ m_LastIntegratedStateReferenceTimeStamp = Reference;
+ _MINIMAL_UNLOCK(m_LastIntegratedStateReferenceTimeStampMutex)
+
+ _MINIMAL___LOCK(m_LastCustomEventReferenceTimeStampMutex)
+ m_LastCustomEventReferenceTimeStamp = Reference;
+ _MINIMAL_UNLOCK(m_LastCustomEventReferenceTimeStampMutex)
+ }
+
+ timeval IIMUEventDispatcher::GetLastStartTimeStamp()
+ {
+ _MINIMAL___LOCK(m_LastStartTimeStampMutex)
+ timeval TimeStampCurrentState = m_LastStartTimeStamp;
+ _MINIMAL_UNLOCK(m_LastStartTimeStampMutex)
+ return TimeStampCurrentState;
+ }
+
+ timeval IIMUEventDispatcher::GetLastStopTimeStamp()
+ {
+ _MINIMAL___LOCK(m_LastStopTimeStampMutex)
+ timeval TimeStampCurrentState = m_LastStopTimeStamp;
+ _MINIMAL_UNLOCK(m_LastStopTimeStampMutex)
+ return TimeStampCurrentState;
+ }
+
+ timeval IIMUEventDispatcher::GetLastCycleReferenceTimeStamp()
+ {
+ _MINIMAL___LOCK(m_LastCycleReferenceTimeStampMutex)
+ timeval TimeStampCurrentState = m_LastCycleReferenceTimeStamp;
+ _MINIMAL_UNLOCK(m_LastCycleReferenceTimeStampMutex)
+ return TimeStampCurrentState;
+ }
+
+ timeval IIMUEventDispatcher::GetLastFusedCycleReferenceTimeStamp()
+ {
+ _MINIMAL___LOCK(m_LastFusedCycleReferenceTimeStampMutex)
+ timeval TimeStampCurrentState = m_LastFusedCycleReferenceTimeStamp;
+ _MINIMAL_UNLOCK(m_LastFusedCycleReferenceTimeStampMutex)
+ return TimeStampCurrentState;
+ }
+
+ timeval IIMUEventDispatcher::GetLastIntegratedStateReferenceTimeStamp()
+ {
+ _MINIMAL___LOCK(m_LastIntegratedStateReferenceTimeStampMutex)
+ timeval TimeStampCurrentState = m_LastIntegratedStateReferenceTimeStamp;
+ _MINIMAL_UNLOCK(m_LastIntegratedStateReferenceTimeStampMutex)
+ return TimeStampCurrentState;
+ }
+
+ timeval IIMUEventDispatcher::GetLastCustomEventReferenceTimeStamp()
+ {
+ _MINIMAL___LOCK(m_LastCustomEventReferenceTimeStampMutex)
+ timeval TimeStampCurrentState = m_LastCustomEventReferenceTimeStamp;
+ _MINIMAL_UNLOCK(m_LastCustomEventReferenceTimeStampMutex)
+ return TimeStampCurrentState;
+ }
+
+ void IIMUEventDispatcher::PurgeEvents()
+ {
+ _MINIMAL___LOCK(m_EventsQueueMutex)
+
+ if (m_EventsQueue.size() >= m_MaximalPendingEvents)
+ {
+ const uint32_t TotalEventsToRemove = (uint32_t(m_EventsQueue.size()) - m_MaximalPendingEvents) + 1;
+
+ for (uint32_t i = 0 ; i < TotalEventsToRemove ; ++i)
+ {
+ m_EventsQueue.pop_front();
+ }
+ }
+
+ _MINIMAL_UNLOCK(m_EventsQueueMutex)
+ }
}
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IIMUEventDispatcher.h b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IIMUEventDispatcher.h
index 2693fca7afa038a4043c4b90bd74ab25854f6c4f..d5a073128d0fa9726e3e7fc47a43d38b854c1b28 100644
--- a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IIMUEventDispatcher.h
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IIMUEventDispatcher.h
@@ -3,7 +3,7 @@
*
* Created on: Mar 16, 2014
* Author: Dr.-Ing. David Israel González Aguirre
- * Mail: david.gonzalez@kit.edu
+ * Mail: david.gonzalez@kit.edu
*/
#ifndef IIMUEVENTDISPATCHER_H_
@@ -15,86 +15,86 @@
namespace IMU
{
- class CIMUDevice;
- class IIMUEventDispatcher
- {
- public:
-
- enum DispatchingMode
- {
- eCoupled, eDecoupled
- };
-
- IIMUEventDispatcher(CIMUDevice* pIMUDevice);
- IIMUEventDispatcher();
-
- virtual ~IIMUEventDispatcher();
-
- void SetIMU(CIMUDevice* pIMUDevice);
-
- uint32_t GetEventFlags();
-
- void SetDispatchingMode(const DispatchingMode Mode);
- DispatchingMode GetDispatchingMode();
-
- void SetMaximalPendingEvents(const uint32_t MaximalPendingEvents);
- uint32_t GetMaximalPendingEvents();
-
- void SetEventHandling(const CIMUEvent::EventType Type, const bool Enabled);
- uint32_t GetEventHandlingFlags();
-
- void ReceiveEvent(const CIMUEvent& Event);
-
- inline uint32_t GetTotalPendingEvents()
- {
- return uint32_t(m_EventsQueue.size());
- }
-
- inline bool HasPendingEvents()
- {
- return m_EventsQueue.size();
- }
-
- bool ProcessPendingEvent();
-
- void SetReferenceTimeStamps(const timeval& Reference);
-
- timeval GetLastStartTimeStamp();
- timeval GetLastStopTimeStamp();
- timeval GetLastCycleReferenceTimeStamp();
- timeval GetLastFusedCycleReferenceTimeStamp();
- timeval GetLastIntegratedStateReferenceTimeStamp();
- timeval GetLastCustomEventReferenceTimeStamp();
-
- virtual void OnIMUEvent(const CIMUEvent& Event) = 0;
-
- private:
-
- void PurgeEvents();
-
- DispatchingMode m_DispatchingMode;
- pthread_mutex_t m_DispatchingModeMutex;
- uint32_t m_MaximalPendingEvents;
- pthread_mutex_t m_MaximalPendingEventsMutex;
- uint32_t m_EventFlags;
- pthread_mutex_t m_EventFlagsMutex;
- CIMUDevice* m_pIMUDevice;
- pthread_mutex_t m_IMUDeviceMutex;
- std::list<CIMUEvent> m_EventsQueue;
- pthread_mutex_t m_EventsQueueMutex;
- timeval m_LastStartTimeStamp;
- pthread_mutex_t m_LastStartTimeStampMutex;
- timeval m_LastStopTimeStamp;
- pthread_mutex_t m_LastStopTimeStampMutex;
- timeval m_LastCycleReferenceTimeStamp;
- pthread_mutex_t m_LastCycleReferenceTimeStampMutex;
- timeval m_LastFusedCycleReferenceTimeStamp;
- pthread_mutex_t m_LastFusedCycleReferenceTimeStampMutex;
- timeval m_LastIntegratedStateReferenceTimeStamp;
- pthread_mutex_t m_LastIntegratedStateReferenceTimeStampMutex;
- timeval m_LastCustomEventReferenceTimeStamp;
- pthread_mutex_t m_LastCustomEventReferenceTimeStampMutex;
- };
+ class CIMUDevice;
+ class IIMUEventDispatcher
+ {
+ public:
+
+ enum DispatchingMode
+ {
+ eCoupled, eDecoupled
+ };
+
+ IIMUEventDispatcher(CIMUDevice* pIMUDevice);
+ IIMUEventDispatcher();
+
+ virtual ~IIMUEventDispatcher();
+
+ void SetIMU(CIMUDevice* pIMUDevice);
+
+ uint32_t GetEventFlags();
+
+ void SetDispatchingMode(const DispatchingMode Mode);
+ DispatchingMode GetDispatchingMode();
+
+ void SetMaximalPendingEvents(const uint32_t MaximalPendingEvents);
+ uint32_t GetMaximalPendingEvents();
+
+ void SetEventHandling(const CIMUEvent::EventType Type, const bool Enabled);
+ uint32_t GetEventHandlingFlags();
+
+ void ReceiveEvent(const CIMUEvent& Event);
+
+ inline uint32_t GetTotalPendingEvents()
+ {
+ return uint32_t(m_EventsQueue.size());
+ }
+
+ inline bool HasPendingEvents()
+ {
+ return m_EventsQueue.size();
+ }
+
+ bool ProcessPendingEvent();
+
+ void SetReferenceTimeStamps(const timeval& Reference);
+
+ timeval GetLastStartTimeStamp();
+ timeval GetLastStopTimeStamp();
+ timeval GetLastCycleReferenceTimeStamp();
+ timeval GetLastFusedCycleReferenceTimeStamp();
+ timeval GetLastIntegratedStateReferenceTimeStamp();
+ timeval GetLastCustomEventReferenceTimeStamp();
+
+ virtual void OnIMUEvent(const CIMUEvent& Event) = 0;
+
+ private:
+
+ void PurgeEvents();
+
+ DispatchingMode m_DispatchingMode;
+ pthread_mutex_t m_DispatchingModeMutex;
+ uint32_t m_MaximalPendingEvents;
+ pthread_mutex_t m_MaximalPendingEventsMutex;
+ uint32_t m_EventFlags;
+ pthread_mutex_t m_EventFlagsMutex;
+ CIMUDevice* m_pIMUDevice;
+ pthread_mutex_t m_IMUDeviceMutex;
+ std::list<CIMUEvent> m_EventsQueue;
+ pthread_mutex_t m_EventsQueueMutex;
+ timeval m_LastStartTimeStamp;
+ pthread_mutex_t m_LastStartTimeStampMutex;
+ timeval m_LastStopTimeStamp;
+ pthread_mutex_t m_LastStopTimeStampMutex;
+ timeval m_LastCycleReferenceTimeStamp;
+ pthread_mutex_t m_LastCycleReferenceTimeStampMutex;
+ timeval m_LastFusedCycleReferenceTimeStamp;
+ pthread_mutex_t m_LastFusedCycleReferenceTimeStampMutex;
+ timeval m_LastIntegratedStateReferenceTimeStamp;
+ pthread_mutex_t m_LastIntegratedStateReferenceTimeStampMutex;
+ timeval m_LastCustomEventReferenceTimeStamp;
+ pthread_mutex_t m_LastCustomEventReferenceTimeStampMutex;
+ };
}
#endif
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMU.h b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMU.h
index 205cc8b596aec2f0247e3e9f81224735c87eca76..0eb554c34a855467d284dc3108c94e0027a0c459 100644
--- a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMU.h
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMU.h
@@ -3,7 +3,7 @@
*
* Created on: Mar 17, 2014
* Author: Dr.-Ing. David Israel González Aguirre
- * Mail: david.gonzalez@kit.edu
+ * Mail: david.gonzalez@kit.edu
*/
#ifndef IMU_H_
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDeducedReckoning.cpp b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDeducedReckoning.cpp
index c386a4fdb8c2ab8ece76ce0f212743da9ba50480..0a1a7f7c3e1cd43a64fe6fb0e1f7d6b2dc8ad27d 100644
--- a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDeducedReckoning.cpp
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDeducedReckoning.cpp
@@ -3,169 +3,188 @@
*
* Created on: Mar 16, 2014
* Author: Dr.-Ing. David Israel González Aguirre
- * Mail: david.gonzalez@kit.edu
+ * Mail: david.gonzalez@kit.edu
*/
#include "IMUDeducedReckoning.h"
namespace IMU
{
- CIMUDeducedReckoning::CIMUDeducedReckoning(const bool IsVerbose) :
- IIMUEventDispatcher(), m_IsVerbose(IsVerbose), m_G(IMU::CGeolocationInformation::GetGravitationalAcceleration())
- {
- SetEventHandling(CIMUEvent::eOnIMUStart,true);
- SetEventHandling(CIMUEvent::eOnIMUStop,true);
- SetEventHandling(CIMUEvent::eOnIMUCycle,false);
- SetEventHandling(CIMUEvent::eOnIMUFusedCycle,true);
- SetEventHandling(CIMUEvent::eOnIMUIntegratedState,false);
- SetEventHandling(CIMUEvent::eOnIMUCustomEvent,true);
- }
-
- CIMUDeducedReckoning::~CIMUDeducedReckoning()
- {
- }
-
- void CIMUDeducedReckoning::OnIMUEvent(const CIMUEvent& Event)
- {
- switch(Event.GetEventType())
- {
- case CIMUEvent::eOnIMUCycle:
- OnIMUCycle(Event.GetTimeStamp(),Event.GetIMU());
- break;
- case CIMUEvent::eOnIMUFusedCycle:
- OnIMUFusedCycle(Event.GetTimeStamp(),Event.GetIMU());
- break;
- case CIMUEvent::eOnIMUIntegratedState:
- OnIMUIntegratedState(Event.GetTimeStamp(),Event.GetIMU());
- break;
- case CIMUEvent::eOnIMUCustomEvent:
- OnIMUCustomEvent(Event);
- break;
- case CIMUEvent::eOnIMUStart:
- OnIMUStart(Event.GetTimeStamp(),Event.GetIMU());
- break;
- case CIMUEvent::eOnIMUStop:
- OnIMUStop(Event.GetTimeStamp(),Event.GetIMU());
- break;
- }
- }
-
- void CIMUDeducedReckoning::OnIMUStart(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
- {
- if (m_IsVerbose)
- {
- std::cout << "OnIMUStart(IMU Device ID = " << pIMUDevice->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << ")" << std::endl;
- std::cout << "\t[ Global time = " << CTimeStamp::GetElapsedSeconds(TimeStamp,pIMUDevice->GetReferenceTimeStamp()) << " s]" << std::endl;
- std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(TimeStamp,GetLastStartTimeStamp()) << "]" << std::endl;
- if (GetDispatchingMode() == eDecoupled)
- {
- std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
- std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
- }
- }
- }
-
- void CIMUDeducedReckoning::OnIMUStop(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
- {
- if (m_IsVerbose)
- {
- std::cout << "OnIMUStop(IMU Device ID = " << pIMUDevice->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << ")" << std::endl;
- std::cout << "\t[ Global time = " << CTimeStamp::GetElapsedSeconds(TimeStamp,pIMUDevice->GetReferenceTimeStamp()) << " s]" << std::endl;
- std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(TimeStamp,GetLastStopTimeStamp()) << "]" << std::endl;
- if (GetDispatchingMode() == eDecoupled)
- {
- std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
- std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
- }
- }
- }
-
- void CIMUDeducedReckoning::OnIMUCycle(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
- {
- const IMUState CurrentState = pIMUDevice->GetIMUState();
- if (m_IsVerbose)
- {
- std::cout << "OnIMUCycle(IMU Device ID = 0x" << std::hex << pIMUDevice->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << std::dec << ")" << std::endl;
- std::cout << "\t[ Global time = " << CTimeStamp::GetElapsedSeconds(TimeStamp,pIMUDevice->GetReferenceTimeStamp()) << " s]" << std::endl;
- std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(TimeStamp,GetLastCycleReferenceTimeStamp()) << " µs]" << std::endl;
- if (GetDispatchingMode() == eDecoupled)
- {
- std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
- std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
- }
- std::cout << "\t[ Cycle = " << CurrentState.m_ControlData.m_MessageCounter << "]" << std::endl;
- std::cout << "\t[ Acceleration = [" << CurrentState.m_PhysicalData.m_Acceleration[0] << "," << CurrentState.m_PhysicalData.m_Acceleration[1] << "," << CurrentState.m_PhysicalData.m_Acceleration[2] << "]" << std::endl;
- std::cout << "\t[ Acceleration Magnitude = [" << std::abs(CurrentState.m_PhysicalData.m_AccelerationMagnitud - m_G) * 1000.0f << " mm/s^2]" << std::endl;
- std::cout << "\t[ Gyroscope Rotation = [" << CurrentState.m_PhysicalData.m_GyroscopeRotation[0] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[1] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[2] << "]" << std::endl;
- std::cout << "\t[ Magnetic Rotation = [" << CurrentState.m_PhysicalData.m_MagneticRotation[0] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[1] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[2] << "]" << std::endl;
- std::cout << "\t[ Quaternion Rotation = [" << CurrentState.m_PhysicalData.m_QuaternionRotation[0] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[1] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[2] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[3] << "]" << std::endl;
- }
- memcpy(m_OrientationQuaternion,CurrentState.m_PhysicalData.m_QuaternionRotation,sizeof(float) * 4);
- memcpy(m_MagneticRotation,CurrentState.m_PhysicalData.m_MagneticRotation,sizeof(float) * 3);
- memcpy(m_GyroscopeRotation,CurrentState.m_PhysicalData.m_GyroscopeRotation,sizeof(float) * 3);
- memcpy(m_Accelaration,CurrentState.m_PhysicalData.m_Acceleration,sizeof(float) * 3);
- }
-
- void CIMUDeducedReckoning::OnIMUFusedCycle(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
- {
- const IMUState CurrentState = pIMUDevice->GetIMUState();
- if (m_IsVerbose)
- {
- std::cout << "OnIMUFusedCycle(IMU Device ID = 0x" << std::hex << pIMUDevice->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << std::dec << ")" << std::endl;
- std::cout << "\t[ Global time = " << CTimeStamp::GetElapsedSeconds(TimeStamp,pIMUDevice->GetReferenceTimeStamp()) << " s]" << std::endl;
- std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(TimeStamp,GetLastFusedCycleReferenceTimeStamp()) << " µs]" << std::endl;
- if (GetDispatchingMode() == eDecoupled)
- {
- std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
- std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
- }
- std::cout << "\t[ Cycle = " << CurrentState.m_ControlData.m_MessageCounter << "]" << std::endl;
- std::cout << "\t[ Acceleration = [" << CurrentState.m_PhysicalData.m_Acceleration[0] << "," << CurrentState.m_PhysicalData.m_Acceleration[1] << "," << CurrentState.m_PhysicalData.m_Acceleration[2] << "]" << std::endl;
- std::cout << "\t[ Acceleration Magnitude = [" << std::abs(CurrentState.m_PhysicalData.m_AccelerationMagnitud - m_G) * 1000.0f << " mm/s^2]" << std::endl;
- std::cout << "\t[ Gyroscope Rotation = [" << CurrentState.m_PhysicalData.m_GyroscopeRotation[0] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[1] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[2] << "]" << std::endl;
- std::cout << "\t[ Magnetic Rotation = [" << CurrentState.m_PhysicalData.m_MagneticRotation[0] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[1] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[2] << "]" << std::endl;
- std::cout << "\t[ Quaternion Rotation = [" << CurrentState.m_PhysicalData.m_QuaternionRotation[0] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[1] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[2] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[3] << "]" << std::endl;
- }
- memcpy(m_OrientationQuaternion,CurrentState.m_PhysicalData.m_QuaternionRotation,sizeof(float) * 4);
- memcpy(m_MagneticRotation,CurrentState.m_PhysicalData.m_MagneticRotation,sizeof(float) * 3);
- memcpy(m_GyroscopeRotation,CurrentState.m_PhysicalData.m_GyroscopeRotation,sizeof(float) * 3);
- memcpy(m_Accelaration,CurrentState.m_PhysicalData.m_Acceleration,sizeof(float) * 3);
- }
-
- void CIMUDeducedReckoning::OnIMUIntegratedState(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
- {
- const IMUState CurrentState = pIMUDevice->GetIMUState();
- if (m_IsVerbose)
- {
- std::cout << "OnIMUStateUpdate(IMU Device ID = 0x" << std::hex << pIMUDevice->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << std::dec << ")" << std::endl;
- std::cout << "\t[ Global time = " << CTimeStamp::GetElapsedSeconds(TimeStamp,pIMUDevice->GetReferenceTimeStamp()) << " s]" << std::endl;
- std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(TimeStamp,GetLastIntegratedStateReferenceTimeStamp()) << " µs]" << std::endl;
- if (GetDispatchingMode() == eDecoupled)
- {
- std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
- std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
- }
- std::cout << "\t[ Cycle = " << CurrentState.m_ControlData.m_MessageCounter << "]" << std::endl;
- std::cout << "\t[ Acceleration = [" << CurrentState.m_PhysicalData.m_Acceleration[0] << "," << CurrentState.m_PhysicalData.m_Acceleration[1] << "," << CurrentState.m_PhysicalData.m_Acceleration[2] << "]" << std::endl;
- std::cout << "\t[ Acceleration Magnitude = [" << std::abs(CurrentState.m_PhysicalData.m_AccelerationMagnitud - m_G) * 1000.0f << " mm/s^2]" << std::endl;
- std::cout << "\t[ Gyroscope Rotation = [" << CurrentState.m_PhysicalData.m_GyroscopeRotation[0] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[1] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[2] << "]" << std::endl;
- std::cout << "\t[ Magnetic Rotation = [" << CurrentState.m_PhysicalData.m_MagneticRotation[0] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[1] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[2] << "]" << std::endl;
- std::cout << "\t[ Quaternion Rotation = [" << CurrentState.m_PhysicalData.m_QuaternionRotation[0] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[1] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[2] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[3] << "]" << std::endl;
- }
- }
-
- void CIMUDeducedReckoning::OnIMUCustomEvent(const CIMUEvent& CustomEvent)
- {
- if (m_IsVerbose)
- {
- std::cout << "OnIMUCustomEvent(IMU Device ID = 0x" << std::hex << CustomEvent.GetIMU()->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << std::dec << ")" << std::endl;
- std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(CustomEvent.GetTimeStamp(),CustomEvent.GetIMU()->GetReferenceTimeStamp()) << " µs]" << std::endl;
- std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(CustomEvent.GetTimeStamp(),GetLastCustomEventReferenceTimeStamp()) << " µs]" << std::endl;
- if (GetDispatchingMode() == eDecoupled)
- {
- std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
- std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
- }
- }
- }
+ CIMUDeducedReckoning::CIMUDeducedReckoning(const bool IsVerbose) :
+ IIMUEventDispatcher(), m_IsVerbose(IsVerbose), m_G(IMU::CGeolocationInformation::GetGravitationalAcceleration())
+ {
+ SetEventHandling(CIMUEvent::eOnIMUStart, true);
+ SetEventHandling(CIMUEvent::eOnIMUStop, true);
+ SetEventHandling(CIMUEvent::eOnIMUCycle, false);
+ SetEventHandling(CIMUEvent::eOnIMUFusedCycle, true);
+ SetEventHandling(CIMUEvent::eOnIMUIntegratedState, false);
+ SetEventHandling(CIMUEvent::eOnIMUCustomEvent, true);
+ }
+
+ CIMUDeducedReckoning::~CIMUDeducedReckoning()
+ {
+ }
+
+ void CIMUDeducedReckoning::OnIMUEvent(const CIMUEvent& Event)
+ {
+ switch (Event.GetEventType())
+ {
+ case CIMUEvent::eOnIMUCycle:
+ OnIMUCycle(Event.GetTimeStamp(), Event.GetIMU());
+ break;
+
+ case CIMUEvent::eOnIMUFusedCycle:
+ OnIMUFusedCycle(Event.GetTimeStamp(), Event.GetIMU());
+ break;
+
+ case CIMUEvent::eOnIMUIntegratedState:
+ OnIMUIntegratedState(Event.GetTimeStamp(), Event.GetIMU());
+ break;
+
+ case CIMUEvent::eOnIMUCustomEvent:
+ OnIMUCustomEvent(Event);
+ break;
+
+ case CIMUEvent::eOnIMUStart:
+ OnIMUStart(Event.GetTimeStamp(), Event.GetIMU());
+ break;
+
+ case CIMUEvent::eOnIMUStop:
+ OnIMUStop(Event.GetTimeStamp(), Event.GetIMU());
+ break;
+ }
+ }
+
+ void CIMUDeducedReckoning::OnIMUStart(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
+ {
+ if (m_IsVerbose)
+ {
+ std::cout << "OnIMUStart(IMU Device ID = " << pIMUDevice->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << ")" << std::endl;
+ std::cout << "\t[ Global time = " << CTimeStamp::GetElapsedSeconds(TimeStamp, pIMUDevice->GetReferenceTimeStamp()) << " s]" << std::endl;
+ std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(TimeStamp, GetLastStartTimeStamp()) << "]" << std::endl;
+
+ if (GetDispatchingMode() == eDecoupled)
+ {
+ std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
+ std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
+ }
+ }
+ }
+
+ void CIMUDeducedReckoning::OnIMUStop(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
+ {
+ if (m_IsVerbose)
+ {
+ std::cout << "OnIMUStop(IMU Device ID = " << pIMUDevice->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << ")" << std::endl;
+ std::cout << "\t[ Global time = " << CTimeStamp::GetElapsedSeconds(TimeStamp, pIMUDevice->GetReferenceTimeStamp()) << " s]" << std::endl;
+ std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(TimeStamp, GetLastStopTimeStamp()) << "]" << std::endl;
+
+ if (GetDispatchingMode() == eDecoupled)
+ {
+ std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
+ std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
+ }
+ }
+ }
+
+ void CIMUDeducedReckoning::OnIMUCycle(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
+ {
+ const IMUState CurrentState = pIMUDevice->GetIMUState();
+
+ if (m_IsVerbose)
+ {
+ std::cout << "OnIMUCycle(IMU Device ID = 0x" << std::hex << pIMUDevice->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << std::dec << ")" << std::endl;
+ std::cout << "\t[ Global time = " << CTimeStamp::GetElapsedSeconds(TimeStamp, pIMUDevice->GetReferenceTimeStamp()) << " s]" << std::endl;
+ std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(TimeStamp, GetLastCycleReferenceTimeStamp()) << " µs]" << std::endl;
+
+ if (GetDispatchingMode() == eDecoupled)
+ {
+ std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
+ std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
+ }
+
+ std::cout << "\t[ Cycle = " << CurrentState.m_ControlData.m_MessageCounter << "]" << std::endl;
+ std::cout << "\t[ Acceleration = [" << CurrentState.m_PhysicalData.m_Acceleration[0] << "," << CurrentState.m_PhysicalData.m_Acceleration[1] << "," << CurrentState.m_PhysicalData.m_Acceleration[2] << "]" << std::endl;
+ std::cout << "\t[ Acceleration Magnitude = [" << std::abs(CurrentState.m_PhysicalData.m_AccelerationMagnitud - m_G) * 1000.0f << " mm/s^2]" << std::endl;
+ std::cout << "\t[ Gyroscope Rotation = [" << CurrentState.m_PhysicalData.m_GyroscopeRotation[0] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[1] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[2] << "]" << std::endl;
+ std::cout << "\t[ Magnetic Rotation = [" << CurrentState.m_PhysicalData.m_MagneticRotation[0] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[1] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[2] << "]" << std::endl;
+ std::cout << "\t[ Quaternion Rotation = [" << CurrentState.m_PhysicalData.m_QuaternionRotation[0] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[1] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[2] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[3] << "]" << std::endl;
+ }
+
+ memcpy(m_OrientationQuaternion, CurrentState.m_PhysicalData.m_QuaternionRotation, sizeof(float) * 4);
+ memcpy(m_MagneticRotation, CurrentState.m_PhysicalData.m_MagneticRotation, sizeof(float) * 3);
+ memcpy(m_GyroscopeRotation, CurrentState.m_PhysicalData.m_GyroscopeRotation, sizeof(float) * 3);
+ memcpy(m_Accelaration, CurrentState.m_PhysicalData.m_Acceleration, sizeof(float) * 3);
+ }
+
+ void CIMUDeducedReckoning::OnIMUFusedCycle(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
+ {
+ const IMUState CurrentState = pIMUDevice->GetIMUState();
+
+ if (m_IsVerbose)
+ {
+ std::cout << "OnIMUFusedCycle(IMU Device ID = 0x" << std::hex << pIMUDevice->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << std::dec << ")" << std::endl;
+ std::cout << "\t[ Global time = " << CTimeStamp::GetElapsedSeconds(TimeStamp, pIMUDevice->GetReferenceTimeStamp()) << " s]" << std::endl;
+ std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(TimeStamp, GetLastFusedCycleReferenceTimeStamp()) << " µs]" << std::endl;
+
+ if (GetDispatchingMode() == eDecoupled)
+ {
+ std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
+ std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
+ }
+
+ std::cout << "\t[ Cycle = " << CurrentState.m_ControlData.m_MessageCounter << "]" << std::endl;
+ std::cout << "\t[ Acceleration = [" << CurrentState.m_PhysicalData.m_Acceleration[0] << "," << CurrentState.m_PhysicalData.m_Acceleration[1] << "," << CurrentState.m_PhysicalData.m_Acceleration[2] << "]" << std::endl;
+ std::cout << "\t[ Acceleration Magnitude = [" << std::abs(CurrentState.m_PhysicalData.m_AccelerationMagnitud - m_G) * 1000.0f << " mm/s^2]" << std::endl;
+ std::cout << "\t[ Gyroscope Rotation = [" << CurrentState.m_PhysicalData.m_GyroscopeRotation[0] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[1] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[2] << "]" << std::endl;
+ std::cout << "\t[ Magnetic Rotation = [" << CurrentState.m_PhysicalData.m_MagneticRotation[0] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[1] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[2] << "]" << std::endl;
+ std::cout << "\t[ Quaternion Rotation = [" << CurrentState.m_PhysicalData.m_QuaternionRotation[0] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[1] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[2] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[3] << "]" << std::endl;
+ }
+
+ memcpy(m_OrientationQuaternion, CurrentState.m_PhysicalData.m_QuaternionRotation, sizeof(float) * 4);
+ memcpy(m_MagneticRotation, CurrentState.m_PhysicalData.m_MagneticRotation, sizeof(float) * 3);
+ memcpy(m_GyroscopeRotation, CurrentState.m_PhysicalData.m_GyroscopeRotation, sizeof(float) * 3);
+ memcpy(m_Accelaration, CurrentState.m_PhysicalData.m_Acceleration, sizeof(float) * 3);
+ }
+
+ void CIMUDeducedReckoning::OnIMUIntegratedState(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
+ {
+ const IMUState CurrentState = pIMUDevice->GetIMUState();
+
+ if (m_IsVerbose)
+ {
+ std::cout << "OnIMUStateUpdate(IMU Device ID = 0x" << std::hex << pIMUDevice->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << std::dec << ")" << std::endl;
+ std::cout << "\t[ Global time = " << CTimeStamp::GetElapsedSeconds(TimeStamp, pIMUDevice->GetReferenceTimeStamp()) << " s]" << std::endl;
+ std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(TimeStamp, GetLastIntegratedStateReferenceTimeStamp()) << " µs]" << std::endl;
+
+ if (GetDispatchingMode() == eDecoupled)
+ {
+ std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
+ std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
+ }
+
+ std::cout << "\t[ Cycle = " << CurrentState.m_ControlData.m_MessageCounter << "]" << std::endl;
+ std::cout << "\t[ Acceleration = [" << CurrentState.m_PhysicalData.m_Acceleration[0] << "," << CurrentState.m_PhysicalData.m_Acceleration[1] << "," << CurrentState.m_PhysicalData.m_Acceleration[2] << "]" << std::endl;
+ std::cout << "\t[ Acceleration Magnitude = [" << std::abs(CurrentState.m_PhysicalData.m_AccelerationMagnitud - m_G) * 1000.0f << " mm/s^2]" << std::endl;
+ std::cout << "\t[ Gyroscope Rotation = [" << CurrentState.m_PhysicalData.m_GyroscopeRotation[0] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[1] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[2] << "]" << std::endl;
+ std::cout << "\t[ Magnetic Rotation = [" << CurrentState.m_PhysicalData.m_MagneticRotation[0] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[1] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[2] << "]" << std::endl;
+ std::cout << "\t[ Quaternion Rotation = [" << CurrentState.m_PhysicalData.m_QuaternionRotation[0] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[1] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[2] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[3] << "]" << std::endl;
+ }
+ }
+
+ void CIMUDeducedReckoning::OnIMUCustomEvent(const CIMUEvent& CustomEvent)
+ {
+ if (m_IsVerbose)
+ {
+ std::cout << "OnIMUCustomEvent(IMU Device ID = 0x" << std::hex << CustomEvent.GetIMU()->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << std::dec << ")" << std::endl;
+ std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(CustomEvent.GetTimeStamp(), CustomEvent.GetIMU()->GetReferenceTimeStamp()) << " µs]" << std::endl;
+ std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(CustomEvent.GetTimeStamp(), GetLastCustomEventReferenceTimeStamp()) << " µs]" << std::endl;
+
+ if (GetDispatchingMode() == eDecoupled)
+ {
+ std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
+ std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
+ }
+ }
+ }
}
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDeducedReckoning.h b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDeducedReckoning.h
index d2617867402ba6e95a3369190de635114737bf9b..8315bb663a9718436cd326c5dca62c42ac66df38 100644
--- a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDeducedReckoning.h
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDeducedReckoning.h
@@ -3,7 +3,7 @@
*
* Created on: Mar 16, 2014
* Author: Dr.-Ing. David Israel González Aguirre
- * Mail: david.gonzalez@kit.edu
+ * Mail: david.gonzalez@kit.edu
*/
#ifndef IMUDEDUCEDRECKONING_H_
@@ -15,52 +15,52 @@
namespace IMU
{
- class CIMUDeducedReckoning : public IIMUEventDispatcher
- {
- public:
+ class CIMUDeducedReckoning : public IIMUEventDispatcher
+ {
+ public:
- CIMUDeducedReckoning(const bool IsVerbose);
- virtual ~CIMUDeducedReckoning();
+ CIMUDeducedReckoning(const bool IsVerbose);
+ virtual ~CIMUDeducedReckoning();
- inline const float* GetOrientationQuaternion() const
- {
- return m_OrientationQuaternion;
- }
+ inline const float* GetOrientationQuaternion() const
+ {
+ return m_OrientationQuaternion;
+ }
- inline const float* GetMagneticRotation() const
- {
- return m_MagneticRotation;
- }
+ inline const float* GetMagneticRotation() const
+ {
+ return m_MagneticRotation;
+ }
- inline const float* GetGyroscopeRotation() const
- {
- return m_GyroscopeRotation;
- }
+ inline const float* GetGyroscopeRotation() const
+ {
+ return m_GyroscopeRotation;
+ }
- inline const float* GetAccelaration() const
- {
- return m_Accelaration;
- }
+ inline const float* GetAccelaration() const
+ {
+ return m_Accelaration;
+ }
- protected:
+ protected:
- virtual void OnIMUEvent(const CIMUEvent& Event);
+ virtual void OnIMUEvent(const CIMUEvent& Event);
- void OnIMUStart(const timeval& TimeStamp, const CIMUDevice* pIMUDevice);
- void OnIMUStop(const timeval& TimeStamp, const CIMUDevice* pIMUDevice);
- void OnIMUFusedCycle(const timeval& TimeStamp, const CIMUDevice* pIMUDevice);
- void OnIMUCycle(const timeval& TimeStamp, const CIMUDevice* pIMUDevice);
- void OnIMUIntegratedState(const timeval& TimeStamp, const CIMUDevice* pIMUDevice);
- void OnIMUCustomEvent(const CIMUEvent& CustomEvent);
+ void OnIMUStart(const timeval& TimeStamp, const CIMUDevice* pIMUDevice);
+ void OnIMUStop(const timeval& TimeStamp, const CIMUDevice* pIMUDevice);
+ void OnIMUFusedCycle(const timeval& TimeStamp, const CIMUDevice* pIMUDevice);
+ void OnIMUCycle(const timeval& TimeStamp, const CIMUDevice* pIMUDevice);
+ void OnIMUIntegratedState(const timeval& TimeStamp, const CIMUDevice* pIMUDevice);
+ void OnIMUCustomEvent(const CIMUEvent& CustomEvent);
- const bool m_IsVerbose;
- const float m_G;
+ const bool m_IsVerbose;
+ const float m_G;
- float m_OrientationQuaternion[4];
- float m_MagneticRotation[3];
- float m_GyroscopeRotation[3];
- float m_Accelaration[3];
- };
+ float m_OrientationQuaternion[4];
+ float m_MagneticRotation[3];
+ float m_GyroscopeRotation[3];
+ float m_Accelaration[3];
+ };
}
#endif
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDevice.cpp b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDevice.cpp
index 77336df645c8aac6b4b561ae4700b92b39a8a8d6..2a69eeb8d2336b3b585e29bd2653effd25586e50 100644
--- a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDevice.cpp
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDevice.cpp
@@ -3,7 +3,7 @@
*
* Created on: Mar 16, 2014
* Author: Dr.-Ing. David Israel González Aguirre
- * Mail: david.gonzalez@kit.edu
+ * Mail: david.gonzalez@kit.edu
*/
#include "IMUDevice.h"
@@ -11,559 +11,646 @@
namespace IMU
{
- CIMUDevice::CIMUDevice() :
- m_DeviceId(0), m_SamplingFrequency(SamplingFrequency(0)), m_PeriodMicroSeconds(0), m_FusionStrategy(eNoFusion), m_SamplesPerFusion(0), m_CollectedFusionSamples(0), m_IsActive(false), m_IsDispatching(false), m_IsInitialized(false), m_pInternalThreadHandel(0), m_IMUEventDispatchers(), m_ReferenceTimeStamp(CTimeStamp::s_Zero), m_LastFrameTimeStamp(CTimeStamp::s_Zero)
+ CIMUDevice::CIMUDevice() :
+ m_DeviceId(0), m_SamplingFrequency(SamplingFrequency(0)), m_PeriodMicroSeconds(0), m_FusionStrategy(eNoFusion), m_SamplesPerFusion(0), m_CollectedFusionSamples(0), m_IsActive(false), m_IsDispatching(false), m_IsInitialized(false), m_pInternalThreadHandel(0), m_IMUEventDispatchers(), m_ReferenceTimeStamp(CTimeStamp::s_Zero), m_LastFrameTimeStamp(CTimeStamp::s_Zero)
#ifdef _IMU_USE_XSENS_DEVICE_
- , m_pXsensMTiModule(NULL)
+ , m_pXsensMTiModule(NULL)
#endif
- {
- pthread_mutex_init(&m_IsActiveMutex,NULL);
- pthread_mutex_init(&m_IsDispatchingMutex,NULL);
- pthread_mutex_init(&m_EventDispatchersMutex,NULL);
- pthread_mutex_init(&m_DeviceMutex,NULL);
- }
-
- CIMUDevice::~CIMUDevice()
- {
- FinalizeModuleDevice();
- }
-
- uint64_t CIMUDevice::GetDeviceId() const
- {
- return m_DeviceId;
- }
-
- bool CIMUDevice::Connect(const std::string& PortName, const SamplingFrequency Frequency)
- {
- if (m_IsInitialized)
- return true;
-
- if (!PortName.length())
- {
- std::cerr << "[IMU Error: Cannot connect to empty port name!]\n\t[Operation result: (PortName.length()==0)]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
- return false;
- }
-
- m_IsInitialized = InitializeDevice(PortName,Frequency);
-
- return m_IsInitialized;
- }
-
- bool CIMUDevice::Start(const bool Blocking)
- {
- if (m_IsInitialized && (!m_IsActive))
- {
- const int Result = pthread_create(&m_pInternalThreadHandel,NULL,CIMUDevice::ThreadLoop,(void*) this);
- if (Result == 0)
- {
- while(Blocking && !m_IsActive)
- pthread_yield();
- return true;
- }
- }
- return false;
- }
-
- void CIMUDevice::Stop(const bool Blocking)
- {
- if (m_IsActive)
- {
- _MINIMAL___LOCK(m_IsActiveMutex)
- m_IsActive = false;
- _MINIMAL_UNLOCK(m_IsActiveMutex)
- pthread_join(m_pInternalThreadHandel,NULL);
- while(Blocking && m_IsDispatching)
- pthread_yield();
- }
- }
-
- bool CIMUDevice::SetFusion(const FusionStrategy Strategy, const ushort SamplesPerFusion)
- {
- if (SamplesPerFusion > 1)
- {
- if ((m_FusionStrategy != Strategy) || (m_SamplesPerFusion != SamplesPerFusion))
- {
- m_FusionStrategy = Strategy;
- m_SamplesPerFusion = SamplesPerFusion;
- m_CollectedFusionSamples = 0;
- }
- return true;
- }
- else
- {
- std::cerr << "[IMU Device error: Cannot set fusion with less than 2 samples per fusion!]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
- return false;
- }
- }
-
- bool CIMUDevice::IsActive() const
- {
- return m_IsActive;
- }
-
- bool CIMUDevice::RegisterEventDispatcher(IIMUEventDispatcher* pIMUEventDispatcher)
- {
- if (pIMUEventDispatcher)
- {
- _MINIMAL___LOCK(m_EventDispatchersMutex)
- if (m_IMUEventDispatchers.find(pIMUEventDispatcher) == m_IMUEventDispatchers.end())
- {
- pIMUEventDispatcher->SetIMU(this);
- pIMUEventDispatcher->SetReferenceTimeStamps(m_ReferenceTimeStamp);
- std::pair<std::set<IIMUEventDispatcher*>::iterator, bool> Result = m_IMUEventDispatchers.insert(pIMUEventDispatcher);
- _MINIMAL_UNLOCK(m_EventDispatchersMutex)
- return Result.second;
- }
- _MINIMAL_UNLOCK(m_EventDispatchersMutex)
- }
- return false;
- }
-
- bool CIMUDevice::UnregisterEventDispatcher(IIMUEventDispatcher* pIMUEventDispatcher)
- {
- if (pIMUEventDispatcher)
- {
- _MINIMAL___LOCK(m_EventDispatchersMutex)
- std::set<IIMUEventDispatcher*>::iterator ppElement = m_IMUEventDispatchers.find(pIMUEventDispatcher);
- if (ppElement != m_IMUEventDispatchers.end())
- {
- pIMUEventDispatcher->SetIMU(NULL);
- m_IMUEventDispatchers.erase(ppElement);
- _MINIMAL_UNLOCK(m_EventDispatchersMutex)
- return true;
- }
- _MINIMAL_UNLOCK(m_EventDispatchersMutex)
- }
- return false;
- }
-
- void CIMUDevice::UnregisterEventDispatchers()
- {
- if (m_IMUEventDispatchers.size())
- {
- _MINIMAL___LOCK(m_EventDispatchersMutex)
- for(std::set<IIMUEventDispatcher*>::iterator ppElement = m_IMUEventDispatchers.begin() ; ppElement != m_IMUEventDispatchers.end() ; ++ppElement)
- (*ppElement)->SetIMU(NULL);
- m_IMUEventDispatchers.clear();
- _MINIMAL_UNLOCK(m_EventDispatchersMutex)
- }
- }
+ {
+ pthread_mutex_init(&m_IsActiveMutex, NULL);
+ pthread_mutex_init(&m_IsDispatchingMutex, NULL);
+ pthread_mutex_init(&m_EventDispatchersMutex, NULL);
+ pthread_mutex_init(&m_DeviceMutex, NULL);
+ }
+
+ CIMUDevice::~CIMUDevice()
+ {
+ FinalizeModuleDevice();
+ }
+
+ uint64_t CIMUDevice::GetDeviceId() const
+ {
+ return m_DeviceId;
+ }
+
+ bool CIMUDevice::Connect(const std::string& PortName, const SamplingFrequency Frequency)
+ {
+ if (m_IsInitialized)
+ {
+ return true;
+ }
+
+ if (!PortName.length())
+ {
+ std::cerr << "[IMU Error: Cannot connect to empty port name!]\n\t[Operation result: (PortName.length()==0)]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ return false;
+ }
+
+ m_IsInitialized = InitializeDevice(PortName, Frequency);
+
+ return m_IsInitialized;
+ }
+
+ bool CIMUDevice::Start(const bool Blocking)
+ {
+ if (m_IsInitialized && (!m_IsActive))
+ {
+ const int Result = pthread_create(&m_pInternalThreadHandel, NULL, CIMUDevice::ThreadLoop, (void*) this);
+
+ if (Result == 0)
+ {
+ while (Blocking && !m_IsActive)
+ {
+ pthread_yield();
+ }
+
+ return true;
+ }
+ }
+
+ return false;
+ }
+
+ void CIMUDevice::Stop(const bool Blocking)
+ {
+ if (m_IsActive)
+ {
+ _MINIMAL___LOCK(m_IsActiveMutex)
+ m_IsActive = false;
+ _MINIMAL_UNLOCK(m_IsActiveMutex)
+ pthread_join(m_pInternalThreadHandel, NULL);
+
+ while (Blocking && m_IsDispatching)
+ {
+ pthread_yield();
+ }
+ }
+ }
+
+ bool CIMUDevice::SetFusion(const FusionStrategy Strategy, const ushort SamplesPerFusion)
+ {
+ if (SamplesPerFusion > 1)
+ {
+ if ((m_FusionStrategy != Strategy) || (m_SamplesPerFusion != SamplesPerFusion))
+ {
+ m_FusionStrategy = Strategy;
+ m_SamplesPerFusion = SamplesPerFusion;
+ m_CollectedFusionSamples = 0;
+ }
+
+ return true;
+ }
+ else
+ {
+ std::cerr << "[IMU Device error: Cannot set fusion with less than 2 samples per fusion!]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ return false;
+ }
+ }
+
+ bool CIMUDevice::IsActive() const
+ {
+ return m_IsActive;
+ }
+
+ bool CIMUDevice::RegisterEventDispatcher(IIMUEventDispatcher* pIMUEventDispatcher)
+ {
+ if (pIMUEventDispatcher)
+ {
+ _MINIMAL___LOCK(m_EventDispatchersMutex)
+
+ if (m_IMUEventDispatchers.find(pIMUEventDispatcher) == m_IMUEventDispatchers.end())
+ {
+ pIMUEventDispatcher->SetIMU(this);
+ pIMUEventDispatcher->SetReferenceTimeStamps(m_ReferenceTimeStamp);
+ std::pair<std::set<IIMUEventDispatcher*>::iterator, bool> Result = m_IMUEventDispatchers.insert(pIMUEventDispatcher);
+ _MINIMAL_UNLOCK(m_EventDispatchersMutex)
+ return Result.second;
+ }
+
+ _MINIMAL_UNLOCK(m_EventDispatchersMutex)
+ }
+
+ return false;
+ }
+
+ bool CIMUDevice::UnregisterEventDispatcher(IIMUEventDispatcher* pIMUEventDispatcher)
+ {
+ if (pIMUEventDispatcher)
+ {
+ _MINIMAL___LOCK(m_EventDispatchersMutex)
+ std::set<IIMUEventDispatcher*>::iterator ppElement = m_IMUEventDispatchers.find(pIMUEventDispatcher);
+
+ if (ppElement != m_IMUEventDispatchers.end())
+ {
+ pIMUEventDispatcher->SetIMU(NULL);
+ m_IMUEventDispatchers.erase(ppElement);
+ _MINIMAL_UNLOCK(m_EventDispatchersMutex)
+ return true;
+ }
+
+ _MINIMAL_UNLOCK(m_EventDispatchersMutex)
+ }
+
+ return false;
+ }
+
+ void CIMUDevice::UnregisterEventDispatchers()
+ {
+ if (m_IMUEventDispatchers.size())
+ {
+ _MINIMAL___LOCK(m_EventDispatchersMutex)
+
+ for (std::set<IIMUEventDispatcher*>::iterator ppElement = m_IMUEventDispatchers.begin() ; ppElement != m_IMUEventDispatchers.end() ; ++ppElement)
+ {
+ (*ppElement)->SetIMU(NULL);
+ }
+
+ m_IMUEventDispatchers.clear();
+ _MINIMAL_UNLOCK(m_EventDispatchersMutex)
+ }
+ }
#ifdef _IMU_USE_XSENS_DEVICE_
- bool CIMUDevice::InitializeXsensDevice(const std::string& PortName, const SamplingFrequency Frequency)
- {
- if (m_IsInitialized)
- return true;
-
- m_pXsensMTiModule = new Xsens::CXsensMTiModule();
-
- if (m_pXsensMTiModule->openPort(PortName.c_str()) != MTRV_OK)
- {
- std::cerr << "[IMU Device error: Cannot open port!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
- DestroyXsensModuleDevice();
- return false;
- }
-
- if (m_pXsensMTiModule->writeMessage(MID_GOTOCONFIG) != MTRV_OK)
- {
- std::cerr << "[IMU Device error: Cannot set configuration state!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
- DestroyXsensModuleDevice();
- return false;
- }
-
- if (m_pXsensMTiModule->setDeviceMode(OUTPUTMODE_CALIB | OUTPUTMODE_ORIENT,OUTPUTSETTINGS_ORIENTMODE_QUATERNION | OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT) != MTRV_OK)
- {
- std::cerr << "[IMU Device error: Cannot set output mode!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
- DestroyXsensModuleDevice();
- return false;
- }
-
- if (m_pXsensMTiModule->setSetting(MID_SETPERIOD,Frequency,LEN_PERIOD) != MTRV_OK)
- {
- std::cerr << "[IMU Device error: Cannot set sampling period!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
- DestroyXsensModuleDevice();
- return false;
- }
-
- unsigned long DeviceId;
- if (m_pXsensMTiModule->reqSetting(MID_REQDID,DeviceId) != MTRV_OK)
- {
- std::cerr << "[IMU Device error: Cannot get device ID!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
- DestroyXsensModuleDevice();
- return false;
- }
- m_DeviceId = DeviceId;
-
- if (m_pXsensMTiModule->writeMessage(MID_GOTOMEASUREMENT) != MTRV_OK)
- {
- std::cerr << "[IMU Device error: Cannot enter measurement state!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
- DestroyXsensModuleDevice();
- return false;
- }
-
- return true;
- }
-
- void CIMUDevice::FinalizeXsensModuleDevice()
- {
- if (m_IsInitialized)
- {
- while(m_IsActive || m_IsDispatching)
- pthread_yield();
-
- _MINIMAL___LOCK(m_DeviceMutex)
- DestroyXsensModuleDevice();
- _MINIMAL_UNLOCK(m_DeviceMutex)
- }
- }
-
- void CIMUDevice::DestroyXsensModuleDevice()
- {
- if (m_pXsensMTiModule)
- {
- if (m_pXsensMTiModule->isPortOpen())
- m_pXsensMTiModule->close();
- delete m_pXsensMTiModule;
- m_pXsensMTiModule = NULL;
- m_DeviceId = 0;
- }
- }
+ bool CIMUDevice::InitializeXsensDevice(const std::string& PortName, const SamplingFrequency Frequency)
+ {
+ if (m_IsInitialized)
+ {
+ return true;
+ }
+
+ m_pXsensMTiModule = new Xsens::CXsensMTiModule();
+
+ if (m_pXsensMTiModule->openPort(PortName.c_str()) != MTRV_OK)
+ {
+ std::cerr << "[IMU Device error: Cannot open port!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ DestroyXsensModuleDevice();
+ return false;
+ }
+
+ if (m_pXsensMTiModule->writeMessage(MID_GOTOCONFIG) != MTRV_OK)
+ {
+ std::cerr << "[IMU Device error: Cannot set configuration state!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ DestroyXsensModuleDevice();
+ return false;
+ }
+
+ if (m_pXsensMTiModule->setDeviceMode(OUTPUTMODE_CALIB | OUTPUTMODE_ORIENT, OUTPUTSETTINGS_ORIENTMODE_QUATERNION | OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT) != MTRV_OK)
+ {
+ std::cerr << "[IMU Device error: Cannot set output mode!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ DestroyXsensModuleDevice();
+ return false;
+ }
+
+ if (m_pXsensMTiModule->setSetting(MID_SETPERIOD, Frequency, LEN_PERIOD) != MTRV_OK)
+ {
+ std::cerr << "[IMU Device error: Cannot set sampling period!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ DestroyXsensModuleDevice();
+ return false;
+ }
+
+ unsigned long DeviceId;
+
+ if (m_pXsensMTiModule->reqSetting(MID_REQDID, DeviceId) != MTRV_OK)
+ {
+ std::cerr << "[IMU Device error: Cannot get device ID!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ DestroyXsensModuleDevice();
+ return false;
+ }
+
+ m_DeviceId = DeviceId;
+
+ if (m_pXsensMTiModule->writeMessage(MID_GOTOMEASUREMENT) != MTRV_OK)
+ {
+ std::cerr << "[IMU Device error: Cannot enter measurement state!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ DestroyXsensModuleDevice();
+ return false;
+ }
+
+ return true;
+ }
+
+ void CIMUDevice::FinalizeXsensModuleDevice()
+ {
+ if (m_IsInitialized)
+ {
+ while (m_IsActive || m_IsDispatching)
+ {
+ pthread_yield();
+ }
+
+ _MINIMAL___LOCK(m_DeviceMutex)
+ DestroyXsensModuleDevice();
+ _MINIMAL_UNLOCK(m_DeviceMutex)
+ }
+ }
+
+ void CIMUDevice::DestroyXsensModuleDevice()
+ {
+ if (m_pXsensMTiModule)
+ {
+ if (m_pXsensMTiModule->isPortOpen())
+ {
+ m_pXsensMTiModule->close();
+ }
+
+ delete m_pXsensMTiModule;
+ m_pXsensMTiModule = NULL;
+ m_DeviceId = 0;
+ }
+ }
#endif
- bool CIMUDevice::LoadCurrentState()
- {
+ bool CIMUDevice::LoadCurrentState()
+ {
- _MINIMAL___LOCK(m_DeviceMutex)
+ _MINIMAL___LOCK(m_DeviceMutex)
#ifdef _IMU_USE_XSENS_DEVICE_
- if (m_pXsensMTiModule->readDataMessage(m_XsensMTiFrame.m_Data,m_XsensMTiFrame.m_DataLength) == MTRV_OK)
- if (m_pXsensMTiModule->getValue(VALUE_CALIB_ACC,m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration,m_XsensMTiFrame.m_Data) == MTRV_OK)
- if (m_pXsensMTiModule->getValue(VALUE_CALIB_GYR,m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_GyroscopeRotation,m_XsensMTiFrame.m_Data) == MTRV_OK)
- if (m_pXsensMTiModule->getValue(VALUE_CALIB_MAG,m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_MagneticRotation,m_XsensMTiFrame.m_Data) == MTRV_OK)
- if (m_pXsensMTiModule->getValue(VALUE_ORIENT_QUAT,m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_QuaternionRotation,m_XsensMTiFrame.m_Data) == MTRV_OK)
- if (m_pXsensMTiModule->getValue(VALUE_SAMPLECNT,m_XsensMTiFrame.m_IMUState.m_ControlData.m_CurrentSampleCount,m_XsensMTiFrame.m_Data) == MTRV_OK)
- {
- if (m_XsensMTiFrame.m_IMUState.m_ControlData.m_PreviousSampleCount != -1)
- m_XsensMTiFrame.m_IMUState.m_ControlData.m_IsConsecutive = ((m_XsensMTiFrame.m_IMUState.m_ControlData.m_PreviousSampleCount + 1) % 65536) == m_XsensMTiFrame.m_IMUState.m_ControlData.m_CurrentSampleCount;
- m_XsensMTiFrame.m_IMUState.m_ControlData.m_PreviousSampleCount = m_XsensMTiFrame.m_IMUState.m_ControlData.m_CurrentSampleCount;
- m_XsensMTiFrame.m_IMUState.m_ControlData.m_MessageCounter++;
- gettimeofday(&m_XsensMTiFrame.m_IMUState.m_ControlData.m_TimeStamp,NULL);
- m_LastFrameTimeStamp = m_XsensMTiFrame.m_IMUState.m_ControlData.m_TimeStamp;
-
- m_XsensMTiFrame.m_IMUState.m_PhysicalData.UpdateAccelerationMagnitud();
-
- _MINIMAL_UNLOCK(m_DeviceMutex)
- return true;
- }
- else
- std::cerr << "[IMU Device error: Fail to get sample count!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
- else
- std::cerr << "[IMU Device error: Fail to get quaternion rotation!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
- else
- std::cerr << "[IMU Device error: Fail to get magnetic rotation!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
- else
- std::cerr << "[IMU Device error: Fail to get gyroscope rotation!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
- else
- std::cerr << "[IMU Device error: Fail to get acceleration vector!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
- else
- std::cerr << "[IMU Device error: Fail to read message!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ if (m_pXsensMTiModule->readDataMessage(m_XsensMTiFrame.m_Data, m_XsensMTiFrame.m_DataLength) == MTRV_OK)
+ if (m_pXsensMTiModule->getValue(VALUE_CALIB_ACC, m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration, m_XsensMTiFrame.m_Data) == MTRV_OK)
+ if (m_pXsensMTiModule->getValue(VALUE_CALIB_GYR, m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_GyroscopeRotation, m_XsensMTiFrame.m_Data) == MTRV_OK)
+ if (m_pXsensMTiModule->getValue(VALUE_CALIB_MAG, m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_MagneticRotation, m_XsensMTiFrame.m_Data) == MTRV_OK)
+ if (m_pXsensMTiModule->getValue(VALUE_ORIENT_QUAT, m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_QuaternionRotation, m_XsensMTiFrame.m_Data) == MTRV_OK)
+ if (m_pXsensMTiModule->getValue(VALUE_SAMPLECNT, m_XsensMTiFrame.m_IMUState.m_ControlData.m_CurrentSampleCount, m_XsensMTiFrame.m_Data) == MTRV_OK)
+ {
+ if (m_XsensMTiFrame.m_IMUState.m_ControlData.m_PreviousSampleCount != -1)
+ {
+ m_XsensMTiFrame.m_IMUState.m_ControlData.m_IsConsecutive = ((m_XsensMTiFrame.m_IMUState.m_ControlData.m_PreviousSampleCount + 1) % 65536) == m_XsensMTiFrame.m_IMUState.m_ControlData.m_CurrentSampleCount;
+ }
+
+ m_XsensMTiFrame.m_IMUState.m_ControlData.m_PreviousSampleCount = m_XsensMTiFrame.m_IMUState.m_ControlData.m_CurrentSampleCount;
+ m_XsensMTiFrame.m_IMUState.m_ControlData.m_MessageCounter++;
+ gettimeofday(&m_XsensMTiFrame.m_IMUState.m_ControlData.m_TimeStamp, NULL);
+ m_LastFrameTimeStamp = m_XsensMTiFrame.m_IMUState.m_ControlData.m_TimeStamp;
+
+ m_XsensMTiFrame.m_IMUState.m_PhysicalData.UpdateAccelerationMagnitud();
+
+ _MINIMAL_UNLOCK(m_DeviceMutex)
+ return true;
+ }
+ else
+ {
+ std::cerr << "[IMU Device error: Fail to get sample count!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ }
+ else
+ {
+ std::cerr << "[IMU Device error: Fail to get quaternion rotation!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ }
+ else
+ {
+ std::cerr << "[IMU Device error: Fail to get magnetic rotation!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ }
+ else
+ {
+ std::cerr << "[IMU Device error: Fail to get gyroscope rotation!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ }
+ else
+ {
+ std::cerr << "[IMU Device error: Fail to get acceleration vector!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ }
+ else
+ {
+ std::cerr << "[IMU Device error: Fail to read message!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ }
#endif
- _MINIMAL_UNLOCK(m_DeviceMutex)
-
- return false;
- }
-
- bool CIMUDevice::MeanFuseCurrentState()
- {
- IncorporateCurrentStateMeanFusion();
- if (m_CollectedFusionSamples == m_SamplesPerFusion)
- {
- MeanFusion();
- return true;
- }
- return false;
- }
-
- void CIMUDevice::IncorporateCurrentStateMeanFusion()
- {
- m_FusedPhysicalData.m_Acceleration[0] += m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[0];
- m_FusedPhysicalData.m_Acceleration[1] += m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[1];
- m_FusedPhysicalData.m_Acceleration[2] += m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[2];
- ++m_CollectedFusionSamples;
- }
-
- void CIMUDevice::MeanFusion()
- {
- //Execution the fusion
- const float NormalizationFactor = 1.0f / float(m_CollectedFusionSamples);
- m_FusedIMUState.m_PhysicalData.m_Acceleration[0] = m_FusedPhysicalData.m_Acceleration[0] * NormalizationFactor;
- m_FusedIMUState.m_PhysicalData.m_Acceleration[1] = m_FusedPhysicalData.m_Acceleration[1] * NormalizationFactor;
- m_FusedIMUState.m_PhysicalData.m_Acceleration[2] = m_FusedPhysicalData.m_Acceleration[2] * NormalizationFactor;
-
- //Derivated from fusion
- m_FusedIMUState.m_PhysicalData.UpdateAccelerationMagnitud();
-
- //Reset counters and accumulators
- memset(m_FusedPhysicalData.m_Acceleration,0,sizeof(float) * 3);
- m_CollectedFusionSamples = 0;
- }
-
- bool CIMUDevice::GaussianFuseCurrentState()
- {
- IncorporateCurrentStateGaussianFusion();
- if (m_CollectedFusionSamples == m_SamplesPerFusion)
- {
- GaussianFusion();
- return true;
- }
- return false;
- }
-
- void CIMUDevice::IncorporateCurrentStateGaussianFusion()
- {
- m_FusedPhysicalData.m_Acceleration[0] += m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[0];
- m_FusedPhysicalData.m_Acceleration[1] += m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[1];
- m_FusedPhysicalData.m_Acceleration[2] += m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[2];
-
- ++m_CollectedFusionSamples;
- }
-
- void CIMUDevice::GaussianFusion()
- {
- //Execution the fusion
- const float NormalizationFactor = 1.0f / float(m_CollectedFusionSamples);
- m_FusedIMUState.m_PhysicalData.m_Acceleration[0] = m_FusedPhysicalData.m_Acceleration[0] * NormalizationFactor;
- m_FusedIMUState.m_PhysicalData.m_Acceleration[1] = m_FusedPhysicalData.m_Acceleration[1] * NormalizationFactor;
- m_FusedIMUState.m_PhysicalData.m_Acceleration[2] = m_FusedPhysicalData.m_Acceleration[2] * NormalizationFactor;
-
- //Derivated from fusion
- m_FusedIMUState.m_PhysicalData.UpdateAccelerationMagnitud();
-
- //Reset counters and accumulators
- memset(m_FusedPhysicalData.m_Acceleration,0,sizeof(float) * 3);
- m_CollectedFusionSamples = 0;
- }
-
- bool CIMUDevice::IntegrateCurrentState()
- {
- if (m_FusionStrategy == eNoFusion)
- {
- return IntegrateWithOutFusion();
- }
- else
- {
- return IntegrateWithFusion();
- }
- }
-
- bool CIMUDevice::IntegrateWithOutFusion()
- {
- return true;
- }
-
- bool CIMUDevice::IntegrateWithFusion()
- {
- return true;
- }
-
- bool CIMUDevice::InitializeDevice(const std::string& PortName, const SamplingFrequency Frequency)
- {
-
- _MINIMAL___LOCK(m_DeviceMutex)
+ _MINIMAL_UNLOCK(m_DeviceMutex)
+
+ return false;
+ }
+
+ bool CIMUDevice::MeanFuseCurrentState()
+ {
+ IncorporateCurrentStateMeanFusion();
+
+ if (m_CollectedFusionSamples == m_SamplesPerFusion)
+ {
+ MeanFusion();
+ return true;
+ }
+
+ return false;
+ }
+
+ void CIMUDevice::IncorporateCurrentStateMeanFusion()
+ {
+ m_FusedPhysicalData.m_Acceleration[0] += m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[0];
+ m_FusedPhysicalData.m_Acceleration[1] += m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[1];
+ m_FusedPhysicalData.m_Acceleration[2] += m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[2];
+ ++m_CollectedFusionSamples;
+ }
+
+ void CIMUDevice::MeanFusion()
+ {
+ //Execution the fusion
+ const float NormalizationFactor = 1.0f / float(m_CollectedFusionSamples);
+ m_FusedIMUState.m_PhysicalData.m_Acceleration[0] = m_FusedPhysicalData.m_Acceleration[0] * NormalizationFactor;
+ m_FusedIMUState.m_PhysicalData.m_Acceleration[1] = m_FusedPhysicalData.m_Acceleration[1] * NormalizationFactor;
+ m_FusedIMUState.m_PhysicalData.m_Acceleration[2] = m_FusedPhysicalData.m_Acceleration[2] * NormalizationFactor;
+
+ //Derivated from fusion
+ m_FusedIMUState.m_PhysicalData.UpdateAccelerationMagnitud();
+
+ //Reset counters and accumulators
+ memset(m_FusedPhysicalData.m_Acceleration, 0, sizeof(float) * 3);
+ m_CollectedFusionSamples = 0;
+ }
+
+ bool CIMUDevice::GaussianFuseCurrentState()
+ {
+ IncorporateCurrentStateGaussianFusion();
+
+ if (m_CollectedFusionSamples == m_SamplesPerFusion)
+ {
+ GaussianFusion();
+ return true;
+ }
+
+ return false;
+ }
+
+ void CIMUDevice::IncorporateCurrentStateGaussianFusion()
+ {
+ m_FusedPhysicalData.m_Acceleration[0] += m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[0];
+ m_FusedPhysicalData.m_Acceleration[1] += m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[1];
+ m_FusedPhysicalData.m_Acceleration[2] += m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[2];
+
+ ++m_CollectedFusionSamples;
+ }
+
+ void CIMUDevice::GaussianFusion()
+ {
+ //Execution the fusion
+ const float NormalizationFactor = 1.0f / float(m_CollectedFusionSamples);
+ m_FusedIMUState.m_PhysicalData.m_Acceleration[0] = m_FusedPhysicalData.m_Acceleration[0] * NormalizationFactor;
+ m_FusedIMUState.m_PhysicalData.m_Acceleration[1] = m_FusedPhysicalData.m_Acceleration[1] * NormalizationFactor;
+ m_FusedIMUState.m_PhysicalData.m_Acceleration[2] = m_FusedPhysicalData.m_Acceleration[2] * NormalizationFactor;
+
+ //Derivated from fusion
+ m_FusedIMUState.m_PhysicalData.UpdateAccelerationMagnitud();
+
+ //Reset counters and accumulators
+ memset(m_FusedPhysicalData.m_Acceleration, 0, sizeof(float) * 3);
+ m_CollectedFusionSamples = 0;
+ }
+
+ bool CIMUDevice::IntegrateCurrentState()
+ {
+ if (m_FusionStrategy == eNoFusion)
+ {
+ return IntegrateWithOutFusion();
+ }
+ else
+ {
+ return IntegrateWithFusion();
+ }
+ }
+
+ bool CIMUDevice::IntegrateWithOutFusion()
+ {
+ return true;
+ }
+
+ bool CIMUDevice::IntegrateWithFusion()
+ {
+ return true;
+ }
+
+ bool CIMUDevice::InitializeDevice(const std::string& PortName, const SamplingFrequency Frequency)
+ {
+
+ _MINIMAL___LOCK(m_DeviceMutex)
#ifdef _IMU_USE_XSENS_DEVICE_
- if (InitializeXsensDevice(PortName,Frequency))
- {
- m_SamplingFrequency = Frequency;
- const int Cylces = 0X1C200 / int(m_SamplingFrequency);
- m_PeriodMicroSeconds = int(round((1000000.0f * _IMU_DEVICE_DEFAUL_CHECK_PERIOD_FACTOR_) / float(Cylces)));
- _MINIMAL_UNLOCK(m_DeviceMutex)
- return true;
- }
- else
- {
- _MINIMAL_UNLOCK(m_DeviceMutex)
- return false;
- }
+ if (InitializeXsensDevice(PortName, Frequency))
+ {
+ m_SamplingFrequency = Frequency;
+ const int Cylces = 0X1C200 / int(m_SamplingFrequency);
+ m_PeriodMicroSeconds = int(round((1000000.0f * _IMU_DEVICE_DEFAUL_CHECK_PERIOD_FACTOR_) / float(Cylces)));
+ _MINIMAL_UNLOCK(m_DeviceMutex)
+ return true;
+ }
+ else
+ {
+ _MINIMAL_UNLOCK(m_DeviceMutex)
+ return false;
+ }
#endif
- }
+ }
- void CIMUDevice::FinalizeModuleDevice()
- {
- Stop(true);
+ void CIMUDevice::FinalizeModuleDevice()
+ {
+ Stop(true);
#ifdef _IMU_USE_XSENS_DEVICE_
- FinalizeXsensModuleDevice();
+ FinalizeXsensModuleDevice();
#endif
- UnregisterEventDispatchers();
-
- }
-
- void CIMUDevice::ShouldYield()
- {
- const long int RemainingTime = m_PeriodMicroSeconds - CTimeStamp::GetElapsedMicroseconds(m_LastFrameTimeStamp);
- if (RemainingTime > 0)
- usleep(__useconds_t(RemainingTime));
- }
-
- bool CIMUDevice::DispatchCylcle()
- {
- if (LoadCurrentState())
- {
- SendEvent(CIMUEvent(m_LastFrameTimeStamp,CIMUEvent::eOnIMUCycle,this));
- switch(m_FusionStrategy)
- {
- case eMeanFusion:
- if (MeanFuseCurrentState())
- {
- SendEvent(CIMUEvent(m_LastFrameTimeStamp,CIMUEvent::eOnIMUFusedCycle,this,m_FusedIMUState));
- if (IntegrateCurrentState())
- SendEvent(CIMUEvent(m_LastFrameTimeStamp,CIMUEvent::eOnIMUIntegratedState,this,m_IntegratedIMUState));
- }
- break;
- case eGaussianFusion:
- if (GaussianFuseCurrentState())
- {
- SendEvent(CIMUEvent(m_LastFrameTimeStamp,CIMUEvent::eOnIMUIntegratedState,this,m_FusedIMUState));
- if (IntegrateCurrentState())
- SendEvent(CIMUEvent(m_LastFrameTimeStamp,CIMUEvent::eOnIMUIntegratedState,this,m_IntegratedIMUState));
- }
- break;
- case eNoFusion:
- if (IntegrateCurrentState())
- SendEvent(CIMUEvent(m_LastFrameTimeStamp,CIMUEvent::eOnIMUIntegratedState,this,m_IntegratedIMUState));
- break;
- }
- return true;
- }
- return false;
- }
-
- void CIMUDevice::SendEvent(const CIMUEvent& Event)
- {
- _MINIMAL___LOCK(m_EventDispatchersMutex)
- const unsigned long int TotalDispatchers = m_IMUEventDispatchers.size();
- if (TotalDispatchers)
- {
- if (TotalDispatchers == 1)
- (*m_IMUEventDispatchers.begin())->ReceiveEvent(Event);
- else
- for(std::set<IIMUEventDispatcher*>::iterator ppIIMUEventDispatcher = m_IMUEventDispatchers.begin() ; ppIIMUEventDispatcher != m_IMUEventDispatchers.end() ; ++ppIIMUEventDispatcher)
- (*ppIIMUEventDispatcher)->ReceiveEvent(Event);
- }
- _MINIMAL_UNLOCK(m_EventDispatchersMutex)
- }
-
- void CIMUDevice::SetReferenceTimeStamps()
- {
- _MINIMAL___LOCK(m_EventDispatchersMutex)
- gettimeofday(&m_ReferenceTimeStamp,NULL);
- gettimeofday(&m_LastFrameTimeStamp,NULL);
- if (m_IMUEventDispatchers.size())
- for(std::set<IIMUEventDispatcher*>::iterator ppIIMUEventDispatcher = m_IMUEventDispatchers.begin() ; ppIIMUEventDispatcher != m_IMUEventDispatchers.end() ; ++ppIIMUEventDispatcher)
- (*ppIIMUEventDispatcher)->SetReferenceTimeStamps(m_ReferenceTimeStamp);
- _MINIMAL_UNLOCK(m_EventDispatchersMutex)
- }
-
- bool CIMUDevice::SetThreadRunnigMode(const ThreadPolicyType ThreadPolicy, const float NormalizedPriority)
- {
- if (m_IsActive)
- {
- int Policy = -1;
- struct sched_param SchedulingParameters;
- if (pthread_getschedparam(m_pInternalThreadHandel,&Policy,&SchedulingParameters) == 0)
- {
- const int MaximalPriority = sched_get_priority_max(ThreadPolicy);
- const int MinimalPriority = sched_get_priority_min(ThreadPolicy);
- const int PriorityRange = MaximalPriority - MinimalPriority;
- const int Priority = int(round(float(PriorityRange) * NormalizedPriority + float(MinimalPriority)));
- SchedulingParameters.sched_priority = Priority;
- const int Result = pthread_setschedparam(m_pInternalThreadHandel,ThreadPolicy,&SchedulingParameters);
- switch(Result)
- {
- case 0:
- return true;
- break;
- case EINVAL:
- std::cerr << "[IMU Device error: SetThreadRunnigMode() returns EINVAL!]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
- break;
- case ENOTSUP:
- std::cerr << "[IMU Device error: SetThreadRunnigMode() returns ENOTSUP!]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
- break;
- case EPERM:
- std::cerr << "[IMU Device error: SetThreadRunnigMode() returns EPERM!]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
- break;
- case ESRCH:
- std::cerr << "[IMU Device error: SetThreadRunnigMode() returns ESRCH!]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
- break;
- }
- }
- return false;
- }
- else
- {
- std::cerr << "[IMU Device error: SetThreadRunnigMode() cannot set running mode while thread is not active!]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
- return false;
- }
- }
-
- void* CIMUDevice::ThreadLoop(void* pData)
- {
- if (pData)
- {
-
- CIMUDevice* pIMUDevice = (CIMUDevice*) pData;
-
- _MINIMAL___LOCK(pIMUDevice->m_IsActiveMutex)
- pIMUDevice->m_IsActive = true;
- _MINIMAL_UNLOCK(pIMUDevice->m_IsActiveMutex)
-
- pIMUDevice->SetReferenceTimeStamps();
-
- pIMUDevice->SendEvent(CIMUEvent(CIMUEvent::eOnIMUStart,pIMUDevice));
-
- while(pIMUDevice->m_IsActive)
- {
- pIMUDevice->ShouldYield();
-
- _MINIMAL___LOCK(pIMUDevice->m_IsDispatchingMutex)
- pIMUDevice->m_IsDispatching = true;
-
- const bool DispatchingResult = pIMUDevice->DispatchCylcle();
-
- pIMUDevice->m_IsDispatching = false;
- _MINIMAL_UNLOCK(pIMUDevice->m_IsDispatchingMutex)
-
- if (!DispatchingResult)
- {
- std::cerr << "[IMU Device error: DispatchCylcle() returns false!]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
- break;
- }
-
- }
- if (pIMUDevice->m_IsActive)
- {
- _MINIMAL___LOCK(pIMUDevice->m_IsActiveMutex)
- pIMUDevice->m_IsActive = false;
- _MINIMAL_UNLOCK(pIMUDevice->m_IsActiveMutex)
- }
-
- pIMUDevice->SendEvent(CIMUEvent(CIMUEvent::eOnIMUStop,pIMUDevice));
- }
- return NULL;
- }
+ UnregisterEventDispatchers();
+
+ }
+
+ void CIMUDevice::ShouldYield()
+ {
+ const long int RemainingTime = m_PeriodMicroSeconds - CTimeStamp::GetElapsedMicroseconds(m_LastFrameTimeStamp);
+
+ if (RemainingTime > 0)
+ {
+ usleep(__useconds_t(RemainingTime));
+ }
+ }
+
+ bool CIMUDevice::DispatchCylcle()
+ {
+ if (LoadCurrentState())
+ {
+ SendEvent(CIMUEvent(m_LastFrameTimeStamp, CIMUEvent::eOnIMUCycle, this));
+
+ switch (m_FusionStrategy)
+ {
+ case eMeanFusion:
+ if (MeanFuseCurrentState())
+ {
+ SendEvent(CIMUEvent(m_LastFrameTimeStamp, CIMUEvent::eOnIMUFusedCycle, this, m_FusedIMUState));
+
+ if (IntegrateCurrentState())
+ {
+ SendEvent(CIMUEvent(m_LastFrameTimeStamp, CIMUEvent::eOnIMUIntegratedState, this, m_IntegratedIMUState));
+ }
+ }
+
+ break;
+
+ case eGaussianFusion:
+ if (GaussianFuseCurrentState())
+ {
+ SendEvent(CIMUEvent(m_LastFrameTimeStamp, CIMUEvent::eOnIMUIntegratedState, this, m_FusedIMUState));
+
+ if (IntegrateCurrentState())
+ {
+ SendEvent(CIMUEvent(m_LastFrameTimeStamp, CIMUEvent::eOnIMUIntegratedState, this, m_IntegratedIMUState));
+ }
+ }
+
+ break;
+
+ case eNoFusion:
+ if (IntegrateCurrentState())
+ {
+ SendEvent(CIMUEvent(m_LastFrameTimeStamp, CIMUEvent::eOnIMUIntegratedState, this, m_IntegratedIMUState));
+ }
+
+ break;
+ }
+
+ return true;
+ }
+
+ return false;
+ }
+
+ void CIMUDevice::SendEvent(const CIMUEvent& Event)
+ {
+ _MINIMAL___LOCK(m_EventDispatchersMutex)
+ const unsigned long int TotalDispatchers = m_IMUEventDispatchers.size();
+
+ if (TotalDispatchers)
+ {
+ if (TotalDispatchers == 1)
+ {
+ (*m_IMUEventDispatchers.begin())->ReceiveEvent(Event);
+ }
+ else
+ for (std::set<IIMUEventDispatcher*>::iterator ppIIMUEventDispatcher = m_IMUEventDispatchers.begin() ; ppIIMUEventDispatcher != m_IMUEventDispatchers.end() ; ++ppIIMUEventDispatcher)
+ {
+ (*ppIIMUEventDispatcher)->ReceiveEvent(Event);
+ }
+ }
+
+ _MINIMAL_UNLOCK(m_EventDispatchersMutex)
+ }
+
+ void CIMUDevice::SetReferenceTimeStamps()
+ {
+ _MINIMAL___LOCK(m_EventDispatchersMutex)
+ gettimeofday(&m_ReferenceTimeStamp, NULL);
+ gettimeofday(&m_LastFrameTimeStamp, NULL);
+
+ if (m_IMUEventDispatchers.size())
+ for (std::set<IIMUEventDispatcher*>::iterator ppIIMUEventDispatcher = m_IMUEventDispatchers.begin() ; ppIIMUEventDispatcher != m_IMUEventDispatchers.end() ; ++ppIIMUEventDispatcher)
+ {
+ (*ppIIMUEventDispatcher)->SetReferenceTimeStamps(m_ReferenceTimeStamp);
+ }
+
+ _MINIMAL_UNLOCK(m_EventDispatchersMutex)
+ }
+
+ bool CIMUDevice::SetThreadRunnigMode(const ThreadPolicyType ThreadPolicy, const float NormalizedPriority)
+ {
+ if (m_IsActive)
+ {
+ int Policy = -1;
+ struct sched_param SchedulingParameters;
+
+ if (pthread_getschedparam(m_pInternalThreadHandel, &Policy, &SchedulingParameters) == 0)
+ {
+ const int MaximalPriority = sched_get_priority_max(ThreadPolicy);
+ const int MinimalPriority = sched_get_priority_min(ThreadPolicy);
+ const int PriorityRange = MaximalPriority - MinimalPriority;
+ const int Priority = int(round(float(PriorityRange) * NormalizedPriority + float(MinimalPriority)));
+ SchedulingParameters.sched_priority = Priority;
+ const int Result = pthread_setschedparam(m_pInternalThreadHandel, ThreadPolicy, &SchedulingParameters);
+
+ switch (Result)
+ {
+ case 0:
+ return true;
+ break;
+
+ case EINVAL:
+ std::cerr << "[IMU Device error: SetThreadRunnigMode() returns EINVAL!]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ break;
+
+ case ENOTSUP:
+ std::cerr << "[IMU Device error: SetThreadRunnigMode() returns ENOTSUP!]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ break;
+
+ case EPERM:
+ std::cerr << "[IMU Device error: SetThreadRunnigMode() returns EPERM!]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ break;
+
+ case ESRCH:
+ std::cerr << "[IMU Device error: SetThreadRunnigMode() returns ESRCH!]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ break;
+ }
+ }
+
+ return false;
+ }
+ else
+ {
+ std::cerr << "[IMU Device error: SetThreadRunnigMode() cannot set running mode while thread is not active!]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ return false;
+ }
+ }
+
+ void* CIMUDevice::ThreadLoop(void* pData)
+ {
+ if (pData)
+ {
+
+ CIMUDevice* pIMUDevice = (CIMUDevice*) pData;
+
+ _MINIMAL___LOCK(pIMUDevice->m_IsActiveMutex)
+ pIMUDevice->m_IsActive = true;
+ _MINIMAL_UNLOCK(pIMUDevice->m_IsActiveMutex)
+
+ pIMUDevice->SetReferenceTimeStamps();
+
+ pIMUDevice->SendEvent(CIMUEvent(CIMUEvent::eOnIMUStart, pIMUDevice));
+
+ while (pIMUDevice->m_IsActive)
+ {
+ pIMUDevice->ShouldYield();
+
+ _MINIMAL___LOCK(pIMUDevice->m_IsDispatchingMutex)
+ pIMUDevice->m_IsDispatching = true;
+
+ const bool DispatchingResult = pIMUDevice->DispatchCylcle();
+
+ pIMUDevice->m_IsDispatching = false;
+ _MINIMAL_UNLOCK(pIMUDevice->m_IsDispatchingMutex)
+
+ if (!DispatchingResult)
+ {
+ std::cerr << "[IMU Device error: DispatchCylcle() returns false!]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ break;
+ }
+
+ }
+
+ if (pIMUDevice->m_IsActive)
+ {
+ _MINIMAL___LOCK(pIMUDevice->m_IsActiveMutex)
+ pIMUDevice->m_IsActive = false;
+ _MINIMAL_UNLOCK(pIMUDevice->m_IsActiveMutex)
+ }
+
+ pIMUDevice->SendEvent(CIMUEvent(CIMUEvent::eOnIMUStop, pIMUDevice));
+ }
+
+ return NULL;
+ }
}
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDevice.h b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDevice.h
index ff37a7d3d9de28038ca765cf83eebf03b5ef9be8..0eccbad7e60cea8d40ccc3559dd83bd22b31eed1 100644
--- a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDevice.h
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDevice.h
@@ -3,7 +3,7 @@
*
* Created on: Mar 16, 2014
* Author: Dr.-Ing. David Israel González Aguirre
- * Mail: david.gonzalez@kit.edu
+ * Mail: david.gonzalez@kit.edu
*/
#ifndef IMUDEVICE_H_
@@ -28,192 +28,192 @@
namespace IMU
{
- //Forward definition
- class IIMUEventDispatcher;
-
- /*!
- \class CIMUDevice
- \ingroup IMU
- \brief This class contains the the devices module and the thread for read the measurements.
-
- CIMUDevice encapsulates the device details for the rest of the library and applications.
- This also includes a thread which is in charge of generate the IMU events
- */
- class CIMUDevice
- {
- public:
-
- /*!
- \brief Enum specifying the running thread policy.
- */
- enum ThreadPolicyType
- {
- eRealTime = SCHED_FIFO, eRoundRobinPriorityBased = SCHED_RR, eBatch = SCHED_BATCH, eIdle = SCHED_IDLE
- };
-
- /*!
- \brief Enum specifying the supported sampling frequencies.
- */
- enum SamplingFrequency
- {
- eSamplingFrequency_10HZ = 0x2D00,
- eSamplingFrequency_12HZ = 0x2580,
- eSamplingFrequency_15HZ = 0x1E00,
- eSamplingFrequency_16HZ = 0x1C20,
- eSamplingFrequency_18HZ = 0x1900,
- eSamplingFrequency_20HZ = 0x1680,
- eSamplingFrequency_24HZ = 0x12C0,
- eSamplingFrequency_25HZ = 0x1200,
- eSamplingFrequency_30HZ = 0x0F00,
- eSamplingFrequency_32HZ = 0x0E10,
- eSamplingFrequency_36HZ = 0x0C80,
- eSamplingFrequency_40HZ = 0x0B40,
- eSamplingFrequency_45HZ = 0x0A00,
- eSamplingFrequency_48HZ = 0x0960,
- eSamplingFrequency_50HZ = 0x0900,
- eSamplingFrequency_60HZ = 0x0780,
- eSamplingFrequency_64HZ = 0x0708,
- eSamplingFrequency_72HZ = 0x0640,
- eSamplingFrequency_75HZ = 0x0600,
- eSamplingFrequency_80HZ = 0x05A0,
- eSamplingFrequency_90HZ = 0x0500,
- eSamplingFrequency_96HZ = 0x04B0,
- eSamplingFrequency_100HZ = 0x0480,
- eSamplingFrequency_120HZ = 0x03C0,
- eSamplingFrequency_128HZ = 0x0384,
- eSamplingFrequency_144HZ = 0x0320,
- eSamplingFrequency_150HZ = 0x0300,
- eSamplingFrequency_160HZ = 0x02D0,
- eSamplingFrequency_180HZ = 0x0280,
- eSamplingFrequency_192HZ = 0x0258,
- eSamplingFrequency_200HZ = 0x0240,
- eSamplingFrequency_225HZ = 0x0200,
- eSamplingFrequency_240HZ = 0x01E0,
- eSamplingFrequency_256HZ = 0x01C2,
- eSamplingFrequency_288HZ = 0x0190,
- eSamplingFrequency_300HZ = 0x0180,
- eSamplingFrequency_320HZ = 0x0168,
- eSamplingFrequency_360HZ = 0x0140,
- eSamplingFrequency_384HZ = 0x012C,
- eSamplingFrequency_400HZ = 0x0120,
- eSamplingFrequency_450HZ = 0x0100,
- eSamplingFrequency_480HZ = 0x00F0,
- eSamplingFrequency_512HZ = 0x00E1
- };
-
- enum FusionStrategy
- {
- eNoFusion, eMeanFusion, eGaussianFusion
- };
-
- /*!
- \brief The default constructor.
- The default constructor sets all member variables to zero, i.e. after construction no valid device nor thread are represented.
- */
- CIMUDevice();
-
- /*!
- \brief The destructor.
- */
- virtual ~CIMUDevice();
-
- uint64_t GetDeviceId() const;
-
- inline IMUState GetIMUState() const
- {
+ //Forward definition
+ class IIMUEventDispatcher;
+
+ /*!
+ \class CIMUDevice
+ \ingroup IMU
+ \brief This class contains the the devices module and the thread for read the measurements.
+
+ CIMUDevice encapsulates the device details for the rest of the library and applications.
+ This also includes a thread which is in charge of generate the IMU events
+ */
+ class CIMUDevice
+ {
+ public:
+
+ /*!
+ \brief Enum specifying the running thread policy.
+ */
+ enum ThreadPolicyType
+ {
+ eRealTime = SCHED_FIFO, eRoundRobinPriorityBased = SCHED_RR, eBatch = SCHED_BATCH, eIdle = SCHED_IDLE
+ };
+
+ /*!
+ \brief Enum specifying the supported sampling frequencies.
+ */
+ enum SamplingFrequency
+ {
+ eSamplingFrequency_10HZ = 0x2D00,
+ eSamplingFrequency_12HZ = 0x2580,
+ eSamplingFrequency_15HZ = 0x1E00,
+ eSamplingFrequency_16HZ = 0x1C20,
+ eSamplingFrequency_18HZ = 0x1900,
+ eSamplingFrequency_20HZ = 0x1680,
+ eSamplingFrequency_24HZ = 0x12C0,
+ eSamplingFrequency_25HZ = 0x1200,
+ eSamplingFrequency_30HZ = 0x0F00,
+ eSamplingFrequency_32HZ = 0x0E10,
+ eSamplingFrequency_36HZ = 0x0C80,
+ eSamplingFrequency_40HZ = 0x0B40,
+ eSamplingFrequency_45HZ = 0x0A00,
+ eSamplingFrequency_48HZ = 0x0960,
+ eSamplingFrequency_50HZ = 0x0900,
+ eSamplingFrequency_60HZ = 0x0780,
+ eSamplingFrequency_64HZ = 0x0708,
+ eSamplingFrequency_72HZ = 0x0640,
+ eSamplingFrequency_75HZ = 0x0600,
+ eSamplingFrequency_80HZ = 0x05A0,
+ eSamplingFrequency_90HZ = 0x0500,
+ eSamplingFrequency_96HZ = 0x04B0,
+ eSamplingFrequency_100HZ = 0x0480,
+ eSamplingFrequency_120HZ = 0x03C0,
+ eSamplingFrequency_128HZ = 0x0384,
+ eSamplingFrequency_144HZ = 0x0320,
+ eSamplingFrequency_150HZ = 0x0300,
+ eSamplingFrequency_160HZ = 0x02D0,
+ eSamplingFrequency_180HZ = 0x0280,
+ eSamplingFrequency_192HZ = 0x0258,
+ eSamplingFrequency_200HZ = 0x0240,
+ eSamplingFrequency_225HZ = 0x0200,
+ eSamplingFrequency_240HZ = 0x01E0,
+ eSamplingFrequency_256HZ = 0x01C2,
+ eSamplingFrequency_288HZ = 0x0190,
+ eSamplingFrequency_300HZ = 0x0180,
+ eSamplingFrequency_320HZ = 0x0168,
+ eSamplingFrequency_360HZ = 0x0140,
+ eSamplingFrequency_384HZ = 0x012C,
+ eSamplingFrequency_400HZ = 0x0120,
+ eSamplingFrequency_450HZ = 0x0100,
+ eSamplingFrequency_480HZ = 0x00F0,
+ eSamplingFrequency_512HZ = 0x00E1
+ };
+
+ enum FusionStrategy
+ {
+ eNoFusion, eMeanFusion, eGaussianFusion
+ };
+
+ /*!
+ \brief The default constructor.
+ The default constructor sets all member variables to zero, i.e. after construction no valid device nor thread are represented.
+ */
+ CIMUDevice();
+
+ /*!
+ \brief The destructor.
+ */
+ virtual ~CIMUDevice();
+
+ uint64_t GetDeviceId() const;
+
+ inline IMUState GetIMUState() const
+ {
#ifdef _IMU_USE_XSENS_DEVICE_
- return m_XsensMTiFrame.m_IMUState;
+ return m_XsensMTiFrame.m_IMUState;
#endif
- }
+ }
- bool Connect(const std::string& PortName, const SamplingFrequency Frequency);
+ bool Connect(const std::string& PortName, const SamplingFrequency Frequency);
- bool Start(const bool Blocking = true);
+ bool Start(const bool Blocking = true);
- bool SetThreadRunnigMode(const ThreadPolicyType ThreadPolicy, const float NormalizedPriority);
+ bool SetThreadRunnigMode(const ThreadPolicyType ThreadPolicy, const float NormalizedPriority);
- void Stop(const bool Blocking = true);
+ void Stop(const bool Blocking = true);
- bool SetFusion(const FusionStrategy Strategy, const ushort SamplesPerFusion);
+ bool SetFusion(const FusionStrategy Strategy, const ushort SamplesPerFusion);
- bool IsActive() const;
+ bool IsActive() const;
- bool RegisterEventDispatcher(IIMUEventDispatcher* pIMUEventDispatcher);
+ bool RegisterEventDispatcher(IIMUEventDispatcher* pIMUEventDispatcher);
- bool UnregisterEventDispatcher(IIMUEventDispatcher* pIMUEventDispatcher);
+ bool UnregisterEventDispatcher(IIMUEventDispatcher* pIMUEventDispatcher);
- inline const timeval& GetReferenceTimeStamp() const
- {
- return m_ReferenceTimeStamp;
- }
+ inline const timeval& GetReferenceTimeStamp() const
+ {
+ return m_ReferenceTimeStamp;
+ }
- protected:
+ protected:
- bool LoadCurrentState();
+ bool LoadCurrentState();
- bool MeanFuseCurrentState();
- void IncorporateCurrentStateMeanFusion();
- void MeanFusion();
+ bool MeanFuseCurrentState();
+ void IncorporateCurrentStateMeanFusion();
+ void MeanFusion();
- bool GaussianFuseCurrentState();
- void IncorporateCurrentStateGaussianFusion();
- void GaussianFusion();
+ bool GaussianFuseCurrentState();
+ void IncorporateCurrentStateGaussianFusion();
+ void GaussianFusion();
- bool IntegrateCurrentState();
- bool IntegrateWithOutFusion();
- bool IntegrateWithFusion();
+ bool IntegrateCurrentState();
+ bool IntegrateWithOutFusion();
+ bool IntegrateWithFusion();
- private:
+ private:
- void UnregisterEventDispatchers();
+ void UnregisterEventDispatchers();
- bool InitializeDevice(const std::string& PortName, const SamplingFrequency Frequency);
- void FinalizeModuleDevice();
- void ShouldYield();
- bool DispatchCylcle();
- void SendEvent(const CIMUEvent& Event);
- void SetReferenceTimeStamps();
+ bool InitializeDevice(const std::string& PortName, const SamplingFrequency Frequency);
+ void FinalizeModuleDevice();
+ void ShouldYield();
+ bool DispatchCylcle();
+ void SendEvent(const CIMUEvent& Event);
+ void SetReferenceTimeStamps();
- static void* ThreadLoop(void* pData);
+ static void* ThreadLoop(void* pData);
- uint64_t m_DeviceId;
- SamplingFrequency m_SamplingFrequency;
- int m_PeriodMicroSeconds;
- FusionStrategy m_FusionStrategy;
- int m_SamplesPerFusion;
- int m_CollectedFusionSamples;
- volatile bool m_IsActive;
- volatile bool m_IsDispatching;
- bool m_IsInitialized;
- pthread_t m_pInternalThreadHandel;
- pthread_mutex_t m_IsActiveMutex;
- pthread_mutex_t m_IsDispatchingMutex;
- pthread_mutex_t m_EventDispatchersMutex;
- pthread_mutex_t m_DeviceMutex;
- std::set<IIMUEventDispatcher*> m_IMUEventDispatchers;
- timeval m_ReferenceTimeStamp;
- timeval m_LastFrameTimeStamp;
+ uint64_t m_DeviceId;
+ SamplingFrequency m_SamplingFrequency;
+ int m_PeriodMicroSeconds;
+ FusionStrategy m_FusionStrategy;
+ int m_SamplesPerFusion;
+ int m_CollectedFusionSamples;
+ volatile bool m_IsActive;
+ volatile bool m_IsDispatching;
+ bool m_IsInitialized;
+ pthread_t m_pInternalThreadHandel;
+ pthread_mutex_t m_IsActiveMutex;
+ pthread_mutex_t m_IsDispatchingMutex;
+ pthread_mutex_t m_EventDispatchersMutex;
+ pthread_mutex_t m_DeviceMutex;
+ std::set<IIMUEventDispatcher*> m_IMUEventDispatchers;
+ timeval m_ReferenceTimeStamp;
+ timeval m_LastFrameTimeStamp;
- IMUState::PhysicalData m_FusedPhysicalData;
- IMUState m_FusedIMUState;
- IMUState m_IntegratedIMUState;
+ IMUState::PhysicalData m_FusedPhysicalData;
+ IMUState m_FusedIMUState;
+ IMUState m_IntegratedIMUState;
#ifdef _IMU_USE_XSENS_DEVICE_
- bool InitializeXsensDevice(const std::string& PortName, const SamplingFrequency Frequency);
- void FinalizeXsensModuleDevice();
- void DestroyXsensModuleDevice();
+ bool InitializeXsensDevice(const std::string& PortName, const SamplingFrequency Frequency);
+ void FinalizeXsensModuleDevice();
+ void DestroyXsensModuleDevice();
- Xsens::CXsensMTiModule* m_pXsensMTiModule;
- Xsens::XsensMTiFrame m_XsensMTiFrame;
+ Xsens::CXsensMTiModule* m_pXsensMTiModule;
+ Xsens::XsensMTiFrame m_XsensMTiFrame;
#endif
- };
+ };
}
#endif
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUEvent.cpp b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUEvent.cpp
index 5a2aaef79521e8e7b64fd464b73287cf8b6f8a2d..c2a97a2091b8070061c0c173551b00e1501b6e17 100644
--- a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUEvent.cpp
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUEvent.cpp
@@ -2,8 +2,8 @@
* IMUEvent.cpp
*
* Created on: Mar 16, 2014
- * Author: Dr.-Ing. David Israel González Aguirre
- * Mail: david.gonzalez@kit.edu
+ * Author: Dr.-Ing. David Israel González Aguirre
+ * Mail: david.gonzalez@kit.edu
*/
#include "IMUEvent.h"
@@ -11,44 +11,44 @@
namespace IMU
{
- uint32_t CIMUEvent::s_IdCounter = 0;
- pthread_mutex_t CIMUEvent::s_IdCounterMutex = PTHREAD_MUTEX_INITIALIZER;
-
- CIMUEvent::CIMUEvent(const timeval& TimeStamp, const EventType EventType, const CIMUDevice* pIMUDevice, const IMUState& EventState) :
- m_Id(CreatId()) ,
-m_TimeStamp (TimeStamp), m_EventType(EventType), m_pIMUDevice(pIMUDevice), m_IMUState(EventState)
- {
- }
-
- CIMUEvent::CIMUEvent(const timeval& TimeStamp, const EventType EventType, const CIMUDevice* pIMUDevice) :
- m_Id(CreatId()), m_TimeStamp(TimeStamp), m_EventType(EventType), m_pIMUDevice(pIMUDevice), m_IMUState(pIMUDevice->GetIMUState())
- {
- }
-
- CIMUEvent::CIMUEvent(const EventType EventType, const CIMUDevice* pIMUDevice) :
- m_Id(CreatId()), m_TimeStamp(CTimeStamp::GetCurrentTimeStamp()), m_EventType(EventType), m_pIMUDevice(pIMUDevice), m_IMUState(pIMUDevice->GetIMUState())
- {
- }
-
- CIMUEvent::CIMUEvent(const CIMUEvent& Event) :
- m_Id(CreatId()), m_TimeStamp(Event.m_TimeStamp), m_EventType(Event.m_EventType), m_pIMUDevice(Event.m_pIMUDevice), m_IMUState(Event.m_IMUState)
- {
- }
-
- CIMUEvent::CIMUEvent() :
- m_Id(CreatId()), m_TimeStamp(CTimeStamp::s_Zero), m_EventType(EventType(0x0)), m_pIMUDevice(NULL), m_IMUState()
- {
- }
-
- CIMUEvent::~CIMUEvent()
- {
- }
-
- uint32_t CIMUEvent::CreatId()
- {
- pthread_mutex_lock(&s_IdCounterMutex);
- uint32_t Id = CIMUEvent::s_IdCounter++;
- pthread_mutex_unlock(&s_IdCounterMutex);
- return Id;
- }
+ uint32_t CIMUEvent::s_IdCounter = 0;
+ pthread_mutex_t CIMUEvent::s_IdCounterMutex = PTHREAD_MUTEX_INITIALIZER;
+
+ CIMUEvent::CIMUEvent(const timeval& TimeStamp, const EventType EventType, const CIMUDevice* pIMUDevice, const IMUState& EventState) :
+ m_Id(CreatId()) ,
+ m_TimeStamp(TimeStamp), m_EventType(EventType), m_pIMUDevice(pIMUDevice), m_IMUState(EventState)
+ {
+ }
+
+ CIMUEvent::CIMUEvent(const timeval& TimeStamp, const EventType EventType, const CIMUDevice* pIMUDevice) :
+ m_Id(CreatId()), m_TimeStamp(TimeStamp), m_EventType(EventType), m_pIMUDevice(pIMUDevice), m_IMUState(pIMUDevice->GetIMUState())
+ {
+ }
+
+ CIMUEvent::CIMUEvent(const EventType EventType, const CIMUDevice* pIMUDevice) :
+ m_Id(CreatId()), m_TimeStamp(CTimeStamp::GetCurrentTimeStamp()), m_EventType(EventType), m_pIMUDevice(pIMUDevice), m_IMUState(pIMUDevice->GetIMUState())
+ {
+ }
+
+ CIMUEvent::CIMUEvent(const CIMUEvent& Event) :
+ m_Id(CreatId()), m_TimeStamp(Event.m_TimeStamp), m_EventType(Event.m_EventType), m_pIMUDevice(Event.m_pIMUDevice), m_IMUState(Event.m_IMUState)
+ {
+ }
+
+ CIMUEvent::CIMUEvent() :
+ m_Id(CreatId()), m_TimeStamp(CTimeStamp::s_Zero), m_EventType(EventType(0x0)), m_pIMUDevice(NULL), m_IMUState()
+ {
+ }
+
+ CIMUEvent::~CIMUEvent()
+ {
+ }
+
+ uint32_t CIMUEvent::CreatId()
+ {
+ pthread_mutex_lock(&s_IdCounterMutex);
+ uint32_t Id = CIMUEvent::s_IdCounter++;
+ pthread_mutex_unlock(&s_IdCounterMutex);
+ return Id;
+ }
}
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUEvent.h b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUEvent.h
index f10f4755e27fd2a4c51ad2f38cf5241491877cd2..b342fb84fd69801ae8d3e477673a079185e42564 100644
--- a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUEvent.h
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUEvent.h
@@ -3,7 +3,7 @@
*
* Created on: Mar 16, 2014
* Author: Dr.-Ing. David Israel González Aguirre
- * Mail: david.gonzalez@kit.edu
+ * Mail: david.gonzalez@kit.edu
*/
#ifndef IMUEVENT_H_
@@ -14,58 +14,58 @@
namespace IMU
{
- class CIMUDevice;
- class CIMUEvent
- {
- public:
+ class CIMUDevice;
+ class CIMUEvent
+ {
+ public:
- enum EventType
- {
- eOnIMUStart = 0X0001, eOnIMUStop = 0X0002, eOnIMUCycle = 0X0004, eOnIMUFusedCycle = 0X0008, eOnIMUIntegratedState = 0X0010, eOnIMUCustomEvent = 0X8000
- };
+ enum EventType
+ {
+ eOnIMUStart = 0X0001, eOnIMUStop = 0X0002, eOnIMUCycle = 0X0004, eOnIMUFusedCycle = 0X0008, eOnIMUIntegratedState = 0X0010, eOnIMUCustomEvent = 0X8000
+ };
- CIMUEvent(const timeval& TimeStamp, const EventType EventType, const CIMUDevice* pIMUDevice, const IMUState& EventState);
- CIMUEvent(const timeval& TimeStamp, const EventType EventType, const CIMUDevice* pIMUDevice);
- CIMUEvent(const EventType EventType, const CIMUDevice* pIMUDevice);
- CIMUEvent(const CIMUEvent& Event);
- CIMUEvent();
+ CIMUEvent(const timeval& TimeStamp, const EventType EventType, const CIMUDevice* pIMUDevice, const IMUState& EventState);
+ CIMUEvent(const timeval& TimeStamp, const EventType EventType, const CIMUDevice* pIMUDevice);
+ CIMUEvent(const EventType EventType, const CIMUDevice* pIMUDevice);
+ CIMUEvent(const CIMUEvent& Event);
+ CIMUEvent();
- virtual ~CIMUEvent();
+ virtual ~CIMUEvent();
- inline uint32_t GetId() const
- {
- return m_Id;
- }
+ inline uint32_t GetId() const
+ {
+ return m_Id;
+ }
- inline EventType GetEventType() const
- {
- return m_EventType;
- }
+ inline EventType GetEventType() const
+ {
+ return m_EventType;
+ }
- inline const CIMUDevice* GetIMU() const
- {
- return m_pIMUDevice;
- }
+ inline const CIMUDevice* GetIMU() const
+ {
+ return m_pIMUDevice;
+ }
- inline const timeval& GetTimeStamp() const
- {
- return m_TimeStamp;
- }
+ inline const timeval& GetTimeStamp() const
+ {
+ return m_TimeStamp;
+ }
- protected:
+ protected:
- uint32_t m_Id;
- const timeval m_TimeStamp;
- const EventType m_EventType;
- const CIMUDevice* m_pIMUDevice;
- const IMUState m_IMUState;
+ uint32_t m_Id;
+ const timeval m_TimeStamp;
+ const EventType m_EventType;
+ const CIMUDevice* m_pIMUDevice;
+ const IMUState m_IMUState;
- private:
+ private:
- static uint32_t CreatId();
- static uint32_t s_IdCounter;
- static pthread_mutex_t s_IdCounterMutex;
- };
+ static uint32_t CreatId();
+ static uint32_t s_IdCounter;
+ static pthread_mutex_t s_IdCounterMutex;
+ };
}
#endif
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUHelpers.cpp b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUHelpers.cpp
index 1ceeff59f0115005639a78973fd904217fe2fa6e..c787ddfaf1de4cb788cd85a20e73c297fd986032 100644
--- a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUHelpers.cpp
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUHelpers.cpp
@@ -3,16 +3,16 @@
*
* Created on: Mar 16, 2014
* Author: Dr.-Ing. David Israel González Aguirre
- * Mail: david.gonzalez@kit.edu
+ * Mail: david.gonzalez@kit.edu
*/
#include "IMUHelpers.h"
namespace IMU
{
- const timeval CTimeStamp::s_Zero = { 0 , 0 };
+ const timeval CTimeStamp::s_Zero = { 0 , 0 };
- const float CGeolocationInformation::s_G_LPoles = 9.832f;
- const float CGeolocationInformation::s_G_L45 = 9.806f;
- const float CGeolocationInformation::s_G_LEquator = 9.780f;
+ const float CGeolocationInformation::s_G_LPoles = 9.832f;
+ const float CGeolocationInformation::s_G_L45 = 9.806f;
+ const float CGeolocationInformation::s_G_LEquator = 9.780f;
}
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUHelpers.h b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUHelpers.h
index 82632ec54165d23e5ba09d15a1a7afa718e0dcf6..8e5dd202c1836cef6068fd9be94e6faf27004e79 100644
--- a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUHelpers.h
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUHelpers.h
@@ -3,7 +3,7 @@
*
* Created on: Mar 17, 2014
* Author: Dr.-Ing. David Israel González Aguirre
- * Mail: david.gonzalez@kit.edu
+ * Mail: david.gonzalez@kit.edu
*/
#ifndef IMUHELPERS_H_
@@ -16,59 +16,59 @@
namespace IMU
{
- class CTimeStamp
- {
- public:
-
- inline static timeval GetCurrentTimeStamp()
- {
- timeval TimeStamp;
- gettimeofday(&TimeStamp,NULL);
- return TimeStamp;
- }
-
- inline static float GetElapsedSeconds(const timeval& Post, const timeval& Pre)
- {
- return float(double(GetElapsedMicroseconds(Post,Pre)) / 1000000.0);
- }
-
- inline static float GetElapsedMilliseconds(const timeval& Post, const timeval& Pre)
- {
- return float(double(GetElapsedMicroseconds(Post,Pre)) / 1000.0);
- }
-
- inline static long GetElapsedMicroseconds(const timeval& Post, const timeval& Pre)
- {
- return ((Post.tv_sec - Pre.tv_sec) * 1000000) + (Post.tv_usec - Pre.tv_usec);
- }
-
- inline static long GetElapsedMicroseconds(const timeval& Pre)
- {
- timeval Post;
- gettimeofday(&Post,NULL);
- return ((Post.tv_sec - Pre.tv_sec) * 1000000) + (Post.tv_usec - Pre.tv_usec);
- }
-
- static const timeval s_Zero;
- };
-
- class CGeolocationInformation
- {
- public:
-
- static const float s_G_LPoles;
- static const float s_G_L45;
- static const float s_G_LEquator;
-
- //LatitudeInDegrees of your location:
- //http://www.mapsofworld.com/lat_long/germany-lat-long.html
- //49.0167 Karlsruhe, Germany
-
- static float GetGravitationalAcceleration(const float LatitudeInDegrees = 49.0167f)
- {
- return s_G_L45 - (s_G_LPoles - s_G_LEquator) * std::cos((float(M_PI) / 90.0f) * LatitudeInDegrees);
- }
- };
+ class CTimeStamp
+ {
+ public:
+
+ inline static timeval GetCurrentTimeStamp()
+ {
+ timeval TimeStamp;
+ gettimeofday(&TimeStamp, NULL);
+ return TimeStamp;
+ }
+
+ inline static float GetElapsedSeconds(const timeval& Post, const timeval& Pre)
+ {
+ return float(double(GetElapsedMicroseconds(Post, Pre)) / 1000000.0);
+ }
+
+ inline static float GetElapsedMilliseconds(const timeval& Post, const timeval& Pre)
+ {
+ return float(double(GetElapsedMicroseconds(Post, Pre)) / 1000.0);
+ }
+
+ inline static long GetElapsedMicroseconds(const timeval& Post, const timeval& Pre)
+ {
+ return ((Post.tv_sec - Pre.tv_sec) * 1000000) + (Post.tv_usec - Pre.tv_usec);
+ }
+
+ inline static long GetElapsedMicroseconds(const timeval& Pre)
+ {
+ timeval Post;
+ gettimeofday(&Post, NULL);
+ return ((Post.tv_sec - Pre.tv_sec) * 1000000) + (Post.tv_usec - Pre.tv_usec);
+ }
+
+ static const timeval s_Zero;
+ };
+
+ class CGeolocationInformation
+ {
+ public:
+
+ static const float s_G_LPoles;
+ static const float s_G_L45;
+ static const float s_G_LEquator;
+
+ //LatitudeInDegrees of your location:
+ //http://www.mapsofworld.com/lat_long/germany-lat-long.html
+ //49.0167 Karlsruhe, Germany
+
+ static float GetGravitationalAcceleration(const float LatitudeInDegrees = 49.0167f)
+ {
+ return s_G_L45 - (s_G_LPoles - s_G_LEquator) * std::cos((float(M_PI) / 90.0f) * LatitudeInDegrees);
+ }
+ };
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUState.cpp b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUState.cpp
index cde825c3c17e262d80f8721531370f053dde7756..b578fc2b661c2d7d05250c624628c29ccaf09676 100644
--- a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUState.cpp
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUState.cpp
@@ -3,7 +3,7 @@
*
* Created on: Mar 16, 2014
* Author: Dr.-Ing. David Israel González Aguirre
- * Mail: david.gonzalez@kit.edu
+ * Mail: david.gonzalez@kit.edu
*/
#include "IMUState.h"
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUState.h b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUState.h
index 7f8fe38c447973993230c921c48176059bc70fe0..f1f3df1c3d0100b2f190feb4a09fd5cc03187c94 100644
--- a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUState.h
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUState.h
@@ -3,7 +3,7 @@
*
* Created on: Mar 16, 2014
* Author: Dr.-Ing. David Israel González Aguirre
- * Mail: david.gonzalez@kit.edu
+ * Mail: david.gonzalez@kit.edu
*/
#ifndef IMUSTATE_H_
@@ -13,54 +13,54 @@
namespace IMU
{
- struct IMUState
- {
- inline IMUState() :
- m_ControlData(), m_PhysicalData()
- {
- }
+ struct IMUState
+ {
+ inline IMUState() :
+ m_ControlData(), m_PhysicalData()
+ {
+ }
- struct ControlData
- {
- ControlData() :
- m_PreviousSampleCount(-1), m_CurrentSampleCount(0), m_MessageCounter(0), m_IsConsecutive(false)
- {
- memset(&m_TimeStamp,0,sizeof(timeval));
- }
+ struct ControlData
+ {
+ ControlData() :
+ m_PreviousSampleCount(-1), m_CurrentSampleCount(0), m_MessageCounter(0), m_IsConsecutive(false)
+ {
+ memset(&m_TimeStamp, 0, sizeof(timeval));
+ }
- long m_PreviousSampleCount;
- unsigned short m_CurrentSampleCount;
- long m_MessageCounter;
- bool m_IsConsecutive;
- timeval m_TimeStamp;
- };
+ long m_PreviousSampleCount;
+ unsigned short m_CurrentSampleCount;
+ long m_MessageCounter;
+ bool m_IsConsecutive;
+ timeval m_TimeStamp;
+ };
- struct PhysicalData
- {
- PhysicalData() :
- m_AccelerationMagnitud(0.0f)
- {
- memset(m_Acceleration,0,sizeof(float) * 3);
- memset(m_GyroscopeRotation,0,sizeof(float) * 3);
- memset(m_MagneticRotation,0,sizeof(float) * 3);
- memset(m_QuaternionRotation,0,sizeof(float) * 4);
- }
+ struct PhysicalData
+ {
+ PhysicalData() :
+ m_AccelerationMagnitud(0.0f)
+ {
+ memset(m_Acceleration, 0, sizeof(float) * 3);
+ memset(m_GyroscopeRotation, 0, sizeof(float) * 3);
+ memset(m_MagneticRotation, 0, sizeof(float) * 3);
+ memset(m_QuaternionRotation, 0, sizeof(float) * 4);
+ }
- float m_Acceleration[3];
- float m_AccelerationMagnitud;
- float m_GyroscopeRotation[3];
- float m_MagneticRotation[3];
- float m_QuaternionRotation[4];
+ float m_Acceleration[3];
+ float m_AccelerationMagnitud;
+ float m_GyroscopeRotation[3];
+ float m_MagneticRotation[3];
+ float m_QuaternionRotation[4];
- inline void UpdateAccelerationMagnitud()
- {
- m_AccelerationMagnitud = std::sqrt(m_Acceleration[0] * m_Acceleration[0] + m_Acceleration[1] * m_Acceleration[1] + m_Acceleration[2] * m_Acceleration[2]);
- }
- };
+ inline void UpdateAccelerationMagnitud()
+ {
+ m_AccelerationMagnitud = std::sqrt(m_Acceleration[0] * m_Acceleration[0] + m_Acceleration[1] * m_Acceleration[1] + m_Acceleration[2] * m_Acceleration[2]);
+ }
+ };
- ControlData m_ControlData;
- PhysicalData m_PhysicalData;
- };
+ ControlData m_ControlData;
+ PhysicalData m_PhysicalData;
+ };
}
#endif
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Includes.h b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Includes.h
index 958dea63913562559ba9b810d6febc65e88be338..364db5615867220318bb2195ed6df24e8ac86923 100644
--- a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Includes.h
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Includes.h
@@ -2,8 +2,8 @@
* Includes.h
*
* Created on: Mar 17, 2014
- * Author: Dr.-Ing. David Israel González Aguirre
- * Mail: david.gonzalez@kit.edu
+ * Author: Dr.-Ing. David Israel González Aguirre
+ * Mail: david.gonzalez@kit.edu
*/
#ifndef INCLUDES_H_
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Xsens/Xsens.h b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Xsens/Xsens.h
index f52c895894940c56b40fdf50c5d4d70a34bbc9ca..3f7c4fe033b81dea8242dab459a91e0c59d5315a 100644
--- a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Xsens/Xsens.h
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Xsens/Xsens.h
@@ -17,21 +17,21 @@
namespace IMU
{
- namespace Xsens
- {
- struct XsensMTiFrame
- {
- XsensMTiFrame() :
- m_DataLength(0)
- {
- memset(m_Data,0,sizeof(unsigned char) * MAXMSGLEN);
- }
-
- short m_DataLength;
- unsigned char m_Data[MAXMSGLEN ];
- IMUState m_IMUState;
- };
- }
+ namespace Xsens
+ {
+ struct XsensMTiFrame
+ {
+ XsensMTiFrame() :
+ m_DataLength(0)
+ {
+ memset(m_Data, 0, sizeof(unsigned char) * MAXMSGLEN);
+ }
+
+ short m_DataLength;
+ unsigned char m_Data[MAXMSGLEN ];
+ IMUState m_IMUState;
+ };
+ }
}
#endif
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.cpp b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.cpp
index 389219d7aa4fd18cf340e366e42e32359b5a4275..cad622231046e90bb58b19abb4bbd23c712670f7 100644
--- a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.cpp
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.cpp
@@ -29,14 +29,14 @@
//
// v1.2.0
// 27-02-2006 - Renamed Xbus class to Motion Tracker C++ Communication class, short MTComm
-// - Defines XBRV_* accordingly renamed to MTRV_*
-// - Fixed device length not correct for bid 0 when using Xbus Master and setDeviceMode function
+// - Defines XBRV_* accordingly renamed to MTRV_*
+// - Fixed device length not correct for bid 0 when using Xbus Master and setDeviceMode function
//
// v1.1.7
// 15-02-2006 - Added fixed point signed 12.20 dataformat support
-// Added selective calibrated data output per sensor type support
-// Added outputmode temperature support
-// Fixed warning C4244: '=' : conversion from '' to '', possible loss of data
+// Added selective calibrated data output per sensor type support
+// Added outputmode temperature support
+// Fixed warning C4244: '=' : conversion from '' to '', possible loss of data
// v1.1.6
// 25-01-2006 - Added escape function for CLRDTR, CLRRTS, SETDTR, SETRTS, SETXOFF, SETXON, SETBREAK, CLRBREAK
//
@@ -45,46 +45,46 @@
//
// v1.1.4
// 08-11-2005 - Changed practically all uses of m_timeOut into uses of the new m_clkEnd
-// - Changed COM timeout in win32 to return immediately if data is available,
-// but wait 1ms otherwise
+// - Changed COM timeout in win32 to return immediately if data is available,
+// but wait 1ms otherwise
//
// v1.1.3
// 18-10-2005 - Added MID_REQPRODUCTCODE, MID_REQ/SETTRANSMITDELAY
-// - Added MTRV_TIMEOUTNODATA indicating timeout occurred due to no data read
+// - Added MTRV_TIMEOUTNODATA indicating timeout occurred due to no data read
//
// v1.1.2
// 16-09-2005 - Added eMTS version 0.1->1.0 changes (EMTS_FACTORYMODE)
-// - Added factory output mode defines
+// - Added factory output mode defines
//
// v1.1.1
// 01-09-2005 - Added defines for Extended output mode
-// - Added reqSetting (byte array in + out & no param variant)
+// - Added reqSetting (byte array in + out & no param variant)
//
// v1.1
// 08-08-2005 - Added file read and write support
-// - Added functions for data retrieval (getValue etc)
-// for easy data retrieval of acc, gyr, mag etc
-// - ReadMessageRaw:
-// - added a temporary buffer for unprocessed bytes
-// - check for invalid length messages
-// - Changed BID_MT into 1 and added BID_MASTER (=0xFF)
-// - Changed various ....SerialPort functions to .....Port
-// - Changed mtSendMessage functions to mtWriteMessage
-// - Added numerous defines
-// - Deleted obsolete functions
-// - Changed function getLastErrorCode into getLastDeviceError
-// - Changed OpenPort function for compatiblity with Bluetooth ports
-// - Added workaround for lockup of USB driver (close function)
-// - Added workaround for clock() problem with read function of USB driver
+// - Added functions for data retrieval (getValue etc)
+// for easy data retrieval of acc, gyr, mag etc
+// - ReadMessageRaw:
+// - added a temporary buffer for unprocessed bytes
+// - check for invalid length messages
+// - Changed BID_MT into 1 and added BID_MASTER (=0xFF)
+// - Changed various ....SerialPort functions to .....Port
+// - Changed mtSendMessage functions to mtWriteMessage
+// - Added numerous defines
+// - Deleted obsolete functions
+// - Changed function getLastErrorCode into getLastDeviceError
+// - Changed OpenPort function for compatiblity with Bluetooth ports
+// - Added workaround for lockup of USB driver (close function)
+// - Added workaround for clock() problem with read function of USB driver
//
// v1.0.2
// 29-06-2005 - Inserted initSerialPort with devicename input
-// - Changed return value defines names from X_ to MTRV_
-// - Removed unneeded includes for linux
+// - Changed return value defines names from X_ to MTRV_
+// - Removed unneeded includes for linux
//
// v1.0.1
// 22-06-2005 - Fixed ReqSetting functions (byte array & param variant)
-// mtSendRawString had wrong length input
+// mtSendRawString had wrong length input
//
// v1.0.0
// 20-06-2005 - Initial release
@@ -92,11 +92,11 @@
// ----------------------------------------------------------------------------
// This file is an Xsens Code Examples.
//
-// Copyright (C) Xsens Technologies B.V., 2005.
+// Copyright (C) Xsens Technologies B.V., 2005.
//
// This source code is intended only as a example of the implementation
-// of the Xsens MT Communication protocol.
-// It was written for cross platform capabilities.
+// of the Xsens MT Communication protocol.
+// It was written for cross platform capabilities.
//
// THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
// KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
@@ -109,7 +109,7 @@
#ifdef _DEBUG
#undef THIS_FILE
-static char THIS_FILE[]=__FILE__;
+static char THIS_FILE[] = __FILE__;
#define new DEBUG_NEW
#endif
@@ -123,2827 +123,3067 @@ static char THIS_FILE[]=__FILE__;
namespace IMU
{
- namespace Xsens
- {
- //////////////////////////////////////////////////////////////////////
- // Construction/Destruction
- //////////////////////////////////////////////////////////////////////
- //
- CXsensMTiModule::CXsensMTiModule()
- {
- m_handle = -1;
- m_portOpen = false;
- m_fileOpen = false;
- m_deviceError = 0; // No error
- m_retVal = MTRV_OK;
- m_timeOut = TO_DEFAULT;
- m_nTempBufferLen = 0;
- m_clkEnd = 0;
- for(int i = 0 ; i < MAXDEVICES + 1 ; i++)
- {
- m_storedOutputMode[i] = INVALIDSETTINGVALUE;
- m_storedOutputSettings[i] = INVALIDSETTINGVALUE;
- m_storedDataLength[i] = 0;
- }
- }
-
- CXsensMTiModule::~CXsensMTiModule()
- {
- close();
- }
-
-////////////////////////////////////////////////////////////////////
-// clockms
-//
-// Calculates the processor time used by the calling process.
-// For linux use gettimeofday instead of clock() function
-// When using read from FTDI serial port in a loop the
-// clock() function very often keeps returning 0.
-//
-// Output
-// returns processor time in milliseconds
-//
- clock_t CXsensMTiModule::clockms()
- {
- clock_t clk;
+ namespace Xsens
+ {
+ //////////////////////////////////////////////////////////////////////
+ // Construction/Destruction
+ //////////////////////////////////////////////////////////////////////
+ //
+ CXsensMTiModule::CXsensMTiModule()
+ {
+ m_handle = -1;
+ m_portOpen = false;
+ m_fileOpen = false;
+ m_deviceError = 0; // No error
+ m_retVal = MTRV_OK;
+ m_timeOut = TO_DEFAULT;
+ m_nTempBufferLen = 0;
+ m_clkEnd = 0;
+
+ for (int i = 0 ; i < MAXDEVICES + 1 ; i++)
+ {
+ m_storedOutputMode[i] = INVALIDSETTINGVALUE;
+ m_storedOutputSettings[i] = INVALIDSETTINGVALUE;
+ m_storedDataLength[i] = 0;
+ }
+ }
+
+ CXsensMTiModule::~CXsensMTiModule()
+ {
+ close();
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // clockms
+ //
+ // Calculates the processor time used by the calling process.
+ // For linux use gettimeofday instead of clock() function
+ // When using read from FTDI serial port in a loop the
+ // clock() function very often keeps returning 0.
+ //
+ // Output
+ // returns processor time in milliseconds
+ //
+ clock_t CXsensMTiModule::clockms()
+ {
+ clock_t clk;
#ifdef WIN32
- clk = clock(); // Get current processor time
+ clk = clock(); // Get current processor time
#if (CLOCKS_PER_SEC != 1000)
- clk /= (CLOCKS_PER_SEC / 1000);
- // clk = (clk * 1000) / CLOCKS_PER_SEC;
+ clk /= (CLOCKS_PER_SEC / 1000);
+ // clk = (clk * 1000) / CLOCKS_PER_SEC;
#endif
#else
- struct timeval tv;
- struct timezone tz;
- gettimeofday(&tv,&tz);
- clk = tv.tv_sec * 1000 + (tv.tv_usec / 1000);
+ struct timeval tv;
+ struct timezone tz;
+ gettimeofday(&tv, &tz);
+ clk = tv.tv_sec * 1000 + (tv.tv_usec / 1000);
#endif
- return clk;
- }
+ return clk;
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // openPort (serial port number as input parameter)
+ //
+ // Opens a 'live' connection to a MT or XM
+ //
+ // Input
+ // portNumber : number of serial port to open (1-99)
+ // baudrate : baudrate value (One of the PBR_* defines), default = PBR_115K2
+ // inqueueSize : size of input queue, default = 4096
+ // outqueueSize: size of output queue, default = 1024
+ //
+ // Output
+ // returns MTRV_OK if serial port is successfully opened, else MTRV_INPUTCANNOTBEOPENED
+ //
+ short CXsensMTiModule::openPort(const int portNumber, const unsigned long baudrate, const unsigned long /*inqueueSize*/, const unsigned long /*outqueueSize*/)
+ {
+ m_clkEnd = 0;
+
+ if (m_fileOpen || m_portOpen)
+ {
+ return (m_retVal = MTRV_ANINPUTALREADYOPEN);
+ }
-////////////////////////////////////////////////////////////////////
-// openPort (serial port number as input parameter)
-//
-// Opens a 'live' connection to a MT or XM
-//
-// Input
-// portNumber : number of serial port to open (1-99)
-// baudrate : baudrate value (One of the PBR_* defines), default = PBR_115K2
-// inqueueSize : size of input queue, default = 4096
-// outqueueSize: size of output queue, default = 1024
-//
-// Output
-// returns MTRV_OK if serial port is successfully opened, else MTRV_INPUTCANNOTBEOPENED
-//
- short CXsensMTiModule::openPort(const int portNumber, const unsigned long baudrate, const unsigned long /*inqueueSize*/, const unsigned long /*outqueueSize*/)
- {
- m_clkEnd = 0;
-
- if (m_fileOpen || m_portOpen)
- {
- return (m_retVal = MTRV_ANINPUTALREADYOPEN);
- }
-#ifdef WIN32
- char pchFileName[10];
-
- sprintf(pchFileName,"\\\\.\\COM%d",portNumber); // Create file name
-
- m_handle = CreateFile(pchFileName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
- if (m_handle == INVALID_HANDLE_VALUE)
- {
- return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
- }
-
- // Once here, port is open
- m_portOpen = true;
-
- //Get the current state & then change it
- DCB dcb;
-
- GetCommState(m_handle, &dcb);// Get current state
-
- dcb.BaudRate = baudrate;// Setup the baud rate
- dcb.Parity = NOPARITY;// Setup the Parity
- dcb.ByteSize = 8;// Setup the data bits
- dcb.StopBits = TWOSTOPBITS;// Setup the stop bits
- dcb.fDsrSensitivity = FALSE;// Setup the flow control
- dcb.fOutxCtsFlow = FALSE;// NoFlowControl:
- dcb.fOutxDsrFlow = FALSE;
- dcb.fOutX = FALSE;
- dcb.fInX = FALSE;
- if (!SetCommState(m_handle, (LPDCB)&dcb))
- { // Set new state
- // Bluetooth ports cannot always be opened with 2 stopbits
- // Now try to open port with 1 stopbit.
- dcb.StopBits = ONESTOPBIT;
- if (!SetCommState(m_handle, (LPDCB)&dcb))
- {
- CloseHandle(m_handle);
- m_handle = INVALID_HANDLE_VALUE;
- m_portOpen = false;
- return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
- }
- }
-
- // Set COM timeouts
- COMMTIMEOUTS CommTimeouts;
-
- GetCommTimeouts(m_handle,&CommTimeouts);// Fill CommTimeouts structure
-
- // immediate return if data is available, wait 1ms otherwise
- CommTimeouts.ReadTotalTimeoutConstant = 1;
- CommTimeouts.ReadIntervalTimeout = MAXDWORD;
- CommTimeouts.ReadTotalTimeoutMultiplier = MAXDWORD;
-
- // immediate return whether data is available or not
-// CommTimeouts.ReadTotalTimeoutConstant = 0;
-// CommTimeouts.ReadIntervalTimeout = MAXDWORD;
-// CommTimeouts.ReadTotalTimeoutMultiplier = 0;
-
- SetCommTimeouts(m_handle, &CommTimeouts);// Set CommTimeouts structure
-
- // Other initialization functions
- EscapeCommFunction(m_handle, SETRTS);// Enable RTS (for Xbus Master use)
- SetupComm(m_handle,inqueueSize,outqueueSize);// Set queue size
-
- // Remove any 'old' data in buffer
- PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
-
- return (m_retVal = MTRV_OK);
-#else
- char chPort[15];
- struct termios options;
-
- /* Open port */
- sprintf(chPort,"/dev/ttyS%d",(portNumber - 1));
- m_handle = open(chPort,O_RDWR | O_NOCTTY);
-
- // O_RDWR: Read+Write
- // O_NOCTTY: Raw input, no "controlling terminal"
- // O_NDELAY: Don't care about DCD signal
-
- if (m_handle < 0)
- {
- // Port not open
- return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
- }
-
- // Once here, port is open
- m_portOpen = true;
-
- /* Start configuring of port for non-canonical transfer mode */
- // Get current options for the port
- tcgetattr(m_handle,&options);
-
- // Set baudrate.
- cfsetispeed(&options,baudrate);
- cfsetospeed(&options,baudrate);
-
- // Enable the receiver and set local mode
- options.c_cflag |= (CLOCAL | CREAD);
- // Set character size to data bits and set no parity Mask the characte size bits
- options.c_cflag &= ~(CSIZE | PARENB);
- options.c_cflag |= CS8; // Select 8 data bits
- options.c_cflag |= CSTOPB; // send 2 stop bits
- // Disable hardware flow control
- options.c_cflag &= ~CRTSCTS;
- options.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
- // Disable software flow control
- options.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
- // Set Raw output
- options.c_oflag &= ~OPOST;
- // Timeout 0.005 sec for first byte, read minimum of 0 bytes
- options.c_cc[VMIN] = 0;
- options.c_cc[VTIME] = 5;
-
- // Set the new options for the port
- tcsetattr(m_handle,TCSANOW,&options);
-
- tcflush(m_handle,TCIOFLUSH);
-
- return (m_retVal = MTRV_OK);
-#endif
- }
-
-////////////////////////////////////////////////////////////////////
-// openPort (string as input parameter)
-//
-// Opens a 'live' connection to a MT or XM
-//
-// Input
-// portName : device name of serial port ("/dev/ttyUSB0" or "\\\\.\\COM1")
-// baudrate : baudrate value (One of the PBR_* defines), default = PBR_115K2
-// inqueueSize : size of input queue, default = 4096
-// outqueueSize: size of output queue, default = 1024
-//
-// Output
-// returns MTRV_OK if serial port is successfully opened, else MTRV_INPUTCANNOTBEOPENED
-//
- short CXsensMTiModule::openPort(const char *portName, const unsigned long baudrate, const unsigned long /*inqueueSize*/, const unsigned long /*outqueueSize*/)
- {
- m_clkEnd = 0;
-
- if (m_fileOpen || m_portOpen)
- {
- return (m_retVal = MTRV_ANINPUTALREADYOPEN);
- }
-#ifdef WIN32
- m_handle = CreateFile(portName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
- if (m_handle == INVALID_HANDLE_VALUE)
- {
- return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
- }
-
- // Once here, port is open
- m_portOpen = true;
-
- //Get the current state & then change it
- DCB dcb;
-
- GetCommState(m_handle, &dcb);// Get current state
-
- dcb.BaudRate = baudrate;// Setup the baud rate
- dcb.Parity = NOPARITY;// Setup the Parity
- dcb.ByteSize = 8;// Setup the data bits
- dcb.StopBits = TWOSTOPBITS;// Setup the stop bits
- dcb.fDsrSensitivity = FALSE;// Setup the flow control
- dcb.fOutxCtsFlow = FALSE;// NoFlowControl:
- dcb.fOutxDsrFlow = FALSE;
- dcb.fOutX = FALSE;
- dcb.fInX = FALSE;
- if (!SetCommState(m_handle, (LPDCB)&dcb))
- { // Set new state
- // Bluetooth ports cannot always be opened with 2 stopbits
- // Now try to open port with 1 stopbit.
- dcb.StopBits = ONESTOPBIT;
- if (!SetCommState(m_handle, (LPDCB)&dcb))
- {
- CloseHandle(m_handle);
- m_handle = INVALID_HANDLE_VALUE;
- m_portOpen = false;
- return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
- }
- }
-
- // Set COM timeouts
- COMMTIMEOUTS CommTimeouts;
-
- GetCommTimeouts(m_handle,&CommTimeouts);// Fill CommTimeouts structure
-
- // immediate return if data is available, wait 1ms otherwise
- CommTimeouts.ReadTotalTimeoutConstant = 1;
- CommTimeouts.ReadIntervalTimeout = MAXDWORD;
- CommTimeouts.ReadTotalTimeoutMultiplier = MAXDWORD;
-
- // immediate return whether data is available or not
-// CommTimeouts.ReadTotalTimeoutConstant = 0;
-// CommTimeouts.ReadIntervalTimeout = MAXDWORD;
-// CommTimeouts.ReadTotalTimeoutMultiplier = 0;
- SetCommTimeouts(m_handle, &CommTimeouts);// Set CommTimeouts structure
-
- // Other initialization functions
- EscapeCommFunction(m_handle, SETRTS);// Enable RTS (for Xbus Master use)
- SetupComm(m_handle,inqueueSize,outqueueSize);// Set queue size
-
- // Remove any 'old' data in buffer
- PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
-
- return (m_retVal = MTRV_OK);
-#else
- struct termios options;
-
- /* Open port */
-
- m_handle = open(portName,O_RDWR | O_NOCTTY);
-
- // O_RDWR: Read+Write
- // O_NOCTTY: Raw input, no "controlling terminal"
- // O_NDELAY: Don't care about DCD signal
-
- if (m_handle < 0)
- {
- // Port not open
- return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
- }
-
- // Once here, port is open
- m_portOpen = true;
-
- /* Start configuring of port for non-canonical transfer mode */
- // Get current options for the port
- tcgetattr(m_handle,&options);
-
- // Set baudrate.
- cfsetispeed(&options,baudrate);
- cfsetospeed(&options,baudrate);
-
- // Enable the receiver and set local mode
- options.c_cflag |= (CLOCAL | CREAD);
- // Set character size to data bits and set no parity Mask the characte size bits
- options.c_cflag &= ~(CSIZE | PARENB);
- options.c_cflag |= CS8; // Select 8 data bits
- options.c_cflag |= CSTOPB; // send 2 stop bits
- // Disable hardware flow control
- options.c_cflag &= ~CRTSCTS;
- options.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
- // Disable software flow control
- options.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
- // Set Raw output
- options.c_oflag &= ~OPOST;
- // Timeout 0.005 sec for first byte, read minimum of 0 bytes
- options.c_cc[VMIN] = 0;
- options.c_cc[VTIME] = 5;
-
- // Set the new options for the port
- tcsetattr(m_handle,TCSANOW,&options);
-
- tcflush(m_handle,TCIOFLUSH);
-
- return (m_retVal = MTRV_OK);
-#endif
- }
-
-////////////////////////////////////////////////////////////////////
-// openFile
-//
-// Open file for reading and writing
-// Filepos is at start of file
-//
-// Input
-// fileName : file name including path
-// createAlways: empties the log file, if existing
-//
-// Output
-// returns MTRV_OK if the file is opened
-// returns MTRV_INPUTCANNOTBEOPENED if the file can not be opened
-// returns MTRV_ANINPUTALREADYOPEN if a serial port / file is already opened
-//
- short CXsensMTiModule::openFile(const char *fileName, bool createAlways)
- {
- m_clkEnd = 0;
-
- if (m_portOpen || m_portOpen)
- {
- return (m_retVal = MTRV_ANINPUTALREADYOPEN);
- }
-#ifdef WIN32
- DWORD disposition = OPEN_ALWAYS;
- if (createAlways == true)
- {
- disposition = CREATE_ALWAYS;
- }
- m_handle = CreateFile(fileName, GENERIC_READ | GENERIC_WRITE, FILE_SHARE_READ, NULL, disposition, 0, NULL);
- if (m_handle == INVALID_HANDLE_VALUE)
- {
- return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
- }
-#else
- int openMode = O_RDWR | O_CREAT;
- if (createAlways == true)
- {
- openMode |= O_TRUNC;
- }
- m_handle = open(fileName,openMode,S_IRWXU);
- if (m_handle < 0)
- {
- return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
- }
-#endif
- m_timeOut = 0; // No timeout when using file input
- m_fileOpen = true;
- return (m_retVal = MTRV_OK);
+#ifdef WIN32
+ char pchFileName[10];
- }
+ sprintf(pchFileName, "\\\\.\\COM%d", portNumber); // Create file name
-////////////////////////////////////////////////////////////////////
-// isPortOpen
-//
-// Return if serial port is open or not
-//
- bool CXsensMTiModule::isPortOpen()
- {
- return m_portOpen;
- }
+ m_handle = CreateFile(pchFileName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
-////////////////////////////////////////////////////////////////////
-// isFileOpen
-//
-// Return if serial port is open or not
-//
- bool CXsensMTiModule::isFileOpen()
- {
- return m_fileOpen;
- }
+ if (m_handle == INVALID_HANDLE_VALUE)
+ {
+ return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
+ }
-////////////////////////////////////////////////////////////////////
-// readData
-//
-// Reads bytes from serial port or file
-//
-// Input
-// msgBuffer : pointer to buffer in which next string will be stored
-// nBytesToRead : number of buffer bytes to read from serial port
-// retval : return value, either MTRV_OK, MTRV_ENDOFFILE or MTRV_NOINPUTINITIALIZED
-//
-// Output
-// number of bytes actually read
- int CXsensMTiModule::readData(unsigned char* msgBuffer, const int nBytesToRead)
- {
- //if(!m_fileOpen && !m_portOpen)
- if (!(m_portOpen || m_fileOpen))
- {
- m_retVal = MTRV_NOINPUTINITIALIZED;
- return 0;
- }
-#ifdef WIN32
- DWORD nBytesRead;
- BOOL retval = ReadFile(m_handle, msgBuffer, nBytesToRead, &nBytesRead, NULL);
- if (retval && nBytesRead == 0 && m_fileOpen)
- {
- m_retVal = MTRV_ENDOFFILE;
- }
- else
- m_retVal = MTRV_OK;
- return nBytesRead;
+ // Once here, port is open
+ m_portOpen = true;
+
+ //Get the current state & then change it
+ DCB dcb;
+
+ GetCommState(m_handle, &dcb);// Get current state
+
+ dcb.BaudRate = baudrate;// Setup the baud rate
+ dcb.Parity = NOPARITY;// Setup the Parity
+ dcb.ByteSize = 8;// Setup the data bits
+ dcb.StopBits = TWOSTOPBITS;// Setup the stop bits
+ dcb.fDsrSensitivity = FALSE;// Setup the flow control
+ dcb.fOutxCtsFlow = FALSE;// NoFlowControl:
+ dcb.fOutxDsrFlow = FALSE;
+ dcb.fOutX = FALSE;
+ dcb.fInX = FALSE;
+
+ if (!SetCommState(m_handle, (LPDCB)&dcb))
+ {
+ // Set new state
+ // Bluetooth ports cannot always be opened with 2 stopbits
+ // Now try to open port with 1 stopbit.
+ dcb.StopBits = ONESTOPBIT;
+
+ if (!SetCommState(m_handle, (LPDCB)&dcb))
+ {
+ CloseHandle(m_handle);
+ m_handle = INVALID_HANDLE_VALUE;
+ m_portOpen = false;
+ return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
+ }
+ }
+
+ // Set COM timeouts
+ COMMTIMEOUTS CommTimeouts;
+
+ GetCommTimeouts(m_handle, &CommTimeouts); // Fill CommTimeouts structure
+
+ // immediate return if data is available, wait 1ms otherwise
+ CommTimeouts.ReadTotalTimeoutConstant = 1;
+ CommTimeouts.ReadIntervalTimeout = MAXDWORD;
+ CommTimeouts.ReadTotalTimeoutMultiplier = MAXDWORD;
+
+ // immediate return whether data is available or not
+ // CommTimeouts.ReadTotalTimeoutConstant = 0;
+ // CommTimeouts.ReadIntervalTimeout = MAXDWORD;
+ // CommTimeouts.ReadTotalTimeoutMultiplier = 0;
+
+ SetCommTimeouts(m_handle, &CommTimeouts);// Set CommTimeouts structure
+
+ // Other initialization functions
+ EscapeCommFunction(m_handle, SETRTS);// Enable RTS (for Xbus Master use)
+ SetupComm(m_handle, inqueueSize, outqueueSize); // Set queue size
+
+ // Remove any 'old' data in buffer
+ PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
+
+ return (m_retVal = MTRV_OK);
#else
- const int nBytesRead = read(m_handle,msgBuffer,nBytesToRead);
-
- m_retVal = MTRV_OK;
- if (nBytesRead)
- return nBytesRead;
- else if (m_fileOpen)
- m_retVal = MTRV_ENDOFFILE;
- return nBytesRead;
- /*
- if (nBytesRead == 0 && m_fileOpen)
- {
- nBytesRead = 0;
- m_retVal = MTRV_ENDOFFILE;
- }
- else
- m_retVal = MTRV_OK;
- return nBytesRead;*/
-
- // In Linux it is sometimes better to read per byte instead of a block of bytes at once
- // Use this block if experiencing problems with the above code
- /*
- int j = 0; // Index in buffer for read data
- int k = 0; // Timeout factor
- int nRead = 0; // Bytes read from port, return by read function
-
- do {
- nRead = read(m_handle, &msgBuffer[j], 1);
- if (nRead == 1) { // Byte read
- k = 0;
- j++;
- }
- else {
- k++;
- }
-
- if (k == 3) { // Timeout, too long no data read from port
- return nRead;
- }
- }
- while (j < nBytesToRead);
-
- return j;
- */
+ char chPort[15];
+ struct termios options;
+
+ /* Open port */
+ sprintf(chPort, "/dev/ttyS%d", (portNumber - 1));
+ m_handle = open(chPort, O_RDWR | O_NOCTTY);
+
+ // O_RDWR: Read+Write
+ // O_NOCTTY: Raw input, no "controlling terminal"
+ // O_NDELAY: Don't care about DCD signal
+
+ if (m_handle < 0)
+ {
+ // Port not open
+ return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
+ }
+
+ // Once here, port is open
+ m_portOpen = true;
+
+ /* Start configuring of port for non-canonical transfer mode */
+ // Get current options for the port
+ tcgetattr(m_handle, &options);
+
+ // Set baudrate.
+ cfsetispeed(&options, baudrate);
+ cfsetospeed(&options, baudrate);
+
+ // Enable the receiver and set local mode
+ options.c_cflag |= (CLOCAL | CREAD);
+ // Set character size to data bits and set no parity Mask the characte size bits
+ options.c_cflag &= ~(CSIZE | PARENB);
+ options.c_cflag |= CS8; // Select 8 data bits
+ options.c_cflag |= CSTOPB; // send 2 stop bits
+ // Disable hardware flow control
+ options.c_cflag &= ~CRTSCTS;
+ options.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
+ // Disable software flow control
+ options.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
+ // Set Raw output
+ options.c_oflag &= ~OPOST;
+ // Timeout 0.005 sec for first byte, read minimum of 0 bytes
+ options.c_cc[VMIN] = 0;
+ options.c_cc[VTIME] = 5;
+
+ // Set the new options for the port
+ tcsetattr(m_handle, TCSANOW, &options);
+
+ tcflush(m_handle, TCIOFLUSH);
+
+ return (m_retVal = MTRV_OK);
#endif
- }
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // openPort (string as input parameter)
+ //
+ // Opens a 'live' connection to a MT or XM
+ //
+ // Input
+ // portName : device name of serial port ("/dev/ttyUSB0" or "\\\\.\\COM1")
+ // baudrate : baudrate value (One of the PBR_* defines), default = PBR_115K2
+ // inqueueSize : size of input queue, default = 4096
+ // outqueueSize: size of output queue, default = 1024
+ //
+ // Output
+ // returns MTRV_OK if serial port is successfully opened, else MTRV_INPUTCANNOTBEOPENED
+ //
+ short CXsensMTiModule::openPort(const char* portName, const unsigned long baudrate, const unsigned long /*inqueueSize*/, const unsigned long /*outqueueSize*/)
+ {
+ m_clkEnd = 0;
+
+ if (m_fileOpen || m_portOpen)
+ {
+ return (m_retVal = MTRV_ANINPUTALREADYOPEN);
+ }
-////////////////////////////////////////////////////////////////////
-// writeData
-//
-// Writes bytes to serial port (to do: file)
-//
-// Input
-// msgBuffer : a pointer to a char buffer containing
-// the characters to be written to serial port
-// nBytesToWrite : number of buffer bytes to write to serial port
-//
-// Output
-// number of bytes actually written
-//
-// The MTComm return value is != MTRV_OK if serial port is closed
- int CXsensMTiModule::writeData(const unsigned char* msgBuffer, const int nBytesToWrite)
- {
- if (!m_fileOpen && !m_portOpen)
- {
- m_retVal = MTRV_NOINPUTINITIALIZED;
- return 0;
- }
-
- m_retVal = MTRV_OK;
#ifdef WIN32
- DWORD nBytesWritten;
- WriteFile(m_handle, msgBuffer, nBytesToWrite, &nBytesWritten, NULL);
- return nBytesWritten;
+ m_handle = CreateFile(portName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
+
+ if (m_handle == INVALID_HANDLE_VALUE)
+ {
+ return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
+ }
+
+ // Once here, port is open
+ m_portOpen = true;
+
+ //Get the current state & then change it
+ DCB dcb;
+
+ GetCommState(m_handle, &dcb);// Get current state
+
+ dcb.BaudRate = baudrate;// Setup the baud rate
+ dcb.Parity = NOPARITY;// Setup the Parity
+ dcb.ByteSize = 8;// Setup the data bits
+ dcb.StopBits = TWOSTOPBITS;// Setup the stop bits
+ dcb.fDsrSensitivity = FALSE;// Setup the flow control
+ dcb.fOutxCtsFlow = FALSE;// NoFlowControl:
+ dcb.fOutxDsrFlow = FALSE;
+ dcb.fOutX = FALSE;
+ dcb.fInX = FALSE;
+
+ if (!SetCommState(m_handle, (LPDCB)&dcb))
+ {
+ // Set new state
+ // Bluetooth ports cannot always be opened with 2 stopbits
+ // Now try to open port with 1 stopbit.
+ dcb.StopBits = ONESTOPBIT;
+
+ if (!SetCommState(m_handle, (LPDCB)&dcb))
+ {
+ CloseHandle(m_handle);
+ m_handle = INVALID_HANDLE_VALUE;
+ m_portOpen = false;
+ return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
+ }
+ }
+
+ // Set COM timeouts
+ COMMTIMEOUTS CommTimeouts;
+
+ GetCommTimeouts(m_handle, &CommTimeouts); // Fill CommTimeouts structure
+
+ // immediate return if data is available, wait 1ms otherwise
+ CommTimeouts.ReadTotalTimeoutConstant = 1;
+ CommTimeouts.ReadIntervalTimeout = MAXDWORD;
+ CommTimeouts.ReadTotalTimeoutMultiplier = MAXDWORD;
+
+ // immediate return whether data is available or not
+ // CommTimeouts.ReadTotalTimeoutConstant = 0;
+ // CommTimeouts.ReadIntervalTimeout = MAXDWORD;
+ // CommTimeouts.ReadTotalTimeoutMultiplier = 0;
+ SetCommTimeouts(m_handle, &CommTimeouts);// Set CommTimeouts structure
+
+ // Other initialization functions
+ EscapeCommFunction(m_handle, SETRTS);// Enable RTS (for Xbus Master use)
+ SetupComm(m_handle, inqueueSize, outqueueSize); // Set queue size
+
+ // Remove any 'old' data in buffer
+ PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
+
+ return (m_retVal = MTRV_OK);
#else
- return write(m_handle,msgBuffer,nBytesToWrite);
+ struct termios options;
+
+ /* Open port */
+
+ m_handle = open(portName, O_RDWR | O_NOCTTY);
+
+ // O_RDWR: Read+Write
+ // O_NOCTTY: Raw input, no "controlling terminal"
+ // O_NDELAY: Don't care about DCD signal
+
+ if (m_handle < 0)
+ {
+ // Port not open
+ return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
+ }
+
+ // Once here, port is open
+ m_portOpen = true;
+
+ /* Start configuring of port for non-canonical transfer mode */
+ // Get current options for the port
+ tcgetattr(m_handle, &options);
+
+ // Set baudrate.
+ cfsetispeed(&options, baudrate);
+ cfsetospeed(&options, baudrate);
+
+ // Enable the receiver and set local mode
+ options.c_cflag |= (CLOCAL | CREAD);
+ // Set character size to data bits and set no parity Mask the characte size bits
+ options.c_cflag &= ~(CSIZE | PARENB);
+ options.c_cflag |= CS8; // Select 8 data bits
+ options.c_cflag |= CSTOPB; // send 2 stop bits
+ // Disable hardware flow control
+ options.c_cflag &= ~CRTSCTS;
+ options.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
+ // Disable software flow control
+ options.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
+ // Set Raw output
+ options.c_oflag &= ~OPOST;
+ // Timeout 0.005 sec for first byte, read minimum of 0 bytes
+ options.c_cc[VMIN] = 0;
+ options.c_cc[VTIME] = 5;
+
+ // Set the new options for the port
+ tcsetattr(m_handle, TCSANOW, &options);
+
+ tcflush(m_handle, TCIOFLUSH);
+
+ return (m_retVal = MTRV_OK);
#endif
- }
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // openFile
+ //
+ // Open file for reading and writing
+ // Filepos is at start of file
+ //
+ // Input
+ // fileName : file name including path
+ // createAlways: empties the log file, if existing
+ //
+ // Output
+ // returns MTRV_OK if the file is opened
+ // returns MTRV_INPUTCANNOTBEOPENED if the file can not be opened
+ // returns MTRV_ANINPUTALREADYOPEN if a serial port / file is already opened
+ //
+ short CXsensMTiModule::openFile(const char* fileName, bool createAlways)
+ {
+ m_clkEnd = 0;
+
+ if (m_portOpen || m_portOpen)
+ {
+ return (m_retVal = MTRV_ANINPUTALREADYOPEN);
+ }
-////////////////////////////////////////////////////////////////////
-// flush
-//
-// Remove any 'old' data in COM port buffer and flushes internal
-// temporary buffer
-//
- void CXsensMTiModule::flush()
- {
- if (m_portOpen)
- {
#ifdef WIN32
- // Remove any 'old' data in buffer
- PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
+ DWORD disposition = OPEN_ALWAYS;
+
+ if (createAlways == true)
+ {
+ disposition = CREATE_ALWAYS;
+ }
+
+ m_handle = CreateFile(fileName, GENERIC_READ | GENERIC_WRITE, FILE_SHARE_READ, NULL, disposition, 0, NULL);
+
+ if (m_handle == INVALID_HANDLE_VALUE)
+ {
+ return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
+ }
+
#else
- tcflush(m_handle,TCIOFLUSH);
+ int openMode = O_RDWR | O_CREAT;
+
+ if (createAlways == true)
+ {
+ openMode |= O_TRUNC;
+ }
+
+ m_handle = open(fileName, openMode, S_IRWXU);
+
+ if (m_handle < 0)
+ {
+ return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
+ }
+
#endif
- }
- m_nTempBufferLen = 0;
- m_retVal = MTRV_OK;
- }
+ m_timeOut = 0; // No timeout when using file input
+ m_fileOpen = true;
+ return (m_retVal = MTRV_OK);
+
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // isPortOpen
+ //
+ // Return if serial port is open or not
+ //
+ bool CXsensMTiModule::isPortOpen()
+ {
+ return m_portOpen;
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // isFileOpen
+ //
+ // Return if serial port is open or not
+ //
+ bool CXsensMTiModule::isFileOpen()
+ {
+ return m_fileOpen;
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // readData
+ //
+ // Reads bytes from serial port or file
+ //
+ // Input
+ // msgBuffer : pointer to buffer in which next string will be stored
+ // nBytesToRead : number of buffer bytes to read from serial port
+ // retval : return value, either MTRV_OK, MTRV_ENDOFFILE or MTRV_NOINPUTINITIALIZED
+ //
+ // Output
+ // number of bytes actually read
+ int CXsensMTiModule::readData(unsigned char* msgBuffer, const int nBytesToRead)
+ {
+ //if(!m_fileOpen && !m_portOpen)
+ if (!(m_portOpen || m_fileOpen))
+ {
+ m_retVal = MTRV_NOINPUTINITIALIZED;
+ return 0;
+ }
-////////////////////////////////////////////////////////////////////
-// escape
-//
-// Directs a COM port to perform an extended function
-//
-// Input
-// function : Windows define. Can be one of following:
-// CLRDTR, CLRRTS, SETDTR, SETRTS, SETXOFF, SETXON, SETBREAK, CLRBREAK
- void CXsensMTiModule::escape(unsigned long /*function*/)
- {
#ifdef WIN32
- EscapeCommFunction(m_handle, function);
+ DWORD nBytesRead;
+ BOOL retval = ReadFile(m_handle, msgBuffer, nBytesToRead, &nBytesRead, NULL);
+
+ if (retval && nBytesRead == 0 && m_fileOpen)
+ {
+ m_retVal = MTRV_ENDOFFILE;
+ }
+ else
+ {
+ m_retVal = MTRV_OK;
+ }
+
+ return nBytesRead;
#else
+ const int nBytesRead = read(m_handle, msgBuffer, nBytesToRead);
+
+ m_retVal = MTRV_OK;
+
+ if (nBytesRead)
+ {
+ return nBytesRead;
+ }
+ else if (m_fileOpen)
+ {
+ m_retVal = MTRV_ENDOFFILE;
+ }
+
+ return nBytesRead;
+ /*
+ if (nBytesRead == 0 && m_fileOpen)
+ {
+ nBytesRead = 0;
+ m_retVal = MTRV_ENDOFFILE;
+ }
+ else
+ m_retVal = MTRV_OK;
+ return nBytesRead;*/
+
+ // In Linux it is sometimes better to read per byte instead of a block of bytes at once
+ // Use this block if experiencing problems with the above code
+ /*
+ int j = 0; // Index in buffer for read data
+ int k = 0; // Timeout factor
+ int nRead = 0; // Bytes read from port, return by read function
+
+ do {
+ nRead = read(m_handle, &msgBuffer[j], 1);
+ if (nRead == 1) { // Byte read
+ k = 0;
+ j++;
+ }
+ else {
+ k++;
+ }
+
+ if (k == 3) { // Timeout, too long no data read from port
+ return nRead;
+ }
+ }
+ while (j < nBytesToRead);
+
+ return j;
+ */
#endif
- }
-
-////////////////////////////////////////////////////////////////////
-// setPortQueueSize
-//
-// Set input and output buffer size of serial port
-//
- void CXsensMTiModule::setPortQueueSize(const unsigned long /*inqueueSize = 4096 */, const unsigned long /*outqueueSize = 1024 */)
- {
- if (m_portOpen)
- {
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // writeData
+ //
+ // Writes bytes to serial port (to do: file)
+ //
+ // Input
+ // msgBuffer : a pointer to a char buffer containing
+ // the characters to be written to serial port
+ // nBytesToWrite : number of buffer bytes to write to serial port
+ //
+ // Output
+ // number of bytes actually written
+ //
+ // The MTComm return value is != MTRV_OK if serial port is closed
+ int CXsensMTiModule::writeData(const unsigned char* msgBuffer, const int nBytesToWrite)
+ {
+ if (!m_fileOpen && !m_portOpen)
+ {
+ m_retVal = MTRV_NOINPUTINITIALIZED;
+ return 0;
+ }
+
+ m_retVal = MTRV_OK;
#ifdef WIN32
- SetupComm(m_handle,inqueueSize,outqueueSize); // Set queue size
+ DWORD nBytesWritten;
+ WriteFile(m_handle, msgBuffer, nBytesToWrite, &nBytesWritten, NULL);
+ return nBytesWritten;
#else
- // Not yet implemented
+ return write(m_handle, msgBuffer, nBytesToWrite);
#endif
- }
- m_retVal = MTRV_OK;
- }
-
-////////////////////////////////////////////////////////////////////
-// setFilePos
-//
-// Sets the current position of the file pointer for file input
-//
-// Input
-// relPos : 32-bit value specifying the relative move in bytes
-// origin is specified in moveMethod
-// moveMethod : FILEPOS_BEGIN, FILEPOS_CURRENT or FILEPOS_END
-// Output
-//
-// returns MTRV_OK if file pointer is successfully set
-//
- short CXsensMTiModule::setFilePos(long relPos, unsigned long moveMethod)
- {
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // flush
+ //
+ // Remove any 'old' data in COM port buffer and flushes internal
+ // temporary buffer
+ //
+ void CXsensMTiModule::flush()
+ {
+ if (m_portOpen)
+ {
#ifdef WIN32
- if (m_fileOpen)
- {
- if(SetFilePointer(m_handle, relPos, NULL, moveMethod) != INVALID_SET_FILE_POINTER)
- {
- return (m_retVal = MTRV_OK);
- }
- }
+ // Remove any 'old' data in buffer
+ PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
#else
- if (m_fileOpen)
- {
- if (lseek(m_handle,relPos,moveMethod) != -1)
- {
- return (m_retVal = MTRV_OK);
- }
- }
+ tcflush(m_handle, TCIOFLUSH);
#endif
- return (m_retVal = MTRV_NOTSUCCESSFUL);
- }
-
-////////////////////////////////////////////////////////////////////
-// getFileSize
-//
-// Retrieves the file size of the opened file
-//
-// Input
-// Output
-// fileSize : Number of bytes of the current file
-//
-// returns MTRV_OK if successful
-//
- short CXsensMTiModule::getFileSize(unsigned long &fileSize)
- {
+ }
+
+ m_nTempBufferLen = 0;
+ m_retVal = MTRV_OK;
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // escape
+ //
+ // Directs a COM port to perform an extended function
+ //
+ // Input
+ // function : Windows define. Can be one of following:
+ // CLRDTR, CLRRTS, SETDTR, SETRTS, SETXOFF, SETXON, SETBREAK, CLRBREAK
+ void CXsensMTiModule::escape(unsigned long /*function*/)
+ {
#ifdef WIN32
- if (m_fileOpen)
- {
- if ((fileSize = GetFileSize(m_handle, NULL)) != INVALID_FILE_SIZE)
- {
- return (m_retVal = MTRV_OK);
- }
- }
+ EscapeCommFunction(m_handle, function);
#else
- if (m_fileOpen)
- {
- struct stat buf;
- if (fstat(m_handle,&buf) == 0)
- {
- fileSize = buf.st_size;
- return (m_retVal = MTRV_OK);
- }
- }
#endif
- return (m_retVal = MTRV_NOTSUCCESSFUL);
- }
-
-////////////////////////////////////////////////////////////////////
-// close
-//
-// Closes handle of file or serial port
-//
- short CXsensMTiModule::close()
- {
- if (m_portOpen || m_fileOpen)
- {
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // setPortQueueSize
+ //
+ // Set input and output buffer size of serial port
+ //
+ void CXsensMTiModule::setPortQueueSize(const unsigned long /*inqueueSize = 4096 */, const unsigned long /*outqueueSize = 1024 */)
+ {
+ if (m_portOpen)
+ {
#ifdef WIN32
- if (m_portOpen)
- { // Because of USB-serial driver bug
- flush();
- }
- CloseHandle(m_handle);
+ SetupComm(m_handle, inqueueSize, outqueueSize); // Set queue size
#else
- ::close(m_handle);
+ // Not yet implemented
#endif
- }
- m_fileOpen = m_portOpen = false;
- m_timeOut = TO_DEFAULT; // Restore timeout value (file input)
- m_clkEnd = 0;
- m_nTempBufferLen = 0;
- m_deviceError = 0; // No error
- for(int i = 0 ; i < MAXDEVICES + 1 ; i++)
- {
- m_storedOutputMode[i] = INVALIDSETTINGVALUE;
- m_storedOutputSettings[i] = INVALIDSETTINGVALUE;
- m_storedDataLength[i] = 0;
- }
- return (m_retVal = MTRV_OK);
- }
-
-////////////////////////////////////////////////////////////////////
-// readMessage
-//
-// Reads the next message from serial port buffer or file.
-// To be read within current time out period
-//
-// Input
-// Output
-// mid : MessageID of message received
-// data : array pointer to data bytes (no header)
-// dataLen : number of data bytes read
-// bid : BID or address of message read (optional)
-//
-// returns MTRV_OK if a message has been read else <>MTRV_OK
-//
-// Remarks
-// allocate enough memory for message buffer
-// use setTimeOut for different timeout value
- short CXsensMTiModule::readMessage(unsigned char &mid, unsigned char data[], short &dataLen, unsigned char *bid)
- {
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
-
- if (!(m_fileOpen || m_portOpen))
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
- }
-
- if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
- {
- // Message read
- mid = buffer[IND_MID];
- if (bid != NULL)
- {
- *bid = buffer[IND_BID];
- }
- if (buffer[IND_LEN] != EXTLENCODE)
- {
- dataLen = buffer[IND_LEN];
- memcpy(data,&buffer[IND_DATA0],dataLen);
- }
- else
- {
- dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
- memcpy(data,&buffer[IND_DATAEXT0],dataLen);
- }
- }
- return m_retVal;
- }
-
-////////////////////////////////////////////////////////////////////
-// readDataMessage
-//
-// Read a MTData or XMData message from serial port (using TimeOut val)
-//
-// Input
-// data : pointer to buffer in which the DATA field of MTData/XMData is stored
-// dataLen : number of bytes in buffer (= number bytes in DATA field)
-// Output
-// returns MTRV_OK if MTData / XMData message has been read else <>MTRV_OK
-//
-// Remarks
-// allocate enough memory for data buffer
-// use setTimeOut for different timeout value
- short CXsensMTiModule::readDataMessage(unsigned char data[], short &dataLen)
- {
- if (!(m_portOpen || m_fileOpen))
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
-
- unsigned char buffer[MAXMSGLEN ];
- short buflen;
-
- if (readMessageRaw(buffer,&buflen) == MTRV_OK)
- {
- if (buffer[IND_MID] == MID_MTDATA)
- { // MID_XMDATA is same
- if (buffer[IND_LEN] != EXTLENCODE)
- {
- dataLen = buffer[IND_LEN];
- memcpy(data,&buffer[IND_DATA0],dataLen);
- }
- else
- {
- dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
- memcpy(data,&buffer[IND_DATAEXT0],dataLen);
- }
- return (m_retVal = MTRV_OK);
- }
- else if (buffer[IND_MID] == MID_ERROR)
- {
- m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
- }
- }
- return m_retVal;
- }
-
-////////////////////////////////////////////////////////////////////
-// readMessageRaw
-//
-// Read a message from serial port for a certain period
-//
-// Input
-// msgBuffer : pointer to buffer in which next msg will be stored
-// msgBufferLength: integer to number of bytes written in buffer (header + data + chksum)
-// Output
-// returns MTRV_OK if a message has been read else <>MTRV_OK
-//
-// Remarks
-// allocate enough memory for message buffer
-// use setTimeOut for different timeout value
- short CXsensMTiModule::readMessageRaw(unsigned char *msgBuffer, short *msgBufferLength)
- {
- int state = 0;
- int nBytesToRead = 1;
- int nBytesRead = 0;
- int nOffset = 0;
- int nMsgDataLen = 0;
- int nMsgLen;
- unsigned char chCheckSum;
- bool Synced = false;
-
- if (!(m_portOpen || m_fileOpen))
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
-
- // Copy previous read bytes if available
- if (m_nTempBufferLen > 0)
- {
- memcpy(msgBuffer,m_tempBuffer,m_nTempBufferLen);
- nBytesRead = m_nTempBufferLen;
- m_nTempBufferLen = 0;
- }
- clock_t clkStart = clockms(); // Get current processor time
- clock_t clkEnd = m_clkEnd; // check if the end timer is already set
- if (clkEnd == 0)
- clkEnd = clkStart + m_timeOut;
-
- while(true)
- {
- do
- {
- // First check if we already have some bytes read
- if (nBytesRead > 0 && nBytesToRead > 0)
- {
- if (nBytesToRead > nBytesRead)
- {
- nOffset += nBytesRead;
- nBytesToRead -= nBytesRead;
- nBytesRead = 0;
- }
- else
- {
- nOffset += nBytesToRead;
- nBytesRead -= nBytesToRead;
- nBytesToRead = 0;
- }
- }
-
- // Check if serial port buffer must be read
- if (nBytesToRead > 0)
- {
- nBytesRead = readData(msgBuffer + nOffset,nBytesToRead);
- if (m_retVal == MTRV_ENDOFFILE)
- return (m_retVal = MTRV_ENDOFFILE);
- nOffset += nBytesRead;
- nBytesToRead -= nBytesRead;
- nBytesRead = 0;
- }
-
- if (!nBytesToRead)
- {
- switch(state)
- {
- case 0: // Check preamble
- if (msgBuffer[IND_PREAMBLE] == PREAMBLE)
- {
- state++;
- nBytesToRead = 3;
- }
- else
- {
- nOffset = 0;
- nBytesToRead = 1;
- }
- break;
- case 1: // Check ADDR, MID, LEN
- if (msgBuffer[IND_LEN] != 0xFF)
- {
- state = 3;
- nBytesToRead = (nMsgDataLen = msgBuffer[IND_LEN]) + 1; // Read LEN + 1 (chksum)
- }
- else
- {
- state = 2;
- nBytesToRead = 2; // Read extended length
- }
- break;
- case 2:
- state = 3;
- nBytesToRead = (nMsgDataLen = msgBuffer[IND_LENEXTH] * 256 + msgBuffer[IND_LENEXTL]) + 1; // Read LENEXT + CS
- if (nMsgDataLen > MAXMSGLEN - LEN_MSGEXTHEADERCS)
- {
- // Not synced - check for preamble in the bytes read
- for(int i = 1 ; i < LEN_MSGEXTHEADER ; i++)
- if (msgBuffer[i] == PREAMBLE)
- {
- // Found a maybe preamble - 'fill buffer'
- nBytesRead = LEN_MSGEXTHEADER - i;
- memmove(msgBuffer,msgBuffer + i,nBytesRead);
- break;
- }
- Synced = false;
- nOffset = 0;
- state = 0;
- nBytesToRead = 1; // Start looking for preamble
- }
- break;
- case 3: // Check msg
- chCheckSum = 0;
- nMsgLen = nMsgDataLen + 5 + (msgBuffer[IND_LEN] == 0xFF ? 2 : 0);
-
- for(int i = 1 ; i < nMsgLen ; i++)
- chCheckSum += msgBuffer[i];
-
- if (chCheckSum == 0)
- { // Checksum ok?
- if (nBytesRead > 0)
- { // Store bytes if read too much
- memcpy(m_tempBuffer,msgBuffer + nMsgLen,nBytesRead);
- m_nTempBufferLen = nBytesRead;
- }
- *msgBufferLength = nMsgLen;
- Synced = true;
- return (m_retVal = MTRV_OK);
- }
- else
- {
- if (!Synced)
- for(int i = 1 ; i < nMsgLen ; i++) // Not synced - maybe recheck for preamble in this incorrect message
- if (msgBuffer[i] == PREAMBLE)
- {
- // Found a maybe preamble - 'fill buffer'
- nBytesRead = nMsgLen - i;
- memmove(msgBuffer,msgBuffer + i,nBytesRead);
- break;
- }
- Synced = false;
- nOffset = 0;
- state = 0;
- nBytesToRead = 1; // Start looking for preamble
- }
- break;
- }
- }
- }
- while((clkEnd >= clockms()) || m_timeOut == 0 || nBytesRead != 0);
-
- // Check if pending message has a valid message
- if (state > 0)
- {
- int i;
- // Search for preamble
- for(i = 1; i < nOffset ; i++)
- if (msgBuffer[i] == PREAMBLE)
- {
- // Found a maybe preamble - 'fill buffer'
- nBytesRead = nOffset - i - 1; // no preamble
- memmove(msgBuffer + 1,msgBuffer + i + 1,nBytesRead);
- break;
- }
- if (i < nOffset)
- {
- // Found preamble in message - recheck
- nOffset = 1;
- state = 1;
- nBytesToRead = 3; // Start looking for preamble
- continue;
- }
- }
- break;
- }
-
- return (m_retVal = MTRV_TIMEOUT);
- }
-
-////////////////////////////////////////////////////////////////////
-// writeMessage (optional: integer value)
-//
-// Sends a message and in case of an serial port interface it checks
-// if the return message (ack, error or timeout). See return value
-// In case an file is opened the functions returns directly after
-// 'sending' the message
-//
-// Use this function for GotoConfig, Reset, ResetOrientation etc
-//
-// Input
-// mid : MessageID of message to send
-// dataValue : A integer value that will be included into the data message field
-// can be a 1,2 or 4 bytes values
-// dataValueLen : Size of dataValue in bytes
-// bid : BID or address to use in message to send (default = 0xFF)
-//
-// Return value
-// = MTRV_OK if an Ack message received / or data successfully written to file
-// = MTRV_RECVERRORMSG if an error message received
-// = MTRV_TIMEOUT if timeout occurred
-// = MTRV_NOINPUTINITIALIZED
-//
- short CXsensMTiModule::writeMessage(const unsigned char mid, const unsigned long dataValue, const unsigned char dataValueLen, const unsigned char bid)
- {
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
-
- if (!(m_fileOpen || m_portOpen))
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
- }
-
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
- buffer[IND_LEN] = dataValueLen;
- if (dataValueLen)
- swapEndian((const unsigned char *) &dataValue,&buffer[IND_DATA0],dataValueLen);
- calcChecksum(buffer,LEN_MSGHEADER + dataValueLen);
-
- // Send message
- writeData(buffer,LEN_MSGHEADERCS + dataValueLen);
-
- // Return if file opened
- if (m_fileOpen)
- {
- return (m_retVal = MTRV_OK);
- }
-
- // Keep reading until an Ack or Error message is received (or timeout)
- clock_t clkStart , clkOld;
- bool msgRead = false;
-
- clkStart = clockms(); // Get current processor time
- clkOld = m_clkEnd;
- if (clkOld == 0)
- m_clkEnd = m_timeOut + clkStart;
-
- while(m_clkEnd >= clockms() || (m_timeOut == 0))
- {
- if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
- {
- // Message received
- msgRead = true;
- if (buffer[IND_MID] == (mid + 1))
- {
- m_clkEnd = clkOld;
- return (m_retVal = MTRV_OK); // Acknowledge received
- }
- else if (buffer[IND_MID] == MID_ERROR)
- {
- m_deviceError = buffer[IND_DATA0];
- m_clkEnd = clkOld;
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
- }
- }
- }
-
- m_clkEnd = clkOld;
- if (msgRead)
- return (m_retVal = MTRV_TIMEOUT);
- else
- return (m_retVal = MTRV_TIMEOUTNODATA);
- }
-
-////////////////////////////////////////////////////////////////////
-// writeMessage (data array)
-//
-// Sends a message and in case of an serial port interface it checks
-// if the return message (ack, error or timeout). See return value
-// In case an file is opened the functions returns directly after
-// 'sending' the message
-//
-// Input
-// mid : MessageID of message to send
-// data : array pointer to data bytes
-// dataLen : number of bytes to include in message
-// bid : BID or address to use in message to send (default = 0xFF)
-//
-// Output
-// = MTRV_OK if an Ack message received
-// = MTRV_RECVERRORMSG if an error message received
-// = MTRV_TIMEOUT if timeout occurred
-// = MTRV_NOINPUTINITIALIZED
-//
- short CXsensMTiModule::writeMessage(const unsigned char mid, const unsigned char data[], const unsigned short &dataLen, const unsigned char bid)
- {
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
- short headerLength;
-
- if (!(m_fileOpen || m_portOpen))
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
- }
-
- // Build message to send
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
-
- if (dataLen < EXTLENCODE)
- {
- buffer[IND_LEN] = (unsigned char) dataLen;
- headerLength = LEN_MSGHEADER;
- }
- else
- {
- buffer[IND_LEN] = EXTLENCODE;
- buffer[IND_LENEXTH] = (unsigned char) (dataLen >> 8);
- buffer[IND_LENEXTL] = (unsigned char) (dataLen & 0x00FF);
- headerLength = LEN_MSGEXTHEADER;
- }
- memcpy(&buffer[headerLength],data,dataLen);
- calcChecksum(buffer,headerLength + dataLen);
-
- // Send message
- writeData(buffer,headerLength + dataLen + LEN_CHECKSUM);
-
- // Return if file opened
- if (m_fileOpen)
- {
- return (m_retVal = MTRV_OK);
- }
-
- // Keep reading until an Ack or Error message is received (or timeout)
- bool msgRead = false;
- clock_t clkStart , clkOld;
-
- clkStart = clockms(); // Get current processor time
- clkOld = m_clkEnd;
- if (clkOld == 0)
- m_clkEnd = m_timeOut + clkStart;
-
- while(m_clkEnd >= clockms() || (m_timeOut == 0))
- {
- if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
- {
- // Message received
- msgRead = true;
- if (buffer[IND_MID] == (mid + 1))
- {
- m_clkEnd = clkOld;
- return (m_retVal = MTRV_OK); // Acknowledge received
- }
- else if (buffer[IND_MID] == MID_ERROR)
- {
- m_deviceError = buffer[IND_DATA0];
- m_clkEnd = clkOld;
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
- }
- }
- }
-
- m_clkEnd = clkOld;
- if (msgRead)
- return (m_retVal = MTRV_TIMEOUT);
- else
- return (m_retVal = MTRV_TIMEOUTNODATA);
- }
-
-////////////////////////////////////////////////////////////////////
-// waitForMessage
-//
-// Read messages from serial port or file using the current timeout period
-// until the received message is equal to a specific message identifier
-// By default the timeout period by file input is set to infinity (= until
-// end of file is reached)
-//
-// Input/Output
-// mid : message identifier of message that should be returned
-// data : pointer to buffer in which the data of the requested msg will be stored
-// dataLen : integer to number of data bytes
-// bid : optional, pointer which holds the bid of the returned message
-// Output
-// returns MTRV_OK if the message has been read else != MTRV_OK
-//
-// Remarks
-// allocate enough memory for data message buffer
-// use setTimeOut for different timeout value
- short CXsensMTiModule::waitForMessage(const unsigned char mid, unsigned char data[], short *dataLen, unsigned char *bid)
- {
- unsigned char buffer[MAXMSGLEN ];
- short buflen;
-
- clock_t clkStart , clkOld;
-
- if (!(m_fileOpen || m_portOpen))
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
- }
-
- clkStart = clockms(); // Get current processor time
- clkOld = m_clkEnd;
- if (clkOld == 0)
- m_clkEnd = m_timeOut + clkStart;
-
- while(m_clkEnd >= clockms() || (m_timeOut == 0))
- {
- if (readMessageRaw(buffer,&buflen) == MTRV_OK)
- {
- if (buffer[IND_MID] == mid)
- {
- if (bid != NULL)
- {
- *bid = buffer[IND_BID];
- }
- if (data != NULL && dataLen != NULL)
- {
- if (buffer[IND_LEN] != EXTLENCODE)
- {
- *dataLen = buffer[IND_LEN];
- memcpy(data,&buffer[IND_DATA0],*dataLen);
- }
- else
- {
- *dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
- memcpy(data,&buffer[IND_DATAEXT0],*dataLen);
- }
- }
- else if (dataLen != NULL)
- dataLen = 0;
- m_clkEnd = clkOld;
- return (m_retVal = MTRV_OK);
- }
- }
- else if (m_retVal == MTRV_ENDOFFILE)
- {
- m_clkEnd = clkOld;
- return (m_retVal = MTRV_ENDOFFILE);
- }
- }
-
- m_clkEnd = clkOld;
- return (m_retVal = MTRV_TIMEOUT);
- }
-
-////////////////////////////////////////////////////////////////////
-// reqSetting (integer & no param variant)
-//
-// Request a integer setting from the device. This setting
-// can be an unsigned 1,2 or 4 bytes setting. Only valid
-// for serial port connections
-//
-// Input
-// mid : Message ID of message to send
-// bid : Bus ID of message to send (def 0xFF)
-//
-// Output
-// = MTRV_OK if an Ack message is received
-// = MTRV_RECVERRORMSG if an error message is received
-// = MTRV_TIMEOUT if timeout occurred
-//
-// value contains the integer value of the data field of the ack message
-//
- short CXsensMTiModule::reqSetting(const unsigned char mid, unsigned long &value, const unsigned char bid)
- {
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
-
- if (m_fileOpen)
- {
- return (m_retVal = MTRV_INVALIDFORFILEINPUT);
- }
- if (!m_portOpen)
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
- }
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
- buffer[IND_LEN] = 0;
- calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
-
- // Send message
- writeData(buffer,LEN_MSGHEADERCS + buffer[IND_LEN]);
-
- // Read next message or else timeout
- if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
- {
- // Message received
- if (buffer[IND_MID] == (mid + 1))
- {
- // Acknowledge received
- value = 0;
- swapEndian(&buffer[IND_DATA0],(unsigned char *) &value,buffer[IND_LEN]);
- return (m_retVal = MTRV_OK);
- }
- else if (buffer[IND_MID] == MID_ERROR)
- {
- m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
- }
- else
- {
- return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
- }
- }
- return m_retVal;
- }
-
-////////////////////////////////////////////////////////////////////
-// reqSetting (integer & param variant)
-//
-// Request a integer setting from the device. This setting
-// can be an unsigned 1,2 or 4 bytes setting. Only valid
-// for serial port connections.
-//
-// Input
-// mid : Message ID of message to send
-// param : For messages that need a parameter
-// bid : Bus ID of message to send (def 0xFF)
-//
-// Output
-// = MTRV_OK if an Ack message is received
-// = MTRV_RECVERRORMSG if an error message is received
-// = MTRV_TIMEOUT if timeout occurred
-//
-// value contains the integer value of the data field of the ack message
-//
- short CXsensMTiModule::reqSetting(const unsigned char mid, const unsigned char param, unsigned long &value, const unsigned char bid)
- {
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
-
- if (m_fileOpen)
- {
- return (m_retVal = MTRV_INVALIDFORFILEINPUT);
- }
- if (!m_portOpen)
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
- }
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
- if (param != 0xFF)
- {
- buffer[IND_LEN] = 1;
- buffer[IND_DATA0] = param;
- }
- else
- {
- buffer[IND_LEN] = 0;
- }
- calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
-
- // Send message
- writeData(buffer,LEN_MSGHEADERCS + buffer[IND_LEN]);
-
- // Read next message or else timeout
- if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
- {
- // Message received
- if (buffer[IND_MID] == (mid + 1))
- {
- // Acknowledge received
- value = 0;
- if (param == 0xFF)
- {
- swapEndian(&buffer[IND_DATA0],(unsigned char *) &value,buffer[IND_LEN]);
- }
- else
- {
- swapEndian(&buffer[IND_DATA0] + 1,(unsigned char *) &value,buffer[IND_LEN] - 1);
- }
- return (m_retVal = MTRV_OK);
- }
- else if (buffer[IND_MID] == MID_ERROR)
- {
- m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
- }
- else
- {
- return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
- }
- }
- return m_retVal;
- }
-
-////////////////////////////////////////////////////////////////////
-// reqSetting (float & no param variant)
-//
-// Request a float setting from the device. Only valid
-// for serial port connections.
-//
-// Input
-// mid : Message ID of message to send
-// bid : Bus ID of message to send (def 0xFF)
-//
-// Output
-// = MTRV_OK if an Ack message is received
-// = MTRV_RECVERRORMSG if an error message is received
-// = MTRV_TIMEOUT if timeout occurred
-//
-// value contains the float value of the acknowledge data field
-//
- short CXsensMTiModule::reqSetting(const unsigned char mid, float &value, const unsigned char bid)
- {
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
-
- if (m_fileOpen)
- {
- return (m_retVal = MTRV_INVALIDFORFILEINPUT);
- }
- if (!m_portOpen)
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
- }
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
- buffer[IND_LEN] = 0;
- calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
-
- // Send message
- writeData(buffer,LEN_MSGHEADERCS + buffer[IND_LEN]);
-
- // Read next message or else timeout
- if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
- {
- // Message received
- if (buffer[IND_MID] == (mid + 1))
- {
- // Acknowledge received
- value = 0;
- swapEndian(&buffer[IND_DATA0],(unsigned char *) &value,buffer[IND_LEN]);
- return (m_retVal = MTRV_OK);
- }
- else if (buffer[IND_MID] == MID_ERROR)
- {
- m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
- }
- else
- {
- return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
- }
- }
- return m_retVal;
- }
-
-////////////////////////////////////////////////////////////////////
-// reqSetting (float & param variant)
-//
-// Request a float setting from the device. Only valid
-// for serial port connections.
-//
-// Input
-// mid : Message ID of message to send
-// param : For messages that need a parameter (optional)
-// bid : Bus ID of message to send (def 0xFF)
-//
-// Output
-// = MTRV_OK if an Ack message is received
-// = MTRV_RECVERRORMSG if an error message is received
-// = MTRV_TIMEOUT if timeout occurred
-//
-// value contains the float value of the acknowledge data field
-//
- short CXsensMTiModule::reqSetting(const unsigned char mid, const unsigned char param, float &value, const unsigned char bid)
- {
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
-
- if (m_fileOpen)
- {
- return (m_retVal = MTRV_INVALIDFORFILEINPUT);
- }
- if (!m_portOpen)
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
- }
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
- if (param != 0xFF)
- {
- buffer[IND_LEN] = 1;
- buffer[IND_DATA0] = param;
- }
- else
- {
- buffer[IND_LEN] = 0;
- }
- calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
-
- // Send message
- writeData(buffer,LEN_MSGHEADERCS + buffer[IND_LEN]);
-
- // Read next message or else timeout
- if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
- {
- // Message received
- if (buffer[IND_MID] == (mid + 1))
- {
- // Acknowledge received
- value = 0;
- if (param == 0xFF)
- {
- swapEndian(&buffer[IND_DATA0],(unsigned char *) &value,buffer[IND_LEN]);
- }
- else
- {
- swapEndian(&buffer[IND_DATA0] + 1,(unsigned char *) &value,buffer[IND_LEN] - 1);
- }
- return (m_retVal = MTRV_OK);
- }
- else if (buffer[IND_MID] == MID_ERROR)
- {
- m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
- }
- else
- {
- return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
- }
- }
- return m_retVal;
- }
-
-////////////////////////////////////////////////////////////////////
-// reqSetting (byte array & no param variant)
-//
-// Send a message to the device and the data of acknowledge message
-// will be returned. Only valid for serial port connections
-//
-// Input
-// mid : Message ID of message to send
-// bid : Bus ID of message to send (def 0xFF)
-//
-// Output
-// = MTRV_OK if an Ack message is received
-// = MTRV_RECVERRORMSG if an error message is received
-// = MTRV_TIMEOUT if timeout occurred
-//
-// data[] contains the data of the acknowledge message
-// dataLen contains the number bytes returned
-//
- short CXsensMTiModule::reqSetting(const unsigned char mid, unsigned char data[], short &dataLen, const unsigned char bid)
- {
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
-
- if (m_fileOpen)
- {
- return (m_retVal = MTRV_INVALIDFORFILEINPUT);
- }
- if (!m_portOpen)
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
- }
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
- buffer[IND_LEN] = 0;
- calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
-
- // Send message
- writeData(buffer,LEN_MSGHEADERCS + buffer[IND_LEN]);
-
- dataLen = 0;
- // Read next message or else timeout
- if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
- {
- // Message received
- if (buffer[IND_MID] == (mid + 1))
- {
- // Acknowledge received
- if (buffer[IND_LEN] != EXTLENCODE)
- {
- dataLen = buffer[IND_LEN];
- memcpy(data,&buffer[IND_DATA0],dataLen);
- }
- else
- {
- dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
- memcpy(data,&buffer[IND_DATAEXT0],dataLen);
- }
- return (m_retVal = MTRV_OK);
- }
- else if (buffer[IND_MID] == MID_ERROR)
- {
- m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
- }
- else
- {
- return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
- }
- }
- return m_retVal;
- }
-
-////////////////////////////////////////////////////////////////////
-// reqSetting (byte array in + out & no param variant)
-//
-// Send a message to the device and the data of acknowledge message
-// will be returned. Only valid for serial port connections
-//
-// Input
-// mid : Message ID of message to send
-// bid : Bus ID of message to send (def 0xFF)
-// dataIn : Data to be included in request
-// dataInLen : Number of bytes in dataIn
-//
-// Output
-// = MTRV_OK if an Ack message is received
-// = MTRV_RECVERRORMSG if an error message is received
-// = MTRV_TIMEOUT if timeout occurred
-//
-// dataOut[] contains the data of the acknowledge message
-// dataOutLen contains the number bytes returned
-//
- short CXsensMTiModule::reqSetting(const unsigned char mid, unsigned char dataIn[], short dataInLen, unsigned char dataOut[], short &dataOutLen, const unsigned char bid)
- {
- unsigned char buffer[MAXMSGLEN ];
- short headerLength;
- short msgLen;
-
- if (m_fileOpen)
- {
- return (m_retVal = MTRV_INVALIDFORFILEINPUT);
- }
- if (!m_portOpen)
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
- }
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
- if (dataInLen < EXTLENCODE)
- {
- buffer[IND_LEN] = (unsigned char) dataInLen;
- headerLength = LEN_MSGHEADER;
- }
- else
- {
- buffer[IND_LEN] = EXTLENCODE;
- buffer[IND_LENEXTH] = (unsigned char) (dataInLen >> 8);
- buffer[IND_LENEXTL] = (unsigned char) (dataInLen & 0x00FF);
- headerLength = LEN_MSGEXTHEADER;
- }
- memcpy(&buffer[headerLength],dataIn,dataInLen);
- calcChecksum(buffer,headerLength + dataInLen);
-
- // Send message
- writeData(buffer,headerLength + dataInLen + LEN_CHECKSUM);
-
- dataOutLen = 0;
- // Read next message or else timeout
- if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
- {
- // Message received
- if (buffer[IND_MID] == (mid + 1))
- {
- // Acknowledge received
- if (buffer[IND_LEN] != EXTLENCODE)
- {
- dataOutLen = buffer[IND_LEN];
- memcpy(dataOut,&buffer[IND_DATA0],dataOutLen);
- }
- else
- {
- dataOutLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
- memcpy(dataOut,&buffer[IND_DATAEXT0],dataOutLen);
- }
- return (m_retVal = MTRV_OK);
- }
- else if (buffer[IND_MID] == MID_ERROR)
- {
- m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
- }
- else
- {
- return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
- }
- }
- return m_retVal;
- }
-
-////////////////////////////////////////////////////////////////////
-// reqSetting (byte array & param variant)
-//
-// Send a message to the device and the data of acknowledge message
-// will be returned. Only valid for serial port connections
-//
-// Input
-// mid : Message ID of message to send
-// param : For messages that need a parameter (optional)
-// bid : Bus ID of message to send (def 0xFF)
-//
-// Output
-// = MTRV_OK if an Ack message is received
-// = MTRV_RECVERRORMSG if an error message is received
-// = MTRV_TIMEOUT if timeout occurred
-//
-// data[] contains the data of the acknowledge message (including param!!)
-// dataLen contains the number bytes returned
-//
- short CXsensMTiModule::reqSetting(const unsigned char mid, const unsigned char param, unsigned char data[], short &dataLen, const unsigned char bid)
- {
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
-
- if (m_fileOpen)
- {
- return (m_retVal = MTRV_INVALIDFORFILEINPUT);
- }
- if (!m_portOpen)
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
- }
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
- if (param != 0xFF)
- {
- buffer[IND_LEN] = 1;
- buffer[IND_DATA0] = param;
- }
- else
- {
- buffer[IND_LEN] = 0;
- }
- calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
-
- // Send message
- writeData(buffer,LEN_MSGHEADERCS + buffer[IND_LEN]);
-
- dataLen = 0;
- // Read next message or else timeout
- if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
- {
- // Message received
- if (buffer[IND_MID] == (mid + 1))
- {
- // Acknowledge received
- if (buffer[IND_LEN] != EXTLENCODE)
- {
- dataLen = buffer[IND_LEN];
- memcpy(data,&buffer[IND_DATA0],dataLen);
- }
- else
- {
- dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
- memcpy(data,&buffer[IND_DATAEXT0],dataLen);
- }
- return (m_retVal = MTRV_OK);
- }
- else if (buffer[IND_MID] == MID_ERROR)
- {
- m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
- }
- else
- {
- return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
- }
- }
- return m_retVal;
- }
-
-////////////////////////////////////////////////////////////////////
-// setSetting (integer & no param variant)
-//
-// Sets a integer setting of the device. This setting
-// can be an unsigned 1,2 or 4 bytes setting. Only valid
-// for serial port connections.
-//
-// Input
-// mid : Message ID of message to send
-// bid : Bus ID of message to send (def 0xFF)
-// value : Contains the integer value to be used
-// valuelen : Length in bytes of the value
-//
-// Output
-// = MTRV_OK if an Ack message is received
-// = MTRV_RECVERRORMSG if an error message is received
-// = MTRV_TIMEOUT if timeout occurred
-//
-//
- short CXsensMTiModule::setSetting(const unsigned char mid, const unsigned long value, const unsigned short valuelen, const unsigned char bid)
- {
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
-
- if (m_fileOpen)
- {
- return (m_retVal = MTRV_INVALIDFORFILEINPUT);
- }
- if (!m_portOpen)
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
- }
- msgLen = LEN_MSGHEADER;
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
- buffer[IND_LEN] = (unsigned char) valuelen;
- swapEndian((unsigned char *) &value,&buffer[msgLen],valuelen);
- calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
-
- // Send message
- writeData(buffer,LEN_MSGHEADERCS + buffer[IND_LEN]);
-
- // Read next received message
- if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
- {
- // Message received
- if (buffer[IND_MID] == (mid + 1))
- {
- return (m_retVal = MTRV_OK); // Acknowledge received
- }
- else if (buffer[IND_MID] == MID_ERROR)
- {
- m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
- }
- }
- return m_retVal;
- }
-
-////////////////////////////////////////////////////////////////////
-// setSetting (integer & param variant)
-//
-// Sets a integer setting of the device. This setting
-// can be an unsigned 1,2 or 4 bytes setting. Only valid
-// for serial port connections.
-//
-// Input
-// mid : Message ID of message to send
-// param : For messages that need a parameter (optional)
-// bid : Bus ID of message to send (def 0xFF)
-// value : Contains the integer value to be used
-// valuelen : Length in bytes of the value
-//
-// Output
-// = MTRV_OK if an Ack message is received
-// = MTRV_RECVERRORMSG if an error message is received
-// = MTRV_TIMEOUT if timeout occurred
-//
-//
- short CXsensMTiModule::setSetting(const unsigned char mid, const unsigned char param, const unsigned long value, const unsigned short valuelen, const unsigned char bid)
- {
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
-
- if (m_fileOpen)
- {
- return (m_retVal = MTRV_INVALIDFORFILEINPUT);
- }
- if (!m_portOpen)
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
- }
- msgLen = LEN_MSGHEADER;
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
- if (param != 0xFF)
- {
- msgLen++;
- buffer[IND_LEN] = valuelen + 1;
- buffer[IND_DATA0] = param;
- }
- else
- {
- buffer[IND_LEN] = (unsigned char) valuelen;
- }
- swapEndian((unsigned char *) &value,&buffer[msgLen],valuelen);
- calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
-
- // Send message
- writeData(buffer,LEN_MSGHEADERCS + buffer[IND_LEN]);
-
- // Read next received message
- if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
- {
- // Message received
- if (buffer[IND_MID] == (mid + 1))
- {
- return (m_retVal = MTRV_OK); // Acknowledge received
- }
- else if (buffer[IND_MID] == MID_ERROR)
- {
- m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
- }
- }
- return m_retVal;
- }
-
-////////////////////////////////////////////////////////////////////
-// setSetting (float & no param variant)
-//
-// Sets a float setting of the device. Only valid
-// for serial port connections.
-//
-// Input
-// mid : Message ID of message to send
-// bid : Bus ID of message to send (def 0xFF)
-// value : Contains the float value to be used
-//
-// Output
-// = MTRV_OK if an Ack message is received
-// = MTRV_RECVERRORMSG if an error message is received
-// = MTRV_TIMEOUT if timeout occurred
-//
- short CXsensMTiModule::setSetting(const unsigned char mid, const float value, const unsigned char bid)
- {
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
-
- if (m_fileOpen)
- {
- return (m_retVal = MTRV_INVALIDFORFILEINPUT);
- }
- if (!m_portOpen)
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
- }
- msgLen = LEN_MSGHEADER;
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
- buffer[IND_LEN] = LEN_FLOAT;
- swapEndian((unsigned char *) &value,&buffer[msgLen],LEN_FLOAT);
- calcChecksum(buffer,LEN_MSGHEADER + LEN_FLOAT);
-
- // Send message
- writeData(buffer,LEN_MSGHEADERCS + LEN_FLOAT);
-
- // Read next received message
- if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
- {
- // Message received
- if (buffer[IND_MID] == (mid + 1))
- {
- return (m_retVal = MTRV_OK); // Acknowledge received
- }
- else if (buffer[IND_MID] == MID_ERROR)
- {
- m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
- }
- }
- return m_retVal;
- }
-
-////////////////////////////////////////////////////////////////////
-// setSetting (float & param variant)
-//
-// Sets a float setting of the device. Only valid
-// for serial port connections.
-//
-// Input
-// mid : Message ID of message to send
-// param : For messages that need a parameter (optional)
-// bid : Bus ID of message to send (def 0xFF)
-// value : Contains the float value to be used
-//
-// Output
-// = MTRV_OK if an Ack message is received
-// = MTRV_RECVERRORMSG if an error message is received
-// = MTRV_TIMEOUT if timeout occurred
-//
-//
- short CXsensMTiModule::setSetting(const unsigned char mid, const unsigned char param, const float value, const unsigned char bid)
- {
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
-
- if (m_fileOpen)
- {
- return (m_retVal = MTRV_INVALIDFORFILEINPUT);
- }
- if (!m_portOpen)
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
- }
- msgLen = LEN_MSGHEADER;
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
- if (param != 0xFF)
- {
- msgLen++;
- buffer[IND_LEN] = LEN_FLOAT + 1;
- buffer[IND_DATA0] = param;
- }
- else
- {
- buffer[IND_LEN] = LEN_FLOAT;
- }
- swapEndian((unsigned char *) &value,&buffer[msgLen],LEN_FLOAT);
- calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
-
- // Send message
- writeData(buffer,LEN_MSGHEADERCS + buffer[IND_LEN]);
-
- // Read next received message
- if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
- {
- // Message received
- if (buffer[IND_MID] == (mid + 1))
- {
- return (m_retVal = MTRV_OK); // Acknowledge received
- }
- else if (buffer[IND_MID] == MID_ERROR)
- {
- m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
- }
- }
- return m_retVal;
- }
-
-////////////////////////////////////////////////////////////////////
-// setSetting (float & param & store variant)
-//
-// Sets a float setting of the device and with the Store field.
-// Only valid for serial port connections
-//
-// Input
-// mid : Message ID of message to send
-// param : For messages that need a parameter (optional)
-// value : Contains the float value to be used
-// store ; Store in non-volatile memory (1) or not (0)
-// bid : Bus ID of message to send (def 0xFF)
-//
-// Output
-// = MTRV_OK if an Ack message is received
-// = MTRV_RECVERRORMSG if an error message is received
-// = MTRV_TIMEOUT if timeout occurred
-//
-//
- short CXsensMTiModule::setSetting(const unsigned char mid, const unsigned char param, const float value, const bool store, const unsigned char bid)
- {
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
-
- if (m_fileOpen)
- {
- return (m_retVal = MTRV_INVALIDFORFILEINPUT);
- }
- if (!m_portOpen)
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
- }
- msgLen = LEN_MSGHEADER;
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
- if (param != 0xFF)
- {
- msgLen++;
- buffer[IND_LEN] = LEN_FLOAT + 2;
- buffer[IND_DATA0] = param;
- }
- else
- {
- buffer[IND_LEN] = LEN_FLOAT + 1;
- }
- swapEndian((unsigned char *) &value,&buffer[msgLen],LEN_FLOAT);
- buffer[msgLen + LEN_FLOAT ] = store;
- calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
-
- // Send message
- writeData(buffer,LEN_MSGHEADERCS + buffer[IND_LEN]);
-
- // Read next received message
- if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
- {
- // Message received
- if (buffer[IND_MID] == (mid + 1))
- {
- return (m_retVal = MTRV_OK); // Acknowledge received
- }
- else if (buffer[IND_MID] == MID_ERROR)
- {
- m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
- }
- }
- return m_retVal;
- }
-
-////////////////////////////////////////////////////////////////////
-// getDeviceMode
-//
-// Requests the current output mode/setting of input (file or serialport)
-// the Outputmode, Outputsettings, DataLength & number of devices
-// are stored in member variables of the MTComm class. These values
-// are needed for the GetValue functions.
-// The function optionally returns the number of devices
-//
-// File: expects the Configuration message at the start of the file
-// which holds the OutputMode & OutputSettings. File position
-// is after the first message
-//
-// Input
-// Output
-// numDevices : [optional] number of devices connected to port or
-// found in configuration file
-//
-// returns MTRV_OK if the mode & settings are read
-//
- short CXsensMTiModule::getDeviceMode(unsigned short *numDevices)
- {
- unsigned char mid = 0 , data[MAXMSGLEN ];
- short datalen;
-
- if (numDevices != NULL)
- {
- *numDevices = 0;
- }
-
- // In case serial port is used (live device / XM or MT)
- if (m_portOpen)
- {
- if (reqSetting(MID_INITBUS,data,datalen) != MTRV_OK)
- {
- return m_retVal;
- }
-
- // Retrieve outputmode + outputsettings
- for(int i = 0 ; i < datalen / LEN_DEVICEID ; i++)
- {
- if (reqSetting(MID_REQOUTPUTMODE,m_storedOutputMode[BID_MT + i],BID_MT + i) != MTRV_OK)
- {
- return m_retVal;
- }
-
- if (reqSetting(MID_REQOUTPUTSETTINGS,m_storedOutputSettings[BID_MT + i],BID_MT + i) != MTRV_OK)
- {
- return m_retVal;
- }
-
- if (reqSetting(MID_REQDATALENGTH,m_storedDataLength[BID_MT + i],BID_MT + i) != MTRV_OK)
- {
- return m_retVal;
- }
- }
-
- if (numDevices != NULL)
- {
- *numDevices = datalen / LEN_DEVICEID;
- }
-
- unsigned char masterDID[4];
- short DIDlen;
-
- if (reqSetting(MID_REQDID,masterDID,DIDlen) != MTRV_OK)
- {
- return m_retVal;
- }
-
- if (memcmp(masterDID,data,LEN_DEVICEID) != 0)
- {
- // Using an XbusMaster
- m_storedOutputMode[0] = OUTPUTMODE_XM;
- m_storedOutputSettings[0] = OUTPUTSETTINGS_XM;
- m_storedDataLength[0] = LEN_SAMPLECNT;
- }
- else
- {
- m_storedOutputMode[0] = m_storedOutputMode[BID_MT ];
- m_storedOutputSettings[0] = m_storedOutputSettings[BID_MT ];
- m_storedDataLength[0] = m_storedDataLength[BID_MT ];
- }
- return (m_retVal = MTRV_OK);
- }
- else if (m_fileOpen)
- {
- // Configuration message should be the first message in the file
- setFilePos(0);
- if (readMessage(mid,data,datalen) == MTRV_OK)
- {
- if (mid == MID_CONFIGURATION)
- {
- unsigned short _numDevices = 0;
- swapEndian(data + CONF_NUMDEVICES,(unsigned char *) &_numDevices,CONF_NUMDEVICESLEN);
- for(unsigned int i = 0 ; i < _numDevices ; i++)
- {
- m_storedOutputMode[BID_MT + i] = 0;
- swapEndian(data + CONF_OUTPUTMODE + i * CONF_BLOCKLEN,(unsigned char *) (m_storedOutputMode + BID_MT + i),
- CONF_OUTPUTMODELEN);
- m_storedOutputSettings[BID_MT + i] = 0;
- swapEndian(data + CONF_OUTPUTSETTINGS + i * CONF_BLOCKLEN,(unsigned char *) (m_storedOutputSettings + BID_MT + i),
- CONF_OUTPUTSETTINGSLEN);
- m_storedDataLength[BID_MT + i] = 0;
- swapEndian(data + CONF_DATALENGTH + i * CONF_BLOCKLEN,(unsigned char *) (m_storedDataLength + BID_MT + i),
- CONF_DATALENGTHLEN);
- }
- if (numDevices != NULL)
- {
- *numDevices = _numDevices;
- }
- if (memcmp(data + CONF_MASTERDID,data + CONF_DID,LEN_DEVICEID) != 0)
- {
- // Using an XbusMaster
- m_storedOutputMode[0] = OUTPUTMODE_XM;
- m_storedOutputSettings[0] = OUTPUTSETTINGS_XM;
- m_storedDataLength[0] = LEN_SAMPLECNT;
- }
- else
- {
- m_storedOutputMode[0] = m_storedOutputMode[BID_MT ];
- m_storedOutputSettings[0] = m_storedOutputSettings[BID_MT ];
- m_storedDataLength[0] = m_storedDataLength[BID_MT ];
- }
- return (m_retVal = MTRV_OK);
- }
- }
- return (m_retVal = MTRV_NOTSUCCESSFUL);
- }
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
- }
-
-////////////////////////////////////////////////////////////////////
-// setDeviceMode
-//
-// Sets the current output mode/setting of input (not for file-based
-// inputs)
-//
-// Input
-// OutputMode : OutputMode to be set in device & stored in MTComm
-// class member variable
-// OutputSettings : OutputSettings to be set in device & stored in
-// MTComm class member variable
-// Output
-//
-// returns MTRV_OK if the mode & settings are read
-//
- short CXsensMTiModule::setDeviceMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid)
- {
- // In case serial port is used (live XM / MT)
- if (m_portOpen)
- {
- // Set OutputMode
- if (setSetting(MID_SETOUTPUTMODE,OutputMode,LEN_OUTPUTMODE,bid) != MTRV_OK)
- {
- return m_retVal;
- }
- if (bid == BID_MASTER || (bid == BID_MT && m_storedOutputMode[0] != OUTPUTMODE_XM))
- {
- m_storedOutputMode[0] = m_storedOutputMode[BID_MT ] = OutputMode;
- }
- else
- {
- m_storedOutputMode[bid] = OutputMode;
- }
- // Set OutputSettings
- if (setSetting(MID_SETOUTPUTSETTINGS,OutputSettings,LEN_OUTPUTSETTINGS,bid) != MTRV_OK)
- {
- return m_retVal;
- }
- if (bid == BID_MASTER || (bid == BID_MT && m_storedOutputMode[0] != OUTPUTMODE_XM))
- {
- m_storedOutputSettings[0] = m_storedOutputSettings[BID_MT ] = OutputSettings;
- }
- else
- {
- m_storedOutputSettings[bid] = OutputSettings;
- }
- // Get DataLength from device
- if (OutputMode != OUTPUTMODE_XM)
- {
- unsigned long value;
- if (reqSetting(MID_REQDATALENGTH,value,bid) == MTRV_OK)
- {
- if ((bid == BID_MASTER ) || ((bid == BID_MT ) && (m_storedOutputMode[0] != OUTPUTMODE_XM)))
- {
- m_storedDataLength[0] = m_storedDataLength[BID_MT ] = value;
- }
- else
- {
- m_storedDataLength[bid] = value;
- }
- }
- }
- else
- {
- m_storedDataLength[0] = LEN_SAMPLECNT;
- }
- return (m_retVal = MTRV_OK);
- }
- return (m_retVal = MTRV_INVALIDFORFILEINPUT);
- }
-
-////////////////////////////////////////////////////////////////////
-// getMode
-//
-// Gets the output mode/setting used in MTComm class and the corresponding
-// datalength. These variables are set by the functions GetDeviceMode,
-// SetDeviceMode or SetMode
-//
-// Input
-// Output
-// OutputMode : OutputMode stored in MTComm class member variable
-// OutputSettings : OutputSettings stored in MTComm class member variable
-//
-// returns always MTRV_OK
-//
- short CXsensMTiModule::getMode(unsigned long &OutputMode, unsigned long &OutputSettings, unsigned short &dataLength, const unsigned char bid)
- {
- unsigned char nbid = (bid == BID_MASTER ) ? 0 : bid;
- OutputMode = m_storedOutputMode[nbid];
- OutputSettings = m_storedOutputSettings[nbid];
- dataLength = (unsigned short) m_storedDataLength[nbid];
- return (m_retVal = MTRV_OK);
- }
-
-////////////////////////////////////////////////////////////////////
-// setMode
-//
-// Sets the output mode/setting used in MTComm class. Use the function
-// GetDeviceMode to retrieve the current values of file/device.
-// This function will also calculate the data length field
-//
-// Input
-// OutputMode : OutputMode to be stored in MTComm class member variable
-// OutputSettings : OutputSettings to be stored in MTComm class member variable
-// Output
-//
-// returns always MTRV_OK
-//
- short CXsensMTiModule::setMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid)
- {
- unsigned char nbid = bid;
-
- if (nbid == BID_MASTER)
- {
- nbid = 0;
- }
- m_storedOutputMode[nbid] = OutputMode;
- m_storedOutputSettings[nbid] = OutputSettings;
- if (OutputMode == INVALIDSETTINGVALUE || OutputSettings == INVALIDSETTINGVALUE)
- {
- m_storedDataLength[nbid] = 0;
- }
- else
- {
- unsigned short dataLength = 0;
- if (OutputMode & OUTPUTMODE_MT9)
- {
- dataLength = ((OutputSettings & OUTPUTSETTINGS_TIMESTAMP_MASK) == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT) ? LEN_SAMPLECNT : 0 + LEN_RAWDATA;
- }
- else if (OutputMode == OUTPUTMODE_XM)
- {
- // XbusMaster concatenates sample counter
- dataLength = LEN_SAMPLECNT;
- }
- else
- {
- if (OutputMode & OUTPUTMODE_RAW)
- {
- dataLength = LEN_RAWDATA;
- }
- else
- {
- if (OutputMode & OUTPUTMODE_CALIB)
- {
- dataLength = LEN_CALIBDATA;
- }
- if (OutputMode & OUTPUTMODE_ORIENT)
- {
- switch(OutputSettings & OUTPUTSETTINGS_ORIENTMODE_MASK)
- {
- case OUTPUTSETTINGS_ORIENTMODE_QUATERNION:
- dataLength += LEN_ORIENT_QUATDATA;
- break;
- case OUTPUTSETTINGS_ORIENTMODE_EULER:
- dataLength += LEN_ORIENT_EULERDATA;
- break;
- case OUTPUTSETTINGS_ORIENTMODE_MATRIX:
- dataLength += LEN_ORIENT_MATRIXDATA;
- break;
- default:
- break;
- }
- }
- }
- switch(OutputSettings & OUTPUTSETTINGS_TIMESTAMP_MASK)
- {
- case OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT:
- dataLength += LEN_SAMPLECNT;
- break;
- default:
- break;
- }
- }
- m_storedDataLength[nbid] = dataLength;
- }
- // If not XbusMaster store also in BID_MT
- if (bid == BID_MASTER && OutputMode != OUTPUTMODE_XM)
- {
- m_storedOutputMode[BID_MT ] = m_storedOutputMode[0];
- m_storedOutputSettings[BID_MT ] = m_storedOutputSettings[0];
- m_storedDataLength[BID_MT ] = m_storedDataLength[0];
- }
- return (m_retVal = MTRV_OK);
- }
-
-////////////////////////////////////////////////////////////////////
-// getValue (unsigned short variant)
-//
-// Retrieves a unsigned short value from the data input parameter
-// This function is valid for the following value specifiers:
-// VALUE_RAW_TEMP
-// VALUE_SAMPLECNT
-//
-// Use getDeviceMode or setMode to initialize the Outputmode
-// and Outputsettings member variables used to retrieve the correct
-// value
-//
-// Input
-// valueSpec : Specifier of the value to be retrieved
-// data[] : Data field of a MTData / BusData message
-// bid : bus identifier of the device of which the
-// value should be returned (default = BID_MT)
-// Output
-// value : reference to unsigned short in which the retrieved
-// value will be returned
-//
-// Return value
-// MTRV_OK : value is successfully retrieved
-// != MTRV_OK : not successful
-//
- short CXsensMTiModule::getValue(const unsigned long valueSpec, unsigned short &value, const unsigned char data[], const unsigned char bid)
- {
- short offset = 0;
- unsigned char nbid = bid;
-
- if (nbid == BID_MASTER)
- {
- nbid = 0;
- }
-
- // Check for invalid mode/settings
- if (m_storedOutputMode[nbid] == INVALIDSETTINGVALUE || m_storedOutputSettings[nbid] == INVALIDSETTINGVALUE)
- {
- return (m_retVal = MTRV_NOVALIDMODESPECIFIED);
- }
-
- // Calculate offset for XM input
- if (m_storedOutputMode[0] == OUTPUTMODE_XM)
- {
- int i = 0;
- while(i < nbid)
- {
- offset += (short) m_storedDataLength[i++];
- }
- }
-
- // Check if data is unsigned short & available in data
- m_retVal = MTRV_INVALIDVALUESPEC;
- if (valueSpec == VALUE_RAW_TEMP)
- {
- if (m_storedOutputMode[nbid] & (OUTPUTMODE_RAW | OUTPUTMODE_MT9))
- {
- offset += (m_storedOutputMode[nbid] == OUTPUTMODE_MT9 && m_storedOutputSettings[nbid] == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT) ? LEN_SAMPLECNT : 0;
- swapEndian(data + offset + valueSpec * LEN_UNSIGSHORT * 3,(unsigned char *) &value,LEN_RAW_TEMP);
- m_retVal = MTRV_OK;
- }
- }
- else if (valueSpec == VALUE_SAMPLECNT)
- {
- if ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_TIMESTAMP_MASK) == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT)
- {
- if (!(m_storedOutputMode[nbid] == OUTPUTMODE_MT9))
- {
- offset += (short) m_storedDataLength[nbid] - LEN_SAMPLECNT;
- }
- swapEndian(data + offset,(unsigned char *) &value,LEN_SAMPLECNT);
- m_retVal = MTRV_OK;
- }
- }
- return m_retVal;
- }
-
-////////////////////////////////////////////////////////////////////
-// getValue (array of unsigned short variant)
-//
-// Retrieves an array of unsigned short values from the data input
-// parameter. This function is valid for the following value specifiers:
-// VALUE_RAW_ACC
-// VALUE_RAW_GYR
-// VALUE_RAW_MAG
-//
-// Use getDeviceMode or setMode to initialize the Outputmode
-// and Outputsettings member variables used to retrieve the correct
-// value
-//
-// Input
-// valueSpec : Specifier of the value to be retrieved
-// data[] : Data field of a MTData / BusData message
-// bid : bus identifier of the device of which the
-// value should be returned (default = BID_MT)
-// Output
-// value[] : pointer to array of unsigned shorts in which the
-// retrieved values will be returned
-//
-// Return value
-// MTRV_OK : value is successfully retrieved
-// != MTRV_OK : not successful
-//
- short CXsensMTiModule::getValue(const unsigned long valueSpec, unsigned short value[], const unsigned char data[], const unsigned char bid)
- {
- short offset = 0;
- unsigned char nbid = bid;
-
- if (nbid == BID_MASTER)
- {
- nbid = 0;
- }
- // Check for invalid mode/settings
- if (m_storedOutputMode[nbid] == INVALIDSETTINGVALUE || m_storedOutputSettings[nbid] == INVALIDSETTINGVALUE)
- {
- return (m_retVal = MTRV_NOVALIDMODESPECIFIED);
- }
-
- // Calculate offset for XM input
- if (m_storedOutputMode[0] == OUTPUTMODE_XM)
- {
- int i = 0;
- while(i < nbid)
- {
- offset += (short) m_storedDataLength[i++];
- }
- }
-
- // Check if data is unsigned short, available in data & retrieve data
- m_retVal = MTRV_INVALIDVALUESPEC;
- //if (valueSpec >= VALUE_RAW_ACC && valueSpec <= VALUE_RAW_MAG)
- if (valueSpec <= VALUE_RAW_MAG)
- {
- if (m_storedOutputMode[nbid] & (OUTPUTMODE_RAW | OUTPUTMODE_MT9))
- {
- offset += (short) (valueSpec * LEN_UNSIGSHORT * 3);
- offset += (m_storedOutputMode[nbid] == OUTPUTMODE_MT9 && m_storedOutputSettings[nbid] == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT) ? LEN_SAMPLECNT : 0;
- for(int i = 0 ; i < 3 ; i++)
- {
- swapEndian(data + offset + i * LEN_UNSIGSHORT,(unsigned char *) value + i * LEN_UNSIGSHORT,LEN_UNSIGSHORT);
- }
- m_retVal = MTRV_OK;
- }
- }
- return m_retVal;
- }
-
-////////////////////////////////////////////////////////////////////
-// getValue (array of floats variant)
-//
-// Retrieves an array of float values from the data input parameter.
-// This function is valid for the following value specifiers:
-// VALUE_TEMP
-// VALUE_CALIB_ACC
-// VALUE_CALIB_GYR
-// VALUE_CALIB_MAG
-// VALUE_ORIENT_QUAT
-// VALUE_ORIENT_EULER
-// VALUE_ORIENT_MATRIX
-//
-// Use getDeviceMode or setMode to initialize the Outputmode
-// and Outputsettings member variables used to retrieve the correct
-// value
-//
-// Input
-// valueSpec : Specifier of the value to be retrieved
-// data[] : Data field of a MTData / BusData message
-// bid : bus identifier of the device of which the
-// value should be returned (default = BID_MT)
-// Output
-// value[] : pointer to array of floats in which the
-// retrieved values will be returned
-//
-// Return value
-// MTRV_OK : value is successfully retrieved
-// != MTRV_OK : not successful
-//
- short CXsensMTiModule::getValue(const unsigned long valueSpec, float value[], const unsigned char data[], const unsigned char bid)
- {
- short offset = 0;
- int nElements = 0;
- unsigned char nbid = bid;
-
- if (nbid == BID_MASTER)
- {
- nbid = 0;
- }
-
- // Check for invalid mode/settings
- if (m_storedOutputMode[nbid] == INVALIDSETTINGVALUE || m_storedOutputSettings[nbid] == INVALIDSETTINGVALUE)
- {
- return (m_retVal = MTRV_NOVALIDMODESPECIFIED);
- }
-
- // Calculate offset for XM input
- if (m_storedOutputMode[0] == OUTPUTMODE_XM)
- {
- int i = 0;
- while(i < nbid)
- {
- offset += (short) m_storedDataLength[i++];
- }
- }
-
- // Check if data is float & available in data
- m_retVal = MTRV_INVALIDVALUESPEC;
- if (valueSpec == VALUE_TEMP)
- {
- if (m_storedOutputMode[nbid] & OUTPUTMODE_TEMP)
- {
- nElements = LEN_TEMPDATA / LEN_FLOAT;
- m_retVal = MTRV_OK;
- }
- }
- else if (valueSpec == VALUE_CALIB_ACC)
- {
- offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0) ? LEN_TEMPDATA : 0;
- if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB) && (m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0)
- {
- nElements = LEN_CALIB_ACCDATA / LEN_FLOAT;
- m_retVal = MTRV_OK;
- }
- }
- else if (valueSpec == VALUE_CALIB_GYR)
- {
- offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0) ? LEN_TEMPDATA : 0;
- if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB) && (m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_GYR_MASK) == 0)
- {
- offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0) ? LEN_CALIB_ACCX * 3 : 0;
- nElements = LEN_CALIB_GYRDATA / LEN_FLOAT;
- m_retVal = MTRV_OK;
- }
- }
- else if (valueSpec == VALUE_CALIB_MAG)
- {
- offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0) ? LEN_TEMPDATA : 0;
- if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB) && (m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_MAG_MASK) == 0)
- {
- offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0) ? LEN_CALIB_ACCX * 3 : 0;
- offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_GYR_MASK) == 0) ? LEN_CALIB_GYRX * 3 : 0;
- nElements = LEN_CALIB_MAGDATA / LEN_FLOAT;
- m_retVal = MTRV_OK;
- }
- }
- else if (valueSpec >= VALUE_ORIENT_QUAT && valueSpec <= VALUE_ORIENT_MATRIX)
- {
- offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0) ? LEN_TEMPDATA : 0;
- if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB))
- {
- offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0) ? LEN_CALIB_ACCX * 3 : 0;
- offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_GYR_MASK) == 0) ? LEN_CALIB_GYRX * 3 : 0;
- offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_MAG_MASK) == 0) ? LEN_CALIB_MAGX * 3 : 0;
- }
- if (m_storedOutputMode[nbid] & OUTPUTMODE_ORIENT)
- {
- unsigned long orientmode = m_storedOutputSettings[nbid] & OUTPUTSETTINGS_ORIENTMODE_MASK;
- switch(valueSpec)
- {
- case VALUE_ORIENT_QUAT:
- if (orientmode == OUTPUTSETTINGS_ORIENTMODE_QUATERNION)
- {
- nElements = LEN_ORIENT_QUATDATA / LEN_FLOAT;
- m_retVal = MTRV_OK;
- }
- break;
- case VALUE_ORIENT_EULER:
- if (orientmode == OUTPUTSETTINGS_ORIENTMODE_EULER)
- {
- nElements = LEN_ORIENT_EULERDATA / LEN_FLOAT;
- m_retVal = MTRV_OK;
- }
- break;
- case VALUE_ORIENT_MATRIX:
- if (orientmode == OUTPUTSETTINGS_ORIENTMODE_MATRIX)
- {
- nElements = LEN_ORIENT_MATRIXDATA / LEN_FLOAT;
- m_retVal = MTRV_OK;
- }
- break;
- default:
- break;
- }
- }
- }
- if (m_retVal == MTRV_OK)
- {
- if ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_DATAFORMAT_F1220) == 0)
- {
- for(int i = 0 ; i < nElements ; i++)
- {
- swapEndian(data + offset + i * LEN_FLOAT,(unsigned char *) value + i * LEN_FLOAT,LEN_FLOAT);
- }
- }
- else
- {
- int temp;
- for(int i = 0 ; i < nElements ; i++)
- {
- swapEndian(data + offset + i * LEN_FLOAT,(unsigned char*) &temp,4);
- value[i] = (float) temp / 1048576;
- }
- }
- }
- return m_retVal;
- }
+ }
+
+ m_retVal = MTRV_OK;
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // setFilePos
+ //
+ // Sets the current position of the file pointer for file input
+ //
+ // Input
+ // relPos : 32-bit value specifying the relative move in bytes
+ // origin is specified in moveMethod
+ // moveMethod : FILEPOS_BEGIN, FILEPOS_CURRENT or FILEPOS_END
+ // Output
+ //
+ // returns MTRV_OK if file pointer is successfully set
+ //
+ short CXsensMTiModule::setFilePos(long relPos, unsigned long moveMethod)
+ {
+#ifdef WIN32
-//////////////////////////////////////////////////////////////////////
-// getLastDeviceError
-//
-// Returns the last reported device error of the latest received Error
-// message
-//
-// Output
-// Error code
- short CXsensMTiModule::getLastDeviceError()
- {
- return m_deviceError;
- }
+ if (m_fileOpen)
+ {
+ if (SetFilePointer(m_handle, relPos, NULL, moveMethod) != INVALID_SET_FILE_POINTER)
+ {
+ return (m_retVal = MTRV_OK);
+ }
+ }
-//////////////////////////////////////////////////////////////////////
-// getLastRetVal
-//
-// Returns the returned value of the last called function
-//
-// Output
-// Return value
- short CXsensMTiModule::getLastRetVal()
- {
- return m_retVal;
- }
+#else
-//////////////////////////////////////////////////////////////////////
-// setTimeOut
-//
-// Sets the time out value in milliseconds used by the functions
-// Use 0 for infinite timeout
-//
-// Output
-// MTRV_OK is set, MTRV_INVALIDTIMEOUT if time value < 0
- short CXsensMTiModule::setTimeOut(short timeOutMs)
- {
- if (timeOutMs >= 0)
- {
- m_timeOut = timeOutMs;
- return (m_retVal = MTRV_OK);
- }
- else
- return (m_retVal = MTRV_INVALIDTIMEOUT);
- }
+ if (m_fileOpen)
+ {
+ if (lseek(m_handle, relPos, moveMethod) != -1)
+ {
+ return (m_retVal = MTRV_OK);
+ }
+ }
-//////////////////////////////////////////////////////////////////////
-// swapEndian
-//
-// Convert 2 or 4 bytes data from little to big endian or back
-//
-// Input
-// input : Pointer to data to be converted
-// output : Pointer where converted data is stored
-// length : Length of setting (0,2 & 4)
-//
-// Remarks:
-// Allocate enough bytes for output buffer
-
- void CXsensMTiModule::swapEndian(const unsigned char input[], unsigned char output[], const int length)
- {
- switch(length)
- {
- case 4:
- output[0] = input[3];
- output[1] = input[2];
- output[2] = input[1];
- output[3] = input[0];
- break;
- case 2:
- output[0] = input[1];
- output[1] = input[0];
- break;
- case 1:
- output[0] = input[0];
- break;
- default:
- for(int i = 0 , j = length - 1 ; i < length ; i++ , j--)
- output[j] = input[i];
- break;
- }
- }
+#endif
+ return (m_retVal = MTRV_NOTSUCCESSFUL);
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // getFileSize
+ //
+ // Retrieves the file size of the opened file
+ //
+ // Input
+ // Output
+ // fileSize : Number of bytes of the current file
+ //
+ // returns MTRV_OK if successful
+ //
+ short CXsensMTiModule::getFileSize(unsigned long& fileSize)
+ {
+#ifdef WIN32
-//////////////////////////////////////////////////////////////////////
-// calcChecksum
-//
-// Calculate and append checksum to msgBuffer
-//
- void CXsensMTiModule::calcChecksum(unsigned char *msgBuffer, const int msgBufferLength)
- {
- unsigned char checkSum = 0;
- int i;
+ if (m_fileOpen)
+ {
+ if ((fileSize = GetFileSize(m_handle, NULL)) != INVALID_FILE_SIZE)
+ {
+ return (m_retVal = MTRV_OK);
+ }
+ }
- for(i = 1; i < msgBufferLength ; i++)
- checkSum += msgBuffer[i];
+#else
- msgBuffer[msgBufferLength] = -checkSum; // Store chksum
- }
+ if (m_fileOpen)
+ {
+ struct stat buf;
-//////////////////////////////////////////////////////////////////////
-// checkChecksum
-//
-// Checks if message checksum is valid
-//
-// Output
-// returns true checksum is OK
- bool CXsensMTiModule::checkChecksum(const unsigned char *msgBuffer, const int msgBufferLength)
- {
- unsigned char checkSum = 0;
- int i;
-
- for(i = 1; i < msgBufferLength ; i++)
- checkSum += msgBuffer[i];
-
- if (checkSum == 0)
- {
- return true;
- }
- else
- {
- return false;
- }
- }
- }
+ if (fstat(m_handle, &buf) == 0)
+ {
+ fileSize = buf.st_size;
+ return (m_retVal = MTRV_OK);
+ }
+ }
+
+#endif
+ return (m_retVal = MTRV_NOTSUCCESSFUL);
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // close
+ //
+ // Closes handle of file or serial port
+ //
+ short CXsensMTiModule::close()
+ {
+ if (m_portOpen || m_fileOpen)
+ {
+#ifdef WIN32
+
+ if (m_portOpen)
+ {
+ // Because of USB-serial driver bug
+ flush();
+ }
+
+ CloseHandle(m_handle);
+#else
+ ::close(m_handle);
+#endif
+ }
+
+ m_fileOpen = m_portOpen = false;
+ m_timeOut = TO_DEFAULT; // Restore timeout value (file input)
+ m_clkEnd = 0;
+ m_nTempBufferLen = 0;
+ m_deviceError = 0; // No error
+
+ for (int i = 0 ; i < MAXDEVICES + 1 ; i++)
+ {
+ m_storedOutputMode[i] = INVALIDSETTINGVALUE;
+ m_storedOutputSettings[i] = INVALIDSETTINGVALUE;
+ m_storedDataLength[i] = 0;
+ }
+
+ return (m_retVal = MTRV_OK);
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // readMessage
+ //
+ // Reads the next message from serial port buffer or file.
+ // To be read within current time out period
+ //
+ // Input
+ // Output
+ // mid : MessageID of message received
+ // data : array pointer to data bytes (no header)
+ // dataLen : number of data bytes read
+ // bid : BID or address of message read (optional)
+ //
+ // returns MTRV_OK if a message has been read else <>MTRV_OK
+ //
+ // Remarks
+ // allocate enough memory for message buffer
+ // use setTimeOut for different timeout value
+ short CXsensMTiModule::readMessage(unsigned char& mid, unsigned char data[], short& dataLen, unsigned char* bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (!(m_fileOpen || m_portOpen))
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ {
+ // Message read
+ mid = buffer[IND_MID];
+
+ if (bid != NULL)
+ {
+ *bid = buffer[IND_BID];
+ }
+
+ if (buffer[IND_LEN] != EXTLENCODE)
+ {
+ dataLen = buffer[IND_LEN];
+ memcpy(data, &buffer[IND_DATA0], dataLen);
+ }
+ else
+ {
+ dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
+ memcpy(data, &buffer[IND_DATAEXT0], dataLen);
+ }
+ }
+
+ return m_retVal;
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // readDataMessage
+ //
+ // Read a MTData or XMData message from serial port (using TimeOut val)
+ //
+ // Input
+ // data : pointer to buffer in which the DATA field of MTData/XMData is stored
+ // dataLen : number of bytes in buffer (= number bytes in DATA field)
+ // Output
+ // returns MTRV_OK if MTData / XMData message has been read else <>MTRV_OK
+ //
+ // Remarks
+ // allocate enough memory for data buffer
+ // use setTimeOut for different timeout value
+ short CXsensMTiModule::readDataMessage(unsigned char data[], short& dataLen)
+ {
+ if (!(m_portOpen || m_fileOpen))
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ unsigned char buffer[MAXMSGLEN ];
+ short buflen;
+
+ if (readMessageRaw(buffer, &buflen) == MTRV_OK)
+ {
+ if (buffer[IND_MID] == MID_MTDATA)
+ {
+ // MID_XMDATA is same
+ if (buffer[IND_LEN] != EXTLENCODE)
+ {
+ dataLen = buffer[IND_LEN];
+ memcpy(data, &buffer[IND_DATA0], dataLen);
+ }
+ else
+ {
+ dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
+ memcpy(data, &buffer[IND_DATAEXT0], dataLen);
+ }
+
+ return (m_retVal = MTRV_OK);
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ }
+
+ return m_retVal;
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // readMessageRaw
+ //
+ // Read a message from serial port for a certain period
+ //
+ // Input
+ // msgBuffer : pointer to buffer in which next msg will be stored
+ // msgBufferLength: integer to number of bytes written in buffer (header + data + chksum)
+ // Output
+ // returns MTRV_OK if a message has been read else <>MTRV_OK
+ //
+ // Remarks
+ // allocate enough memory for message buffer
+ // use setTimeOut for different timeout value
+ short CXsensMTiModule::readMessageRaw(unsigned char* msgBuffer, short* msgBufferLength)
+ {
+ int state = 0;
+ int nBytesToRead = 1;
+ int nBytesRead = 0;
+ int nOffset = 0;
+ int nMsgDataLen = 0;
+ int nMsgLen;
+ unsigned char chCheckSum;
+ bool Synced = false;
+
+ if (!(m_portOpen || m_fileOpen))
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ // Copy previous read bytes if available
+ if (m_nTempBufferLen > 0)
+ {
+ memcpy(msgBuffer, m_tempBuffer, m_nTempBufferLen);
+ nBytesRead = m_nTempBufferLen;
+ m_nTempBufferLen = 0;
+ }
+
+ clock_t clkStart = clockms(); // Get current processor time
+ clock_t clkEnd = m_clkEnd; // check if the end timer is already set
+
+ if (clkEnd == 0)
+ {
+ clkEnd = clkStart + m_timeOut;
+ }
+
+ while (true)
+ {
+ do
+ {
+ // First check if we already have some bytes read
+ if (nBytesRead > 0 && nBytesToRead > 0)
+ {
+ if (nBytesToRead > nBytesRead)
+ {
+ nOffset += nBytesRead;
+ nBytesToRead -= nBytesRead;
+ nBytesRead = 0;
+ }
+ else
+ {
+ nOffset += nBytesToRead;
+ nBytesRead -= nBytesToRead;
+ nBytesToRead = 0;
+ }
+ }
+
+ // Check if serial port buffer must be read
+ if (nBytesToRead > 0)
+ {
+ nBytesRead = readData(msgBuffer + nOffset, nBytesToRead);
+
+ if (m_retVal == MTRV_ENDOFFILE)
+ {
+ return (m_retVal = MTRV_ENDOFFILE);
+ }
+
+ nOffset += nBytesRead;
+ nBytesToRead -= nBytesRead;
+ nBytesRead = 0;
+ }
+
+ if (!nBytesToRead)
+ {
+ switch (state)
+ {
+ case 0: // Check preamble
+ if (msgBuffer[IND_PREAMBLE] == PREAMBLE)
+ {
+ state++;
+ nBytesToRead = 3;
+ }
+ else
+ {
+ nOffset = 0;
+ nBytesToRead = 1;
+ }
+
+ break;
+
+ case 1: // Check ADDR, MID, LEN
+ if (msgBuffer[IND_LEN] != 0xFF)
+ {
+ state = 3;
+ nBytesToRead = (nMsgDataLen = msgBuffer[IND_LEN]) + 1; // Read LEN + 1 (chksum)
+ }
+ else
+ {
+ state = 2;
+ nBytesToRead = 2; // Read extended length
+ }
+
+ break;
+
+ case 2:
+ state = 3;
+ nBytesToRead = (nMsgDataLen = msgBuffer[IND_LENEXTH] * 256 + msgBuffer[IND_LENEXTL]) + 1; // Read LENEXT + CS
+
+ if (nMsgDataLen > MAXMSGLEN - LEN_MSGEXTHEADERCS)
+ {
+ // Not synced - check for preamble in the bytes read
+ for (int i = 1 ; i < LEN_MSGEXTHEADER ; i++)
+ if (msgBuffer[i] == PREAMBLE)
+ {
+ // Found a maybe preamble - 'fill buffer'
+ nBytesRead = LEN_MSGEXTHEADER - i;
+ memmove(msgBuffer, msgBuffer + i, nBytesRead);
+ break;
+ }
+
+ Synced = false;
+ nOffset = 0;
+ state = 0;
+ nBytesToRead = 1; // Start looking for preamble
+ }
+
+ break;
+
+ case 3: // Check msg
+ chCheckSum = 0;
+ nMsgLen = nMsgDataLen + 5 + (msgBuffer[IND_LEN] == 0xFF ? 2 : 0);
+
+ for (int i = 1 ; i < nMsgLen ; i++)
+ {
+ chCheckSum += msgBuffer[i];
+ }
+
+ if (chCheckSum == 0)
+ {
+ // Checksum ok?
+ if (nBytesRead > 0)
+ {
+ // Store bytes if read too much
+ memcpy(m_tempBuffer, msgBuffer + nMsgLen, nBytesRead);
+ m_nTempBufferLen = nBytesRead;
+ }
+
+ *msgBufferLength = nMsgLen;
+ Synced = true;
+ return (m_retVal = MTRV_OK);
+ }
+ else
+ {
+ if (!Synced)
+ for (int i = 1 ; i < nMsgLen ; i++) // Not synced - maybe recheck for preamble in this incorrect message
+ if (msgBuffer[i] == PREAMBLE)
+ {
+ // Found a maybe preamble - 'fill buffer'
+ nBytesRead = nMsgLen - i;
+ memmove(msgBuffer, msgBuffer + i, nBytesRead);
+ break;
+ }
+
+ Synced = false;
+ nOffset = 0;
+ state = 0;
+ nBytesToRead = 1; // Start looking for preamble
+ }
+
+ break;
+ }
+ }
+ }
+ while ((clkEnd >= clockms()) || m_timeOut == 0 || nBytesRead != 0);
+
+ // Check if pending message has a valid message
+ if (state > 0)
+ {
+ int i;
+
+ // Search for preamble
+ for (i = 1; i < nOffset ; i++)
+ if (msgBuffer[i] == PREAMBLE)
+ {
+ // Found a maybe preamble - 'fill buffer'
+ nBytesRead = nOffset - i - 1; // no preamble
+ memmove(msgBuffer + 1, msgBuffer + i + 1, nBytesRead);
+ break;
+ }
+
+ if (i < nOffset)
+ {
+ // Found preamble in message - recheck
+ nOffset = 1;
+ state = 1;
+ nBytesToRead = 3; // Start looking for preamble
+ continue;
+ }
+ }
+
+ break;
+ }
+
+ return (m_retVal = MTRV_TIMEOUT);
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // writeMessage (optional: integer value)
+ //
+ // Sends a message and in case of an serial port interface it checks
+ // if the return message (ack, error or timeout). See return value
+ // In case an file is opened the functions returns directly after
+ // 'sending' the message
+ //
+ // Use this function for GotoConfig, Reset, ResetOrientation etc
+ //
+ // Input
+ // mid : MessageID of message to send
+ // dataValue : A integer value that will be included into the data message field
+ // can be a 1,2 or 4 bytes values
+ // dataValueLen : Size of dataValue in bytes
+ // bid : BID or address to use in message to send (default = 0xFF)
+ //
+ // Return value
+ // = MTRV_OK if an Ack message received / or data successfully written to file
+ // = MTRV_RECVERRORMSG if an error message received
+ // = MTRV_TIMEOUT if timeout occurred
+ // = MTRV_NOINPUTINITIALIZED
+ //
+ short CXsensMTiModule::writeMessage(const unsigned char mid, const unsigned long dataValue, const unsigned char dataValueLen, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (!(m_fileOpen || m_portOpen))
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+ buffer[IND_LEN] = dataValueLen;
+
+ if (dataValueLen)
+ {
+ swapEndian((const unsigned char*) &dataValue, &buffer[IND_DATA0], dataValueLen);
+ }
+
+ calcChecksum(buffer, LEN_MSGHEADER + dataValueLen);
+
+ // Send message
+ writeData(buffer, LEN_MSGHEADERCS + dataValueLen);
+
+ // Return if file opened
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_OK);
+ }
+
+ // Keep reading until an Ack or Error message is received (or timeout)
+ clock_t clkStart , clkOld;
+ bool msgRead = false;
+
+ clkStart = clockms(); // Get current processor time
+ clkOld = m_clkEnd;
+
+ if (clkOld == 0)
+ {
+ m_clkEnd = m_timeOut + clkStart;
+ }
+
+ while (m_clkEnd >= clockms() || (m_timeOut == 0))
+ {
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ {
+ // Message received
+ msgRead = true;
+
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ m_clkEnd = clkOld;
+ return (m_retVal = MTRV_OK); // Acknowledge received
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ m_clkEnd = clkOld;
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ }
+ }
+
+ m_clkEnd = clkOld;
+
+ if (msgRead)
+ {
+ return (m_retVal = MTRV_TIMEOUT);
+ }
+ else
+ {
+ return (m_retVal = MTRV_TIMEOUTNODATA);
+ }
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // writeMessage (data array)
+ //
+ // Sends a message and in case of an serial port interface it checks
+ // if the return message (ack, error or timeout). See return value
+ // In case an file is opened the functions returns directly after
+ // 'sending' the message
+ //
+ // Input
+ // mid : MessageID of message to send
+ // data : array pointer to data bytes
+ // dataLen : number of bytes to include in message
+ // bid : BID or address to use in message to send (default = 0xFF)
+ //
+ // Output
+ // = MTRV_OK if an Ack message received
+ // = MTRV_RECVERRORMSG if an error message received
+ // = MTRV_TIMEOUT if timeout occurred
+ // = MTRV_NOINPUTINITIALIZED
+ //
+ short CXsensMTiModule::writeMessage(const unsigned char mid, const unsigned char data[], const unsigned short& dataLen, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+ short headerLength;
+
+ if (!(m_fileOpen || m_portOpen))
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ // Build message to send
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+
+ if (dataLen < EXTLENCODE)
+ {
+ buffer[IND_LEN] = (unsigned char) dataLen;
+ headerLength = LEN_MSGHEADER;
+ }
+ else
+ {
+ buffer[IND_LEN] = EXTLENCODE;
+ buffer[IND_LENEXTH] = (unsigned char)(dataLen >> 8);
+ buffer[IND_LENEXTL] = (unsigned char)(dataLen & 0x00FF);
+ headerLength = LEN_MSGEXTHEADER;
+ }
+
+ memcpy(&buffer[headerLength], data, dataLen);
+ calcChecksum(buffer, headerLength + dataLen);
+
+ // Send message
+ writeData(buffer, headerLength + dataLen + LEN_CHECKSUM);
+
+ // Return if file opened
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_OK);
+ }
+
+ // Keep reading until an Ack or Error message is received (or timeout)
+ bool msgRead = false;
+ clock_t clkStart , clkOld;
+
+ clkStart = clockms(); // Get current processor time
+ clkOld = m_clkEnd;
+
+ if (clkOld == 0)
+ {
+ m_clkEnd = m_timeOut + clkStart;
+ }
+
+ while (m_clkEnd >= clockms() || (m_timeOut == 0))
+ {
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ {
+ // Message received
+ msgRead = true;
+
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ m_clkEnd = clkOld;
+ return (m_retVal = MTRV_OK); // Acknowledge received
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ m_clkEnd = clkOld;
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ }
+ }
+
+ m_clkEnd = clkOld;
+
+ if (msgRead)
+ {
+ return (m_retVal = MTRV_TIMEOUT);
+ }
+ else
+ {
+ return (m_retVal = MTRV_TIMEOUTNODATA);
+ }
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // waitForMessage
+ //
+ // Read messages from serial port or file using the current timeout period
+ // until the received message is equal to a specific message identifier
+ // By default the timeout period by file input is set to infinity (= until
+ // end of file is reached)
+ //
+ // Input/Output
+ // mid : message identifier of message that should be returned
+ // data : pointer to buffer in which the data of the requested msg will be stored
+ // dataLen : integer to number of data bytes
+ // bid : optional, pointer which holds the bid of the returned message
+ // Output
+ // returns MTRV_OK if the message has been read else != MTRV_OK
+ //
+ // Remarks
+ // allocate enough memory for data message buffer
+ // use setTimeOut for different timeout value
+ short CXsensMTiModule::waitForMessage(const unsigned char mid, unsigned char data[], short* dataLen, unsigned char* bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short buflen;
+
+ clock_t clkStart , clkOld;
+
+ if (!(m_fileOpen || m_portOpen))
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ clkStart = clockms(); // Get current processor time
+ clkOld = m_clkEnd;
+
+ if (clkOld == 0)
+ {
+ m_clkEnd = m_timeOut + clkStart;
+ }
+
+ while (m_clkEnd >= clockms() || (m_timeOut == 0))
+ {
+ if (readMessageRaw(buffer, &buflen) == MTRV_OK)
+ {
+ if (buffer[IND_MID] == mid)
+ {
+ if (bid != NULL)
+ {
+ *bid = buffer[IND_BID];
+ }
+
+ if (data != NULL && dataLen != NULL)
+ {
+ if (buffer[IND_LEN] != EXTLENCODE)
+ {
+ *dataLen = buffer[IND_LEN];
+ memcpy(data, &buffer[IND_DATA0], *dataLen);
+ }
+ else
+ {
+ *dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
+ memcpy(data, &buffer[IND_DATAEXT0], *dataLen);
+ }
+ }
+ else if (dataLen != NULL)
+ {
+ dataLen = 0;
+ }
+
+ m_clkEnd = clkOld;
+ return (m_retVal = MTRV_OK);
+ }
+ }
+ else if (m_retVal == MTRV_ENDOFFILE)
+ {
+ m_clkEnd = clkOld;
+ return (m_retVal = MTRV_ENDOFFILE);
+ }
+ }
+
+ m_clkEnd = clkOld;
+ return (m_retVal = MTRV_TIMEOUT);
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // reqSetting (integer & no param variant)
+ //
+ // Request a integer setting from the device. This setting
+ // can be an unsigned 1,2 or 4 bytes setting. Only valid
+ // for serial port connections
+ //
+ // Input
+ // mid : Message ID of message to send
+ // bid : Bus ID of message to send (def 0xFF)
+ //
+ // Output
+ // = MTRV_OK if an Ack message is received
+ // = MTRV_RECVERRORMSG if an error message is received
+ // = MTRV_TIMEOUT if timeout occurred
+ //
+ // value contains the integer value of the data field of the ack message
+ //
+ short CXsensMTiModule::reqSetting(const unsigned char mid, unsigned long& value, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+ buffer[IND_LEN] = 0;
+ calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
+
+ // Send message
+ writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
+
+ // Read next message or else timeout
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ // Acknowledge received
+ value = 0;
+ swapEndian(&buffer[IND_DATA0], (unsigned char*) &value, buffer[IND_LEN]);
+ return (m_retVal = MTRV_OK);
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ else
+ {
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
+ }
+ }
+
+ return m_retVal;
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // reqSetting (integer & param variant)
+ //
+ // Request a integer setting from the device. This setting
+ // can be an unsigned 1,2 or 4 bytes setting. Only valid
+ // for serial port connections.
+ //
+ // Input
+ // mid : Message ID of message to send
+ // param : For messages that need a parameter
+ // bid : Bus ID of message to send (def 0xFF)
+ //
+ // Output
+ // = MTRV_OK if an Ack message is received
+ // = MTRV_RECVERRORMSG if an error message is received
+ // = MTRV_TIMEOUT if timeout occurred
+ //
+ // value contains the integer value of the data field of the ack message
+ //
+ short CXsensMTiModule::reqSetting(const unsigned char mid, const unsigned char param, unsigned long& value, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+
+ if (param != 0xFF)
+ {
+ buffer[IND_LEN] = 1;
+ buffer[IND_DATA0] = param;
+ }
+ else
+ {
+ buffer[IND_LEN] = 0;
+ }
+
+ calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
+
+ // Send message
+ writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
+
+ // Read next message or else timeout
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ // Acknowledge received
+ value = 0;
+
+ if (param == 0xFF)
+ {
+ swapEndian(&buffer[IND_DATA0], (unsigned char*) &value, buffer[IND_LEN]);
+ }
+ else
+ {
+ swapEndian(&buffer[IND_DATA0] + 1, (unsigned char*) &value, buffer[IND_LEN] - 1);
+ }
+
+ return (m_retVal = MTRV_OK);
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ else
+ {
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
+ }
+ }
+
+ return m_retVal;
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // reqSetting (float & no param variant)
+ //
+ // Request a float setting from the device. Only valid
+ // for serial port connections.
+ //
+ // Input
+ // mid : Message ID of message to send
+ // bid : Bus ID of message to send (def 0xFF)
+ //
+ // Output
+ // = MTRV_OK if an Ack message is received
+ // = MTRV_RECVERRORMSG if an error message is received
+ // = MTRV_TIMEOUT if timeout occurred
+ //
+ // value contains the float value of the acknowledge data field
+ //
+ short CXsensMTiModule::reqSetting(const unsigned char mid, float& value, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+ buffer[IND_LEN] = 0;
+ calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
+
+ // Send message
+ writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
+
+ // Read next message or else timeout
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ // Acknowledge received
+ value = 0;
+ swapEndian(&buffer[IND_DATA0], (unsigned char*) &value, buffer[IND_LEN]);
+ return (m_retVal = MTRV_OK);
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ else
+ {
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
+ }
+ }
+
+ return m_retVal;
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // reqSetting (float & param variant)
+ //
+ // Request a float setting from the device. Only valid
+ // for serial port connections.
+ //
+ // Input
+ // mid : Message ID of message to send
+ // param : For messages that need a parameter (optional)
+ // bid : Bus ID of message to send (def 0xFF)
+ //
+ // Output
+ // = MTRV_OK if an Ack message is received
+ // = MTRV_RECVERRORMSG if an error message is received
+ // = MTRV_TIMEOUT if timeout occurred
+ //
+ // value contains the float value of the acknowledge data field
+ //
+ short CXsensMTiModule::reqSetting(const unsigned char mid, const unsigned char param, float& value, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+
+ if (param != 0xFF)
+ {
+ buffer[IND_LEN] = 1;
+ buffer[IND_DATA0] = param;
+ }
+ else
+ {
+ buffer[IND_LEN] = 0;
+ }
+
+ calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
+
+ // Send message
+ writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
+
+ // Read next message or else timeout
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ // Acknowledge received
+ value = 0;
+
+ if (param == 0xFF)
+ {
+ swapEndian(&buffer[IND_DATA0], (unsigned char*) &value, buffer[IND_LEN]);
+ }
+ else
+ {
+ swapEndian(&buffer[IND_DATA0] + 1, (unsigned char*) &value, buffer[IND_LEN] - 1);
+ }
+
+ return (m_retVal = MTRV_OK);
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ else
+ {
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
+ }
+ }
+
+ return m_retVal;
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // reqSetting (byte array & no param variant)
+ //
+ // Send a message to the device and the data of acknowledge message
+ // will be returned. Only valid for serial port connections
+ //
+ // Input
+ // mid : Message ID of message to send
+ // bid : Bus ID of message to send (def 0xFF)
+ //
+ // Output
+ // = MTRV_OK if an Ack message is received
+ // = MTRV_RECVERRORMSG if an error message is received
+ // = MTRV_TIMEOUT if timeout occurred
+ //
+ // data[] contains the data of the acknowledge message
+ // dataLen contains the number bytes returned
+ //
+ short CXsensMTiModule::reqSetting(const unsigned char mid, unsigned char data[], short& dataLen, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+ buffer[IND_LEN] = 0;
+ calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
+
+ // Send message
+ writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
+
+ dataLen = 0;
+
+ // Read next message or else timeout
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ // Acknowledge received
+ if (buffer[IND_LEN] != EXTLENCODE)
+ {
+ dataLen = buffer[IND_LEN];
+ memcpy(data, &buffer[IND_DATA0], dataLen);
+ }
+ else
+ {
+ dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
+ memcpy(data, &buffer[IND_DATAEXT0], dataLen);
+ }
+
+ return (m_retVal = MTRV_OK);
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ else
+ {
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
+ }
+ }
+
+ return m_retVal;
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // reqSetting (byte array in + out & no param variant)
+ //
+ // Send a message to the device and the data of acknowledge message
+ // will be returned. Only valid for serial port connections
+ //
+ // Input
+ // mid : Message ID of message to send
+ // bid : Bus ID of message to send (def 0xFF)
+ // dataIn : Data to be included in request
+ // dataInLen : Number of bytes in dataIn
+ //
+ // Output
+ // = MTRV_OK if an Ack message is received
+ // = MTRV_RECVERRORMSG if an error message is received
+ // = MTRV_TIMEOUT if timeout occurred
+ //
+ // dataOut[] contains the data of the acknowledge message
+ // dataOutLen contains the number bytes returned
+ //
+ short CXsensMTiModule::reqSetting(const unsigned char mid, unsigned char dataIn[], short dataInLen, unsigned char dataOut[], short& dataOutLen, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short headerLength;
+ short msgLen;
+
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+
+ if (dataInLen < EXTLENCODE)
+ {
+ buffer[IND_LEN] = (unsigned char) dataInLen;
+ headerLength = LEN_MSGHEADER;
+ }
+ else
+ {
+ buffer[IND_LEN] = EXTLENCODE;
+ buffer[IND_LENEXTH] = (unsigned char)(dataInLen >> 8);
+ buffer[IND_LENEXTL] = (unsigned char)(dataInLen & 0x00FF);
+ headerLength = LEN_MSGEXTHEADER;
+ }
+
+ memcpy(&buffer[headerLength], dataIn, dataInLen);
+ calcChecksum(buffer, headerLength + dataInLen);
+
+ // Send message
+ writeData(buffer, headerLength + dataInLen + LEN_CHECKSUM);
+
+ dataOutLen = 0;
+
+ // Read next message or else timeout
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ // Acknowledge received
+ if (buffer[IND_LEN] != EXTLENCODE)
+ {
+ dataOutLen = buffer[IND_LEN];
+ memcpy(dataOut, &buffer[IND_DATA0], dataOutLen);
+ }
+ else
+ {
+ dataOutLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
+ memcpy(dataOut, &buffer[IND_DATAEXT0], dataOutLen);
+ }
+
+ return (m_retVal = MTRV_OK);
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ else
+ {
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
+ }
+ }
+
+ return m_retVal;
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // reqSetting (byte array & param variant)
+ //
+ // Send a message to the device and the data of acknowledge message
+ // will be returned. Only valid for serial port connections
+ //
+ // Input
+ // mid : Message ID of message to send
+ // param : For messages that need a parameter (optional)
+ // bid : Bus ID of message to send (def 0xFF)
+ //
+ // Output
+ // = MTRV_OK if an Ack message is received
+ // = MTRV_RECVERRORMSG if an error message is received
+ // = MTRV_TIMEOUT if timeout occurred
+ //
+ // data[] contains the data of the acknowledge message (including param!!)
+ // dataLen contains the number bytes returned
+ //
+ short CXsensMTiModule::reqSetting(const unsigned char mid, const unsigned char param, unsigned char data[], short& dataLen, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+
+ if (param != 0xFF)
+ {
+ buffer[IND_LEN] = 1;
+ buffer[IND_DATA0] = param;
+ }
+ else
+ {
+ buffer[IND_LEN] = 0;
+ }
+
+ calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
+
+ // Send message
+ writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
+
+ dataLen = 0;
+
+ // Read next message or else timeout
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ // Acknowledge received
+ if (buffer[IND_LEN] != EXTLENCODE)
+ {
+ dataLen = buffer[IND_LEN];
+ memcpy(data, &buffer[IND_DATA0], dataLen);
+ }
+ else
+ {
+ dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
+ memcpy(data, &buffer[IND_DATAEXT0], dataLen);
+ }
+
+ return (m_retVal = MTRV_OK);
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ else
+ {
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
+ }
+ }
+
+ return m_retVal;
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // setSetting (integer & no param variant)
+ //
+ // Sets a integer setting of the device. This setting
+ // can be an unsigned 1,2 or 4 bytes setting. Only valid
+ // for serial port connections.
+ //
+ // Input
+ // mid : Message ID of message to send
+ // bid : Bus ID of message to send (def 0xFF)
+ // value : Contains the integer value to be used
+ // valuelen : Length in bytes of the value
+ //
+ // Output
+ // = MTRV_OK if an Ack message is received
+ // = MTRV_RECVERRORMSG if an error message is received
+ // = MTRV_TIMEOUT if timeout occurred
+ //
+ //
+ short CXsensMTiModule::setSetting(const unsigned char mid, const unsigned long value, const unsigned short valuelen, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ msgLen = LEN_MSGHEADER;
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+ buffer[IND_LEN] = (unsigned char) valuelen;
+ swapEndian((unsigned char*) &value, &buffer[msgLen], valuelen);
+ calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
+
+ // Send message
+ writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
+
+ // Read next received message
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ return (m_retVal = MTRV_OK); // Acknowledge received
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ }
+
+ return m_retVal;
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // setSetting (integer & param variant)
+ //
+ // Sets a integer setting of the device. This setting
+ // can be an unsigned 1,2 or 4 bytes setting. Only valid
+ // for serial port connections.
+ //
+ // Input
+ // mid : Message ID of message to send
+ // param : For messages that need a parameter (optional)
+ // bid : Bus ID of message to send (def 0xFF)
+ // value : Contains the integer value to be used
+ // valuelen : Length in bytes of the value
+ //
+ // Output
+ // = MTRV_OK if an Ack message is received
+ // = MTRV_RECVERRORMSG if an error message is received
+ // = MTRV_TIMEOUT if timeout occurred
+ //
+ //
+ short CXsensMTiModule::setSetting(const unsigned char mid, const unsigned char param, const unsigned long value, const unsigned short valuelen, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ msgLen = LEN_MSGHEADER;
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+
+ if (param != 0xFF)
+ {
+ msgLen++;
+ buffer[IND_LEN] = valuelen + 1;
+ buffer[IND_DATA0] = param;
+ }
+ else
+ {
+ buffer[IND_LEN] = (unsigned char) valuelen;
+ }
+
+ swapEndian((unsigned char*) &value, &buffer[msgLen], valuelen);
+ calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
+
+ // Send message
+ writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
+
+ // Read next received message
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ return (m_retVal = MTRV_OK); // Acknowledge received
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ }
+
+ return m_retVal;
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // setSetting (float & no param variant)
+ //
+ // Sets a float setting of the device. Only valid
+ // for serial port connections.
+ //
+ // Input
+ // mid : Message ID of message to send
+ // bid : Bus ID of message to send (def 0xFF)
+ // value : Contains the float value to be used
+ //
+ // Output
+ // = MTRV_OK if an Ack message is received
+ // = MTRV_RECVERRORMSG if an error message is received
+ // = MTRV_TIMEOUT if timeout occurred
+ //
+ short CXsensMTiModule::setSetting(const unsigned char mid, const float value, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ msgLen = LEN_MSGHEADER;
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+ buffer[IND_LEN] = LEN_FLOAT;
+ swapEndian((unsigned char*) &value, &buffer[msgLen], LEN_FLOAT);
+ calcChecksum(buffer, LEN_MSGHEADER + LEN_FLOAT);
+
+ // Send message
+ writeData(buffer, LEN_MSGHEADERCS + LEN_FLOAT);
+
+ // Read next received message
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ return (m_retVal = MTRV_OK); // Acknowledge received
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ }
+
+ return m_retVal;
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // setSetting (float & param variant)
+ //
+ // Sets a float setting of the device. Only valid
+ // for serial port connections.
+ //
+ // Input
+ // mid : Message ID of message to send
+ // param : For messages that need a parameter (optional)
+ // bid : Bus ID of message to send (def 0xFF)
+ // value : Contains the float value to be used
+ //
+ // Output
+ // = MTRV_OK if an Ack message is received
+ // = MTRV_RECVERRORMSG if an error message is received
+ // = MTRV_TIMEOUT if timeout occurred
+ //
+ //
+ short CXsensMTiModule::setSetting(const unsigned char mid, const unsigned char param, const float value, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ msgLen = LEN_MSGHEADER;
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+
+ if (param != 0xFF)
+ {
+ msgLen++;
+ buffer[IND_LEN] = LEN_FLOAT + 1;
+ buffer[IND_DATA0] = param;
+ }
+ else
+ {
+ buffer[IND_LEN] = LEN_FLOAT;
+ }
+
+ swapEndian((unsigned char*) &value, &buffer[msgLen], LEN_FLOAT);
+ calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
+
+ // Send message
+ writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
+
+ // Read next received message
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ return (m_retVal = MTRV_OK); // Acknowledge received
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ }
+
+ return m_retVal;
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // setSetting (float & param & store variant)
+ //
+ // Sets a float setting of the device and with the Store field.
+ // Only valid for serial port connections
+ //
+ // Input
+ // mid : Message ID of message to send
+ // param : For messages that need a parameter (optional)
+ // value : Contains the float value to be used
+ // store ; Store in non-volatile memory (1) or not (0)
+ // bid : Bus ID of message to send (def 0xFF)
+ //
+ // Output
+ // = MTRV_OK if an Ack message is received
+ // = MTRV_RECVERRORMSG if an error message is received
+ // = MTRV_TIMEOUT if timeout occurred
+ //
+ //
+ short CXsensMTiModule::setSetting(const unsigned char mid, const unsigned char param, const float value, const bool store, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ msgLen = LEN_MSGHEADER;
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+
+ if (param != 0xFF)
+ {
+ msgLen++;
+ buffer[IND_LEN] = LEN_FLOAT + 2;
+ buffer[IND_DATA0] = param;
+ }
+ else
+ {
+ buffer[IND_LEN] = LEN_FLOAT + 1;
+ }
+
+ swapEndian((unsigned char*) &value, &buffer[msgLen], LEN_FLOAT);
+ buffer[msgLen + LEN_FLOAT ] = store;
+ calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
+
+ // Send message
+ writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
+
+ // Read next received message
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ return (m_retVal = MTRV_OK); // Acknowledge received
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ }
+
+ return m_retVal;
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // getDeviceMode
+ //
+ // Requests the current output mode/setting of input (file or serialport)
+ // the Outputmode, Outputsettings, DataLength & number of devices
+ // are stored in member variables of the MTComm class. These values
+ // are needed for the GetValue functions.
+ // The function optionally returns the number of devices
+ //
+ // File: expects the Configuration message at the start of the file
+ // which holds the OutputMode & OutputSettings. File position
+ // is after the first message
+ //
+ // Input
+ // Output
+ // numDevices : [optional] number of devices connected to port or
+ // found in configuration file
+ //
+ // returns MTRV_OK if the mode & settings are read
+ //
+ short CXsensMTiModule::getDeviceMode(unsigned short* numDevices)
+ {
+ unsigned char mid = 0 , data[MAXMSGLEN ];
+ short datalen;
+
+ if (numDevices != NULL)
+ {
+ *numDevices = 0;
+ }
+
+ // In case serial port is used (live device / XM or MT)
+ if (m_portOpen)
+ {
+ if (reqSetting(MID_INITBUS, data, datalen) != MTRV_OK)
+ {
+ return m_retVal;
+ }
+
+ // Retrieve outputmode + outputsettings
+ for (int i = 0 ; i < datalen / LEN_DEVICEID ; i++)
+ {
+ if (reqSetting(MID_REQOUTPUTMODE, m_storedOutputMode[BID_MT + i], BID_MT + i) != MTRV_OK)
+ {
+ return m_retVal;
+ }
+
+ if (reqSetting(MID_REQOUTPUTSETTINGS, m_storedOutputSettings[BID_MT + i], BID_MT + i) != MTRV_OK)
+ {
+ return m_retVal;
+ }
+
+ if (reqSetting(MID_REQDATALENGTH, m_storedDataLength[BID_MT + i], BID_MT + i) != MTRV_OK)
+ {
+ return m_retVal;
+ }
+ }
+
+ if (numDevices != NULL)
+ {
+ *numDevices = datalen / LEN_DEVICEID;
+ }
+
+ unsigned char masterDID[4];
+ short DIDlen;
+
+ if (reqSetting(MID_REQDID, masterDID, DIDlen) != MTRV_OK)
+ {
+ return m_retVal;
+ }
+
+ if (memcmp(masterDID, data, LEN_DEVICEID) != 0)
+ {
+ // Using an XbusMaster
+ m_storedOutputMode[0] = OUTPUTMODE_XM;
+ m_storedOutputSettings[0] = OUTPUTSETTINGS_XM;
+ m_storedDataLength[0] = LEN_SAMPLECNT;
+ }
+ else
+ {
+ m_storedOutputMode[0] = m_storedOutputMode[BID_MT ];
+ m_storedOutputSettings[0] = m_storedOutputSettings[BID_MT ];
+ m_storedDataLength[0] = m_storedDataLength[BID_MT ];
+ }
+
+ return (m_retVal = MTRV_OK);
+ }
+ else if (m_fileOpen)
+ {
+ // Configuration message should be the first message in the file
+ setFilePos(0);
+
+ if (readMessage(mid, data, datalen) == MTRV_OK)
+ {
+ if (mid == MID_CONFIGURATION)
+ {
+ unsigned short _numDevices = 0;
+ swapEndian(data + CONF_NUMDEVICES, (unsigned char*) &_numDevices, CONF_NUMDEVICESLEN);
+
+ for (unsigned int i = 0 ; i < _numDevices ; i++)
+ {
+ m_storedOutputMode[BID_MT + i] = 0;
+ swapEndian(data + CONF_OUTPUTMODE + i * CONF_BLOCKLEN, (unsigned char*)(m_storedOutputMode + BID_MT + i),
+ CONF_OUTPUTMODELEN);
+ m_storedOutputSettings[BID_MT + i] = 0;
+ swapEndian(data + CONF_OUTPUTSETTINGS + i * CONF_BLOCKLEN, (unsigned char*)(m_storedOutputSettings + BID_MT + i),
+ CONF_OUTPUTSETTINGSLEN);
+ m_storedDataLength[BID_MT + i] = 0;
+ swapEndian(data + CONF_DATALENGTH + i * CONF_BLOCKLEN, (unsigned char*)(m_storedDataLength + BID_MT + i),
+ CONF_DATALENGTHLEN);
+ }
+
+ if (numDevices != NULL)
+ {
+ *numDevices = _numDevices;
+ }
+
+ if (memcmp(data + CONF_MASTERDID, data + CONF_DID, LEN_DEVICEID) != 0)
+ {
+ // Using an XbusMaster
+ m_storedOutputMode[0] = OUTPUTMODE_XM;
+ m_storedOutputSettings[0] = OUTPUTSETTINGS_XM;
+ m_storedDataLength[0] = LEN_SAMPLECNT;
+ }
+ else
+ {
+ m_storedOutputMode[0] = m_storedOutputMode[BID_MT ];
+ m_storedOutputSettings[0] = m_storedOutputSettings[BID_MT ];
+ m_storedDataLength[0] = m_storedDataLength[BID_MT ];
+ }
+
+ return (m_retVal = MTRV_OK);
+ }
+ }
+
+ return (m_retVal = MTRV_NOTSUCCESSFUL);
+ }
+
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // setDeviceMode
+ //
+ // Sets the current output mode/setting of input (not for file-based
+ // inputs)
+ //
+ // Input
+ // OutputMode : OutputMode to be set in device & stored in MTComm
+ // class member variable
+ // OutputSettings : OutputSettings to be set in device & stored in
+ // MTComm class member variable
+ // Output
+ //
+ // returns MTRV_OK if the mode & settings are read
+ //
+ short CXsensMTiModule::setDeviceMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid)
+ {
+ // In case serial port is used (live XM / MT)
+ if (m_portOpen)
+ {
+ // Set OutputMode
+ if (setSetting(MID_SETOUTPUTMODE, OutputMode, LEN_OUTPUTMODE, bid) != MTRV_OK)
+ {
+ return m_retVal;
+ }
+
+ if (bid == BID_MASTER || (bid == BID_MT && m_storedOutputMode[0] != OUTPUTMODE_XM))
+ {
+ m_storedOutputMode[0] = m_storedOutputMode[BID_MT ] = OutputMode;
+ }
+ else
+ {
+ m_storedOutputMode[bid] = OutputMode;
+ }
+
+ // Set OutputSettings
+ if (setSetting(MID_SETOUTPUTSETTINGS, OutputSettings, LEN_OUTPUTSETTINGS, bid) != MTRV_OK)
+ {
+ return m_retVal;
+ }
+
+ if (bid == BID_MASTER || (bid == BID_MT && m_storedOutputMode[0] != OUTPUTMODE_XM))
+ {
+ m_storedOutputSettings[0] = m_storedOutputSettings[BID_MT ] = OutputSettings;
+ }
+ else
+ {
+ m_storedOutputSettings[bid] = OutputSettings;
+ }
+
+ // Get DataLength from device
+ if (OutputMode != OUTPUTMODE_XM)
+ {
+ unsigned long value;
+
+ if (reqSetting(MID_REQDATALENGTH, value, bid) == MTRV_OK)
+ {
+ if ((bid == BID_MASTER) || ((bid == BID_MT) && (m_storedOutputMode[0] != OUTPUTMODE_XM)))
+ {
+ m_storedDataLength[0] = m_storedDataLength[BID_MT ] = value;
+ }
+ else
+ {
+ m_storedDataLength[bid] = value;
+ }
+ }
+ }
+ else
+ {
+ m_storedDataLength[0] = LEN_SAMPLECNT;
+ }
+
+ return (m_retVal = MTRV_OK);
+ }
+
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // getMode
+ //
+ // Gets the output mode/setting used in MTComm class and the corresponding
+ // datalength. These variables are set by the functions GetDeviceMode,
+ // SetDeviceMode or SetMode
+ //
+ // Input
+ // Output
+ // OutputMode : OutputMode stored in MTComm class member variable
+ // OutputSettings : OutputSettings stored in MTComm class member variable
+ //
+ // returns always MTRV_OK
+ //
+ short CXsensMTiModule::getMode(unsigned long& OutputMode, unsigned long& OutputSettings, unsigned short& dataLength, const unsigned char bid)
+ {
+ unsigned char nbid = (bid == BID_MASTER) ? 0 : bid;
+ OutputMode = m_storedOutputMode[nbid];
+ OutputSettings = m_storedOutputSettings[nbid];
+ dataLength = (unsigned short) m_storedDataLength[nbid];
+ return (m_retVal = MTRV_OK);
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // setMode
+ //
+ // Sets the output mode/setting used in MTComm class. Use the function
+ // GetDeviceMode to retrieve the current values of file/device.
+ // This function will also calculate the data length field
+ //
+ // Input
+ // OutputMode : OutputMode to be stored in MTComm class member variable
+ // OutputSettings : OutputSettings to be stored in MTComm class member variable
+ // Output
+ //
+ // returns always MTRV_OK
+ //
+ short CXsensMTiModule::setMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid)
+ {
+ unsigned char nbid = bid;
+
+ if (nbid == BID_MASTER)
+ {
+ nbid = 0;
+ }
+
+ m_storedOutputMode[nbid] = OutputMode;
+ m_storedOutputSettings[nbid] = OutputSettings;
+
+ if (OutputMode == INVALIDSETTINGVALUE || OutputSettings == INVALIDSETTINGVALUE)
+ {
+ m_storedDataLength[nbid] = 0;
+ }
+ else
+ {
+ unsigned short dataLength = 0;
+
+ if (OutputMode & OUTPUTMODE_MT9)
+ {
+ dataLength = ((OutputSettings & OUTPUTSETTINGS_TIMESTAMP_MASK) == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT) ? LEN_SAMPLECNT : 0 + LEN_RAWDATA;
+ }
+ else if (OutputMode == OUTPUTMODE_XM)
+ {
+ // XbusMaster concatenates sample counter
+ dataLength = LEN_SAMPLECNT;
+ }
+ else
+ {
+ if (OutputMode & OUTPUTMODE_RAW)
+ {
+ dataLength = LEN_RAWDATA;
+ }
+ else
+ {
+ if (OutputMode & OUTPUTMODE_CALIB)
+ {
+ dataLength = LEN_CALIBDATA;
+ }
+
+ if (OutputMode & OUTPUTMODE_ORIENT)
+ {
+ switch (OutputSettings & OUTPUTSETTINGS_ORIENTMODE_MASK)
+ {
+ case OUTPUTSETTINGS_ORIENTMODE_QUATERNION:
+ dataLength += LEN_ORIENT_QUATDATA;
+ break;
+
+ case OUTPUTSETTINGS_ORIENTMODE_EULER:
+ dataLength += LEN_ORIENT_EULERDATA;
+ break;
+
+ case OUTPUTSETTINGS_ORIENTMODE_MATRIX:
+ dataLength += LEN_ORIENT_MATRIXDATA;
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+
+ switch (OutputSettings & OUTPUTSETTINGS_TIMESTAMP_MASK)
+ {
+ case OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT:
+ dataLength += LEN_SAMPLECNT;
+ break;
+
+ default:
+ break;
+ }
+ }
+
+ m_storedDataLength[nbid] = dataLength;
+ }
+
+ // If not XbusMaster store also in BID_MT
+ if (bid == BID_MASTER && OutputMode != OUTPUTMODE_XM)
+ {
+ m_storedOutputMode[BID_MT ] = m_storedOutputMode[0];
+ m_storedOutputSettings[BID_MT ] = m_storedOutputSettings[0];
+ m_storedDataLength[BID_MT ] = m_storedDataLength[0];
+ }
+
+ return (m_retVal = MTRV_OK);
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // getValue (unsigned short variant)
+ //
+ // Retrieves a unsigned short value from the data input parameter
+ // This function is valid for the following value specifiers:
+ // VALUE_RAW_TEMP
+ // VALUE_SAMPLECNT
+ //
+ // Use getDeviceMode or setMode to initialize the Outputmode
+ // and Outputsettings member variables used to retrieve the correct
+ // value
+ //
+ // Input
+ // valueSpec : Specifier of the value to be retrieved
+ // data[] : Data field of a MTData / BusData message
+ // bid : bus identifier of the device of which the
+ // value should be returned (default = BID_MT)
+ // Output
+ // value : reference to unsigned short in which the retrieved
+ // value will be returned
+ //
+ // Return value
+ // MTRV_OK : value is successfully retrieved
+ // != MTRV_OK : not successful
+ //
+ short CXsensMTiModule::getValue(const unsigned long valueSpec, unsigned short& value, const unsigned char data[], const unsigned char bid)
+ {
+ short offset = 0;
+ unsigned char nbid = bid;
+
+ if (nbid == BID_MASTER)
+ {
+ nbid = 0;
+ }
+
+ // Check for invalid mode/settings
+ if (m_storedOutputMode[nbid] == INVALIDSETTINGVALUE || m_storedOutputSettings[nbid] == INVALIDSETTINGVALUE)
+ {
+ return (m_retVal = MTRV_NOVALIDMODESPECIFIED);
+ }
+
+ // Calculate offset for XM input
+ if (m_storedOutputMode[0] == OUTPUTMODE_XM)
+ {
+ int i = 0;
+
+ while (i < nbid)
+ {
+ offset += (short) m_storedDataLength[i++];
+ }
+ }
+
+ // Check if data is unsigned short & available in data
+ m_retVal = MTRV_INVALIDVALUESPEC;
+
+ if (valueSpec == VALUE_RAW_TEMP)
+ {
+ if (m_storedOutputMode[nbid] & (OUTPUTMODE_RAW | OUTPUTMODE_MT9))
+ {
+ offset += (m_storedOutputMode[nbid] == OUTPUTMODE_MT9 && m_storedOutputSettings[nbid] == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT) ? LEN_SAMPLECNT : 0;
+ swapEndian(data + offset + valueSpec * LEN_UNSIGSHORT * 3, (unsigned char*) &value, LEN_RAW_TEMP);
+ m_retVal = MTRV_OK;
+ }
+ }
+ else if (valueSpec == VALUE_SAMPLECNT)
+ {
+ if ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_TIMESTAMP_MASK) == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT)
+ {
+ if (!(m_storedOutputMode[nbid] == OUTPUTMODE_MT9))
+ {
+ offset += (short) m_storedDataLength[nbid] - LEN_SAMPLECNT;
+ }
+
+ swapEndian(data + offset, (unsigned char*) &value, LEN_SAMPLECNT);
+ m_retVal = MTRV_OK;
+ }
+ }
+
+ return m_retVal;
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // getValue (array of unsigned short variant)
+ //
+ // Retrieves an array of unsigned short values from the data input
+ // parameter. This function is valid for the following value specifiers:
+ // VALUE_RAW_ACC
+ // VALUE_RAW_GYR
+ // VALUE_RAW_MAG
+ //
+ // Use getDeviceMode or setMode to initialize the Outputmode
+ // and Outputsettings member variables used to retrieve the correct
+ // value
+ //
+ // Input
+ // valueSpec : Specifier of the value to be retrieved
+ // data[] : Data field of a MTData / BusData message
+ // bid : bus identifier of the device of which the
+ // value should be returned (default = BID_MT)
+ // Output
+ // value[] : pointer to array of unsigned shorts in which the
+ // retrieved values will be returned
+ //
+ // Return value
+ // MTRV_OK : value is successfully retrieved
+ // != MTRV_OK : not successful
+ //
+ short CXsensMTiModule::getValue(const unsigned long valueSpec, unsigned short value[], const unsigned char data[], const unsigned char bid)
+ {
+ short offset = 0;
+ unsigned char nbid = bid;
+
+ if (nbid == BID_MASTER)
+ {
+ nbid = 0;
+ }
+
+ // Check for invalid mode/settings
+ if (m_storedOutputMode[nbid] == INVALIDSETTINGVALUE || m_storedOutputSettings[nbid] == INVALIDSETTINGVALUE)
+ {
+ return (m_retVal = MTRV_NOVALIDMODESPECIFIED);
+ }
+
+ // Calculate offset for XM input
+ if (m_storedOutputMode[0] == OUTPUTMODE_XM)
+ {
+ int i = 0;
+
+ while (i < nbid)
+ {
+ offset += (short) m_storedDataLength[i++];
+ }
+ }
+
+ // Check if data is unsigned short, available in data & retrieve data
+ m_retVal = MTRV_INVALIDVALUESPEC;
+
+ //if (valueSpec >= VALUE_RAW_ACC && valueSpec <= VALUE_RAW_MAG)
+ if (valueSpec <= VALUE_RAW_MAG)
+ {
+ if (m_storedOutputMode[nbid] & (OUTPUTMODE_RAW | OUTPUTMODE_MT9))
+ {
+ offset += (short)(valueSpec * LEN_UNSIGSHORT * 3);
+ offset += (m_storedOutputMode[nbid] == OUTPUTMODE_MT9 && m_storedOutputSettings[nbid] == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT) ? LEN_SAMPLECNT : 0;
+
+ for (int i = 0 ; i < 3 ; i++)
+ {
+ swapEndian(data + offset + i * LEN_UNSIGSHORT, (unsigned char*) value + i * LEN_UNSIGSHORT, LEN_UNSIGSHORT);
+ }
+
+ m_retVal = MTRV_OK;
+ }
+ }
+
+ return m_retVal;
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // getValue (array of floats variant)
+ //
+ // Retrieves an array of float values from the data input parameter.
+ // This function is valid for the following value specifiers:
+ // VALUE_TEMP
+ // VALUE_CALIB_ACC
+ // VALUE_CALIB_GYR
+ // VALUE_CALIB_MAG
+ // VALUE_ORIENT_QUAT
+ // VALUE_ORIENT_EULER
+ // VALUE_ORIENT_MATRIX
+ //
+ // Use getDeviceMode or setMode to initialize the Outputmode
+ // and Outputsettings member variables used to retrieve the correct
+ // value
+ //
+ // Input
+ // valueSpec : Specifier of the value to be retrieved
+ // data[] : Data field of a MTData / BusData message
+ // bid : bus identifier of the device of which the
+ // value should be returned (default = BID_MT)
+ // Output
+ // value[] : pointer to array of floats in which the
+ // retrieved values will be returned
+ //
+ // Return value
+ // MTRV_OK : value is successfully retrieved
+ // != MTRV_OK : not successful
+ //
+ short CXsensMTiModule::getValue(const unsigned long valueSpec, float value[], const unsigned char data[], const unsigned char bid)
+ {
+ short offset = 0;
+ int nElements = 0;
+ unsigned char nbid = bid;
+
+ if (nbid == BID_MASTER)
+ {
+ nbid = 0;
+ }
+
+ // Check for invalid mode/settings
+ if (m_storedOutputMode[nbid] == INVALIDSETTINGVALUE || m_storedOutputSettings[nbid] == INVALIDSETTINGVALUE)
+ {
+ return (m_retVal = MTRV_NOVALIDMODESPECIFIED);
+ }
+
+ // Calculate offset for XM input
+ if (m_storedOutputMode[0] == OUTPUTMODE_XM)
+ {
+ int i = 0;
+
+ while (i < nbid)
+ {
+ offset += (short) m_storedDataLength[i++];
+ }
+ }
+
+ // Check if data is float & available in data
+ m_retVal = MTRV_INVALIDVALUESPEC;
+
+ if (valueSpec == VALUE_TEMP)
+ {
+ if (m_storedOutputMode[nbid] & OUTPUTMODE_TEMP)
+ {
+ nElements = LEN_TEMPDATA / LEN_FLOAT;
+ m_retVal = MTRV_OK;
+ }
+ }
+ else if (valueSpec == VALUE_CALIB_ACC)
+ {
+ offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0) ? LEN_TEMPDATA : 0;
+
+ if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB) && (m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0)
+ {
+ nElements = LEN_CALIB_ACCDATA / LEN_FLOAT;
+ m_retVal = MTRV_OK;
+ }
+ }
+ else if (valueSpec == VALUE_CALIB_GYR)
+ {
+ offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0) ? LEN_TEMPDATA : 0;
+
+ if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB) && (m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_GYR_MASK) == 0)
+ {
+ offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0) ? LEN_CALIB_ACCX * 3 : 0;
+ nElements = LEN_CALIB_GYRDATA / LEN_FLOAT;
+ m_retVal = MTRV_OK;
+ }
+ }
+ else if (valueSpec == VALUE_CALIB_MAG)
+ {
+ offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0) ? LEN_TEMPDATA : 0;
+
+ if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB) && (m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_MAG_MASK) == 0)
+ {
+ offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0) ? LEN_CALIB_ACCX * 3 : 0;
+ offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_GYR_MASK) == 0) ? LEN_CALIB_GYRX * 3 : 0;
+ nElements = LEN_CALIB_MAGDATA / LEN_FLOAT;
+ m_retVal = MTRV_OK;
+ }
+ }
+ else if (valueSpec >= VALUE_ORIENT_QUAT && valueSpec <= VALUE_ORIENT_MATRIX)
+ {
+ offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0) ? LEN_TEMPDATA : 0;
+
+ if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB))
+ {
+ offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0) ? LEN_CALIB_ACCX * 3 : 0;
+ offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_GYR_MASK) == 0) ? LEN_CALIB_GYRX * 3 : 0;
+ offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_MAG_MASK) == 0) ? LEN_CALIB_MAGX * 3 : 0;
+ }
+
+ if (m_storedOutputMode[nbid] & OUTPUTMODE_ORIENT)
+ {
+ unsigned long orientmode = m_storedOutputSettings[nbid] & OUTPUTSETTINGS_ORIENTMODE_MASK;
+
+ switch (valueSpec)
+ {
+ case VALUE_ORIENT_QUAT:
+ if (orientmode == OUTPUTSETTINGS_ORIENTMODE_QUATERNION)
+ {
+ nElements = LEN_ORIENT_QUATDATA / LEN_FLOAT;
+ m_retVal = MTRV_OK;
+ }
+
+ break;
+
+ case VALUE_ORIENT_EULER:
+ if (orientmode == OUTPUTSETTINGS_ORIENTMODE_EULER)
+ {
+ nElements = LEN_ORIENT_EULERDATA / LEN_FLOAT;
+ m_retVal = MTRV_OK;
+ }
+
+ break;
+
+ case VALUE_ORIENT_MATRIX:
+ if (orientmode == OUTPUTSETTINGS_ORIENTMODE_MATRIX)
+ {
+ nElements = LEN_ORIENT_MATRIXDATA / LEN_FLOAT;
+ m_retVal = MTRV_OK;
+ }
+
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+
+ if (m_retVal == MTRV_OK)
+ {
+ if ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_DATAFORMAT_F1220) == 0)
+ {
+ for (int i = 0 ; i < nElements ; i++)
+ {
+ swapEndian(data + offset + i * LEN_FLOAT, (unsigned char*) value + i * LEN_FLOAT, LEN_FLOAT);
+ }
+ }
+ else
+ {
+ int temp;
+
+ for (int i = 0 ; i < nElements ; i++)
+ {
+ swapEndian(data + offset + i * LEN_FLOAT, (unsigned char*) &temp, 4);
+ value[i] = (float) temp / 1048576;
+ }
+ }
+ }
+
+ return m_retVal;
+ }
+
+ //////////////////////////////////////////////////////////////////////
+ // getLastDeviceError
+ //
+ // Returns the last reported device error of the latest received Error
+ // message
+ //
+ // Output
+ // Error code
+ short CXsensMTiModule::getLastDeviceError()
+ {
+ return m_deviceError;
+ }
+
+ //////////////////////////////////////////////////////////////////////
+ // getLastRetVal
+ //
+ // Returns the returned value of the last called function
+ //
+ // Output
+ // Return value
+ short CXsensMTiModule::getLastRetVal()
+ {
+ return m_retVal;
+ }
+
+ //////////////////////////////////////////////////////////////////////
+ // setTimeOut
+ //
+ // Sets the time out value in milliseconds used by the functions
+ // Use 0 for infinite timeout
+ //
+ // Output
+ // MTRV_OK is set, MTRV_INVALIDTIMEOUT if time value < 0
+ short CXsensMTiModule::setTimeOut(short timeOutMs)
+ {
+ if (timeOutMs >= 0)
+ {
+ m_timeOut = timeOutMs;
+ return (m_retVal = MTRV_OK);
+ }
+ else
+ {
+ return (m_retVal = MTRV_INVALIDTIMEOUT);
+ }
+ }
+
+ //////////////////////////////////////////////////////////////////////
+ // swapEndian
+ //
+ // Convert 2 or 4 bytes data from little to big endian or back
+ //
+ // Input
+ // input : Pointer to data to be converted
+ // output : Pointer where converted data is stored
+ // length : Length of setting (0,2 & 4)
+ //
+ // Remarks:
+ // Allocate enough bytes for output buffer
+
+ void CXsensMTiModule::swapEndian(const unsigned char input[], unsigned char output[], const int length)
+ {
+ switch (length)
+ {
+ case 4:
+ output[0] = input[3];
+ output[1] = input[2];
+ output[2] = input[1];
+ output[3] = input[0];
+ break;
+
+ case 2:
+ output[0] = input[1];
+ output[1] = input[0];
+ break;
+
+ case 1:
+ output[0] = input[0];
+ break;
+
+ default:
+ for (int i = 0 , j = length - 1 ; i < length ; i++ , j--)
+ {
+ output[j] = input[i];
+ }
+
+ break;
+ }
+ }
+
+ //////////////////////////////////////////////////////////////////////
+ // calcChecksum
+ //
+ // Calculate and append checksum to msgBuffer
+ //
+ void CXsensMTiModule::calcChecksum(unsigned char* msgBuffer, const int msgBufferLength)
+ {
+ unsigned char checkSum = 0;
+ int i;
+
+ for (i = 1; i < msgBufferLength ; i++)
+ {
+ checkSum += msgBuffer[i];
+ }
+
+ msgBuffer[msgBufferLength] = -checkSum; // Store chksum
+ }
+
+ //////////////////////////////////////////////////////////////////////
+ // checkChecksum
+ //
+ // Checks if message checksum is valid
+ //
+ // Output
+ // returns true checksum is OK
+ bool CXsensMTiModule::checkChecksum(const unsigned char* msgBuffer, const int msgBufferLength)
+ {
+ unsigned char checkSum = 0;
+ int i;
+
+ for (i = 1; i < msgBufferLength ; i++)
+ {
+ checkSum += msgBuffer[i];
+ }
+
+ if (checkSum == 0)
+ {
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+ }
+ }
}
#endif
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.h b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.h
index 8c4d0d0d0a17c06bf409c228934a978c8c1c3144..54ba05d75115b066cbdf6adb377df62b2a3422cb 100644
--- a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.h
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.h
@@ -29,14 +29,14 @@
//
// v1.2.0
// 27-02-2006 - Renamed Xbus class to Motion Tracker C++ Communication class, short MTComm
-// - Defines XBRV_* accordingly renamed to MTRV_*
-// - Fixed device length not correct for bid 0 when using Xbus Master and setDeviceMode function
+// - Defines XBRV_* accordingly renamed to MTRV_*
+// - Fixed device length not correct for bid 0 when using Xbus Master and setDeviceMode function
//
// v1.1.7
// 15-02-2006 - Added fixed point signed 12.20 dataformat support
-// Added selective calibrated data output per sensor type support
-// Added outputmode temperature support
-// Fixed warning C4244: '=' : conversion from '' to '', possible loss of data
+// Added selective calibrated data output per sensor type support
+// Added outputmode temperature support
+// Fixed warning C4244: '=' : conversion from '' to '', possible loss of data
// v1.1.6
// 25-01-2006 - Added escape function for CLRDTR, CLRRTS, SETDTR, SETRTS, SETXOFF, SETXON, SETBREAK, CLRBREAK
//
@@ -45,46 +45,46 @@
//
// v1.1.4
// 08-11-2005 - Changed practically all uses of m_timeOut into uses of the new m_clkEnd
-// - Changed COM timeout in win32 to return immediately if data is available,
-// but wait 1ms otherwise
+// - Changed COM timeout in win32 to return immediately if data is available,
+// but wait 1ms otherwise
//
// v1.1.3
// 18-10-2005 - Added MID_REQPRODUCTCODE, MID_REQ/SETTRANSMITDELAY
-// - Added XBRV_TIMEOUTNODATA indicating timeout occurred due to no data read
+// - Added XBRV_TIMEOUTNODATA indicating timeout occurred due to no data read
//
// v1.1.2
// 16-09-2005 - Added eMTS version 0.1->1.0 changes (EMTS_FACTORYMODE)
-// - Added factory output mode defines
+// - Added factory output mode defines
//
// v1.1.1
// 01-09-2005 - Added defines for Extended output mode
-// - Added reqSetting (byte array in + out & no param variant)
+// - Added reqSetting (byte array in + out & no param variant)
//
// v1.1
// 08-08-2005 - Added file read and write support
-// - Added functions for data retrieval (getValue etc)
-// for easy data retrieval of acc, gyr, mag etc
-// - ReadMessageRaw:
-// - added a temporary buffer for unprocessed bytes
-// - check for invalid length messages
-// - Changed BID_MT into 1 and added BID_MASTER (=0xFF)
-// - Changed various ....SerialPort functions to .....Port
-// - Changed mtSendMessage functions to mtWriteMessage
-// - Added numerous defines
-// - Deleted obsolete functions
-// - Changed function getLastErrorCode into getLastDeviceError
-// - Changed OpenPort function for compatiblity with Bluetooth ports
-// - Added workaround for lockup of USB driver (close function)
-// - Added workaround for clock() problem with read function of USB driver
+// - Added functions for data retrieval (getValue etc)
+// for easy data retrieval of acc, gyr, mag etc
+// - ReadMessageRaw:
+// - added a temporary buffer for unprocessed bytes
+// - check for invalid length messages
+// - Changed BID_MT into 1 and added BID_MASTER (=0xFF)
+// - Changed various ....SerialPort functions to .....Port
+// - Changed mtSendMessage functions to mtWriteMessage
+// - Added numerous defines
+// - Deleted obsolete functions
+// - Changed function getLastErrorCode into getLastDeviceError
+// - Changed OpenPort function for compatiblity with Bluetooth ports
+// - Added workaround for lockup of USB driver (close function)
+// - Added workaround for clock() problem with read function of USB driver
//
// v1.0.2
// 29-06-2005 - Inserted initSerialPort with devicename input
-// - Changed return value defines names from X_ to XBRV_
-// - Removed unneeded includes for linux
+// - Changed return value defines names from X_ to XBRV_
+// - Removed unneeded includes for linux
//
// v1.0.1
// 22-06-2005 - Fixed ReqSetting functions (byte array & param variant)
-// mtSendRawString had wrong length input
+// mtSendRawString had wrong length input
//
// v1.0.0
// 20-06-2005 - Initial release
@@ -95,8 +95,8 @@
// Copyright (C) Xsens Technologies B.V., 2005.
//
// This source code is intended only as a example of the implementation
-// of the Xsens MT Communication protocol.
-// It was written for cross platform capabilities.
+// of the Xsens MT Communication protocol.
+// It was written for cross platform capabilities.
//
// THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
// KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
@@ -117,11 +117,11 @@
#include <conio.h>
#include <time.h>
#else
-#include <fcntl.h> /* POSIX Standard: 6.5 File Control Operations */
-#include <termios.h> /* terminal i/o system, talks to /dev/tty* ports */
-#include <unistd.h> /* Read function */
-#include <sys/time.h> /* gettimeofday function */
-#include <sys/stat.h> /* stat calls and structures*/
+#include <fcntl.h> /* POSIX Standard: 6.5 File Control Operations */
+#include <termios.h> /* terminal i/o system, talks to /dev/tty* ports */
+#include <unistd.h> /* Read function */
+#include <sys/time.h> /* gettimeofday function */
+#include <sys/stat.h> /* stat calls and structures*/
#endif
#include "../Includes.h"
@@ -130,887 +130,887 @@
namespace IMU
{
- namespace Xsens
- {
+ namespace Xsens
+ {
-#ifndef INVALID_SET_FILE_POINTER
-#define INVALID_SET_FILE_POINTER ((DWORD)(-1))
+#ifndef INVALID_SET_FILE_POINTER
+#define INVALID_SET_FILE_POINTER ((DWORD)(-1))
#endif
-// Field message indexes
-#define IND_PREAMBLE 0
-#define IND_BID 1
-#define IND_MID 2
-#define IND_LEN 3
-#define IND_DATA0 4
-#define IND_LENEXTH 4
-#define IND_LENEXTL 5
-#define IND_DATAEXT0 6
-
-// Maximum number of sensors supported
-#define MAXDEVICES 20
-
-#define PREAMBLE (const unsigned char)0xFA
-#define BID_MASTER (const unsigned char)0xFF
-#define BID_MT (const unsigned char)0x01
-#define EXTLENCODE 0xFF
-
-#define LEN_MSGHEADER (const unsigned short)4
-#define LEN_MSGEXTHEADER (const unsigned short)6
-#define LEN_MSGHEADERCS (const unsigned short)5
-#define LEN_MSGEXTHEADERCS (const unsigned short)7
-#define LEN_CHECKSUM (const unsigned short)1
-#define LEN_UNSIGSHORT (const unsigned short)2
-#define LEN_UNSIGINT (const unsigned short)4
-#define LEN_FLOAT (const unsigned short)4
-
-// Maximum message/data length
-#define MAXDATALEN (const unsigned short)2048
-#define MAXSHORTDATALEN (const unsigned short)254
-#define MAXMSGLEN (const unsigned short)(MAXDATALEN+7)
-#define MAXSHORTMSGLEN (const unsigned short)(MAXSHORTDATALEN+5)
-
-// DID Type (high nibble)
-#define DID_TYPEH_MASK (const unsigned long)0x00F00000
-#define DID_TYPEH_MT (const unsigned long)0x00000000
-#define DID_TYPEH_XM (const unsigned long)0x00100000
-#define DID_TYPEH_MTI_MTX (const unsigned long)0x00300000
-
-// All Message identifiers
-// WakeUp state messages
-#define MID_WAKEUP (const unsigned char)0x3E
-#define MID_WAKEUPACK (const unsigned char)0x3F
-
-// Config state messages
-#define MID_REQDID (const unsigned char)0x00
-#define MID_DEVICEID (const unsigned char)0x01
-#define LEN_DEVICEID (const unsigned short)4
-#define MID_INITBUS (const unsigned char)0x02
-#define MID_INITBUSRESULTS (const unsigned char)0x03
-#define LEN_INITBUSRESULTS (const unsigned short)4
-#define MID_REQPERIOD (const unsigned char)0x04
-#define MID_REQPERIODACK (const unsigned char)0x05
-#define LEN_PERIOD (const unsigned short)2
-#define MID_SETPERIOD (const unsigned char)0x04
-#define MID_SETPERIODACK (const unsigned char)0x05
-// XbusMaster
-#define MID_SETBID (const unsigned char)0x06
-#define MID_SETBIDACK (const unsigned char)0x07
-#define MID_AUTOSTART (const unsigned char)0x06
-#define MID_AUTOSTARTACK (const unsigned char)0x07
-#define MID_BUSPWROFF (const unsigned char)0x08
-#define MID_BUSPWROFFACK (const unsigned char)0x09
-// End XbusMaster
-#define MID_REQDATALENGTH (const unsigned char)0x0A
-#define MID_DATALENGTH (const unsigned char)0x0B
-#define LEN_DATALENGTH (const unsigned short)2
-#define MID_REQCONFIGURATION (const unsigned char)0x0C
-#define MID_CONFIGURATION (const unsigned char)0x0D
-#define LEN_CONFIGURATION (const unsigned short)118
-#define MID_RESTOREFACTORYDEF (const unsigned char)0x0E
-#define MID_RESTOREFACTORYDEFACK (const unsigned char)0x0F
-
-#define MID_GOTOMEASUREMENT (const unsigned char)0x10
-#define MID_GOTOMEASUREMENTACK (const unsigned char)0x11
-#define MID_REQFWREV (const unsigned char)0x12
-#define MID_FIRMWAREREV (const unsigned char)0x13
-#define LEN_FIRMWAREREV (const unsigned short)3
-// XbusMaster
-#define MID_REQBTDISABLE (const unsigned char)0x14
-#define MID_REQBTDISABLEACK (const unsigned char)0x15
-#define MID_DISABLEBT (const unsigned char)0x14
-#define MID_DISABLEBTACK (const unsigned char)0x15
-#define MID_REQOPMODE (const unsigned char)0x16
-#define MID_REQOPMODEACK (const unsigned char)0x17
-#define MID_SETOPMODE (const unsigned char)0x16
-#define MID_SETOPMODEACK (const unsigned char)0x17
-// End XbusMaster
-#define MID_REQBAUDRATE (const unsigned char)0x18
-#define MID_REQBAUDRATEACK (const unsigned char)0x19
-#define LEN_BAUDRATE (const unsigned short)1
-#define MID_SETBAUDRATE (const unsigned char)0x18
-#define MID_SETBAUDRATEACK (const unsigned char)0x19
-// XbusMaster
-#define MID_REQSYNCMODE (const unsigned char)0x1A
-#define MID_REQSYNCMODEACK (const unsigned char)0x1B
-#define MID_SETSYNCMODE (const unsigned char)0x1A
-#define MID_SETSYNCMODEACK (const unsigned char)0x1B
-// End XbusMaster
-#define MID_REQPRODUCTCODE (const unsigned char)0x1C
-#define MID_PRODUCTCODE (const unsigned char)0x1D
-
-#define MID_REQOUTPUTMODE (const unsigned char)0xD0
-#define MID_REQOUTPUTMODEACK (const unsigned char)0xD1
-#define LEN_OUTPUTMODE (const unsigned short)2
-#define MID_SETOUTPUTMODE (const unsigned char)0xD0
-#define MID_SETOUTPUTMODEACK (const unsigned char)0xD1
-
-#define MID_REQOUTPUTSETTINGS (const unsigned char)0xD2
-#define MID_REQOUTPUTSETTINGSACK (const unsigned char)0xD3
-#define LEN_OUTPUTSETTINGS (const unsigned short)4
-#define MID_SETOUTPUTSETTINGS (const unsigned char)0xD2
-#define MID_SETOUTPUTSETTINGSACK (const unsigned char)0xD3
-
-#define MID_REQOUTPUTSKIPFACTOR (const unsigned char)0xD4
-#define MID_REQOUTPUTSKIPFACTORACK (const unsigned char)0xD5
-#define LEN_OUTPUTSKIPFACTOR (const unsigned short)2
-#define MID_SETOUTPUTSKIPFACTOR (const unsigned char)0xD4
-#define MID_SETOUTPUTSKIPFACTORACK (const unsigned char)0xD5
-
-#define MID_REQSYNCINSETTINGS (const unsigned char)0xD6
-#define MID_REQSYNCINSETTINGSACK (const unsigned char)0xD7
-#define LEN_SYNCINMODE (const unsigned short)2
-#define LEN_SYNCINSKIPFACTOR (const unsigned short)2
-#define LEN_SYNCINOFFSET (const unsigned short)4
-#define MID_SETSYNCINSETTINGS (const unsigned char)0xD6
-#define MID_SETSYNCINSETTINGSACK (const unsigned char)0xD7
-
-#define MID_REQSYNCOUTSETTINGS (const unsigned char)0xD8
-#define MID_REQSYNCOUTSETTINGSACK (const unsigned char)0xD9
-#define LEN_SYNCOUTMODE (const unsigned short)2
-#define LEN_SYNCOUTSKIPFACTOR (const unsigned short)2
-#define LEN_SYNCOUTOFFSET (const unsigned short)4
-#define LEN_SYNCOUTPULSEWIDTH (const unsigned short)4
-#define MID_SETSYNCOUTSETTINGS (const unsigned char)0xD8
-#define MID_SETSYNCOUTSETTINGSACK (const unsigned char)0xD9
-
-#define MID_REQERRORMODE (const unsigned char)0xDA
-#define MID_REQERRORMODEACK (const unsigned char)0xDB
-#define LEN_ERRORMODE (const unsigned short)2
-#define MID_SETERRORMODE (const unsigned char)0xDA
-#define MID_SETERRORMODEACK (const unsigned char)0xDB
-
-#define MID_REQTRANSMITDELAY (const unsigned char)0xDC
-#define MID_REQTRANSMITDELAYACK (const unsigned char)0xDD
-#define LEN_TRANSMITDELAY (const unsigned short)2
-#define MID_SETTRANSMITDELAY (const unsigned char)0xDC
-#define MID_SETTRANSMITDELAYACK (const unsigned char)0xDD
-
-// Xbus Master
-#define MID_REQXMERRORMODE (const unsigned char)0x82
-#define MID_REQXMERRORMODEACK (const unsigned char)0x83
-#define LEN_XMERRORMODE (const unsigned short)2
-#define MID_SETXMERRORMODE (const unsigned char)0x82
-#define MID_SETXMERRORMODEACK (const unsigned char)0x83
-
-#define MID_REQBUFFERSIZE (const unsigned char)0x84
-#define MID_REQBUFFERSIZEACK (const unsigned char)0x85
-#define LEN_BUFFERSIZE (const unsigned short)2
-#define MID_SETBUFFERSIZE (const unsigned char)0x84
-#define MID_SETBUFFERSIZEACK (const unsigned char)0x85
-// End Xbus Master
-
-#define MID_REQHEADING (const unsigned char)0x82
-#define MID_REQHEADINGACK (const unsigned char)0x83
-#define LEN_HEADING (const unsigned short)4
-#define MID_SETHEADING (const unsigned char)0x82
-#define MID_SETHEADINGACK (const unsigned char)0x83
-
-#define MID_REQLOCATIONID (const unsigned char)0x84
-#define MID_REQLOCATIONIDACK (const unsigned char)0x85
-#define LEN_LOCATIONID (const unsigned short)2
-#define MID_SETLOCATIONID (const unsigned char)0x84
-#define MID_SETLOCATIONIDACK (const unsigned char)0x85
-
-#define MID_REQEXTOUTPUTMODE (const unsigned char)0x86
-#define MID_REQEXTOUTPUTMODEACK (const unsigned char)0x87
-#define LEN_EXTOUTPUTMODE (const unsigned short)2
-#define MID_SETEXTOUTPUTMODE (const unsigned char)0x86
-#define MID_SETEXTOUTPUTMODEACK (const unsigned char)0x87
-
-// XbusMaster
-#define MID_REQBATLEVEL (const unsigned char)0x88
-#define MID_BATLEVEL (const unsigned char)0x89
-// End XbusMaster
-
-#define MID_REQINITTRACKMODE (const unsigned char)0x88
-#define MID_REQINITTRACKMODEACK (const unsigned char)0x89
-#define LEN_INITTRACKMODE (const unsigned short)2
-#define MID_SETINITTRACKMODE (const unsigned char)0x88
-#define MID_SETINITTRACKMODEACK (const unsigned char)0x89
-
-#define MID_STOREFILTERSTATE (const unsigned char)0x8A
-#define MID_STOREFILTERSTATEACK (const unsigned char)0x8B
-
-// Measurement state
-#define MID_GOTOCONFIG (const unsigned char)0x30
-#define MID_GOTOCONFIGACK (const unsigned char)0x31
-#define MID_BUSDATA (const unsigned char)0x32
-#define MID_MTDATA (const unsigned char)0x32
-
-// Manual
-#define MID_PREPAREDATA (const unsigned char)0x32
-#define MID_REQDATA (const unsigned char)0x34
-#define MID_REQDATAACK (const unsigned char)0x35
-
-// MTData defines
-// Length of data blocks in bytes
-#define LEN_RAWDATA (const unsigned short)20
-#define LEN_CALIBDATA (const unsigned short)36
-#define LEN_CALIB_ACCDATA (const unsigned short)12
-#define LEN_CALIB_GYRDATA (const unsigned short)12
-#define LEN_CALIB_MAGDATA (const unsigned short)12
-#define LEN_ORIENT_QUATDATA (const unsigned short)16
-#define LEN_ORIENT_EULERDATA (const unsigned short)12
-#define LEN_ORIENT_MATRIXDATA (const unsigned short)36
-#define LEN_SAMPLECNT (const unsigned short)2
-#define LEN_TEMPDATA (const unsigned short)4
-
-// Length of data blocks in floats
-#define LEN_CALIBDATA_FLT (const unsigned short)9
-#define LEN_ORIENT_QUATDATA_FLT (const unsigned short)4
-#define LEN_ORIENT_EULERDATA_FLT (const unsigned short)3
-#define LEN_ORIENT_MATRIXDATA_FLT (const unsigned short)9
-
-// Indices of fields in DATA field of MTData message in bytes
-// use in combination with LEN_CALIB etc
-// Un-calibrated raw data
-#define IND_RAW_ACCX 0
-#define IND_RAW_ACCY 2
-#define IND_RAW_ACCZ 4
-#define IND_RAW_GYRX 6
-#define IND_RAW_GYRY 8
-#define IND_RAW_GYRZ 10
-#define IND_RAW_MAGX 12
-#define IND_RAW_MAGY 14
-#define IND_RAW_MAGZ 16
-#define IND_RAW_TEMP 18
-// Calibrated data
-#define IND_CALIB_ACCX 0
-#define IND_CALIB_ACCY 4
-#define IND_CALIB_ACCZ 8
-#define IND_CALIB_GYRX 12
-#define IND_CALIB_GYRY 16
-#define IND_CALIB_GYRZ 20
-#define IND_CALIB_MAGX 24
-#define IND_CALIB_MAGY 28
-#define IND_CALIB_MAGZ 32
-// Orientation data - quat
-#define IND_ORIENT_Q0 0
-#define IND_ORIENT_Q1 4
-#define IND_ORIENT_Q2 8
-#define IND_ORIENT_Q3 12
-// Orientation data - euler
-#define IND_ORIENT_ROLL 0
-#define IND_ORIENT_PITCH 4
-#define IND_ORIENT_YAW 8
-// Orientation data - matrix
-#define IND_ORIENT_A 0
-#define IND_ORIENT_B 4
-#define IND_ORIENT_C 8
-#define IND_ORIENT_D 12
-#define IND_ORIENT_E 16
-#define IND_ORIENT_F 20
-#define IND_ORIENT_G 24
-#define IND_ORIENT_H 28
-#define IND_ORIENT_I 32
-// Orientation data - euler
-#define IND_SAMPLECNTH 0
-#define IND_SAMPLECNTL 1
-
-// Indices of fields in DATA field of MTData message
-// Un-calibrated raw data
-#define FLDNUM_RAW_ACCX 0
-#define FLDNUM_RAW_ACCY 1
-#define FLDNUM_RAW_ACCZ 2
-#define FLDNUM_RAW_GYRX 3
-#define FLDNUM_RAW_GYRY 4
-#define FLDNUM_RAW_GYRZ 5
-#define FLDNUM_RAW_MAGX 6
-#define FLDNUM_RAW_MAGY 7
-#define FLDNUM_RAW_MAGZ 8
-#define FLDNUM_RAW_TEMP 9
-// Calibrated data
-#define FLDNUM_CALIB_ACCX 0
-#define FLDNUM_CALIB_ACCY 1
-#define FLDNUM_CALIB_ACCZ 2
-#define FLDNUM_CALIB_GYRX 3
-#define FLDNUM_CALIB_GYRY 4
-#define FLDNUM_CALIB_GYRZ 5
-#define FLDNUM_CALIB_MAGX 6
-#define FLDNUM_CALIB_MAGY 7
-#define FLDNUM_CALIB_MAGZ 8
-// Orientation data - quat
-#define FLDNUM_ORIENT_Q0 0
-#define FLDNUM_ORIENT_Q1 1
-#define FLDNUM_ORIENT_Q2 2
-#define FLDNUM_ORIENT_Q3 3
-// Orientation data - euler
-#define FLDNUM_ORIENT_ROLL 0
-#define FLDNUM_ORIENT_PITCH 1
-#define FLDNUM_ORIENT_YAW 2
-// Orientation data - matrix
-#define FLDNUM_ORIENT_A 0
-#define FLDNUM_ORIENT_B 1
-#define FLDNUM_ORIENT_C 2
-#define FLDNUM_ORIENT_D 3
-#define FLDNUM_ORIENT_E 4
-#define FLDNUM_ORIENT_F 5
-#define FLDNUM_ORIENT_G 6
-#define FLDNUM_ORIENT_H 7
-#define FLDNUM_ORIENT_I 8
-// Length
-// Uncalibrated raw data
-#define LEN_RAW_ACCX 2
-#define LEN_RAW_ACCY 2
-#define LEN_RAW_ACCZ 2
-#define LEN_RAW_GYRX 2
-#define LEN_RAW_GYRY 2
-#define LEN_RAW_GYRZ 2
-#define LEN_RAW_MAGX 2
-#define LEN_RAW_MAGY 2
-#define LEN_RAW_MAGZ 2
-#define LEN_RAW_TEMP 2
-// Calibrated data
-#define LEN_CALIB_ACCX 4
-#define LEN_CALIB_ACCY 4
-#define LEN_CALIB_ACCZ 4
-#define LEN_CALIB_GYRX 4
-#define LEN_CALIB_GYRY 4
-#define LEN_CALIB_GYRZ 4
-#define LEN_CALIB_MAGX 4
-#define LEN_CALIB_MAGY 4
-#define LEN_CALIB_MAGZ 4
-// Orientation data - quat
-#define LEN_ORIENT_Q0 4
-#define LEN_ORIENT_Q1 4
-#define LEN_ORIENT_Q2 4
-#define LEN_ORIENT_Q3 4
-// Orientation data - euler
-#define LEN_ORIENT_ROLL 4
-#define LEN_ORIENT_PITCH 4
-#define LEN_ORIENT_YAW 4
-// Orientation data - matrix
-#define LEN_ORIENT_A 4
-#define LEN_ORIENT_B 4
-#define LEN_ORIENT_C 4
-#define LEN_ORIENT_D 4
-#define LEN_ORIENT_E 4
-#define LEN_ORIENT_F 4
-#define LEN_ORIENT_G 4
-#define LEN_ORIENT_H 4
-#define LEN_ORIENT_I 4
-
-// Defines for getDataValue
-#define VALUE_RAW_ACC 0
-#define VALUE_RAW_GYR 1
-#define VALUE_RAW_MAG 2
-#define VALUE_RAW_TEMP 3
-#define VALUE_CALIB_ACC 4
-#define VALUE_CALIB_GYR 5
-#define VALUE_CALIB_MAG 6
-#define VALUE_ORIENT_QUAT 7
-#define VALUE_ORIENT_EULER 8
-#define VALUE_ORIENT_MATRIX 9
-#define VALUE_SAMPLECNT 10
-#define VALUE_TEMP 11
-
-#define INVALIDSETTINGVALUE 0xFFFFFFFF
-
-// Valid in all states
-#define MID_RESET (const unsigned char)0x40
-#define MID_RESETACK (const unsigned char)0x41
-#define MID_ERROR (const unsigned char)0x42
-#define LEN_ERROR (const unsigned short)1
-// XbusMaster
-#define MID_XMPWROFF (const unsigned char)0x44
-// End XbusMaster
-
-#define MID_REQFILTERSETTINGS (const unsigned char)0xA0
-#define MID_REQFILTERSETTINGSACK (const unsigned char)0xA1
-#define LEN_FILTERSETTINGS (const unsigned short)4
-#define MID_SETFILTERSETTINGS (const unsigned char)0xA0
-#define MID_SETFILTERSETTINGSACK (const unsigned char)0xA1
-#define MID_REQAMD (const unsigned char)0xA2
-#define MID_REQAMDACK (const unsigned char)0xA3
-#define LEN_AMD (const unsigned short)2
-#define MID_SETAMD (const unsigned char)0xA2
-#define MID_SETAMDACK (const unsigned char)0xA3
-#define MID_RESETORIENTATION (const unsigned char)0xA4
-#define MID_RESETORIENTATIONACK (const unsigned char)0xA5
-#define LEN_RESETORIENTATION (const unsigned short)2
-
-// All Messages
-// WakeUp state messages
-#define MSG_WAKEUPLEN 5
-#define MSG_WAKEUPACK (const unsigned char *)"\xFA\xFF\x3F\x00"
-#define MSG_WAKEUPACKLEN 4
-// Config state messages
-#define MSG_REQDID (const unsigned char *)"\xFA\xFF\x00\x00"
-#define MSG_REQDIDLEN 4
-#define MSG_DEVICEIDLEN 9
-#define MSG_INITBUS (const unsigned char *)"\xFA\xFF\x02\x00"
-#define MSG_INITBUSLEN 4
-#define MSG_INITBUSRESMAXLEN (5 + 4 * MAXSENSORS)
-#define MSG_REQPERIOD (const unsigned char *)"\xFA\xFF\x04\x00"
-#define MSG_REQPERIODLEN 4
-#define MSG_REQPERIODACKLEN 7
-#define MSG_SETPERIOD (const unsigned char *)"\xFA\xFF\x04\x02"
-#define MSG_SETPERIODLEN 6
-#define MSG_SETPERIODACKLEN 5
-#define MSG_SETBID (const unsigned char *)"\xFA\xFF\x06\x05"
-#define MSG_SETBIDLEN 9
-#define MSG_SETBIDACKLEN 5
-#define MSG_AUTOSTART (const unsigned char *)"\xFA\xFF\x06\x00"
-#define MSG_AUTOSTARTLEN 4
-#define MSG_AUTOSTARTACKLEN 5
-#define MSG_BUSPWROFF (const unsigned char *)"\xFA\xFF\x08\x00"
-#define MSG_BUSPWROFFLEN 4
-#define MSG_BUSPWROFFACKLEN 5
-#define MSG_RESTOREFACTORYDEF (const unsigned char *)"\xFA\xFF\x0E\x00"
-#define MSG_RESTOREFACTORYDEFLEN 4
-#define MSG_RESTOREFACTORYDEFACKLEN 5
-#define MSG_REQDATALENGTH (const unsigned char *)"\xFA\xFF\x0A\x00"
-#define MSG_REQDATALENGTHLEN 4
-#define MSG_DATALENGTHLEN 7
-#define MSG_REQCONFIGURATION (const unsigned char *)"\xFA\xFF\x0C\x00"
-#define MSG_REQCONFIGURATIONLEN 4
-#define MSG_GOTOMEASUREMENT (const unsigned char *)"\xFA\xFF\x10\x00"
-#define MSG_GOTOMEASUREMENTLEN 4
-#define MSG_GOTOMEASMAN (const unsigned char *)"\xFA\x01\x10\x00"
-#define MSG_GOTOMEASMANLEN 4
-#define MSG_GOTOMEASACKLEN 5
-#define MSG_REQFWREV (const unsigned char *)"\xFA\xFF\x12\x00"
-#define MSG_REQFWREVLEN 4
-#define MSG_FIRMWAREREVLEN 8
-#define MSG_REQBTDISABLED (const unsigned char *)"\xFA\xFF\x14\x00"
-#define MSG_REQBTDISABLEDLEN 4
-#define MSG_REQBTDISABLEDACKLEN 6
-#define MSG_DISABLEBT (const unsigned char *)"\xFA\xFF\x14\x01"
-#define MSG_DISABLEBTLEN 5
-#define MSG_DISABLEBTACKLEN 5
-#define MSG_REQOPMODE (const unsigned char *)"\xFA\xFF\x16\x00"
-#define MSG_REQOPMODELEN 4
-#define MSG_REQOPMODEACKLEN 6
-#define MSG_SETOPMODE (const unsigned char *)"\xFA\xFF\x16\x01"
-#define MSG_SETOPMODELEN 5
-#define MSG_SETOPMODEACKLEN 5
-#define MSG_REQBAUDRATE (const unsigned char *)"\xFA\xFF\x18\x00"
-#define MSG_REQBAUDRATELEN 4
-#define MSG_REQBAUDRATEACKLEN 6
-#define MSG_SETBAUDRATE (const unsigned char *)"\xFA\xFF\x18\x01"
-#define MSG_SETBAUDRATELEN 5
-#define MSG_SETBAUDRATEACKLEN 5
-#define MSG_REQSYNCMODE (const unsigned char *)"\xFA\xFF\x1A\x00"
-#define MSG_REQSYNCMODELEN 4
-#define MSG_REQSYNCMODEACKLEN 6
-#define MSG_SETSYNCMODE (const unsigned char *)"\xFA\xFF\x1A\x01"
-#define MSG_SETSYNCMODELEN 5
-#define MSG_SETSYNCMODEACKLEN 6
-#define MSG_REQMTS (const unsigned char *)"\xFA\xFF\x90\x01"
-#define MSG_REQMTSLEN 5
-#define MSG_MTSDATA 61
-#define MSG_STORECUSMTS (const unsigned char *)"\xFA\xFF\x92\x58"
-#define MSG_STORECUSMTSLEN 92
-#define MSG_STORECUSMTSACKLEN 5
-#define MSG_REVTOORGMTS (const unsigned char *)"\xFA\xFF\x94\x00"
-#define MSG_REVTOORGMTSLEN 4
-#define MSG_REVTOORGMTSACKLEN 5
-#define MSG_STOREMTS (const unsigned char *)"\xFA\xFF\x96\x41"
-#define MSG_STOREMTSLEN 69
-#define MSG_STOREMTSACKLEN 5
-#define MSG_REQSYNCOUTMODE (const unsigned char *)"\xFA\xFF\xD8\x01\x00"
-#define MSG_REQSYNCOUTMODELEN 5
-#define MSG_REQSYNCOUTSKIPFACTOR (const unsigned char *)"\xFA\xFF\xD8\x01\x01"
-#define MSG_REQSYNCOUTSKIPFACTORLEN 5
-#define MSG_REQSYNCOUTOFFSET (const unsigned char *)"\xFA\xFF\xD8\x01\x02"
-#define MSG_REQSYNCOUTOFFSETLEN 5
-#define MSG_REQSYNCOUTPULSEWIDTH (const unsigned char *)"\xFA\xFF\xD8\x01\x03"
-#define MSG_REQSYNCOUTPULSEWIDTHLEN 5
-#define MSG_REQERRORMODE (const unsigned char *)"\xFA\xFF\xDA\x00"
-#define MSG_REQERRORMODELEN 4
-#define MSG_REQERRORMODEACKLEN 7
-// Measurement state - auto messages
-#define MSG_GOTOCONFIG (const unsigned char *)"\xFA\xFF\x30\x00"
-#define MSG_GOTOCONFIGLEN 4
-#define MSG_GOTOCONFIGACKLEN 5
-// Measurement state - manual messages (Use DID = 0x01)
-#define MSG_GOTOCONFIGM (const unsigned char *)"\xFA\x01\x30\x00"
-#define MSG_GOTOCONFIGMLEN 4
-#define MSG_GOTOCONFIGMACKLEN 5
-#define MSG_PREPAREDATA (const unsigned char *)"\xFA\x01\x32\x00"
-#define MSG_PREPAREDATALEN 4
-#define MSG_REQDATA (const unsigned char *)"\xFA\x01\x34\x00"
-#define MSG_REQDATALEN 4
-// Valid in all states
-#define MSG_RESET (const unsigned char *)"\xFA\xFF\x40\x00"
-#define MSG_RESETLEN 4
-#define MSG_RESETACKLEN 5
-#define MSG_XMPWROFF (const unsigned char *)"\xFA\xFF\x44\x00"
-#define MSG_XMPWROFFLEN 4
-#define MSG_XMPWROFFACKLEN 5
-
-// Baudrate defines for SetBaudrate message
-#define BAUDRATE_9K6 0x09
-#define BAUDRATE_14K4 0x08
-#define BAUDRATE_19K2 0x07
-#define BAUDRATE_28K8 0x06
-#define BAUDRATE_38K4 0x05
-#define BAUDRATE_57K6 0x04
-#define BAUDRATE_76K8 0x03
-#define BAUDRATE_115K2 0x02
-#define BAUDRATE_230K4 0x01
-#define BAUDRATE_460K8 0x00
-#define BAUDRATE_921K6 0x80
-
-// Xbus protocol error codes (Error)
-#define ERROR_NOBUSCOMM 0x01
-#define ERROR_BUSNOTREADY 0x02
-#define ERROR_PERIODINVALID 0x03
-#define ERROR_MESSAGEINVALID 0x04
-#define ERROR_INITOFBUSFAILED1 0x10
-#define ERROR_INITOFBUSFAILED2 0x11
-#define ERROR_INITOFBUSFAILED3 0x12
-#define ERROR_SETBIDPROCFAILED1 0x14
-#define ERROR_SETBIDPROCFAILED2 0x15
-#define ERROR_MEASUREMENTFAILED1 0x18
-#define ERROR_MEASUREMENTFAILED2 0x19
-#define ERROR_MEASUREMENTFAILED3 0x1A
-#define ERROR_MEASUREMENTFAILED4 0x1B
-#define ERROR_MEASUREMENTFAILED5 0x1C
-#define ERROR_MEASUREMENTFAILED6 0x1D
-#define ERROR_TIMEROVERFLOW 0x1E
-#define ERROR_BAUDRATEINVALID 0x20
-#define ERROR_PARAMETERINVALID 0x21
-#define ERROR_MEASUREMENTFAILED7 0x23
-
-// Error modes (SetErrorMode)
-#define ERRORMODE_IGNORE 0x0000
-#define ERRORMODE_INCSAMPLECNT 0x0001
-#define ERRORMODE_INCSAMPLECNT_SENDERROR 0x0002
-#define ERRORMODE_SENDERROR_GOTOCONFIG 0x0003
-
-// Configuration message defines
-#define CONF_MASTERDID 0
-#define CONF_PERIOD 4
-#define CONF_OUTPUTSKIPFACTOR 6
-#define CONF_SYNCIN_MODE 8
-#define CONF_SYNCIN_SKIPFACTOR 10
-#define CONF_SYNCIN_OFFSET 12
-#define CONF_DATE 16
-#define CONF_TIME 24
-#define CONF_NUMDEVICES 96
-// Configuration sensor block properties
-#define CONF_DID 98
-#define CONF_DATALENGTH 102
-#define CONF_OUTPUTMODE 104
-#define CONF_OUTPUTSETTINGS 106
-#define CONF_BLOCKLEN 20
-// To calculate the offset in data field for output mode of sensor #2 use
-// CONF_OUTPUTMODE + 1*CONF_BLOCKLEN
-#define CONF_MASTERDIDLEN 4
-#define CONF_PERIODLEN 2
-#define CONF_OUTPUTSKIPFACTORLEN 2
-#define CONF_SYNCIN_MODELEN 2
-#define CONF_SYNCIN_SKIPFACTORLEN 2
-#define CONF_SYNCIN_OFFSETLEN 4
-#define CONF_DATELEN 8
-#define CONF_TIMELEN 8
-#define CONF_RESERVED_CLIENTLEN 32
-#define CONF_RESERVED_HOSTLEN 32
-#define CONF_NUMDEVICESLEN 2
-// Configuration sensor block properties
-#define CONF_DIDLEN 4
-#define CONF_DATALENGTHLEN 2
-#define CONF_OUTPUTMODELEN 2
-#define CONF_OUTPUTSETTINGSLEN 4
-
-// Clock frequency for offset & pulse width
-#define SYNC_CLOCKFREQ 29.4912e6
-
-// SyncIn params
-#define PARAM_SYNCIN_MODE (const unsigned char)0x00
-#define PARAM_SYNCIN_SKIPFACTOR (const unsigned char)0x01
-#define PARAM_SYNCIN_OFFSET (const unsigned char)0x02
-
-// SyncIn mode
-#define SYNCIN_DISABLED 0x0000
-#define SYNCIN_EDGE_RISING 0x0001
-#define SYNCIN_EDGE_FALLING 0x0002
-#define SYNCIN_EDGE_BOTH 0x0003
-#define SYNCIN_TYPE_SENDLASTDATA 0x0004
-#define SYNCIN_TYPE_DOSAMPLING 0x0000
-#define SYNCIN_EDGE_MASK 0x0003
-#define SYNCIN_TYPE_MASK 0x000C
-
-// SyncOut params
-#define PARAM_SYNCOUT_MODE (const unsigned char)0x00
-#define PARAM_SYNCOUT_SKIPFACTOR (const unsigned char)0x01
-#define PARAM_SYNCOUT_OFFSET (const unsigned char)0x02
-#define PARAM_SYNCOUT_PULSEWIDTH (const unsigned char)0x03
-
-// SyncOut mode
-#define SYNCOUT_DISABLED 0x0000
-#define SYNCOUT_TYPE_TOGGLE 0x0001
-#define SYNCOUT_TYPE_PULSE 0x0002
-#define SYNCOUT_POL_NEG 0x0000
-#define SYNCOUT_POL_POS 0x0010
-#define SYNCOUT_TYPE_MASK 0x000F
-#define SYNCOUT_POL_MASK 0x0010
-
-// Sample frequencies (SetPeriod)
-#define PERIOD_10HZ 0x2D00
-#define PERIOD_12HZ 0x2580
-#define PERIOD_15HZ 0x1E00
-#define PERIOD_16HZ 0x1C20
-#define PERIOD_18HZ 0x1900
-#define PERIOD_20HZ 0x1680
-#define PERIOD_24HZ 0x12C0
-#define PERIOD_25HZ 0x1200
-#define PERIOD_30HZ 0x0F00
-#define PERIOD_32HZ 0x0E10
-#define PERIOD_36HZ 0x0C80
-#define PERIOD_40HZ 0x0B40
-#define PERIOD_45HZ 0x0A00
-#define PERIOD_48HZ 0x0960
-#define PERIOD_50HZ 0x0900
-#define PERIOD_60HZ 0x0780
-#define PERIOD_64HZ 0x0708
-#define PERIOD_72HZ 0x0640
-#define PERIOD_75HZ 0x0600
-#define PERIOD_80HZ 0x05A0
-#define PERIOD_90HZ 0x0500
-#define PERIOD_96HZ 0x04B0
-#define PERIOD_100HZ 0x0480
-#define PERIOD_120HZ 0x03C0
-#define PERIOD_128HZ 0x0384
-#define PERIOD_144HZ 0x0320
-#define PERIOD_150HZ 0x0300
-#define PERIOD_160HZ 0x02D0
-#define PERIOD_180HZ 0x0280
-#define PERIOD_192HZ 0x0258
-#define PERIOD_200HZ 0x0240
-#define PERIOD_225HZ 0x0200
-#define PERIOD_240HZ 0x01E0
-#define PERIOD_256HZ 0x01C2
-#define PERIOD_288HZ 0x0190
-#define PERIOD_300HZ 0x0180
-#define PERIOD_320HZ 0x0168
-#define PERIOD_360HZ 0x0140
-#define PERIOD_384HZ 0x012C
-#define PERIOD_400HZ 0x0120
-#define PERIOD_450HZ 0x0100
-#define PERIOD_480HZ 0x00F0
-#define PERIOD_512HZ 0x00E1
-
-// OutputModes
-#define OUTPUTMODE_MT9 0x8000
-#define OUTPUTMODE_XM 0x0000
-#define OUTPUTMODE_RAW 0x4000
-#define OUTPUTMODE_TEMP 0x0001
-#define OUTPUTMODE_CALIB 0x0002
-#define OUTPUTMODE_ORIENT 0x0004
-
-// OutputSettings
-#define OUTPUTSETTINGS_XM 0x00000001
-#define OUTPUTSETTINGS_TIMESTAMP_NONE 0x00000000
-#define OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT 0x00000001
-#define OUTPUTSETTINGS_ORIENTMODE_QUATERNION 0x00000000
-#define OUTPUTSETTINGS_ORIENTMODE_EULER 0x00000004
-#define OUTPUTSETTINGS_ORIENTMODE_MATRIX 0x00000008
-#define OUTPUTSETTINGS_CALIBMODE_ACCGYRMAG 0x00000000
-#define OUTPUTSETTINGS_CALIBMODE_ACC 0x00000060
-#define OUTPUTSETTINGS_CALIBMODE_ACCGYR 0x00000040
-#define OUTPUTSETTINGS_CALIBMODE_ACCMAG 0x00000020
-#define OUTPUTSETTINGS_CALIBMODE_GYR 0x00000050
-#define OUTPUTSETTINGS_CALIBMODE_GYRMAG 0x00000010
-#define OUTPUTSETTINGS_CALIBMODE_MAG 0x00000030
-#define OUTPUTSETTINGS_DATAFORMAT_FLOAT 0x00000000
-#define OUTPUTSETTINGS_DATAFORMAT_F1220 0x00000100
-#define OUTPUTSETTINGS_TIMESTAMP_MASK 0x00000003
-#define OUTPUTSETTINGS_ORIENTMODE_MASK 0x0000000C
-#define OUTPUTSETTINGS_CALIBMODE_ACC_MASK 0x00000010
-#define OUTPUTSETTINGS_CALIBMODE_GYR_MASK 0x00000020
-#define OUTPUTSETTINGS_CALIBMODE_MAG_MASK 0x00000040
-#define OUTPUTSETTINGS_CALIBMODE_MASK 0x00000070
-#define OUTPUTSETTINGS_DATAFORMAT_MASK 0x00000300
-
-// Extended Output Modes
-#define EXTOUTPUTMODE_DISABLED 0x0000
-#define EXTOUTPUTMODE_EULER 0x0001
-
-// Factory Output Mode
-#define FACTORYOUTPUTMODE_DISABLE 0x0000
-#define FACTORYOUTPUTMODE_DEFAULT 0x0001
-#define FACTORYOUTPUTMODE_CUSTOM 0x0002
-
-// Initial tracking mode (SetInitTrackMode)
-#define INITTRACKMODE_DISABLED 0x0000
-#define INITTRACKMODE_ENABLED 0x0001
-
-// Filter settings params
-#define PARAM_FILTER_GAIN (const unsigned char)0x00
-#define PARAM_FILTER_RHO (const unsigned char)0x01
-#define DONOTSTORE 0x00
-#define STORE 0x01
-
-// AMDSetting (SetAMD)
-#define AMDSETTING_DISABLED 0x0000
-#define AMDSETTING_ENABLED 0x0001
-
-// Reset orientation message type
-#define RESETORIENTATION_STORE 0
-#define RESETORIENTATION_HEADING 1
-#define RESETORIENTATION_GLOBAL 2
-#define RESETORIENTATION_OBJECT 3
-#define RESETORIENTATION_ALIGN 4
-
-// Send raw string mode
-#define SENDRAWSTRING_INIT 0
-#define SENDRAWSTRING_DEFAULT 1
-#define SENDRAWSTRING_SEND 2
-
-// Timeouts
-#define TO_DEFAULT 500
-#define TO_INIT 250
-#define TO_RETRY 50
-
-// openPort baudrates
+ // Field message indexes
+#define IND_PREAMBLE 0
+#define IND_BID 1
+#define IND_MID 2
+#define IND_LEN 3
+#define IND_DATA0 4
+#define IND_LENEXTH 4
+#define IND_LENEXTL 5
+#define IND_DATAEXT0 6
+
+ // Maximum number of sensors supported
+#define MAXDEVICES 20
+
+#define PREAMBLE (const unsigned char)0xFA
+#define BID_MASTER (const unsigned char)0xFF
+#define BID_MT (const unsigned char)0x01
+#define EXTLENCODE 0xFF
+
+#define LEN_MSGHEADER (const unsigned short)4
+#define LEN_MSGEXTHEADER (const unsigned short)6
+#define LEN_MSGHEADERCS (const unsigned short)5
+#define LEN_MSGEXTHEADERCS (const unsigned short)7
+#define LEN_CHECKSUM (const unsigned short)1
+#define LEN_UNSIGSHORT (const unsigned short)2
+#define LEN_UNSIGINT (const unsigned short)4
+#define LEN_FLOAT (const unsigned short)4
+
+ // Maximum message/data length
+#define MAXDATALEN (const unsigned short)2048
+#define MAXSHORTDATALEN (const unsigned short)254
+#define MAXMSGLEN (const unsigned short)(MAXDATALEN+7)
+#define MAXSHORTMSGLEN (const unsigned short)(MAXSHORTDATALEN+5)
+
+ // DID Type (high nibble)
+#define DID_TYPEH_MASK (const unsigned long)0x00F00000
+#define DID_TYPEH_MT (const unsigned long)0x00000000
+#define DID_TYPEH_XM (const unsigned long)0x00100000
+#define DID_TYPEH_MTI_MTX (const unsigned long)0x00300000
+
+ // All Message identifiers
+ // WakeUp state messages
+#define MID_WAKEUP (const unsigned char)0x3E
+#define MID_WAKEUPACK (const unsigned char)0x3F
+
+ // Config state messages
+#define MID_REQDID (const unsigned char)0x00
+#define MID_DEVICEID (const unsigned char)0x01
+#define LEN_DEVICEID (const unsigned short)4
+#define MID_INITBUS (const unsigned char)0x02
+#define MID_INITBUSRESULTS (const unsigned char)0x03
+#define LEN_INITBUSRESULTS (const unsigned short)4
+#define MID_REQPERIOD (const unsigned char)0x04
+#define MID_REQPERIODACK (const unsigned char)0x05
+#define LEN_PERIOD (const unsigned short)2
+#define MID_SETPERIOD (const unsigned char)0x04
+#define MID_SETPERIODACK (const unsigned char)0x05
+ // XbusMaster
+#define MID_SETBID (const unsigned char)0x06
+#define MID_SETBIDACK (const unsigned char)0x07
+#define MID_AUTOSTART (const unsigned char)0x06
+#define MID_AUTOSTARTACK (const unsigned char)0x07
+#define MID_BUSPWROFF (const unsigned char)0x08
+#define MID_BUSPWROFFACK (const unsigned char)0x09
+ // End XbusMaster
+#define MID_REQDATALENGTH (const unsigned char)0x0A
+#define MID_DATALENGTH (const unsigned char)0x0B
+#define LEN_DATALENGTH (const unsigned short)2
+#define MID_REQCONFIGURATION (const unsigned char)0x0C
+#define MID_CONFIGURATION (const unsigned char)0x0D
+#define LEN_CONFIGURATION (const unsigned short)118
+#define MID_RESTOREFACTORYDEF (const unsigned char)0x0E
+#define MID_RESTOREFACTORYDEFACK (const unsigned char)0x0F
+
+#define MID_GOTOMEASUREMENT (const unsigned char)0x10
+#define MID_GOTOMEASUREMENTACK (const unsigned char)0x11
+#define MID_REQFWREV (const unsigned char)0x12
+#define MID_FIRMWAREREV (const unsigned char)0x13
+#define LEN_FIRMWAREREV (const unsigned short)3
+ // XbusMaster
+#define MID_REQBTDISABLE (const unsigned char)0x14
+#define MID_REQBTDISABLEACK (const unsigned char)0x15
+#define MID_DISABLEBT (const unsigned char)0x14
+#define MID_DISABLEBTACK (const unsigned char)0x15
+#define MID_REQOPMODE (const unsigned char)0x16
+#define MID_REQOPMODEACK (const unsigned char)0x17
+#define MID_SETOPMODE (const unsigned char)0x16
+#define MID_SETOPMODEACK (const unsigned char)0x17
+ // End XbusMaster
+#define MID_REQBAUDRATE (const unsigned char)0x18
+#define MID_REQBAUDRATEACK (const unsigned char)0x19
+#define LEN_BAUDRATE (const unsigned short)1
+#define MID_SETBAUDRATE (const unsigned char)0x18
+#define MID_SETBAUDRATEACK (const unsigned char)0x19
+ // XbusMaster
+#define MID_REQSYNCMODE (const unsigned char)0x1A
+#define MID_REQSYNCMODEACK (const unsigned char)0x1B
+#define MID_SETSYNCMODE (const unsigned char)0x1A
+#define MID_SETSYNCMODEACK (const unsigned char)0x1B
+ // End XbusMaster
+#define MID_REQPRODUCTCODE (const unsigned char)0x1C
+#define MID_PRODUCTCODE (const unsigned char)0x1D
+
+#define MID_REQOUTPUTMODE (const unsigned char)0xD0
+#define MID_REQOUTPUTMODEACK (const unsigned char)0xD1
+#define LEN_OUTPUTMODE (const unsigned short)2
+#define MID_SETOUTPUTMODE (const unsigned char)0xD0
+#define MID_SETOUTPUTMODEACK (const unsigned char)0xD1
+
+#define MID_REQOUTPUTSETTINGS (const unsigned char)0xD2
+#define MID_REQOUTPUTSETTINGSACK (const unsigned char)0xD3
+#define LEN_OUTPUTSETTINGS (const unsigned short)4
+#define MID_SETOUTPUTSETTINGS (const unsigned char)0xD2
+#define MID_SETOUTPUTSETTINGSACK (const unsigned char)0xD3
+
+#define MID_REQOUTPUTSKIPFACTOR (const unsigned char)0xD4
+#define MID_REQOUTPUTSKIPFACTORACK (const unsigned char)0xD5
+#define LEN_OUTPUTSKIPFACTOR (const unsigned short)2
+#define MID_SETOUTPUTSKIPFACTOR (const unsigned char)0xD4
+#define MID_SETOUTPUTSKIPFACTORACK (const unsigned char)0xD5
+
+#define MID_REQSYNCINSETTINGS (const unsigned char)0xD6
+#define MID_REQSYNCINSETTINGSACK (const unsigned char)0xD7
+#define LEN_SYNCINMODE (const unsigned short)2
+#define LEN_SYNCINSKIPFACTOR (const unsigned short)2
+#define LEN_SYNCINOFFSET (const unsigned short)4
+#define MID_SETSYNCINSETTINGS (const unsigned char)0xD6
+#define MID_SETSYNCINSETTINGSACK (const unsigned char)0xD7
+
+#define MID_REQSYNCOUTSETTINGS (const unsigned char)0xD8
+#define MID_REQSYNCOUTSETTINGSACK (const unsigned char)0xD9
+#define LEN_SYNCOUTMODE (const unsigned short)2
+#define LEN_SYNCOUTSKIPFACTOR (const unsigned short)2
+#define LEN_SYNCOUTOFFSET (const unsigned short)4
+#define LEN_SYNCOUTPULSEWIDTH (const unsigned short)4
+#define MID_SETSYNCOUTSETTINGS (const unsigned char)0xD8
+#define MID_SETSYNCOUTSETTINGSACK (const unsigned char)0xD9
+
+#define MID_REQERRORMODE (const unsigned char)0xDA
+#define MID_REQERRORMODEACK (const unsigned char)0xDB
+#define LEN_ERRORMODE (const unsigned short)2
+#define MID_SETERRORMODE (const unsigned char)0xDA
+#define MID_SETERRORMODEACK (const unsigned char)0xDB
+
+#define MID_REQTRANSMITDELAY (const unsigned char)0xDC
+#define MID_REQTRANSMITDELAYACK (const unsigned char)0xDD
+#define LEN_TRANSMITDELAY (const unsigned short)2
+#define MID_SETTRANSMITDELAY (const unsigned char)0xDC
+#define MID_SETTRANSMITDELAYACK (const unsigned char)0xDD
+
+ // Xbus Master
+#define MID_REQXMERRORMODE (const unsigned char)0x82
+#define MID_REQXMERRORMODEACK (const unsigned char)0x83
+#define LEN_XMERRORMODE (const unsigned short)2
+#define MID_SETXMERRORMODE (const unsigned char)0x82
+#define MID_SETXMERRORMODEACK (const unsigned char)0x83
+
+#define MID_REQBUFFERSIZE (const unsigned char)0x84
+#define MID_REQBUFFERSIZEACK (const unsigned char)0x85
+#define LEN_BUFFERSIZE (const unsigned short)2
+#define MID_SETBUFFERSIZE (const unsigned char)0x84
+#define MID_SETBUFFERSIZEACK (const unsigned char)0x85
+ // End Xbus Master
+
+#define MID_REQHEADING (const unsigned char)0x82
+#define MID_REQHEADINGACK (const unsigned char)0x83
+#define LEN_HEADING (const unsigned short)4
+#define MID_SETHEADING (const unsigned char)0x82
+#define MID_SETHEADINGACK (const unsigned char)0x83
+
+#define MID_REQLOCATIONID (const unsigned char)0x84
+#define MID_REQLOCATIONIDACK (const unsigned char)0x85
+#define LEN_LOCATIONID (const unsigned short)2
+#define MID_SETLOCATIONID (const unsigned char)0x84
+#define MID_SETLOCATIONIDACK (const unsigned char)0x85
+
+#define MID_REQEXTOUTPUTMODE (const unsigned char)0x86
+#define MID_REQEXTOUTPUTMODEACK (const unsigned char)0x87
+#define LEN_EXTOUTPUTMODE (const unsigned short)2
+#define MID_SETEXTOUTPUTMODE (const unsigned char)0x86
+#define MID_SETEXTOUTPUTMODEACK (const unsigned char)0x87
+
+ // XbusMaster
+#define MID_REQBATLEVEL (const unsigned char)0x88
+#define MID_BATLEVEL (const unsigned char)0x89
+ // End XbusMaster
+
+#define MID_REQINITTRACKMODE (const unsigned char)0x88
+#define MID_REQINITTRACKMODEACK (const unsigned char)0x89
+#define LEN_INITTRACKMODE (const unsigned short)2
+#define MID_SETINITTRACKMODE (const unsigned char)0x88
+#define MID_SETINITTRACKMODEACK (const unsigned char)0x89
+
+#define MID_STOREFILTERSTATE (const unsigned char)0x8A
+#define MID_STOREFILTERSTATEACK (const unsigned char)0x8B
+
+ // Measurement state
+#define MID_GOTOCONFIG (const unsigned char)0x30
+#define MID_GOTOCONFIGACK (const unsigned char)0x31
+#define MID_BUSDATA (const unsigned char)0x32
+#define MID_MTDATA (const unsigned char)0x32
+
+ // Manual
+#define MID_PREPAREDATA (const unsigned char)0x32
+#define MID_REQDATA (const unsigned char)0x34
+#define MID_REQDATAACK (const unsigned char)0x35
+
+ // MTData defines
+ // Length of data blocks in bytes
+#define LEN_RAWDATA (const unsigned short)20
+#define LEN_CALIBDATA (const unsigned short)36
+#define LEN_CALIB_ACCDATA (const unsigned short)12
+#define LEN_CALIB_GYRDATA (const unsigned short)12
+#define LEN_CALIB_MAGDATA (const unsigned short)12
+#define LEN_ORIENT_QUATDATA (const unsigned short)16
+#define LEN_ORIENT_EULERDATA (const unsigned short)12
+#define LEN_ORIENT_MATRIXDATA (const unsigned short)36
+#define LEN_SAMPLECNT (const unsigned short)2
+#define LEN_TEMPDATA (const unsigned short)4
+
+ // Length of data blocks in floats
+#define LEN_CALIBDATA_FLT (const unsigned short)9
+#define LEN_ORIENT_QUATDATA_FLT (const unsigned short)4
+#define LEN_ORIENT_EULERDATA_FLT (const unsigned short)3
+#define LEN_ORIENT_MATRIXDATA_FLT (const unsigned short)9
+
+ // Indices of fields in DATA field of MTData message in bytes
+ // use in combination with LEN_CALIB etc
+ // Un-calibrated raw data
+#define IND_RAW_ACCX 0
+#define IND_RAW_ACCY 2
+#define IND_RAW_ACCZ 4
+#define IND_RAW_GYRX 6
+#define IND_RAW_GYRY 8
+#define IND_RAW_GYRZ 10
+#define IND_RAW_MAGX 12
+#define IND_RAW_MAGY 14
+#define IND_RAW_MAGZ 16
+#define IND_RAW_TEMP 18
+ // Calibrated data
+#define IND_CALIB_ACCX 0
+#define IND_CALIB_ACCY 4
+#define IND_CALIB_ACCZ 8
+#define IND_CALIB_GYRX 12
+#define IND_CALIB_GYRY 16
+#define IND_CALIB_GYRZ 20
+#define IND_CALIB_MAGX 24
+#define IND_CALIB_MAGY 28
+#define IND_CALIB_MAGZ 32
+ // Orientation data - quat
+#define IND_ORIENT_Q0 0
+#define IND_ORIENT_Q1 4
+#define IND_ORIENT_Q2 8
+#define IND_ORIENT_Q3 12
+ // Orientation data - euler
+#define IND_ORIENT_ROLL 0
+#define IND_ORIENT_PITCH 4
+#define IND_ORIENT_YAW 8
+ // Orientation data - matrix
+#define IND_ORIENT_A 0
+#define IND_ORIENT_B 4
+#define IND_ORIENT_C 8
+#define IND_ORIENT_D 12
+#define IND_ORIENT_E 16
+#define IND_ORIENT_F 20
+#define IND_ORIENT_G 24
+#define IND_ORIENT_H 28
+#define IND_ORIENT_I 32
+ // Orientation data - euler
+#define IND_SAMPLECNTH 0
+#define IND_SAMPLECNTL 1
+
+ // Indices of fields in DATA field of MTData message
+ // Un-calibrated raw data
+#define FLDNUM_RAW_ACCX 0
+#define FLDNUM_RAW_ACCY 1
+#define FLDNUM_RAW_ACCZ 2
+#define FLDNUM_RAW_GYRX 3
+#define FLDNUM_RAW_GYRY 4
+#define FLDNUM_RAW_GYRZ 5
+#define FLDNUM_RAW_MAGX 6
+#define FLDNUM_RAW_MAGY 7
+#define FLDNUM_RAW_MAGZ 8
+#define FLDNUM_RAW_TEMP 9
+ // Calibrated data
+#define FLDNUM_CALIB_ACCX 0
+#define FLDNUM_CALIB_ACCY 1
+#define FLDNUM_CALIB_ACCZ 2
+#define FLDNUM_CALIB_GYRX 3
+#define FLDNUM_CALIB_GYRY 4
+#define FLDNUM_CALIB_GYRZ 5
+#define FLDNUM_CALIB_MAGX 6
+#define FLDNUM_CALIB_MAGY 7
+#define FLDNUM_CALIB_MAGZ 8
+ // Orientation data - quat
+#define FLDNUM_ORIENT_Q0 0
+#define FLDNUM_ORIENT_Q1 1
+#define FLDNUM_ORIENT_Q2 2
+#define FLDNUM_ORIENT_Q3 3
+ // Orientation data - euler
+#define FLDNUM_ORIENT_ROLL 0
+#define FLDNUM_ORIENT_PITCH 1
+#define FLDNUM_ORIENT_YAW 2
+ // Orientation data - matrix
+#define FLDNUM_ORIENT_A 0
+#define FLDNUM_ORIENT_B 1
+#define FLDNUM_ORIENT_C 2
+#define FLDNUM_ORIENT_D 3
+#define FLDNUM_ORIENT_E 4
+#define FLDNUM_ORIENT_F 5
+#define FLDNUM_ORIENT_G 6
+#define FLDNUM_ORIENT_H 7
+#define FLDNUM_ORIENT_I 8
+ // Length
+ // Uncalibrated raw data
+#define LEN_RAW_ACCX 2
+#define LEN_RAW_ACCY 2
+#define LEN_RAW_ACCZ 2
+#define LEN_RAW_GYRX 2
+#define LEN_RAW_GYRY 2
+#define LEN_RAW_GYRZ 2
+#define LEN_RAW_MAGX 2
+#define LEN_RAW_MAGY 2
+#define LEN_RAW_MAGZ 2
+#define LEN_RAW_TEMP 2
+ // Calibrated data
+#define LEN_CALIB_ACCX 4
+#define LEN_CALIB_ACCY 4
+#define LEN_CALIB_ACCZ 4
+#define LEN_CALIB_GYRX 4
+#define LEN_CALIB_GYRY 4
+#define LEN_CALIB_GYRZ 4
+#define LEN_CALIB_MAGX 4
+#define LEN_CALIB_MAGY 4
+#define LEN_CALIB_MAGZ 4
+ // Orientation data - quat
+#define LEN_ORIENT_Q0 4
+#define LEN_ORIENT_Q1 4
+#define LEN_ORIENT_Q2 4
+#define LEN_ORIENT_Q3 4
+ // Orientation data - euler
+#define LEN_ORIENT_ROLL 4
+#define LEN_ORIENT_PITCH 4
+#define LEN_ORIENT_YAW 4
+ // Orientation data - matrix
+#define LEN_ORIENT_A 4
+#define LEN_ORIENT_B 4
+#define LEN_ORIENT_C 4
+#define LEN_ORIENT_D 4
+#define LEN_ORIENT_E 4
+#define LEN_ORIENT_F 4
+#define LEN_ORIENT_G 4
+#define LEN_ORIENT_H 4
+#define LEN_ORIENT_I 4
+
+ // Defines for getDataValue
+#define VALUE_RAW_ACC 0
+#define VALUE_RAW_GYR 1
+#define VALUE_RAW_MAG 2
+#define VALUE_RAW_TEMP 3
+#define VALUE_CALIB_ACC 4
+#define VALUE_CALIB_GYR 5
+#define VALUE_CALIB_MAG 6
+#define VALUE_ORIENT_QUAT 7
+#define VALUE_ORIENT_EULER 8
+#define VALUE_ORIENT_MATRIX 9
+#define VALUE_SAMPLECNT 10
+#define VALUE_TEMP 11
+
+#define INVALIDSETTINGVALUE 0xFFFFFFFF
+
+ // Valid in all states
+#define MID_RESET (const unsigned char)0x40
+#define MID_RESETACK (const unsigned char)0x41
+#define MID_ERROR (const unsigned char)0x42
+#define LEN_ERROR (const unsigned short)1
+ // XbusMaster
+#define MID_XMPWROFF (const unsigned char)0x44
+ // End XbusMaster
+
+#define MID_REQFILTERSETTINGS (const unsigned char)0xA0
+#define MID_REQFILTERSETTINGSACK (const unsigned char)0xA1
+#define LEN_FILTERSETTINGS (const unsigned short)4
+#define MID_SETFILTERSETTINGS (const unsigned char)0xA0
+#define MID_SETFILTERSETTINGSACK (const unsigned char)0xA1
+#define MID_REQAMD (const unsigned char)0xA2
+#define MID_REQAMDACK (const unsigned char)0xA3
+#define LEN_AMD (const unsigned short)2
+#define MID_SETAMD (const unsigned char)0xA2
+#define MID_SETAMDACK (const unsigned char)0xA3
+#define MID_RESETORIENTATION (const unsigned char)0xA4
+#define MID_RESETORIENTATIONACK (const unsigned char)0xA5
+#define LEN_RESETORIENTATION (const unsigned short)2
+
+ // All Messages
+ // WakeUp state messages
+#define MSG_WAKEUPLEN 5
+#define MSG_WAKEUPACK (const unsigned char *)"\xFA\xFF\x3F\x00"
+#define MSG_WAKEUPACKLEN 4
+ // Config state messages
+#define MSG_REQDID (const unsigned char *)"\xFA\xFF\x00\x00"
+#define MSG_REQDIDLEN 4
+#define MSG_DEVICEIDLEN 9
+#define MSG_INITBUS (const unsigned char *)"\xFA\xFF\x02\x00"
+#define MSG_INITBUSLEN 4
+#define MSG_INITBUSRESMAXLEN (5 + 4 * MAXSENSORS)
+#define MSG_REQPERIOD (const unsigned char *)"\xFA\xFF\x04\x00"
+#define MSG_REQPERIODLEN 4
+#define MSG_REQPERIODACKLEN 7
+#define MSG_SETPERIOD (const unsigned char *)"\xFA\xFF\x04\x02"
+#define MSG_SETPERIODLEN 6
+#define MSG_SETPERIODACKLEN 5
+#define MSG_SETBID (const unsigned char *)"\xFA\xFF\x06\x05"
+#define MSG_SETBIDLEN 9
+#define MSG_SETBIDACKLEN 5
+#define MSG_AUTOSTART (const unsigned char *)"\xFA\xFF\x06\x00"
+#define MSG_AUTOSTARTLEN 4
+#define MSG_AUTOSTARTACKLEN 5
+#define MSG_BUSPWROFF (const unsigned char *)"\xFA\xFF\x08\x00"
+#define MSG_BUSPWROFFLEN 4
+#define MSG_BUSPWROFFACKLEN 5
+#define MSG_RESTOREFACTORYDEF (const unsigned char *)"\xFA\xFF\x0E\x00"
+#define MSG_RESTOREFACTORYDEFLEN 4
+#define MSG_RESTOREFACTORYDEFACKLEN 5
+#define MSG_REQDATALENGTH (const unsigned char *)"\xFA\xFF\x0A\x00"
+#define MSG_REQDATALENGTHLEN 4
+#define MSG_DATALENGTHLEN 7
+#define MSG_REQCONFIGURATION (const unsigned char *)"\xFA\xFF\x0C\x00"
+#define MSG_REQCONFIGURATIONLEN 4
+#define MSG_GOTOMEASUREMENT (const unsigned char *)"\xFA\xFF\x10\x00"
+#define MSG_GOTOMEASUREMENTLEN 4
+#define MSG_GOTOMEASMAN (const unsigned char *)"\xFA\x01\x10\x00"
+#define MSG_GOTOMEASMANLEN 4
+#define MSG_GOTOMEASACKLEN 5
+#define MSG_REQFWREV (const unsigned char *)"\xFA\xFF\x12\x00"
+#define MSG_REQFWREVLEN 4
+#define MSG_FIRMWAREREVLEN 8
+#define MSG_REQBTDISABLED (const unsigned char *)"\xFA\xFF\x14\x00"
+#define MSG_REQBTDISABLEDLEN 4
+#define MSG_REQBTDISABLEDACKLEN 6
+#define MSG_DISABLEBT (const unsigned char *)"\xFA\xFF\x14\x01"
+#define MSG_DISABLEBTLEN 5
+#define MSG_DISABLEBTACKLEN 5
+#define MSG_REQOPMODE (const unsigned char *)"\xFA\xFF\x16\x00"
+#define MSG_REQOPMODELEN 4
+#define MSG_REQOPMODEACKLEN 6
+#define MSG_SETOPMODE (const unsigned char *)"\xFA\xFF\x16\x01"
+#define MSG_SETOPMODELEN 5
+#define MSG_SETOPMODEACKLEN 5
+#define MSG_REQBAUDRATE (const unsigned char *)"\xFA\xFF\x18\x00"
+#define MSG_REQBAUDRATELEN 4
+#define MSG_REQBAUDRATEACKLEN 6
+#define MSG_SETBAUDRATE (const unsigned char *)"\xFA\xFF\x18\x01"
+#define MSG_SETBAUDRATELEN 5
+#define MSG_SETBAUDRATEACKLEN 5
+#define MSG_REQSYNCMODE (const unsigned char *)"\xFA\xFF\x1A\x00"
+#define MSG_REQSYNCMODELEN 4
+#define MSG_REQSYNCMODEACKLEN 6
+#define MSG_SETSYNCMODE (const unsigned char *)"\xFA\xFF\x1A\x01"
+#define MSG_SETSYNCMODELEN 5
+#define MSG_SETSYNCMODEACKLEN 6
+#define MSG_REQMTS (const unsigned char *)"\xFA\xFF\x90\x01"
+#define MSG_REQMTSLEN 5
+#define MSG_MTSDATA 61
+#define MSG_STORECUSMTS (const unsigned char *)"\xFA\xFF\x92\x58"
+#define MSG_STORECUSMTSLEN 92
+#define MSG_STORECUSMTSACKLEN 5
+#define MSG_REVTOORGMTS (const unsigned char *)"\xFA\xFF\x94\x00"
+#define MSG_REVTOORGMTSLEN 4
+#define MSG_REVTOORGMTSACKLEN 5
+#define MSG_STOREMTS (const unsigned char *)"\xFA\xFF\x96\x41"
+#define MSG_STOREMTSLEN 69
+#define MSG_STOREMTSACKLEN 5
+#define MSG_REQSYNCOUTMODE (const unsigned char *)"\xFA\xFF\xD8\x01\x00"
+#define MSG_REQSYNCOUTMODELEN 5
+#define MSG_REQSYNCOUTSKIPFACTOR (const unsigned char *)"\xFA\xFF\xD8\x01\x01"
+#define MSG_REQSYNCOUTSKIPFACTORLEN 5
+#define MSG_REQSYNCOUTOFFSET (const unsigned char *)"\xFA\xFF\xD8\x01\x02"
+#define MSG_REQSYNCOUTOFFSETLEN 5
+#define MSG_REQSYNCOUTPULSEWIDTH (const unsigned char *)"\xFA\xFF\xD8\x01\x03"
+#define MSG_REQSYNCOUTPULSEWIDTHLEN 5
+#define MSG_REQERRORMODE (const unsigned char *)"\xFA\xFF\xDA\x00"
+#define MSG_REQERRORMODELEN 4
+#define MSG_REQERRORMODEACKLEN 7
+ // Measurement state - auto messages
+#define MSG_GOTOCONFIG (const unsigned char *)"\xFA\xFF\x30\x00"
+#define MSG_GOTOCONFIGLEN 4
+#define MSG_GOTOCONFIGACKLEN 5
+ // Measurement state - manual messages (Use DID = 0x01)
+#define MSG_GOTOCONFIGM (const unsigned char *)"\xFA\x01\x30\x00"
+#define MSG_GOTOCONFIGMLEN 4
+#define MSG_GOTOCONFIGMACKLEN 5
+#define MSG_PREPAREDATA (const unsigned char *)"\xFA\x01\x32\x00"
+#define MSG_PREPAREDATALEN 4
+#define MSG_REQDATA (const unsigned char *)"\xFA\x01\x34\x00"
+#define MSG_REQDATALEN 4
+ // Valid in all states
+#define MSG_RESET (const unsigned char *)"\xFA\xFF\x40\x00"
+#define MSG_RESETLEN 4
+#define MSG_RESETACKLEN 5
+#define MSG_XMPWROFF (const unsigned char *)"\xFA\xFF\x44\x00"
+#define MSG_XMPWROFFLEN 4
+#define MSG_XMPWROFFACKLEN 5
+
+ // Baudrate defines for SetBaudrate message
+#define BAUDRATE_9K6 0x09
+#define BAUDRATE_14K4 0x08
+#define BAUDRATE_19K2 0x07
+#define BAUDRATE_28K8 0x06
+#define BAUDRATE_38K4 0x05
+#define BAUDRATE_57K6 0x04
+#define BAUDRATE_76K8 0x03
+#define BAUDRATE_115K2 0x02
+#define BAUDRATE_230K4 0x01
+#define BAUDRATE_460K8 0x00
+#define BAUDRATE_921K6 0x80
+
+ // Xbus protocol error codes (Error)
+#define ERROR_NOBUSCOMM 0x01
+#define ERROR_BUSNOTREADY 0x02
+#define ERROR_PERIODINVALID 0x03
+#define ERROR_MESSAGEINVALID 0x04
+#define ERROR_INITOFBUSFAILED1 0x10
+#define ERROR_INITOFBUSFAILED2 0x11
+#define ERROR_INITOFBUSFAILED3 0x12
+#define ERROR_SETBIDPROCFAILED1 0x14
+#define ERROR_SETBIDPROCFAILED2 0x15
+#define ERROR_MEASUREMENTFAILED1 0x18
+#define ERROR_MEASUREMENTFAILED2 0x19
+#define ERROR_MEASUREMENTFAILED3 0x1A
+#define ERROR_MEASUREMENTFAILED4 0x1B
+#define ERROR_MEASUREMENTFAILED5 0x1C
+#define ERROR_MEASUREMENTFAILED6 0x1D
+#define ERROR_TIMEROVERFLOW 0x1E
+#define ERROR_BAUDRATEINVALID 0x20
+#define ERROR_PARAMETERINVALID 0x21
+#define ERROR_MEASUREMENTFAILED7 0x23
+
+ // Error modes (SetErrorMode)
+#define ERRORMODE_IGNORE 0x0000
+#define ERRORMODE_INCSAMPLECNT 0x0001
+#define ERRORMODE_INCSAMPLECNT_SENDERROR 0x0002
+#define ERRORMODE_SENDERROR_GOTOCONFIG 0x0003
+
+ // Configuration message defines
+#define CONF_MASTERDID 0
+#define CONF_PERIOD 4
+#define CONF_OUTPUTSKIPFACTOR 6
+#define CONF_SYNCIN_MODE 8
+#define CONF_SYNCIN_SKIPFACTOR 10
+#define CONF_SYNCIN_OFFSET 12
+#define CONF_DATE 16
+#define CONF_TIME 24
+#define CONF_NUMDEVICES 96
+ // Configuration sensor block properties
+#define CONF_DID 98
+#define CONF_DATALENGTH 102
+#define CONF_OUTPUTMODE 104
+#define CONF_OUTPUTSETTINGS 106
+#define CONF_BLOCKLEN 20
+ // To calculate the offset in data field for output mode of sensor #2 use
+ // CONF_OUTPUTMODE + 1*CONF_BLOCKLEN
+#define CONF_MASTERDIDLEN 4
+#define CONF_PERIODLEN 2
+#define CONF_OUTPUTSKIPFACTORLEN 2
+#define CONF_SYNCIN_MODELEN 2
+#define CONF_SYNCIN_SKIPFACTORLEN 2
+#define CONF_SYNCIN_OFFSETLEN 4
+#define CONF_DATELEN 8
+#define CONF_TIMELEN 8
+#define CONF_RESERVED_CLIENTLEN 32
+#define CONF_RESERVED_HOSTLEN 32
+#define CONF_NUMDEVICESLEN 2
+ // Configuration sensor block properties
+#define CONF_DIDLEN 4
+#define CONF_DATALENGTHLEN 2
+#define CONF_OUTPUTMODELEN 2
+#define CONF_OUTPUTSETTINGSLEN 4
+
+ // Clock frequency for offset & pulse width
+#define SYNC_CLOCKFREQ 29.4912e6
+
+ // SyncIn params
+#define PARAM_SYNCIN_MODE (const unsigned char)0x00
+#define PARAM_SYNCIN_SKIPFACTOR (const unsigned char)0x01
+#define PARAM_SYNCIN_OFFSET (const unsigned char)0x02
+
+ // SyncIn mode
+#define SYNCIN_DISABLED 0x0000
+#define SYNCIN_EDGE_RISING 0x0001
+#define SYNCIN_EDGE_FALLING 0x0002
+#define SYNCIN_EDGE_BOTH 0x0003
+#define SYNCIN_TYPE_SENDLASTDATA 0x0004
+#define SYNCIN_TYPE_DOSAMPLING 0x0000
+#define SYNCIN_EDGE_MASK 0x0003
+#define SYNCIN_TYPE_MASK 0x000C
+
+ // SyncOut params
+#define PARAM_SYNCOUT_MODE (const unsigned char)0x00
+#define PARAM_SYNCOUT_SKIPFACTOR (const unsigned char)0x01
+#define PARAM_SYNCOUT_OFFSET (const unsigned char)0x02
+#define PARAM_SYNCOUT_PULSEWIDTH (const unsigned char)0x03
+
+ // SyncOut mode
+#define SYNCOUT_DISABLED 0x0000
+#define SYNCOUT_TYPE_TOGGLE 0x0001
+#define SYNCOUT_TYPE_PULSE 0x0002
+#define SYNCOUT_POL_NEG 0x0000
+#define SYNCOUT_POL_POS 0x0010
+#define SYNCOUT_TYPE_MASK 0x000F
+#define SYNCOUT_POL_MASK 0x0010
+
+ // Sample frequencies (SetPeriod)
+#define PERIOD_10HZ 0x2D00
+#define PERIOD_12HZ 0x2580
+#define PERIOD_15HZ 0x1E00
+#define PERIOD_16HZ 0x1C20
+#define PERIOD_18HZ 0x1900
+#define PERIOD_20HZ 0x1680
+#define PERIOD_24HZ 0x12C0
+#define PERIOD_25HZ 0x1200
+#define PERIOD_30HZ 0x0F00
+#define PERIOD_32HZ 0x0E10
+#define PERIOD_36HZ 0x0C80
+#define PERIOD_40HZ 0x0B40
+#define PERIOD_45HZ 0x0A00
+#define PERIOD_48HZ 0x0960
+#define PERIOD_50HZ 0x0900
+#define PERIOD_60HZ 0x0780
+#define PERIOD_64HZ 0x0708
+#define PERIOD_72HZ 0x0640
+#define PERIOD_75HZ 0x0600
+#define PERIOD_80HZ 0x05A0
+#define PERIOD_90HZ 0x0500
+#define PERIOD_96HZ 0x04B0
+#define PERIOD_100HZ 0x0480
+#define PERIOD_120HZ 0x03C0
+#define PERIOD_128HZ 0x0384
+#define PERIOD_144HZ 0x0320
+#define PERIOD_150HZ 0x0300
+#define PERIOD_160HZ 0x02D0
+#define PERIOD_180HZ 0x0280
+#define PERIOD_192HZ 0x0258
+#define PERIOD_200HZ 0x0240
+#define PERIOD_225HZ 0x0200
+#define PERIOD_240HZ 0x01E0
+#define PERIOD_256HZ 0x01C2
+#define PERIOD_288HZ 0x0190
+#define PERIOD_300HZ 0x0180
+#define PERIOD_320HZ 0x0168
+#define PERIOD_360HZ 0x0140
+#define PERIOD_384HZ 0x012C
+#define PERIOD_400HZ 0x0120
+#define PERIOD_450HZ 0x0100
+#define PERIOD_480HZ 0x00F0
+#define PERIOD_512HZ 0x00E1
+
+ // OutputModes
+#define OUTPUTMODE_MT9 0x8000
+#define OUTPUTMODE_XM 0x0000
+#define OUTPUTMODE_RAW 0x4000
+#define OUTPUTMODE_TEMP 0x0001
+#define OUTPUTMODE_CALIB 0x0002
+#define OUTPUTMODE_ORIENT 0x0004
+
+ // OutputSettings
+#define OUTPUTSETTINGS_XM 0x00000001
+#define OUTPUTSETTINGS_TIMESTAMP_NONE 0x00000000
+#define OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT 0x00000001
+#define OUTPUTSETTINGS_ORIENTMODE_QUATERNION 0x00000000
+#define OUTPUTSETTINGS_ORIENTMODE_EULER 0x00000004
+#define OUTPUTSETTINGS_ORIENTMODE_MATRIX 0x00000008
+#define OUTPUTSETTINGS_CALIBMODE_ACCGYRMAG 0x00000000
+#define OUTPUTSETTINGS_CALIBMODE_ACC 0x00000060
+#define OUTPUTSETTINGS_CALIBMODE_ACCGYR 0x00000040
+#define OUTPUTSETTINGS_CALIBMODE_ACCMAG 0x00000020
+#define OUTPUTSETTINGS_CALIBMODE_GYR 0x00000050
+#define OUTPUTSETTINGS_CALIBMODE_GYRMAG 0x00000010
+#define OUTPUTSETTINGS_CALIBMODE_MAG 0x00000030
+#define OUTPUTSETTINGS_DATAFORMAT_FLOAT 0x00000000
+#define OUTPUTSETTINGS_DATAFORMAT_F1220 0x00000100
+#define OUTPUTSETTINGS_TIMESTAMP_MASK 0x00000003
+#define OUTPUTSETTINGS_ORIENTMODE_MASK 0x0000000C
+#define OUTPUTSETTINGS_CALIBMODE_ACC_MASK 0x00000010
+#define OUTPUTSETTINGS_CALIBMODE_GYR_MASK 0x00000020
+#define OUTPUTSETTINGS_CALIBMODE_MAG_MASK 0x00000040
+#define OUTPUTSETTINGS_CALIBMODE_MASK 0x00000070
+#define OUTPUTSETTINGS_DATAFORMAT_MASK 0x00000300
+
+ // Extended Output Modes
+#define EXTOUTPUTMODE_DISABLED 0x0000
+#define EXTOUTPUTMODE_EULER 0x0001
+
+ // Factory Output Mode
+#define FACTORYOUTPUTMODE_DISABLE 0x0000
+#define FACTORYOUTPUTMODE_DEFAULT 0x0001
+#define FACTORYOUTPUTMODE_CUSTOM 0x0002
+
+ // Initial tracking mode (SetInitTrackMode)
+#define INITTRACKMODE_DISABLED 0x0000
+#define INITTRACKMODE_ENABLED 0x0001
+
+ // Filter settings params
+#define PARAM_FILTER_GAIN (const unsigned char)0x00
+#define PARAM_FILTER_RHO (const unsigned char)0x01
+#define DONOTSTORE 0x00
+#define STORE 0x01
+
+ // AMDSetting (SetAMD)
+#define AMDSETTING_DISABLED 0x0000
+#define AMDSETTING_ENABLED 0x0001
+
+ // Reset orientation message type
+#define RESETORIENTATION_STORE 0
+#define RESETORIENTATION_HEADING 1
+#define RESETORIENTATION_GLOBAL 2
+#define RESETORIENTATION_OBJECT 3
+#define RESETORIENTATION_ALIGN 4
+
+ // Send raw string mode
+#define SENDRAWSTRING_INIT 0
+#define SENDRAWSTRING_DEFAULT 1
+#define SENDRAWSTRING_SEND 2
+
+ // Timeouts
+#define TO_DEFAULT 500
+#define TO_INIT 250
+#define TO_RETRY 50
+
+ // openPort baudrates
#ifdef WIN32
-#define PBR_9600 CBR_9600
-#define PBR_14K4 CBR_14400
-#define PBR_19K2 CBR_19200
-#define PBR_28K8 28800
-#define PBR_38K4 CBR_38400
-#define PBR_57K6 CBR_57600
-#define PBR_76K8 76800
-#define PBR_115K2 CBR_115200
-#define PBR_230K4 230400
-#define PBR_460K8 460800
-#define PBR_921K6 921600
+#define PBR_9600 CBR_9600
+#define PBR_14K4 CBR_14400
+#define PBR_19K2 CBR_19200
+#define PBR_28K8 28800
+#define PBR_38K4 CBR_38400
+#define PBR_57K6 CBR_57600
+#define PBR_76K8 76800
+#define PBR_115K2 CBR_115200
+#define PBR_230K4 230400
+#define PBR_460K8 460800
+#define PBR_921K6 921600
#else
-#define PBR_9600 B9600
-#define PBR_14K4 B14400
-#define PBR_19K2 B19200
-#define PBR_28K8 B28800
-#define PBR_38K4 B38400
-#define PBR_57K6 B57600
-#define PBR_76K8 B76800
-#define PBR_115K2 B115200
-#define PBR_230K4 B230400
-#define PBR_460K8 B460800
-#define PBR_921K6 B921600
+#define PBR_9600 B9600
+#define PBR_14K4 B14400
+#define PBR_19K2 B19200
+#define PBR_28K8 B28800
+#define PBR_38K4 B38400
+#define PBR_57K6 B57600
+#define PBR_76K8 B76800
+#define PBR_115K2 B115200
+#define PBR_230K4 B230400
+#define PBR_460K8 B460800
+#define PBR_921K6 B921600
#endif
-// setFilePos defines
+ // setFilePos defines
#ifdef WIN32
-#define FILEPOS_BEGIN FILE_BEGIN
-#define FILEPOS_CURRENT FILE_CURRENT
-#define FILEPOS_END FILE_END
+#define FILEPOS_BEGIN FILE_BEGIN
+#define FILEPOS_CURRENT FILE_CURRENT
+#define FILEPOS_END FILE_END
#else
-#define FILEPOS_BEGIN SEEK_SET
-#define FILEPOS_CURRENT SEEK_CUR
-#define FILEPOS_END SEEK_END
+#define FILEPOS_BEGIN SEEK_SET
+#define FILEPOS_CURRENT SEEK_CUR
+#define FILEPOS_END SEEK_END
#endif
-// Return values
-#define MTRV_OK 0 // Operation successful
-#define MTRV_NOTSUCCESSFUL 1 // General no success return value
-#define MTRV_TIMEOUT 2 // Operation aborted because of a timeout
-#define MTRV_TIMEOUTNODATA 3 // Operation aborted because of no data read
-#define MTRV_CHECKSUMFAULT 4 // Checksum fault occured
-#define MTRV_NODATA 5 // No data is received
-#define MTRV_RECVERRORMSG 6 // A error message is received
-#define MTRV_OUTOFMEMORY 7 // No internal memory available
-#define MTRV_UNKNOWDATA 8 // An invalid message is read
-#define MTRV_INVALIDTIMEOUT 9 // An invalid value is used to set the timeout
-#define MTRV_UNEXPECTEDMSG 10 // Unexpected message received (e.g. no acknowledge message received)
-#define MTRV_INPUTCANNOTBEOPENED 11 // The specified file / serial port can not be opened
-#define MTRV_ANINPUTALREADYOPEN 12 // File and serial port can not be opened at same time
-#define MTRV_ENDOFFILE 13 // End of file is reached
-#define MTRV_NOINPUTINITIALIZED 14 // No file or serial port opened for reading/writing
-#define MTRV_NOVALIDMODESPECIFIED 15 // No valid outputmode or outputsettings are specified (use
-// mtGetDeviceMode or mtSetMode)
-#define MTRV_INVALIDVALUESPEC 16 // Value specifier does not match value type or not available in data
-#define MTRV_INVALIDFORFILEINPUT 17 // Function is not valid for file based interfaces
-
- class CXsensMTiModule
- {
- public:
- CXsensMTiModule();
- virtual ~CXsensMTiModule();
-
- // Low level general functions
- clock_t clockms();
-
- // Low level COM port / file functions
- short openPort(const int portNumber, const unsigned long baudrate = PBR_115K2, const unsigned long inqueueSize = 4096, const unsigned long outqueueSize = 1024);
- short openPort(const char *portName, const unsigned long baudrate = PBR_115K2, const unsigned long inqueueSize = 4096, const unsigned long outqueueSize = 1024);
- short openFile(const char *fileName, bool createAlways = false);
- bool isPortOpen();
- bool isFileOpen();
- int readData(unsigned char* msgBuffer, const int nBytesToRead);
- int writeData(const unsigned char* msgBuffer, const int nBytesToWrite);
- void flush();
- void escape(unsigned long function);
- void setPortQueueSize(const unsigned long inqueueSize = 4096, const unsigned long outqueueSize = 1024);
- short setFilePos(long relPos, unsigned long moveMethod = FILEPOS_BEGIN);
- short getFileSize(unsigned long &fileSize);
- short close();
-
- // Read & write message functions
- short readMessage(unsigned char &mid, unsigned char data[], short &dataLen, unsigned char *bid = NULL);
- short readDataMessage(unsigned char data[], short &dataLen);
- short readMessageRaw(unsigned char *msgBuffer, short *msgBufferLength);
- short writeMessage(const unsigned char mid, const unsigned long dataValue = 0, const unsigned char dataValueLen = 0, const unsigned char bid = BID_MASTER);
- short writeMessage(const unsigned char mid, const unsigned char data[], const unsigned short &dataLen, const unsigned char bid = BID_MASTER);
- short waitForMessage(const unsigned char mid, unsigned char data[] = NULL, short *dataLen = NULL, unsigned char *bid = NULL);
-
- // Request & set setting functions
- short reqSetting(const unsigned char mid, unsigned long &value, const unsigned char bid = BID_MASTER);
- short reqSetting(const unsigned char mid, const unsigned char param, unsigned long &value, const unsigned char bid = BID_MASTER);
- short reqSetting(const unsigned char mid, float &value, const unsigned char bid = BID_MASTER);
- short reqSetting(const unsigned char mid, const unsigned char param, float &value, const unsigned char bid = BID_MASTER);
- short reqSetting(const unsigned char mid, unsigned char data[], short &dataLen, const unsigned char bid = BID_MASTER);
- short reqSetting(const unsigned char mid, unsigned char dataIn[], short dataInLen, unsigned char dataOut[], short &dataOutLen, const unsigned char bid = BID_MASTER);
- short reqSetting(const unsigned char mid, const unsigned char param, unsigned char data[], short &dataLen, const unsigned char bid = BID_MASTER);
- short setSetting(const unsigned char mid, const unsigned long value, const unsigned short valuelen, const unsigned char bid = BID_MASTER);
- short setSetting(const unsigned char mid, const unsigned char param, const unsigned long value, const unsigned short valuelen, const unsigned char bid = BID_MASTER);
- short setSetting(const unsigned char mid, const float value, const unsigned char bid = BID_MASTER);
- short setSetting(const unsigned char mid, const unsigned char param, const float value, const unsigned char bid = BID_MASTER);
- short setSetting(const unsigned char mid, const unsigned char param, const float value, const bool store, const unsigned char bid = BID_MASTER);
- // Data-related functions
- short getDeviceMode(unsigned short *numDevices = NULL);
- short setDeviceMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid = BID_MASTER);
- short getMode(unsigned long &OutputMode, unsigned long &OutputSettings, unsigned short &dataLength, const unsigned char bid = BID_MASTER);
- short setMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid = BID_MASTER);
- short getValue(const unsigned long valueSpec, unsigned short &value, const unsigned char data[], const unsigned char bid = BID_MT);
- short getValue(const unsigned long valueSpec, unsigned short value[], const unsigned char data[], const unsigned char bid = BID_MT);
- short getValue(const unsigned long valueSpec, float value[], const unsigned char data[], const unsigned char bid = BID_MT);
-
- // Generic MTComm functions
- short getLastDeviceError();
- short getLastRetVal();
- short setTimeOut(short timeOutMs);
- static void swapEndian(const unsigned char input[], unsigned char output[], const int length);
- void calcChecksum(unsigned char *msgBuffer, const int msgBufferLength);
- bool checkChecksum(const unsigned char *msgBuffer, const int msgBufferLength);
- protected:
- // member variables
+ // Return values
+#define MTRV_OK 0 // Operation successful
+#define MTRV_NOTSUCCESSFUL 1 // General no success return value
+#define MTRV_TIMEOUT 2 // Operation aborted because of a timeout
+#define MTRV_TIMEOUTNODATA 3 // Operation aborted because of no data read
+#define MTRV_CHECKSUMFAULT 4 // Checksum fault occured
+#define MTRV_NODATA 5 // No data is received
+#define MTRV_RECVERRORMSG 6 // A error message is received
+#define MTRV_OUTOFMEMORY 7 // No internal memory available
+#define MTRV_UNKNOWDATA 8 // An invalid message is read
+#define MTRV_INVALIDTIMEOUT 9 // An invalid value is used to set the timeout
+#define MTRV_UNEXPECTEDMSG 10 // Unexpected message received (e.g. no acknowledge message received)
+#define MTRV_INPUTCANNOTBEOPENED 11 // The specified file / serial port can not be opened
+#define MTRV_ANINPUTALREADYOPEN 12 // File and serial port can not be opened at same time
+#define MTRV_ENDOFFILE 13 // End of file is reached
+#define MTRV_NOINPUTINITIALIZED 14 // No file or serial port opened for reading/writing
+#define MTRV_NOVALIDMODESPECIFIED 15 // No valid outputmode or outputsettings are specified (use
+ // mtGetDeviceMode or mtSetMode)
+#define MTRV_INVALIDVALUESPEC 16 // Value specifier does not match value type or not available in data
+#define MTRV_INVALIDFORFILEINPUT 17 // Function is not valid for file based interfaces
+
+ class CXsensMTiModule
+ {
+ public:
+ CXsensMTiModule();
+ virtual ~CXsensMTiModule();
+
+ // Low level general functions
+ clock_t clockms();
+
+ // Low level COM port / file functions
+ short openPort(const int portNumber, const unsigned long baudrate = PBR_115K2, const unsigned long inqueueSize = 4096, const unsigned long outqueueSize = 1024);
+ short openPort(const char* portName, const unsigned long baudrate = PBR_115K2, const unsigned long inqueueSize = 4096, const unsigned long outqueueSize = 1024);
+ short openFile(const char* fileName, bool createAlways = false);
+ bool isPortOpen();
+ bool isFileOpen();
+ int readData(unsigned char* msgBuffer, const int nBytesToRead);
+ int writeData(const unsigned char* msgBuffer, const int nBytesToWrite);
+ void flush();
+ void escape(unsigned long function);
+ void setPortQueueSize(const unsigned long inqueueSize = 4096, const unsigned long outqueueSize = 1024);
+ short setFilePos(long relPos, unsigned long moveMethod = FILEPOS_BEGIN);
+ short getFileSize(unsigned long& fileSize);
+ short close();
+
+ // Read & write message functions
+ short readMessage(unsigned char& mid, unsigned char data[], short& dataLen, unsigned char* bid = NULL);
+ short readDataMessage(unsigned char data[], short& dataLen);
+ short readMessageRaw(unsigned char* msgBuffer, short* msgBufferLength);
+ short writeMessage(const unsigned char mid, const unsigned long dataValue = 0, const unsigned char dataValueLen = 0, const unsigned char bid = BID_MASTER);
+ short writeMessage(const unsigned char mid, const unsigned char data[], const unsigned short& dataLen, const unsigned char bid = BID_MASTER);
+ short waitForMessage(const unsigned char mid, unsigned char data[] = NULL, short* dataLen = NULL, unsigned char* bid = NULL);
+
+ // Request & set setting functions
+ short reqSetting(const unsigned char mid, unsigned long& value, const unsigned char bid = BID_MASTER);
+ short reqSetting(const unsigned char mid, const unsigned char param, unsigned long& value, const unsigned char bid = BID_MASTER);
+ short reqSetting(const unsigned char mid, float& value, const unsigned char bid = BID_MASTER);
+ short reqSetting(const unsigned char mid, const unsigned char param, float& value, const unsigned char bid = BID_MASTER);
+ short reqSetting(const unsigned char mid, unsigned char data[], short& dataLen, const unsigned char bid = BID_MASTER);
+ short reqSetting(const unsigned char mid, unsigned char dataIn[], short dataInLen, unsigned char dataOut[], short& dataOutLen, const unsigned char bid = BID_MASTER);
+ short reqSetting(const unsigned char mid, const unsigned char param, unsigned char data[], short& dataLen, const unsigned char bid = BID_MASTER);
+ short setSetting(const unsigned char mid, const unsigned long value, const unsigned short valuelen, const unsigned char bid = BID_MASTER);
+ short setSetting(const unsigned char mid, const unsigned char param, const unsigned long value, const unsigned short valuelen, const unsigned char bid = BID_MASTER);
+ short setSetting(const unsigned char mid, const float value, const unsigned char bid = BID_MASTER);
+ short setSetting(const unsigned char mid, const unsigned char param, const float value, const unsigned char bid = BID_MASTER);
+ short setSetting(const unsigned char mid, const unsigned char param, const float value, const bool store, const unsigned char bid = BID_MASTER);
+ // Data-related functions
+ short getDeviceMode(unsigned short* numDevices = NULL);
+ short setDeviceMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid = BID_MASTER);
+ short getMode(unsigned long& OutputMode, unsigned long& OutputSettings, unsigned short& dataLength, const unsigned char bid = BID_MASTER);
+ short setMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid = BID_MASTER);
+ short getValue(const unsigned long valueSpec, unsigned short& value, const unsigned char data[], const unsigned char bid = BID_MT);
+ short getValue(const unsigned long valueSpec, unsigned short value[], const unsigned char data[], const unsigned char bid = BID_MT);
+ short getValue(const unsigned long valueSpec, float value[], const unsigned char data[], const unsigned char bid = BID_MT);
+
+ // Generic MTComm functions
+ short getLastDeviceError();
+ short getLastRetVal();
+ short setTimeOut(short timeOutMs);
+ static void swapEndian(const unsigned char input[], unsigned char output[], const int length);
+ void calcChecksum(unsigned char* msgBuffer, const int msgBufferLength);
+ bool checkChecksum(const unsigned char* msgBuffer, const int msgBufferLength);
+ protected:
+ // member variables
#ifdef WIN32
- HANDLE m_handle;
+ HANDLE m_handle;
#else
- int m_handle;
+ int m_handle;
#endif
- bool m_portOpen;
- bool m_fileOpen;
- short m_deviceError;
- short m_retVal;
- short m_timeOut;
- clock_t m_clkEnd;
-
- // OutputMode, OutputSettings & DataLength for connected devices + 1 for master
- unsigned long m_storedOutputMode[MAXDEVICES + 1];
- unsigned long m_storedOutputSettings[MAXDEVICES + 1];
- unsigned long m_storedDataLength[MAXDEVICES + 1];
-
- // Temporary buffer for excess bytes read in ReadMessageRaw
- unsigned char m_tempBuffer[MAXMSGLEN ];
- int m_nTempBufferLen;
-
- private:
- };
- }
+ bool m_portOpen;
+ bool m_fileOpen;
+ short m_deviceError;
+ short m_retVal;
+ short m_timeOut;
+ clock_t m_clkEnd;
+
+ // OutputMode, OutputSettings & DataLength for connected devices + 1 for master
+ unsigned long m_storedOutputMode[MAXDEVICES + 1];
+ unsigned long m_storedOutputSettings[MAXDEVICES + 1];
+ unsigned long m_storedDataLength[MAXDEVICES + 1];
+
+ // Temporary buffer for excess bytes read in ReadMessageRaw
+ unsigned char m_tempBuffer[MAXMSGLEN ];
+ int m_nTempBufferLen;
+
+ private:
+ };
+ }
}
#endif
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/XsensIMU.cpp b/source/RobotAPI/libraries/drivers/XsensIMU/XsensIMU.cpp
index 9bd91962232ea60c553257346676fb54dc032195..6b90025cab677fdd40a37d60f249babbfa37b094 100644
--- a/source/RobotAPI/libraries/drivers/XsensIMU/XsensIMU.cpp
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/XsensIMU.cpp
@@ -35,37 +35,40 @@ PropertyDefinitionsPtr XsensIMU::createPropertyDefinitions()
void XsensIMU::frameAcquisitionTaskLoop()
{
- while(!sensorTask->isStopped()) {
+ while (!sensorTask->isStopped())
+ {
- while(HasPendingEvents()) {
+ while (HasPendingEvents())
+ {
- ProcessPendingEvent();
+ ProcessPendingEvent();
- TimestampVariantPtr now = TimestampVariant::nowPtr();
- IMUData data;
+ TimestampVariantPtr now = TimestampVariant::nowPtr();
+ IMUData data;
- data.acceleration.push_back(m_Accelaration[0]);
- data.acceleration.push_back(m_Accelaration[1]);
- data.acceleration.push_back(m_Accelaration[2]);
+ data.acceleration.push_back(m_Accelaration[0]);
+ data.acceleration.push_back(m_Accelaration[1]);
+ data.acceleration.push_back(m_Accelaration[2]);
- data.gyroscopeRotation.push_back(m_GyroscopeRotation[0]);
- data.gyroscopeRotation.push_back(m_GyroscopeRotation[1]);
- data.gyroscopeRotation.push_back(m_GyroscopeRotation[2]);
+ data.gyroscopeRotation.push_back(m_GyroscopeRotation[0]);
+ data.gyroscopeRotation.push_back(m_GyroscopeRotation[1]);
+ data.gyroscopeRotation.push_back(m_GyroscopeRotation[2]);
- data.magneticRotation.push_back(m_MagneticRotation[0]);
- data.magneticRotation.push_back(m_MagneticRotation[1]);
- data.magneticRotation.push_back(m_MagneticRotation[2]);
+ data.magneticRotation.push_back(m_MagneticRotation[0]);
+ data.magneticRotation.push_back(m_MagneticRotation[1]);
+ data.magneticRotation.push_back(m_MagneticRotation[2]);
- data.orientationQuaternion.push_back(m_OrientationQuaternion[0]);
- data.orientationQuaternion.push_back(m_OrientationQuaternion[1]);
- data.orientationQuaternion.push_back(m_OrientationQuaternion[2]);
- data.orientationQuaternion.push_back(m_OrientationQuaternion[3]);
+ data.orientationQuaternion.push_back(m_OrientationQuaternion[0]);
+ data.orientationQuaternion.push_back(m_OrientationQuaternion[1]);
+ data.orientationQuaternion.push_back(m_OrientationQuaternion[2]);
+ data.orientationQuaternion.push_back(m_OrientationQuaternion[3]);
- IMUTopicPrx->reportSensorValues("device", "name", data, now);
+ IMUTopicPrx->reportSensorValues("device", "name", data, now);
}
+
usleep(10000);
}
}
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/XsensIMU.h b/source/RobotAPI/libraries/drivers/XsensIMU/XsensIMU.h
index d412f84412d890d842c4f1f7496c62763279afcf..abc5540be19d3b43f4044653f7e148d66a73f929 100644
--- a/source/RobotAPI/libraries/drivers/XsensIMU/XsensIMU.h
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/XsensIMU.h
@@ -95,7 +95,7 @@ namespace armarx
void onExitIMU();
- private:
+ private:
RunningTask<XsensIMU>::pointer_type sensorTask;
IMU::CIMUDevice IMUDevice;
diff --git a/source/RobotAPI/statecharts/StatechartProfilesTestGroup/TestState.cpp b/source/RobotAPI/statecharts/StatechartProfilesTestGroup/TestState.cpp
index 63b26f24cdde1f13bd73c874b4a0d7cc1af5a114..2fce6241e256a383313569c5e266fe4afbbc2089 100644
--- a/source/RobotAPI/statecharts/StatechartProfilesTestGroup/TestState.cpp
+++ b/source/RobotAPI/statecharts/StatechartProfilesTestGroup/TestState.cpp
@@ -59,11 +59,11 @@ void TestState::run()
// runs in seperate thread, thus can do complex operations
// should check constantly whether isRunningTaskStopped() returns true
-// uncomment this if you need a continous run function. Make sure to use sleep or use blocking wait to reduce cpu load.
-// while (!isRunningTaskStopped()) // stop run function if returning true
-// {
-// // do your calculations
-// }
+ // uncomment this if you need a continous run function. Make sure to use sleep or use blocking wait to reduce cpu load.
+ // while (!isRunningTaskStopped()) // stop run function if returning true
+ // {
+ // // do your calculations
+ // }
}
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupStatechartContext.generated.h b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupStatechartContext.generated.h
index fa5b65d74818547e072123ad5c082e7c5976a9b3..a9286cbdde6732fe3bff44c5b4bd969176476347 100644
--- a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupStatechartContext.generated.h
+++ b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupStatechartContext.generated.h
@@ -26,11 +26,11 @@ namespace armarx
defineOptionalProperty<std::string>("WeissHapticUnitName", "WeissHapticUnit", "Name of the weiss haptic unit that should be used");
}
}; // class PropertyDefinitions
-
+
private:
HapticUnitObserverInterfacePrx hapticObserver;
WeissHapticUnitInterfacePrx weissHapticUnit;
-
+
public:
std::string getDefaultName() const
{
@@ -46,8 +46,14 @@ namespace armarx
hapticObserver = getProxy<HapticUnitObserverInterfacePrx>(getProperty<std::string>("HapticUnitObserverName").getValue());
weissHapticUnit = getProxy<WeissHapticUnitInterfacePrx>(getProperty<std::string>("WeissHapticUnitName").getValue());
}
- HapticUnitObserverInterfacePrx getHapticObserver() const { return hapticObserver; }
- WeissHapticUnitInterfacePrx getWeissHapticUnit() const { return weissHapticUnit; }
+ HapticUnitObserverInterfacePrx getHapticObserver() const
+ {
+ return hapticObserver;
+ }
+ WeissHapticUnitInterfacePrx getWeissHapticUnit() const
+ {
+ return weissHapticUnit;
+ }
virtual PropertyDefinitionsPtr createPropertyDefinitions()
{
return PropertyDefinitionsPtr(new WeissHapticGroupStatechartContext::PropertyDefinitions(getConfigIdentifier()));
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.cpp b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.cpp
index 769c40d0c1011f8463898270dd12b93fdbcc1492..88a2741e8a690e1188f4a728ae1f93092e2d1040 100644
--- a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.cpp
+++ b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.cpp
@@ -44,14 +44,16 @@ void WeissHapticSensorTest::onEnter()
HapticUnitObserverInterfacePrx hapticObserver = context->getHapticObserver();
ChannelRegistry channels = hapticObserver->getAvailableChannels(false);
std::map<std::string, DatafieldRefPtr> tactileDatafields_MaximumValueMap;
- if(channels.size() == 0)
+
+ if (channels.size() == 0)
{
ARMARX_WARNING << "No tactile pads found";
}
else
{
ARMARX_INFO << "Creating tactile channels";
- for(std::pair<std::string, ChannelRegistryEntry> pair : channels)
+
+ for (std::pair<std::string, ChannelRegistryEntry> pair : channels)
{
std::string tactilePad = pair.first;
DatafieldRefBasePtr matrixDatafield = new DatafieldRef(hapticObserver, tactilePad, "matrix");
@@ -60,6 +62,7 @@ void WeissHapticSensorTest::onEnter()
tactileDatafields_MaximumValueMap.insert(std::make_pair(tactilePad, DatafieldRefPtr::dynamicCast(matrixMax)));
}
}
+
local.setTactileDatafields_MaximumValue(tactileDatafields_MaximumValueMap);
@@ -74,7 +77,8 @@ void WeissHapticSensorTest::run()
std::stringstream ss;
std::stringstream ssNames;
int max = 0;
- for(std::pair<std::string, DatafieldRefPtr> pair : tactileDatafields_MaximumValueMap)
+
+ for (std::pair<std::string, DatafieldRefPtr> pair : tactileDatafields_MaximumValueMap)
{
std::string tactilePad = pair.first;
DatafieldRefPtr matrixMax = pair.second;
@@ -83,6 +87,7 @@ void WeissHapticSensorTest::run()
ssNames << tactilePad << "; ";
max = std::max(max, padMax);
}
+
ARMARX_IMPORTANT << "tactile max value: " << max << "; \n\n" << ss.str() << "\n" << ssNames.str();
usleep(10000); // 100ms
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.generated.h b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.generated.h
index 302e35b1a80f661317bed96394b5fa8e397e94f1..8e2979669ade0da92d0a7f9a409e7617f3e5b4f3 100644
--- a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.generated.h
+++ b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.generated.h
@@ -20,32 +20,32 @@ namespace armarx
class WeissHapticSensorTestIn
{
private:
- WeissHapticSensorTestGeneratedBase<StateType> *parent;
-
+ WeissHapticSensorTestGeneratedBase<StateType>* parent;
+
public:
- WeissHapticSensorTestIn(WeissHapticSensorTestGeneratedBase<StateType> *parent)
+ WeissHapticSensorTestIn(WeissHapticSensorTestGeneratedBase<StateType>* parent)
: parent(parent)
{
}
}; // class WeissHapticSensorTestIn
-
+
class WeissHapticSensorTestLocal
{
private:
- WeissHapticSensorTestGeneratedBase<StateType> *parent;
-
+ WeissHapticSensorTestGeneratedBase<StateType>* parent;
+
public:
- WeissHapticSensorTestLocal(WeissHapticSensorTestGeneratedBase<StateType> *parent)
+ WeissHapticSensorTestLocal(WeissHapticSensorTestGeneratedBase<StateType>* parent)
: parent(parent)
{
}
-
+
public:
std::map<std::string, ::armarx::DatafieldRefPtr> getTactileDatafields_MaximumValue() const
{
return parent->State::getLocal< ::armarx::StringValueMap>("TactileDatafields_MaximumValue")->::armarx::StringValueMap::toStdMap< ::armarx::DatafieldRefPtr>();
}
- void setTactileDatafields_MaximumValue(const std::map<std::string, ::armarx::DatafieldRefPtr> & value) const
+ void setTactileDatafields_MaximumValue(const std::map<std::string, ::armarx::DatafieldRefPtr>& value) const
{
::armarx::StringValueMapPtr container = ::armarx::StringValueMap::FromStdMap< ::armarx::DatafieldRefPtr>(value);
parent->State::setLocal("TactileDatafields_MaximumValue", *container);
@@ -55,24 +55,24 @@ namespace armarx
return parent->State::isLocalParameterSet("TactileDatafields_MaximumValue");
}
}; // class WeissHapticSensorTestLocal
-
+
class WeissHapticSensorTestOut
{
private:
- WeissHapticSensorTestGeneratedBase<StateType> *parent;
-
+ WeissHapticSensorTestGeneratedBase<StateType>* parent;
+
public:
- WeissHapticSensorTestOut(WeissHapticSensorTestGeneratedBase<StateType> *parent)
+ WeissHapticSensorTestOut(WeissHapticSensorTestGeneratedBase<StateType>* parent)
: parent(parent)
{
}
}; // class WeissHapticSensorTestOut
-
+
protected:
const WeissHapticSensorTestIn in;
const WeissHapticSensorTestLocal local;
const WeissHapticSensorTestOut out;
-
+
public:
WeissHapticSensorTestGeneratedBase(const XMLStateConstructorParams& stateData)
: XMLStateTemplate < StateType > (stateData),
@@ -81,7 +81,7 @@ namespace armarx
out(WeissHapticSensorTestOut(this))
{
}
- WeissHapticSensorTestGeneratedBase(const WeissHapticSensorTestGeneratedBase &source)
+ WeissHapticSensorTestGeneratedBase(const WeissHapticSensorTestGeneratedBase& source)
: IceUtil::Shared(source),
armarx::StateIceBase(source),
armarx::StateBase(source),
@@ -93,7 +93,7 @@ namespace armarx
out(WeissHapticSensorTestOut(this))
{
}
-
+
public:
HapticUnitObserverInterfacePrx getHapticObserver() const
{
diff --git a/source/RobotAPI/statecharts/operations/RobotControl.cpp b/source/RobotAPI/statecharts/operations/RobotControl.cpp
index 9f15e6ec637bac582c3bd8f6ecafc9ced80df892..47c178faafd6d44ddb8c597dc59cad5fdb4085c2 100644
--- a/source/RobotAPI/statecharts/operations/RobotControl.cpp
+++ b/source/RobotAPI/statecharts/operations/RobotControl.cpp
@@ -58,7 +58,7 @@ namespace armarx
}
- void RobotControl::hardReset(const Ice::Current &)
+ void RobotControl::hardReset(const Ice::Current&)
{
removeInstance(robotFunctionalState->getLocalUniqueId());
startRobotStatechart();
@@ -73,7 +73,7 @@ namespace armarx
{
getObjectScheduler()->waitForObjectState(eManagedIceObjectStarted, 5000);
stateId = createRemoteStateInstance("RobotControl", NULL, "RobotStatechart", "");
- std::map<int, StateBasePtr> stateList = getChildStatesByName(stateId,"Functional");
+ std::map<int, StateBasePtr> stateList = getChildStatesByName(stateId, "Functional");
ARMARX_INFO << "Functional State Id:" << stateList.begin()->first << flush;
assert(stateList.size() > 0);
int robotFunctionalStateId = stateList.begin()->first;
@@ -83,17 +83,19 @@ namespace armarx
const std::string proxyName = getProperty<std::string>("proxyName").getValue();
const std::string stateName = getProperty<std::string>("stateName").getValue();
ARMARX_IMPORTANT << VAROUT(proxyName) << VAROUT(stateName);
- if(!proxyName.empty() && !stateName.empty())
+
+ if (!proxyName.empty() && !stateName.empty())
{
auto statebase = robotFunctionalState->findSubstateByName("DynamicRemoteState");
ARMARX_CHECK_EXPRESSION(statebase);
DynamicRemoteStatePtr state = DynamicRemoteStatePtr::dynamicCast(statebase);
- if(!state)
+ if (!state)
{
ARMARX_ERROR << "dynamic state pointer null";
return;
}
+
state->getObjectScheduler()->waitForObjectState(eManagedIceObjectStarted, 5000);
EventPtr evt = createEvent<EvLoadScenario>();
evt->eventReceiverName = "toAll";
@@ -139,7 +141,7 @@ namespace armarx
addTransition<EvInit>(stateIdling, stateRobotPreInitialized);
addTransition<EvInitialized>(stateRobotPreInitialized, stateRobotInitialized);
addTransition<EvLoadScenario>(stateRobotInitialized, stateScenarioRunning,
- PM::createMapping()->mapFromEvent("*","*"));
+ PM::createMapping()->mapFromEvent("*", "*"));
addTransition<EvInit>(stateScenarioRunning, stateRobotPreInitialized);
addTransition<Success>(stateScenarioRunning, stateRobotInitialized);
@@ -153,9 +155,9 @@ namespace armarx
void StateRobotControl::onEnter()
{
// install global running conditions for the robot (e.g. temperature < xx°C)
-// KinematicUnitObserverInterfacePrx prx = getContext()->getProxy<KinematicUnitObserverInterfacePrx>("KinematicUnitObserver");
-// installCondition<Failure>(Expression::create()
-// ->add(channels::KinematicUnitObserver::jointTemperatures.getDatafield("KinematicUnitObserver", "NECK_JOINT0"), checks::KinematicUnitObserver::larger->getCheckStr(), 50.f));
+ // KinematicUnitObserverInterfacePrx prx = getContext()->getProxy<KinematicUnitObserverInterfacePrx>("KinematicUnitObserver");
+ // installCondition<Failure>(Expression::create()
+ // ->add(channels::KinematicUnitObserver::jointTemperatures.getDatafield("KinematicUnitObserver", "NECK_JOINT0"), checks::KinematicUnitObserver::larger->getCheckStr(), 50.f));
}
diff --git a/source/RobotAPI/statecharts/operations/RobotControl.h b/source/RobotAPI/statecharts/operations/RobotControl.h
index 897a2b62d575f5bb7dfd7c4f88385aaf7c2ac945..c0d2cb409b03da05d24c8ea87e18ee8d7ee16949 100644
--- a/source/RobotAPI/statecharts/operations/RobotControl.h
+++ b/source/RobotAPI/statecharts/operations/RobotControl.h
@@ -75,7 +75,7 @@ namespace armarx
void onExitRemoteStateOfferer();
void startRobotStatechart();
- void hardReset(const Ice::Current & = ::Ice::Current());
+ void hardReset(const Ice::Current& = ::Ice::Current());
virtual PropertyDefinitionsPtr createPropertyDefinitions();
@@ -105,7 +105,7 @@ namespace armarx
\enddot
*/
struct Statechart_Robot :
- StateTemplate<Statechart_Robot>
+ StateTemplate<Statechart_Robot>
{
void defineState();
void defineSubstates();
@@ -142,7 +142,7 @@ namespace armarx
\enddot
*/
struct StateRobotControl :
- StateTemplate<StateRobotControl>
+ StateTemplate<StateRobotControl>
{
void onEnter();
void defineSubstates();
@@ -152,7 +152,7 @@ namespace armarx
* Robot is in the state preinitialized.
*/
struct RobotPreInitialized :
- StateTemplate<RobotPreInitialized>
+ StateTemplate<RobotPreInitialized>
{
RobotPreInitialized();
void onEnter();