Update to changes in gepetto-viewer-corba

plugins/hppmanipulationwidgetsplugin/hppmanipulationwidgetsplugin.cc

@@ -146,31 +146,30 @@ namespace hpp {
 							  hpp::intSeq_var& indexes,
 							  hpp::floatSeqSeq_var& points,
 							  CORBA::ULong j,
-							  double epsilon)
+							  float epsilon)
     {
       gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
-      const float color[] = {0, 1, 0, 1};
-      QVector3D norm(0, 0, 0);
+      osgVector4 color (0, 1, 0, 1);
+      osgVector3 norm(0, 0, 0);
       CORBA::Long iPts = (j == 0) ? 0 : indexes[j - 1];
-      gepetto::corbaserver::PositionSeq ps; ps.length (indexes[j] - iPts);
-      if (ps.length() > 3) {
-	QVector3D a(points[iPts][0] - points[iPts + 1][0],
-		    points[iPts][1] - points[iPts + 1][1],
-		    points[iPts][2] - points[iPts + 1][2]);
-	QVector3D b(points[iPts + 1][0] - points[iPts + 2][0],
-		    points[iPts + 1][1] - points[iPts + 2][1],
-		    points[iPts + 1][2] - points[iPts + 2][2]);
-	QVector3D c = QVector3D::crossProduct(a, b);
-	if (c.length() > 0.00001 || c.length() < -0.00001)
+      graphics::WindowsManager::Vec3ArrayPtr_t ps;
+      ps->resize (indexes[j] - iPts);
+      if (ps->size() > 3) {
+	osgVector3 a((float)(points[iPts][0] - points[iPts + 1][0]),
+		     (float)(points[iPts][1] - points[iPts + 1][1]),
+		     (float)(points[iPts][2] - points[iPts + 1][2]));
+	osgVector3 b((float)(points[iPts + 1][0] - points[iPts + 2][0]),
+		     (float)(points[iPts + 1][1] - points[iPts + 2][1]),
+		     (float)(points[iPts + 1][2] - points[iPts + 2][2]));
+	osgVector3 c = a ^ b;
+	if (c.length() > 0.00001)
 	  norm = c / c.length();
       }
       for (CORBA::Long k = iPts; k < indexes[j]; ++k) {
-	ps[k - iPts][0] = (float)points[k][0] + norm.x() * epsilon;
-	ps[k - iPts][1] = (float)points[k][1] + norm.y() * epsilon;
-	ps[k - iPts][2] = (float)points[k][2] + norm.z() * epsilon;
+        (*ps)[k - iPts] = osgVector3((float)points[k][0],(float)points[k][1],(float)points[k][2]) + norm * epsilon;
       }
-      main->osg()->addCurve (name.c_str(), ps, color);
-      main->osg()->setCurveMode (name.c_str(), GL_POLYGON);
+      main->osg()->addCurve (name, ps, color);
+      main->osg()->setCurveMode (name, GL_POLYGON);
     }
 
     void HppManipulationWidgetsPlugin::drawRobotContacts()
@@ -221,17 +220,17 @@ namespace hpp {
       gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
       hpp::Names_t_var rcs = hpp_->problem()->getAvailable("handle");
       hpp::Transform__var t (new Transform_);
-      graphics::WindowsManager::value_type t_gv[7];
-      const float color[] = {0, 1, 0, 1};
+      graphics::Configuration config;
+      const graphics::WindowsManager::Color_t color (0, 1, 0, 1);
       for (CORBA::ULong i = 0; i < rcs->length(); ++i) {
         const std::string jn =
           hpp_->robot()->getHandlePositionInJoint (rcs[i],t.out());
-        std::string groupName = createJointGroup (jn.c_str());
+        std::string groupName = createJointGroup (jn);
         std::string hn = "handle_" + escapeJointName (std::string(rcs[i]));
-        fromHPP(t, t_gv);
-        main->osg()->addXYZaxis (hn.c_str(), color, 0.005f, 1.f);
-        main->osg()->applyConfiguration (hn.c_str(), t_gv);
-        main->osg()->addToGroup (hn.c_str(), groupName.c_str());
+        fromHPP(t, config);
+        main->osg()->addXYZaxis (hn, color, 0.005f, 1.f);
+        main->osg()->applyConfiguration (hn, config);
+        main->osg()->addToGroup (hn, groupName);
       }
       main->osg()->refresh();
       gepetto::gui::MainWindow::instance()->requestRefresh();
@@ -242,17 +241,17 @@ namespace hpp {
       gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
       hpp::Names_t_var rcs = hpp_->problem()->getAvailable("gripper");
       hpp::Transform__var t (new Transform_);
-      graphics::WindowsManager::value_type t_gv[7];
-      const float color[] = {0, 1, 0, 1};
+      graphics::Configuration config;
+      const graphics::WindowsManager::Color_t color (0, 1, 0, 1);
       for (CORBA::ULong i = 0; i < rcs->length(); ++i) {
         const std::string jn =
           hpp_->robot()->getGripperPositionInJoint (rcs[i],t.out());
-        std::string groupName = createJointGroup (jn.c_str());
+        std::string groupName = createJointGroup (jn);
         std::string hn = "gripper_" + escapeJointName (std::string(rcs[i]));
-        fromHPP(t, t_gv);
-        main->osg()->addXYZaxis (hn.c_str(), color, 0.005f, 1.f);
-        main->osg()->applyConfiguration (hn.c_str(), t_gv);
-        main->osg()->addToGroup (hn.c_str(), groupName.c_str());
+        fromHPP(t, config);
+        main->osg()->addXYZaxis (hn, color, 0.005f, 1.f);
+        main->osg()->applyConfiguration (hn, config);
+        main->osg()->addToGroup (hn, groupName);
       }
       main->osg()->refresh();
       gepetto::gui::MainWindow::instance()->requestRefresh();

plugins/hppmanipulationwidgetsplugin/hppmanipulationwidgetsplugin.hh

@@ -119,7 +119,7 @@ signals:
       /// \param j index in table indexes
       /// \param epsilon offset
       void drawContactSurface(const std::string& name, hpp::intSeq_var& indexes,
-			      hpp::floatSeqSeq_var& points, CORBA::ULong j, double epsilon = 0.0001);
+			      hpp::floatSeqSeq_var& points, CORBA::ULong j, float epsilon = 0.0001f);
 
         HppManipClient* hpp_;
 

plugins/hppwidgetsplugin/conversions.hh

 #ifndef HPP_GUI_CONVERSIONS_HH
 #define HPP_GUI_CONVERSIONS_HH
 
+#include <gepetto/gui/windows-manager.hh>
+
 namespace hpp {
   namespace gui {
-    template <typename Out>
-    void fromHPP(const hpp::Transform__var& in, Out out[7])
+    inline void fromHPP(const hpp::Transform__var& in, osgVector3 v)
+    {
+      typedef graphics::WindowsManager::value_type type;
+      const hpp::Transform__slice* t (in.in());
+      v.set((type)t[0], (type)t[1], (type)t[2]);
+    }
+
+    inline void fromHPP(const hpp::Transform__var& in, osgQuat q)
     {
-      for (int i = 0; i < 3; ++i) {
-        out[i] = (Out)in.in()[i];
-        out[4+i] = (Out)in.in()[3+i];
-      }
-      out[3] = (Out)in.in()[6];
+      typedef graphics::WindowsManager::value_type type;
+      const hpp::Transform__slice* t (in.in());
+      q.set((type)t[3], (type)t[4], (type)t[5], (type)t[6]);
     }
+
+    inline void fromHPP(const hpp::Transform__var& in, graphics::Configuration c)
+    {
+      fromHPP(in, c.position);
+      fromHPP(in, c.quat);
+    }
+
   } // namespace gui
 } // namespace hpp
 

plugins/hppwidgetsplugin/hppwidgetsplugin.cc

@@ -29,6 +29,8 @@ using CORBA::ULong;
 namespace hpp {
   namespace gui {
     using gepetto::gui::MainWindow;
+    typedef graphics::WindowsManager::Color_t OsgColor_t;
+    typedef graphics::Configuration OsgConfiguration_t;
 
     HppWidgetsPlugin::JointElement::JointElement (
         const std::string& n, const std::string& prefix,
@@ -37,7 +39,7 @@ namespace hpp {
       bodyNames (bns.length()), item (i), updateViewer (bns.length(), updateV)
     {
       for (std::size_t i = 0; i < bns.length(); ++i)
-        bodyNames[i] = std::string(bns[i]);
+        bodyNames[i] = std::string(bns[(CORBA::ULong)i]);
     }
 
     HppWidgetsPlugin::HppWidgetsPlugin() :
@@ -224,7 +226,9 @@ namespace hpp {
                             "interface) and you did not refresh this GUI. "
                             "Use the refresh button \"Tools\" menu.");
       }
-      float T[7];
+      // Something smarter could be done here.
+      // For instance, the joint tree item could know the NodePtr_t of their bodies.
+      OsgConfiguration_t T;
       for (JointMap::iterator ite = jointMap_.begin ();
           ite != jointMap_.end (); ite++) {
         for (std::size_t i = 0; i < ite->bodyNames.size(); ++i)
@@ -233,7 +237,7 @@ namespace hpp {
           fromHPP(t, T);
           if (ite->updateViewer[i]) {
             std::string n = ite->prefix + ite->bodyNames[i];
-            ite->updateViewer[i] = main->osg()->applyConfiguration(n.c_str(), T);
+            ite->updateViewer[i] = main->osg()->applyConfiguration(n, T);
           }
         }
         if (!ite->item) continue;
@@ -245,7 +249,7 @@ namespace hpp {
         std::string n = escapeJointName(*it);
         hpp::Transform__var t = client()->robot()->getJointPosition(it->c_str());
         fromHPP(t, T);
-        main->osg()->applyConfiguration (n.c_str (), T);
+        main->osg()->applyConfiguration (n, T);
       }
       main->osg()->refresh();
     }
@@ -304,7 +308,7 @@ namespace hpp {
         std::string group; group.assign(what[1].first, what[1].second);
         std::string joint; joint.assign(what[2].first, what[2].second);
         std::string type;  type .assign(what[3].first, what[3].second);
-        std::size_t n = std::atoi (what[4].first);
+        CORBA::ULong n = (CORBA::ULong)std::atoi (what[4].first);
         qDebug () << "Detected the" << group.c_str() << type.c_str() << n << "of joint" << joint.c_str();
         if (group == "roadmap") {
           if (type == "node") {
@@ -439,14 +443,14 @@ namespace hpp {
       gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
       std::string target = createJointGroup(jointName);
       const std::string n = target + "/XYZ";
-      const float color[4] = {1,0,0,1};
+      const OsgColor_t color(1,0,0,1);
 
       /// This returns false if the frame already exists
-      if (main->osg()->addXYZaxis (n.c_str(), color, 0.005f, 1.f)) {
-        main->osg()->setVisibility (n.c_str(), "ALWAYS_ON_TOP");
+      if (main->osg()->addXYZaxis (n, color, 0.005f, 1.f)) {
+        main->osg()->setVisibility (n, "ALWAYS_ON_TOP");
         return;
       } else {
-        main->osg()->setVisibility (n.c_str(), "ALWAYS_ON_TOP");
+        main->osg()->setVisibility (n, "ALWAYS_ON_TOP");
       }
     }
 
@@ -455,11 +459,11 @@ namespace hpp {
       gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
       hpp::Names_t_var obs = client()->obstacle()->getObstacleNames (true, false);
       hpp::Transform__var cfg = hpp::Transform__alloc () ;
-      float d[7];
-      for (size_t i = 0; i < obs->length(); ++i) {
-        client()->obstacle()->getObstaclePosition (obs[(ULong) i], cfg.out());
+      OsgConfiguration_t d;
+      for (ULong i = 0; i < obs->length(); ++i) {
+        client()->obstacle()->getObstaclePosition (obs[i], cfg.out());
         fromHPP(cfg, d);
-        main->osg ()->applyConfiguration(obs[(ULong) i], d);
+        main->osg ()->applyConfiguration(std::string(obs[i]), d);
       }
       main->osg()->refresh();
     }
@@ -494,12 +498,11 @@ namespace hpp {
       if (main->osg()->createGroup(target.c_str())) {
         main->osg()->addToGroup(target.c_str(), "joints");
 
-        hpp::Transform__var t = client()->robot()->getJointPosition
-          (jn.c_str());
-        float p[7];
+        hpp::Transform__var t = client()->robot()->getJointPosition (jn.c_str());
+        OsgConfiguration_t p;
         fromHPP(t, p);
         jointFrames_.push_back(jn);
-        main->osg()->applyConfiguration (target.c_str(), p);
+        main->osg()->applyConfiguration (target, p);
         main->osg()->refresh();
       }
       return target;

plugins/hppwidgetsplugin/pathplayer.cc

@@ -58,31 +58,35 @@ namespace hpp {
       int pid = pathIndex()->value();
       std::stringstream ss; ss << "path" << pid << "_" << jointName;
       std::string pn = ss.str();
-      float colorN[] = {0.f, 0.f, 1.f, 1.f};
-      float colorE[] = {1.f, 0.f, 0.f, 1.f};
+      const osgVector4 colorN (0.f, 0.f, 1.f, 1.f),
+                       colorE (1.f, 0.f, 0.f, 1.f);
       gepetto::gui::WindowsManagerPtr_t wsm = main->osg();
       HppWidgetsPlugin::HppClient* hpp = plugin_->client();
       hpp::floatSeqSeq_var waypoints = hpp->problem()->getWaypoints((CORBA::UShort)pid);
-      wsm->createScene (pn.c_str());
+      wsm->createScene (pn);
       hpp::floatSeq_var curCfg = hpp->robot()->getCurrentConfig();
+      graphics::Configuration pos;
+      osgVector3 pos1, pos2;
       for (unsigned int i = 0; i < waypoints->length(); ++i) {
-        float pos[7];
-        float pos1[3], pos2[3];
+        // Make name
+        ss.clear(); ss.str(std::string()); ss << pn << "/node" << i;
+        std::string xyzName = ss.str();
+        // Get positions
         hpp->robot()->setCurrentConfig(waypoints[i]);
         hpp::Transform__var t = hpp->robot()->getLinkPosition(jointName.c_str());
         fromHPP(t, pos);
-        for (int j = 0; j < 3; ++j) { pos1[j] = pos2[j]; }
-        for (int j = 0; j < 3; ++j) { pos2[j] = pos[j]; }
-        QString xyzName = QString::fromStdString(pn).append("/node%1").arg (i);
-        wsm->addXYZaxis(xyzName.toLocal8Bit().data(), colorN, 0.01f, 1.f);
-        wsm->applyConfiguration(xyzName.toLocal8Bit().data(), pos);
+        pos1 = pos2; pos2 = pos.position;
+        // Create the nodes
+        wsm->addXYZaxis(xyzName, colorN, 0.01f, 1.f);
+        wsm->applyConfiguration(xyzName, pos);
         if  (i > 0) {
-          QString lineName = QString::fromStdString(pn).append("/edge%1").arg (i);
-          wsm->addLine(lineName.toLocal8Bit().data(), pos1, pos2, colorE);
+          xyzName.replace(pn.length() + 1, 4, "edge");
+          qDebug () << xyzName.c_str();
+          wsm->addLine(xyzName, pos1, pos2, colorE);
         }
       }
       hpp->robot()->setCurrentConfig(curCfg.in());
-      wsm->addToGroup(pn.c_str(), "hpp-gui");
+      wsm->addToGroup(pn, "hpp-gui");
       wsm->refresh();
     }
 
@@ -110,26 +114,26 @@ namespace hpp {
       CORBA::UShort pid = (CORBA::UShort) pathIndex()->value();
       std::stringstream ss; ss << "curvedpath_" << pid << "_" << jointName;
       std::string pn = ss.str();
-      float colorE[] = {1.f, 0.f, 0.f, 1.f};
+      graphics::WindowsManager::Color_t colorE (1.f, 0.f, 0.f, 1.f);
       gepetto::gui::WindowsManagerPtr_t wsm = main->osg();
       HppWidgetsPlugin::HppClient* hpp = plugin_->client();
       hpp::floatSeq_var curCfg = hpp->robot()->getCurrentConfig();
       CORBA::Double length = hpp->problem()->pathLength(pid);
       double dt = lengthBetweenRefresh();
-      std::size_t nbPos = (std::size_t)(length / dt) + 1;
-      gepetto::corbaserver::PositionSeq_var posSeq = new gepetto::corbaserver::PositionSeq (nbPos);
-      posSeq->length(nbPos);
+      CORBA::ULong nbPos = (CORBA::ULong)(length / dt) + 1;
+      ::osg::Vec3ArrayRefPtr posSeq = new ::osg::Vec3Array;
       float statusStep = 100.f / (float) nbPos;
       emit displayPath_status(0);
-      for (std::size_t i = 0; i < nbPos; ++i) {
+      for (CORBA::ULong i = 0; i < nbPos; ++i) {
         double t = std::min (i * dt, length);
         hpp::floatSeq_var q = hpp->problem()->configAtParam(pid, t);
         hpp->robot()->setCurrentConfig(q);
         hpp::Transform__var transform = hpp->robot()->getLinkPosition(jointName.c_str());
-        for (int j = 0; j < 3; ++j) { posSeq[i][j] = (float)transform.in()[j]; }
+        const hpp::Transform__slice* tt = transform.in();
+        posSeq->push_back(::osg::Vec3 ((float)tt[0],(float)tt[1],(float)tt[2]));
         emit displayPath_status(qFloor ((float)i * statusStep));
       }
-      wsm->addCurve(pn.c_str(), posSeq.in(), colorE);
+      wsm->addCurve(pn, posSeq, colorE);
       hpp->robot()->setCurrentConfig(curCfg.in());
       wsm->addToGroup(pn.c_str(), "hpp-gui");
       wsm->refresh();

plugins/hppwidgetsplugin/roadmap.cc

@@ -58,7 +58,7 @@ namespace hpp {
       if (currentNodeId_ >= nbNodes && currentEdgeId_ >= nbEdges) return;
 
       std::string rn = roadmapName ();
-      float color[4];
+      Color color;
       gepetto::gui::WindowsManagerPtr_t wsm = gepetto::gui::MainWindow::instance()->osg();
       if (nbNodes == 0) {
         gepetto::gui::MainWindow::instance()->logError("There is no node in the roadmap.");
@@ -70,17 +70,17 @@ namespace hpp {
         nodePosition (currentNodeId_, pos);
         nodeColor (currentNodeId_, color);
         std::string name = nodeName (currentNodeId_);
-        wsm->addXYZaxis(name.c_str(), color, radius, axisSize);
-        wsm->applyConfiguration(name.c_str(), pos);
+        wsm->addXYZaxis(name, color, radius, axisSize);
+        wsm->applyConfiguration(name, pos);
       }
       for (; currentEdgeId_ < nbEdges; ++currentEdgeId_) {
         Position pos1, pos2;
         edgePositions (currentEdgeId_, pos1, pos2);
         edgeColor (currentEdgeId_, color);
         std::string name = edgeName (currentEdgeId_);
-        wsm->addLine(name.c_str(), pos1, pos2, color);
+        wsm->addLine(name, pos1, pos2, color);
       }
-      wsm->addToGroup(rn.c_str(), "hpp-gui");
+      wsm->addToGroup(rn, "hpp-gui");
       afterDisplay ();
       wsm->refresh();
     }

plugins/hppwidgetsplugin/roadmap.hh

@@ -3,6 +3,7 @@
 
 #include <hpp/corbaserver/common.hh>
 #include <gepetto/gui/color-map.hh>
+#include <gepetto/gui/windows-manager.hh>
 #include <hppwidgetsplugin/hppwidgetsplugin.hh>
 
 namespace hpp {
@@ -11,9 +12,9 @@ namespace hpp {
       public:
         typedef unsigned int NodeID;
         typedef unsigned int EdgeID;
-        typedef float Frame[7];
-        typedef float Position[3];
-        typedef float Color[4];
+        typedef graphics::Configuration Frame;
+        typedef osgVector3 Position;
+        typedef graphics::WindowsManager::Color_t Color;
 
         float radius, axisSize;