added joint acceleration to KinematicUnitObserver

source/RobotAPI/components/units/KinematicUnitObserver.cpp

@@ -98,7 +98,8 @@ void KinematicUnitObserver::onConnectObserver()
 
     // register all channels
     offerChannel("jointangles", "Joint values of the " + robotNodeSetName + " kinematic chain");
-    offerChannel("jointvelocities", "Joint velocities of the " + robotNodeSetName + "kinematic chain");
+    offerChannel("jointvelocities", "Joint velocities of the " + robotNodeSetName + " kinematic chain");
+    offerChannel("jointaccelerations", "Joint accelerations 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");
@@ -111,11 +112,12 @@ void KinematicUnitObserver::onConnectObserver()
         std::string jointName = (*it)->getName();
         ARMARX_VERBOSE << "* " << jointName << std::endl;
         offerDataFieldWithDefault("jointcontrolmodes", jointName, ControlModeToString(eUnknown), "Controlmode of the " + jointName + " joint");
-        offerDataField("jointangles", jointName, VariantType::Float, "Joint angle of the " + jointName + "  joint in radians");
-        offerDataField("jointvelocities", jointName, VariantType::Float, "Joint velocity of the " + jointName + "  joint");
-        offerDataField("jointtorques", jointName, VariantType::Float, "Joint torque of the " + jointName + "  joint");
-        offerDataField("jointcurrents", jointName, VariantType::Float, "Joint current of the " + jointName + "  joint");
-        offerDataField("jointmotortemperatures", jointName, VariantType::Float, "Joint motor temperature of the " + jointName + "  joint");
+        offerDataField("jointangles", jointName, VariantType::Float, "Joint angle of the " + jointName + " joint in radians");
+        offerDataField("jointvelocities", jointName, VariantType::Float, "Joint velocity of the " + jointName + " joint");
+        offerDataField("jointaccelerations", jointName, VariantType::Float, "Joint acceleration of the " + jointName + " joint");
+        offerDataField("jointtorques", jointName, VariantType::Float, "Joint torque of the " + jointName + " joint");
+        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");
@@ -214,7 +216,22 @@ void KinematicUnitObserver::reportJointTorques(const NameValueMap& jointTorques,
 
 void KinematicUnitObserver::reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
 {
+    try
+    {
+        if (jointAccelerations.size() == 0)
+        {
+            return;
+        }
+
 
+        nameValueMapToDataFields("jointaccelerations", jointAccelerations, timestamp, aValueChanged);
+
+        updateChannel("jointaccelerations");
+    }
+    catch (...)
+    {
+        handleExceptions();
+    }
 }
 
 void KinematicUnitObserver::reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
@@ -279,14 +296,14 @@ void KinematicUnitObserver::nameValueMapToDataFields(const std::string& channelN
         boost::unordered_map< ::std::string, ::armarx::VariantBasePtr> map;
         if (timestamp < 0)
         {
-            for (const auto& it : nameValueMap)
+            for (const auto & it : nameValueMap)
             {
                 map[it.first] = new Variant(it.second);
             }
         }
         else
         {
-            for (const auto& it : nameValueMap)
+            for (const auto & it : nameValueMap)
             {
                 map[it.first] = new TimedVariant(new Variant(it.second), IceUtil::Time::microSeconds(timestamp));
             }