Merge remote-tracking branch 'origin/master' into RobotStateHistory

* origin/master:
  Scaling support for RobotStateComponent (also fixes issues with poses of scaled models in the world)
  fixed statechart profile prefix
  RobotStateComponentName is added as an optional property..It was hard coded
  fixed kinematicunit gui dialog: combobox update was not correctly handled
  fixed ft observer visu update cycle
  optimized force torque observer performance

Conflicts:
	source/RobotAPI/components/RobotState/RobotStateComponent.h

scenarios/tests/StatechartProfilesTestArmar3Simulation/config/RobotControl.cfg

@@ -112,6 +112,6 @@ ArmarX.RobotControlStateOfferer.proxyName = "StatechartProfilesTestGroupRemoteSt
 #  - Case sensitivity:   no
 #  - Required:           no
 # ArmarX.RobotControlStateOfferer.ObjectName = ""
-
+ArmarX.RobotControlStateOfferer.XMLStatechartProfile = "Armar3Simulation"
 
 

scenarios/tests/StatechartProfilesTestArmar3a/config/RobotControl.cfg

@@ -113,5 +113,5 @@ ArmarX.RobotControlStateOfferer.proxyName = "StatechartProfilesTestGroupRemoteSt
 #  - Required:           no
 # ArmarX.RobotControlStateOfferer.ObjectName = ""
 
-
+ArmarX.XMLStateComponent.XMLStatechartProfile = "Armar3a"
 

source/RobotAPI/components/RobotState/RobotStateComponent.cpp

@@ -72,6 +72,14 @@ namespace armarx
         this->_synchronized = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
         _synchronized->setName(getProperty<std::string>("AgentName").getValue());
 
+        robotModelScaling = getProperty<float>("RobotModelScaling").getValue();
+        ARMARX_INFO << "scale factor: " << robotModelScaling;
+        if (robotModelScaling != 1.0f)
+        {
+            ARMARX_INFO << "Scaling robot model with scale factor " << robotModelScaling;
+            _synchronized = _synchronized->clone(_synchronized->getName(), _synchronized->getCollisionChecker(), robotModelScaling);
+        }
+
         if (this->_synchronized)
         {
             ARMARX_VERBOSE << "Loaded robot from file " << robotFile << ". Robot name: " << this->_synchronized->getName();
@@ -105,6 +113,7 @@ namespace armarx
         }
         usingTopic(robotNodeSetName + "State");
 
+
         /*VirtualRobot::RobotNodeSetPtr pns = this->_synchronized->getRobotNodeSet("Platform");
         if (pns)
         {
@@ -319,6 +328,11 @@ namespace armarx
         return relativeRobotFile;
     }
 
+    float RobotStateComponent::getScaling(const Ice::Current&) const
+    {
+        return robotModelScaling;
+    }
+
     std::vector<string> RobotStateComponent::getArmarXPackages(const Current&) const
     {
         std::vector<string> result;

source/RobotAPI/components/RobotState/RobotStateComponent.h

@@ -52,6 +52,7 @@ namespace armarx
             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");
             defineOptionalProperty<int>("HistoryLength", 10000, "Number of entries in the robot state history");
+            defineOptionalProperty<float>("RobotModelScaling", 1.0f, "Scaling of the robot model");
         }
     };
 
@@ -132,6 +133,8 @@ namespace armarx
         void setRobotStateObserver(RobotStateObserverPtr observer);
 
         bool interpolate(long time, RobotStateConfig& config) const;
+
+        float getScaling(const Ice::Current&) const;
     protected:
         /**
          * Load and create a VirtualRobot::Robot instance from the RobotFileName
@@ -171,7 +174,7 @@ namespace armarx
 
         std::string robotNodeSetName;
 
-
+        float robotModelScaling;
     };
 
 }

source/RobotAPI/components/units/ForceTorqueObserver.cpp

@@ -76,17 +76,17 @@ void ForceTorqueObserver::onInitObserver()
     offerConditionCheck("magnitudelarger", new ConditionCheckMagnitudeLarger());
     offerConditionCheck("magnitudesmaller", new ConditionCheckMagnitudeSmaller());
 
-    usingProxy("RobotStateComponent");
+    usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
     offeringTopic("DebugDrawerUpdates");
 }
 
 void ForceTorqueObserver::onConnectObserver()
 {
-
+    robot = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
     debugDrawer = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
     if (getProperty<bool>("VisualizeForce").getValue())
     {
-        visualizerTask = new PeriodicTask<ForceTorqueObserver>(this, &ForceTorqueObserver::visualizerFunction, getProperty<int>("VisualizeForceUpdateFrequency").getValue(), false, "visualizerFunction");
+        visualizerTask = new PeriodicTask<ForceTorqueObserver>(this, &ForceTorqueObserver::visualizerFunction, 1000 / getProperty<int>("VisualizeForceUpdateFrequency").getValue(), false, "visualizerFunction");
         visualizerTask->start();
     }
 
@@ -125,7 +125,7 @@ void ForceTorqueObserver::visualizerFunction()
         float forceFactor = getProperty<float>("ForceVisualizerFactor").getValue();
         RemoteRobot::synchronizeLocalClone(localRobot, robot);
         auto channels = getAvailableChannels(false);
-        for (auto& channel : channels)
+        for (auto & channel : channels)
         {
             if (localRobot->hasRobotNode(channel.first))
             {
@@ -164,38 +164,61 @@ PropertyDefinitionsPtr ForceTorqueObserver::createPropertyDefinitions()
 }
 
 
-void ForceTorqueObserver::offerValue(std::string nodeName, const std::string& type, FramedDirectionBasePtr value, const DataFieldIdentifierPtr& id)
+void ForceTorqueObserver::offerValue(const std::string& nodeName, const std::string& type, const FramedDirectionBasePtr& value, const DataFieldIdentifierPtr& id)
 {
     FramedDirectionPtr vec = FramedDirectionPtr::dynamicCast(value);
 
 
 
-    if (!existsDataField(id->channelName, id->datafieldName))
+
+    try
     {
-        if (!existsChannel(id->channelName))
-        {
-            offerChannel(id->channelName, type + " vectors on specific parts of the robot.");
-        }
-        offerDataFieldWithDefault(id->channelName, id->datafieldName, Variant(value), "3D vector for " + type + " of " + nodeName);
+        setDataFieldFlatCopy(id->channelName, id->datafieldName, new Variant(value));
     }
-    else
+    catch (...)
     {
-        setDataFieldFlatCopy(id->channelName, id->datafieldName, new Variant(value));
+        ARMARX_INFO << "Creating force/torque fields";
+        if (!existsDataField(id->channelName, id->datafieldName))
+        {
+            if (!existsChannel(id->channelName))
+            {
+                offerChannel(id->channelName, type + " vectors on specific parts of the robot.");
+            }
+            offerDataFieldWithDefault(id->channelName, id->datafieldName, Variant(value), "3D vector for " + type + " of " + nodeName);
+        }
     }
 
 
     // pod = plain old data
     std::string pod_channelName = id->channelName + POD_SUFFIX;
 
-    if (!existsChannel(pod_channelName))
+
+
+    try
+    {
+        StringVariantBaseMap values;
+        values[id->datafieldName + "_x"    ] = new Variant(vec->x);
+        values[id->datafieldName + "_y"    ] = new Variant(vec->y);
+        values[id->datafieldName + "_z"    ] = new Variant(vec->z);
+        values[id->datafieldName + "_frame"] = new Variant(vec->frame);
+        setDataFieldsFlatCopy(pod_channelName, values);
+    }
+    catch (...)
     {
-        offerChannel(pod_channelName, id->datafieldName + " on " + nodeName + " as plain old data (pod)");
+        ARMARX_INFO << "Creating force/torque pod fields";
+        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);
     }
 
-    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&)
@@ -223,7 +246,7 @@ void armarx::ForceTorqueObserver::reportSensorValues(const std::string& sensorNo
         updateChannel(id->channelName);
         updateChannel(id->channelName + POD_SUFFIX);
     }
-    catch (std::exception& e)
+    catch (std::exception&)
     {
         ARMARX_ERROR << "Reporting force torque for " << sensorNodeName << " failed! ";
         handleExceptions();

source/RobotAPI/components/units/ForceTorqueObserver.h

@@ -43,9 +43,11 @@ namespace armarx
         {
             defineRequiredProperty<std::string>("ForceTorqueTopicName", "Name of the ForceTorqueUnit Topic");
             defineOptionalProperty<bool>("VisualizeForce", true, "Visualize the force with an arrow in the debug drawer");
-            defineOptionalProperty<int>("VisualizeForceUpdateFrequency", 20, "Frequency with which the force is visualized");
-            defineOptionalProperty<float>("ForceVisualizerFactor", 0.01, "Factor by which the forces are scaled to fit into 0..1 (only for visulization) ");
+            defineOptionalProperty<int>("VisualizeForceUpdateFrequency", 30, "Frequency with which the force is visualized");
+            defineOptionalProperty<float>("ForceVisualizerFactor", 0.01f, "Factor by which the forces are scaled to fit into 0..1 (only for visulization) ");
             defineOptionalProperty<float>("MaxForceArrowLength", 150, "Length of the force visu arrow in mm");
+            defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the RobotStateComponent that should be used");
+
         }
     };
 
@@ -97,7 +99,7 @@ namespace armarx
         DebugDrawerInterfacePrx debugDrawer;
         PeriodicTask<ForceTorqueObserver>::pointer_type visualizerTask;
 
-        void offerValue(std::string nodeName, const std::string& type, FramedDirectionBasePtr value, const DataFieldIdentifierPtr& id);
+        void offerValue(const std::string& nodeName, const std::string& type, const FramedDirectionBasePtr& value, const DataFieldIdentifierPtr& id);
 
         // ForceTorqueUnitObserverInterface interface
     public:

source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp

@@ -49,10 +49,11 @@ KinematicUnitConfigDialog::KinematicUnitConfigDialog(QWidget* parent) :
     connect(this->ui->buttonBox, SIGNAL(accepted()), this, SLOT(verifyConfig()));
     ui->buttonBox->button(QDialogButtonBox::Ok)->setDefault(true);
     proxyFinder = new IceProxyFinder<KinematicUnitInterfacePrx>(this);
-    proxyFinder->setSearchMask("*");
+    proxyFinder->setSearchMask("*Unit");
     ui->gridLayout->addWidget(proxyFinder, 0, 1, 1, 2);
 
     connect(proxyFinder->getUi()->cbProxyName, SIGNAL(currentIndexChanged(int)), this, SLOT(selectionChanged(int)));
+    connect(proxyFinder->getUi()->cbProxyName, SIGNAL(editTextChanged(QString)), this, SLOT(proxyNameChanged(QString)));
 }
 
 KinematicUnitConfigDialog::~KinematicUnitConfigDialog()
@@ -88,20 +89,13 @@ void KinematicUnitConfigDialog::verifyConfig()
     }
 }
 
-void KinematicUnitConfigDialog::selectionChanged(int nr)
+void KinematicUnitConfigDialog::updateSubconfig(std::string kinematicUnitName)
 {
-    ARMARX_LOG << "Selected entry:" << nr;
-    ui->labelTopic->setText("");
-    ui->labelRobotModel->setText("");
-    ui->labelRNS->setText("");
-    if (nr < 0)
-    {
-        return;
-    }
-    std::string kinematicUnitName = proxyFinder->getUi()->cbProxyName->currentText().toStdString();
+
     try
     {
         ARMARX_INFO << "Connecting to KinematicUnitProxy " << kinematicUnitName;
+
         KinematicUnitInterfacePrx kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
         std::string topicName = kinematicUnitInterfacePrx->getReportTopicName();
         std::string robotNodeSetName = kinematicUnitInterfacePrx->getRobotNodeSetName();
@@ -114,7 +108,30 @@ void KinematicUnitConfigDialog::selectionChanged(int nr)
     {
         ARMARX_INFO << "Could not connect to KinematicUnitProxy " << kinematicUnitName;
     }
+}
+
+void KinematicUnitConfigDialog::selectionChanged(int nr)
+{
+    ARMARX_LOG << "Selected entry:" << nr;
+    ui->labelTopic->setText("");
+    ui->labelRobotModel->setText("");
+    ui->labelRNS->setText("");
+    if (nr < 0)
+    {
+        return;
+    }
+    std::string kinematicUnitName = proxyFinder->getUi()->cbProxyName->currentText().toStdString();
+
+    updateSubconfig(kinematicUnitName);
+
+}
 
+void KinematicUnitConfigDialog::proxyNameChanged(QString kinematicUnitName)
+{
+    ui->labelTopic->setText("");
+    ui->labelRobotModel->setText("");
+    ui->labelRNS->setText("");
+    updateSubconfig(kinematicUnitName.toStdString());
 }
 
 

source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.h

@@ -65,6 +65,10 @@ namespace armarx
         void verifyConfig();
 
         void selectionChanged(int nr);
+        void proxyNameChanged(QString);
+    protected slots:
+        void updateSubconfig(std::string kinematicUnitName);
+
     private:
 
         IceProxyFinderBase* proxyFinder;

source/RobotAPI/interface/core/RobotState.ice

@@ -189,6 +189,14 @@ module armarx
        ["cpp:const"]
        idempotent string getRobotName() throws NotInitializedException;
 
+
+       /**
+        * @return The scaling of the robot model represented by this component.
+        *
+        */
+       ["cpp:const"]
+       idempotent float getScaling();
+
        /**
         * @return The name of the robot node set that is represented by this component.
         *

source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp

@@ -327,10 +327,10 @@ namespace armarx
 
     VirtualRobot::RobotPtr RemoteRobot::createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const string& filename)
     {
-        return createLocalClone(robotStatePrx->getSynchronizedRobot(), filename);
+        return createLocalClone(robotStatePrx->getSynchronizedRobot(), filename, robotStatePrx->getScaling());
     }
 
-    RobotPtr RemoteRobot::createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, const string& filename)
+    RobotPtr RemoteRobot::createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, const string& filename, float scaling)
     {
         boost::recursive_mutex::scoped_lock cloneLock(m);
         ARMARX_VERBOSE_S << "Creating local clone of remote robot (filename:" << filename << ")" << endl;
@@ -362,6 +362,12 @@ namespace armarx
                 ARMARX_ERROR_S << "Could not load robot file " << filename << ". Aborting..." << endl;
                 return result;
             }
+
+            if (scaling != 1.0f)
+            {
+                ARMARX_INFO_S << "Scaling robot to " << scaling;
+                result = result->clone(result->getName(), result->getCollisionChecker(), scaling);
+            }
         }
 
         synchronizeLocalClone(result, sharedRobotPrx);

source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h

@@ -184,7 +184,7 @@ namespace armarx
             */
         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(), float scaling = 1.0f);
 
         /*!
                 Use this method to synchronize (i.e. copy the joint values) from the remote robot to the local clone.

source/RobotAPI/statecharts/operations/RobotControl.cpp

@@ -80,7 +80,7 @@ namespace armarx
         robotFunctionalState = stateList.begin()->second;
         callRemoteState(stateId, StringVariantContainerBaseMap());
 
-        const std::string proxyName = getProperty<std::string>("proxyName").getValue();
+        const std::string proxyName = getProperty<std::string>("XMLStatechartProfile").getValue() + getProperty<std::string>("proxyName").getValue();
         const std::string stateName = getProperty<std::string>("stateName").getValue();
         ARMARX_IMPORTANT << VAROUT(proxyName) << VAROUT(stateName);
 

source/RobotAPI/statecharts/operations/RobotControl.h

@@ -38,6 +38,8 @@ namespace armarx
         RobotControlContextProperties(std::string prefix):
             StatechartContextPropertyDefinitions(prefix)
         {
+
+            defineOptionalProperty<std::string>("XMLStatechartProfile", "", "Name of the statechart profile to be used. This is used as prefix to the proxyName. So GraspGroupRemoteStateOfferer will be Armar3aGraspGroupRemoteStateOfferer");
             defineOptionalProperty<std::string>("proxyName", "", "name of the proxy to load");
             defineOptionalProperty<std::string>("stateName", "", "name of the state to load");
         }