ft sim can now provide multiple sensors

source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp

@@ -32,18 +32,20 @@ using namespace armarx;
 void ForceTorqueUnitSimulation::onInitForceTorqueUnit()
 {
     int intervalMs = getProperty<int>("IntervalMs").getValue();
-    std::string sensorName = getProperty<std::string>("SensorName").getValue();
 
+    sensorNamesList = Split(getProperty<std::string>("SensorNames").getValue(), ",");
+    for (auto & sensor : sensorNamesList)
+    {
+        boost::trim(sensor);
+    }
     //std::vector<std::string> sensorNamesList;
     //boost::split(sensorNamesList, sensorNames, boost::is_any_of(","));
 
-    //for(std::vector<std::string>::iterator s = sensorNamesList.begin(); s != sensorNamesList.end(); s++)
-    //{
-    //    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());
+    for (std::vector<std::string>::iterator s = sensorNamesList.begin(); s != sensorNamesList.end(); s++)
+    {
+        forces[*s] = new armarx::FramedDirection(Eigen::Vector3f(0, 0, 0), *s, getProperty<std::string>("AgentName").getValue());
+        torques[*s] = new armarx::FramedDirection(Eigen::Vector3f(0, 0, 0), *s, getProperty<std::string>("AgentName").getValue());
+    }
 
     ARMARX_VERBOSE << "Starting ForceTorqueUnitSimulation with " << intervalMs << " ms interval";
     simulationTask = new PeriodicTask<ForceTorqueUnitSimulation>(this, &ForceTorqueUnitSimulation::simulationFunction, intervalMs, false, "ForceTorqueUnitSimulation");
@@ -63,7 +65,11 @@ void ForceTorqueUnitSimulation::onExitForceTorqueUnit()
 
 void ForceTorqueUnitSimulation::simulationFunction()
 {
-    listenerPrx->reportSensorValues(getProperty<std::string>("SensorName").getValue(), forces, torques);
+
+    for (auto & sensor : sensorNamesList)
+    {
+        listenerPrx->reportSensorValues(sensor, forces[sensor], torques[sensor]);
+    }
 
     //listenerPrx->reportSensorValues(forces);
     //listenerPrx->reportSensorValues(torques);

source/RobotAPI/components/units/ForceTorqueUnitSimulation.h

@@ -48,8 +48,9 @@ namespace armarx
         ForceTorqueUnitSimulationPropertyDefinitions(std::string prefix) :
             ForceTorqueUnitPropertyDefinitions(prefix)
         {
-            defineRequiredProperty<std::string>("SensorName", "simulated sensor name");
-            defineOptionalProperty<int>("IntervalMs", 10, "The time in milliseconds between two calls to the simulation method.");
+            defineRequiredProperty<std::string>("SensorNames", "simulated sensor name. seperated by comma");
+            defineRequiredProperty<std::string>("AgentName", "name of the robot agent");
+            defineOptionalProperty<int>("IntervalMs", 50, "The time in milliseconds between two calls to the simulation method.");
         }
     };
 
@@ -92,9 +93,9 @@ namespace armarx
         virtual PropertyDefinitionsPtr createPropertyDefinitions();
 
     protected:
-        armarx::FramedDirectionPtr forces;
-        armarx::FramedDirectionPtr torques;
-
+        std::map<std::string, armarx::FramedDirectionPtr> forces;
+        std::map<std::string, armarx::FramedDirectionPtr> torques;
+        Ice::StringSeq sensorNamesList;
         PeriodicTask<ForceTorqueUnitSimulation>::pointer_type simulationTask;
     };
 }