Added target position and target joint values to callback.

source/RobotAPI/components/units/HeadIKUnit.cpp

@@ -52,7 +52,6 @@ namespace armarx
 
         headIKUnitListener = getTopic<armarx::HeadIKUnitListenerPrx>(getProperty<std::string>("HeadIKUnitTopicName").getValue());
 
-
         //std::string robotModelFile;
         //ArmarXDataPath::getAbsolutePath("Armar3/robotmodel/ArmarIII.xml", robotModelFile);
         //localRobot = VirtualRobot::RobotIO::loadRobot(robotModelFile.c_str(), VirtualRobot::RobotIO::eStructure);
@@ -123,6 +122,7 @@ namespace armarx
         this->targetPosition->y = targetPosition->y;
         this->targetPosition->z = targetPosition->z;
         this->targetPosition->frame = targetPosition->frame;
+
         FramedPositionPtr globalTarget = FramedPositionPtr::dynamicCast(targetPosition)->toGlobal(robotStateComponentPrx->getSynchronizedRobot());
 
         if (drawer && getProperty<bool>("VisualizeIKTarget").getValue())
@@ -138,8 +138,6 @@ namespace armarx
         ARMARX_DEBUG << "new Head target set: " << *globalTarget;
 
         newTargetSet = true;
-
-        headIKUnitListener->reportHeadTargetChanged();
     }
 
 
@@ -269,6 +267,7 @@ namespace armarx
                     //                                controlModes[kinematicChain->getNode(i)->getName()] = ePositionControl;
                 }
             }
+            FramedPositionPtr globalPos;
             float error = -1;
             //VirtualRobot::RobotPtr robotSnapshot(new RemoteRobot(remoteRobotPrx));
             //localRobot->setConfig(robotSnapshot->getConfig());
@@ -304,7 +303,8 @@ namespace armarx
                 ikSolver.enableJointLimitAvoidance(true);
                 ikSolver.setup(10, 30, 20);
                 //            ikSolver.setVerbose(true);
-                auto globalPos = targetPosition->toGlobal(localRobot);
+
+                globalPos = targetPosition->toGlobal(localRobot);
                 ARMARX_DEBUG << "Calculating IK for target position " << globalPos->output();
                 auto start = IceUtil::Time::now();
                 bool solutionFound = ikSolver.solve(globalPos->toEigen());
@@ -329,6 +329,8 @@ namespace armarx
                 }
                 else
                 {
+
+
                     foundSolution = true;
                     selectedRobotNodeSetName = robotNodeSetName;
                     break;
@@ -361,6 +363,13 @@ namespace armarx
                 ARMARX_DEBUG << kinematicChain->getNode(i)->getName() << ": " << kinematicChain->getNode(i)->getJointValue();
             }
 
+
+
+            if (headIKUnitListener)
+            {
+                headIKUnitListener->reportHeadTargetChanged(targetJointAngles, globalPos);
+            }
+
             try
             {
                 kinematicUnitPrx->switchControlMode(controlModes);

source/RobotAPI/interface/units/HeadIKUnit.ice

@@ -59,7 +59,7 @@ module armarx
 
     interface HeadIKUnitListener
     {
-        void reportHeadTargetChanged();
+        void reportHeadTargetChanged(NameValueMap targetJointAngles, FramedPositionBase targetPosition);
     };
 
 };