From d3d69fb68a8559bb71e33a7ce496c3a96cee8fb1 Mon Sep 17 00:00:00 2001
From: Mathieu Geisert <mgeisert@laas.fr>
Date: Fri, 5 Dec 2014 18:17:38 +0100
Subject: [PATCH] add Colors. add LeafNodeLine.

---
 .../corbaserver/graphicalinterface.idl        |  24 +--
 .../corbaserver/graphicalinterface.idl~       |  55 -------
 src/GraphicalInterface.impl.cpp               | 155 ++++++++++++------
 src/GraphicalInterface.impl.hh                |  25 +--
 src/client-cpp.cc                             |  49 ++++--
 src/client-cpp.hh                             |  22 +--
 6 files changed, 174 insertions(+), 156 deletions(-)
 delete mode 100644 idl/Graphics/corbaserver/graphicalinterface.idl~

diff --git a/idl/Graphics/corbaserver/graphicalinterface.idl b/idl/Graphics/corbaserver/graphicalinterface.idl
index 2c6d9e6..ca15ba8 100644
--- a/idl/Graphics/corbaserver/graphicalinterface.idl
+++ b/idl/Graphics/corbaserver/graphicalinterface.idl
@@ -3,6 +3,8 @@ module Graphics {
 module corbaserver {
 
 typedef double Transform [7];
+typedef double Position [3];
+typedef double Color [4];
 
   /// Corba exception travelling through the Corba channel
   interface Graphicalinterface {
@@ -23,22 +25,24 @@ typedef double Transform [7];
        boolean createSceneWithFloor(in string sceneName) ;
        boolean addSceneToWindow(in string sceneName, in string windowName) ;
 
-       //void addBox(in string boxName, in float boxSize1, in float boxSize2, in float boxSize3, in string colorName) ;
-       boolean addBox(in string boxName, in float boxSize1, in float boxSize2, in float boxSize3) ;
+       boolean addBox(in string boxName, in float boxSize1, in float boxSize2, in float boxSize3, in Color RGBAcolor) ;
+       //boolean addBox(in string boxName, in float boxSize1, in float boxSize2, in float boxSize3) ;
 
-       boolean addCapsule(in string capsuleName, in float radius, in float height) ;
-       //void addCapsule(in string capsuleName, in float radius, in float height, in string colorName) ;
+       //boolean addCapsule(in string capsuleName, in float radius, in float height) ;
+       boolean addCapsule(in string capsuleName, in float radius, in float height, in Color RGBAcolorid) ;
 
        boolean addMesh(in string meshName, in string meshPath) ;
 
-       boolean addCone(in string coneName, in float radius, in float height) ;
-       //void addCone(in string coneName, in float radius, in float height, in string colorName) ;
+       //boolean addCone(in string coneName, in float radius, in float height) ;
+       boolean addCone(in string coneName, in float radius, in float height, in Color RGBAcolor) ;
 
-       boolean addCylinder(in string cylinderName, in float radius, in float height) ;
-       //void addCylinder(in string cylinderName, in float radius, in float height, in string colorName) ;
+       //boolean addCylinder(in string cylinderName, in float radius, in float height) ;
+       boolean addCylinder(in string cylinderName, in float radius, in float height, in Color RGBAcolor) ;
 
-       boolean addSphere(in string sphereName, in float radius) ;
-       //void addSphere(in string sphereName, in float radius, in string colorName) ;
+       //boolean addSphere(in string sphereName, in float radius) ;
+       boolean addSphere(in string sphereName, in float radius, in Color RGBAcolor) ;
+
+       boolean addLine(in string lineName, in Position pos1, in Position pos2, in Color RGBAColor) ;
 
        boolean addURDF(in string urdfName, in string urdfPath, in string urdfPackage) ;
 
diff --git a/idl/Graphics/corbaserver/graphicalinterface.idl~ b/idl/Graphics/corbaserver/graphicalinterface.idl~
deleted file mode 100644
index b63fd43..0000000
--- a/idl/Graphics/corbaserver/graphicalinterface.idl~
+++ /dev/null
@@ -1,55 +0,0 @@
-// file sceneViewer.idl
-module Graphics {
-module corbaserver {
-
-typedef double Transform [7];
-
-  /// Corba exception travelling through the Corba channel
-  interface Graphicalinterface {
-
-       // set refreching rate of windows
-       boolean setRate(in long rate);
-
-       // refresh configurations
-       void refresh();
-
-       boolean createWindow(in string name);
-
-       void getNodeList();
-       void getWindowList();
-       //void createWindow(in string name, in unsigned long x, in unsigned long y, in unsigned long width, in unsigned long height) ;
-
-       boolean createScene(in string sceneName) ;
-       boolean createSceneWithFloor(in string sceneName) ;
-       boolean addSceneToWindow(in string sceneName, in string windowName) ;
-
-       //void addBox(in string boxName, in float boxSize1, in float boxSize2, in float boxSize3, in string colorName) ;
-       boolean addBox(in string boxName, in float boxSize1, in float boxSize2, in float boxSize3) ;
-
-       boolean addCapsule(in string capsuleName, in float radius, in float height) ;
-       //void addCapsule(in string capsuleName, in float radius, in float height, in string colorName) ;
-
-       boolean addMesh(in string meshName, in string meshPath) ;
-
-       boolean addCone(in string coneName, in float radius, in float height) ;
-       //void addCone(in string coneName, in float radius, in float height, in string colorName) ;
-
-       boolean addCylinder(in string cylinderName, in float radius, in float height) ;
-       //void addCylinder(in string cylinderName, in float radius, in float height, in string colorName) ;
-
-       boolean addSphere(in string sphereName, in float radius) ;
-       //void addSphere(in string sphereName, in float radius, in string colorName) ;
-
-       boolean addURDF(in string urdfName, in string urdfPath, in string urdfPackage) ;
-
-       boolean createGroup(in string groupName) ;
-       boolean addToGroup(in string nodeName, in string groupName) ;
-
-       boolean applyConfiguration(in string nodeName, in Transform configuration) ;
-
-       boolean setVisibility(in string nodeName, in string visibilityMode) ;
-       boolean setWireFrameMode(in string nodeName, in string wireFrameMode) ;
-       boolean setLightingMode(in string nodeName, in string lightingMode) ;
-  };
-};
-};
diff --git a/src/GraphicalInterface.impl.cpp b/src/GraphicalInterface.impl.cpp
index 186949b..7e87784 100644
--- a/src/GraphicalInterface.impl.cpp
+++ b/src/GraphicalInterface.impl.cpp
@@ -31,6 +31,11 @@ osgVector4 GraphicalInterface::getColor(const std::string& colorName)
         return osgVector4(0.f, 0.f, 0.f, 1.f);
 }
 
+osgVector4 GraphicalInterface::getColor(const double* colorCorba)
+{
+    return osgVector4(colorCorba[0], colorCorba[1], colorCorba[2], colorCorba[3]);
+}
+
 VisibilityMode GraphicalInterface::getVisibility(const std::string& visibilityName)
 {
     if (visibilityName == "OFF")
@@ -229,17 +234,7 @@ bool GraphicalInterface::addSceneToWindow(const char* sceneNameCorba,const char*
     }
 }
 
-/*void GraphicalInterface::addBox(const char* boxName, const float boxSize1, const float boxSize2, const float boxSize3, const char* colorName)
-
-     {
-         osgVector4 color = getColor(colorName);
-         LeafNodeBoxPtr_t box = LeafNodeBox::create(boxName, osgVector3(boxSize1, boxSize2, boxSize3), color);
-         GraphicalInterface::initParent(boxName, box);
-         addNode(boxName, box);
-     }*/
-
-bool GraphicalInterface::addBox(const char* boxNameCorba, const float boxSize1, const float boxSize2, const float boxSize3)
-
+bool GraphicalInterface::addBox(const char* boxNameCorba, const float boxSize1, const float boxSize2, const float boxSize3, const double* colorCorba)
 {
     std::string boxName(boxNameCorba);
     if (nodes_.find(boxName) != nodes_.end()) {
@@ -247,15 +242,29 @@ bool GraphicalInterface::addBox(const char* boxNameCorba, const float boxSize1,
         return false;
     }
     else {
-        LeafNodeBoxPtr_t box = LeafNodeBox::create(boxName, osgVector3(boxSize1, boxSize2, boxSize3));
+        LeafNodeBoxPtr_t box = LeafNodeBox::create(boxName, osgVector3(boxSize1, boxSize2, boxSize3), getColor(colorCorba));
         GraphicalInterface::initParent(boxName, box);
         addNode(boxName, box);
         return true;
     }
 }
 
-bool GraphicalInterface::addCapsule(const char* capsuleNameCorba, const float radius, const float height)
+/*bool GraphicalInterface::addBox(const char* boxNameCorba, const float boxSize1, const float boxSize2, const float boxSize3)
+{
+    std::string boxName(boxNameCorba);
+    if (nodes_.find(boxName) != nodes_.end()) {
+        std::cout << "You need to chose an other name, \"" << boxName << "\" already exist." << std::endl;
+        return false;
+    }
+    else {
+        LeafNodeBoxPtr_t box = LeafNodeBox::create(boxName, osgVector3(boxSize1, boxSize2, boxSize3));
+        GraphicalInterface::initParent(boxName, box);
+        addNode(boxName, box);
+        return true;
+    }
+}*/
 
+/*bool GraphicalInterface::addCapsule(const char* capsuleNameCorba, const float radius, const float height)
 {
     const std::string capsuleName(capsuleNameCorba);
     if (nodes_.find(capsuleName) != nodes_.end()) {
@@ -268,19 +277,24 @@ bool GraphicalInterface::addCapsule(const char* capsuleNameCorba, const float ra
         addNode(capsuleName, capsule);
         return true;
     }
-}
-
-/*void GraphicalInterface::addCapsule(const char* capsuleName, const float radius, const float height, const char* colorName)
+}*/
 
-     {
-         osgVector4 color = getColor(colorName);
-         LeafNodeCapsulePtr_t capsule = LeafNodeCapsule::create(capsuleName, radius, height, color);
-         GraphicalInterface::initParent(capsuleName, capsule);
-         addNode(capsuleName, capsule);
-     }*/
+bool GraphicalInterface::addCapsule(const char* capsuleNameCorba, const float radius, const float height, const double* colorCorba)
+{
+    const std::string capsuleName(capsuleNameCorba);
+    if (nodes_.find(capsuleName) != nodes_.end()) {
+        std::cout << "You need to chose an other name, \"" << capsuleName << "\" already exist." << std::endl;
+        return false;
+    }
+    else {
+        LeafNodeCapsulePtr_t capsule = LeafNodeCapsule::create(capsuleName, radius, height, getColor(colorCorba));
+        GraphicalInterface::initParent(capsuleName, capsule);
+        addNode(capsuleName, capsule);
+        return true;
+    }
+}
 
 bool GraphicalInterface::addMesh(const char* meshNameCorba, const char* meshPathCorba)
-
 {
     std::string meshName(meshNameCorba);
     std::string meshPath(meshPathCorba);
@@ -301,8 +315,22 @@ bool GraphicalInterface::addMesh(const char* meshNameCorba, const char* meshPath
     }
 }
 
-bool GraphicalInterface::addCone(const char* coneNameCorba, const float radius, const float height)
+/*bool GraphicalInterface::addCone(const char* coneNameCorba, const float radius, const float height)
+{
+    std::string coneName(coneNameCorba);
+    if (nodes_.find(coneName) != nodes_.end()) {
+        std::cout << "You need to chose an other name, \"" << coneName << "\" already exist." << std::endl;
+        return false;
+    }
+    else {
+        LeafNodeConePtr_t cone = LeafNodeCone::create(coneName, radius, height);
+        GraphicalInterface::initParent(coneName, cone);
+        addNode(coneName, cone);
+        return true;
+    }
+}*/
 
+bool GraphicalInterface::addCone(const char* coneNameCorba, const float radius, const float height, const double* colorCorba)
 {
     std::string coneName(coneNameCorba);
     if (nodes_.find(coneName) != nodes_.end()) {
@@ -317,16 +345,7 @@ bool GraphicalInterface::addCone(const char* coneNameCorba, const float radius,
     }
 }
 
-/*void GraphicalInterface::addCone(const char* coneName, const float radius, const float height, const char* colorName)
-
-     {
-         osgVector4 color = getColor(colorName);
-         LeafNodeConePtr_t cone = LeafNodeCone::create(coneName, radius, height, color);
-         GraphicalInterface::initParent(coneName, cone);
-         addNode(coneName, cone);
-     }*/
-
-bool GraphicalInterface::addCylinder(const char* cylinderNameCorba, const float radius, const float height)
+/*bool GraphicalInterface::addCylinder(const char* cylinderNameCorba, const float radius, const float height)
 
 {
     std::string cylinderName(cylinderNameCorba);
@@ -340,20 +359,24 @@ bool GraphicalInterface::addCylinder(const char* cylinderNameCorba, const float
         addNode(cylinderName, cylinder);
         return true;
     }
-}
-
-/*void GraphicalInterface::addCylinder(const char* cylinderName, const float radius, const float height, const char* colorName)
-
-     {
-         osgVector4 color = getColor(colorName);
-         LeafNodeCylinderPtr_t cylinder = LeafNodeCylinder::create(cylinderName, radius, height);
-         GraphicalInterface::initParent(cylinderName, cylinder);
-         addNode(cylinderName, cylinder);
-
-     }*/
+}*/
 
-bool GraphicalInterface::addSphere(const char* sphereNameCorba, const float radius)
+bool GraphicalInterface::addCylinder(const char* cylinderNameCorba, const float radius, const float height, const double* colorCorba)
+{
+    std::string cylinderName(cylinderNameCorba);
+    if (nodes_.find(cylinderName) != nodes_.end()) {
+        std::cout << "You need to chose an other name, \"" << cylinderName << "\" already exist." << std::endl;
+        return false;
+    }
+    else {
+        LeafNodeCylinderPtr_t cylinder = LeafNodeCylinder::create(cylinderName, radius, height, getColor(colorCorba));
+        GraphicalInterface::initParent(cylinderName, cylinder);
+        addNode(cylinderName, cylinder);
+        return true;
+    }
+}
 
+/*bool GraphicalInterface::addSphere(const char* sphereNameCorba, const float radius)
 {
     std::string sphereName(sphereNameCorba);
     if (nodes_.find(sphereName) != nodes_.end()) {
@@ -366,6 +389,38 @@ bool GraphicalInterface::addSphere(const char* sphereNameCorba, const float radi
         addNode(sphereName, sphere);
         return true;
     }
+}*/
+
+bool GraphicalInterface::addSphere(const char* sphereNameCorba, const float radius, const double* colorCorba)
+{
+    std::string sphereName(sphereNameCorba);
+    if (nodes_.find(sphereName) != nodes_.end()) {
+        std::cout << "You need to chose an other name, \"" << sphereName << "\" already exist." << std::endl;
+        return false;
+    }
+    else {
+        LeafNodeSpherePtr_t sphere = LeafNodeSphere::create(sphereName, radius, getColor(colorCorba));
+        GraphicalInterface::initParent(sphereName, sphere);
+        addNode(sphereName, sphere);
+        return true;
+    }
+}
+
+bool GraphicalInterface::addLine(const char* lineNameCorba, const double* posCorba1, const double* posCorba2, const double* colorCorba)
+{
+    std::string lineName(lineNameCorba);
+    if (nodes_.find(lineName) != nodes_.end()) {
+        std::cout << "You need to chose an other name, \"" << lineName << "\" already exist." << std::endl;
+        return false;
+    }
+    else {
+        osgVector3 pos1(posCorba1[0], posCorba1[1], posCorba1[2]);
+        osgVector3 pos2(posCorba2[0], posCorba2[1], posCorba2[2]);
+        LeafNodeLinePtr_t line = LeafNodeLine::create(lineName, pos1, pos2, getColor(colorCorba));
+        GraphicalInterface::initParent(lineName, line);
+        addNode(lineName, line);
+        return true;
+    }
 }
 
 void GraphicalInterface::getNodeList()
@@ -384,14 +439,6 @@ void GraphicalInterface::getWindowList()
     for (std::map<std::string, WindowManagerPtr_t>::iterator it=windowManagers_.begin(); it!=windowManagers_.end(); ++it)
         std::cout << "   " << it->first << std::endl;
 }
-/*void GraphicalInterface::addSphere(const char* sphereName, const float radius, const char* colorName)
-
-     {
-         osgVector4 color = getColor(colorName);
-         LeafNodeSpherePtr_t sphere = LeafNodeSphere::create(sphereName, radius);
-         GraphicalInterface::initParent(sphereName, sphere);
-         addNode(sphereName, sphere);
-     }*/
 
 bool GraphicalInterface::createGroup(const char* groupNameCorba)
 {
diff --git a/src/GraphicalInterface.impl.hh b/src/GraphicalInterface.impl.hh
index 5347215..3b8e837 100644
--- a/src/GraphicalInterface.impl.hh
+++ b/src/GraphicalInterface.impl.hh
@@ -53,6 +53,7 @@ private:
     std::list<NodeConfiguration> newNodeConfigurations_;
 
     static osgVector4 getColor(const std::string& colorName);
+    static osgVector4 getColor(const double* color);
     static std::string parentName(const std::string& name);
     static VisibilityMode getVisibility(const std::string& visibilityName);
     static WireFrameMode getWire(const std::string& wireName);
@@ -79,8 +80,6 @@ public:
     virtual void refresh();
 
     virtual bool createWindow(const char* windowNameCorba);
-
-
     //virtual void createWindow(const char* name, CORBA::ULong x, CORBA::ULong y, CORBA::ULong width, CORBA::ULong height) ;
 
     virtual bool createScene(const char* sceneNameCorba) ;
@@ -88,22 +87,24 @@ public:
     virtual bool addSceneToWindow(const char* sceneNameCorba, const char* windowNameCorba) ;
 
 
-    //virtual void addBox(const char* boxName, float boxSize1, float boxSize2, float boxSize3, const char* colorName);
-    virtual bool addBox(const char* boxNameCorba, float boxSize1, float boxSize2, float boxSize3) ;
+    virtual bool addBox(const char* boxName, float boxSize1, float boxSize2, float boxSize3, const double* color);
+    //virtual bool addBox(const char* boxNameCorba, float boxSize1, float boxSize2, float boxSize3) ;
 
-    virtual bool addCapsule(const char* capsuleNameCorba, float radius, float height) ;
-    //virtual void addCapsule(const char* capsuleName, float radius, float height, const char* colorName);
+    //virtual bool addCapsule(const char* capsuleNameCorba, float radius, float height) ;
+    virtual bool addCapsule(const char* capsuleName, float radius, float height, const double* color);
 
     virtual bool addMesh(const char* meshNameCorba, const char* meshPathCorba) ;
 
-    virtual bool addCone(const char* coneNameCorba, float radius, float height) ;
-    //virtual void addCone(const char* coneName, float radius, float height, const char* colorName);
+    //virtual bool addCone(const char* coneNameCorba, float radius, float height) ;
+    virtual bool addCone(const char* coneName, float radius, float height, const double* color);
+
+    //virtual bool addCylinder(const char* cylinderNameCorba, float radius, float height) ;
+    virtual bool addCylinder(const char* cylinderName, float radius, float height, const double* color);
 
-    virtual bool addCylinder(const char* cylinderNameCorba, float radius, float height) ;
-    //virtual void addCylinder(const char* cylinderName, float radius, float height, const char* colorName);
+    //virtual bool addSphere(const char* sphereNameCorba, float radius) ;
+    virtual bool addSphere(const char* sphereName, float radius, const double* color);
 
-    virtual bool addSphere(const char* sphereNameCorba, float radius) ;
-    //virtual void addSphere(const char* sphereName, float radius, const char* colorName);
+    virtual bool addLine(const char* lineName, const double* pos1, const double* pos2, const double* color);
 
     virtual bool addURDF(const char* urdfNameCorba, const char* urdfPathCorba, const char* urdfPackagePathCorba);
 
diff --git a/src/client-cpp.cc b/src/client-cpp.cc
index 25e10fa..d8b355d 100644
--- a/src/client-cpp.cc
+++ b/src/client-cpp.cc
@@ -23,7 +23,6 @@ void ClientCpp::se3ToCorba(CORBA::Double* corbaPosition, const se3::SE3& se3posi
     corbaPosition[5] = q.z();        
 }
 
-
 ClientCpp::ClientCpp()
 {
     int    argc=0;       // Dummy variables to support following call.
@@ -105,45 +104,65 @@ bool ClientCpp::addSceneToWindow(const char* sceneName, const char* windowName)
    return  manager_->addSceneToWindow(sceneName, windowName);
 }
 
-    //void ClientCpp::addBox(const char* boxName, float boxSize1, float boxSize2, float boxSize3, const char* colorName);
-
-bool ClientCpp::addBox(const char* boxName, float boxSize1, float boxSize2, float boxSize3)
+/*bool ClientCpp::addBox(const char* boxName, float boxSize1, float boxSize2, float boxSize3)
 {
     return manager_->addBox(boxName, boxSize1, boxSize2, boxSize3);
+}*/
+
+bool ClientCpp::addBox(const char* boxName, const float boxSize1, const float boxSize2, const float boxSize3, const double* color)
+{
+    return manager_->addBox(boxName, boxSize1, boxSize2, boxSize3, color);
 }
 
-bool ClientCpp::addCapsule(const char* capsuleName, float radius, float height)
+/*bool ClientCpp::addCapsule(const char* capsuleName, float radius, float height)
 {
     return manager_->addCapsule(capsuleName, radius, height);
-}
+}*/
 
-    //void ClientCpp::addCapsule(const char* capsuleName, float radius, float height, const char* colorName);
+bool ClientCpp::addCapsule(const char* capsuleName, const float radius, const float height, const double* color)
+{
+    return manager_->addCapsule(capsuleName, radius, height, color);
+}
 
 bool ClientCpp::addMesh(const char* meshName, const char* meshPath)
 {
     return manager_->addMesh(meshName, meshPath);
 }
 
-bool ClientCpp::addCone(const char* coneName, float radius, float height)
+/*bool ClientCpp::addCone(const char* coneName, float radius, float height)
 {
     return manager_->addCone(coneName, radius, height);
-}
+}*/
 
-    //void ClientCpp::addCone(const char* coneName, float radius, float height, const char* colorName);
+bool ClientCpp::addCone(const char* coneName, const float radius, const float height, const double* color)
+{
+    return manager_->addCone(coneName, radius, height, color);
+}
 
-bool ClientCpp::addCylinder(const char* cylinderName, float radius, float height)
+/*bool ClientCpp::addCylinder(const char* cylinderName, float radius, float height)
 {
     return manager_->addCylinder(cylinderName, radius, height);
-}
+}*/
 
-    //void ClientCpp::addCylinder(const char* cylinderName, float radius, float height, const char* colorName);
+bool ClientCpp::addCylinder(const char* cylinderName, const float radius, const float height, const double* color)
+{
+    return manager_->addCylinder(cylinderName, radius, height, color);
+}
 
-bool ClientCpp::addSphere(const char* sphereName, float radius)
+/*bool ClientCpp::addSphere(const char* sphereName, float radius)
 {
     return manager_->addSphere(sphereName, radius);
+}*/
+
+bool ClientCpp::addSphere(const char* sphereName, const float radius, const double* color)
+{
+    return manager_->addSphere(sphereName, radius, color);
 }
 
-    //void ClientCpp::addSphere(const char* sphereName, float radius, const char* colorName);
+bool ClientCpp::addLine(const char* lineName, const double* pos1, const double* pos2, const double* color)
+{
+    return manager_->addLine(lineName, pos1, pos2, color);
+}
 
 bool ClientCpp::addURDF(const char* urdfName, const char* urdfPath, const char* urdfPackagePath)
 {
diff --git a/src/client-cpp.hh b/src/client-cpp.hh
index 2bba572..5bd27d0 100644
--- a/src/client-cpp.hh
+++ b/src/client-cpp.hh
@@ -50,22 +50,24 @@ public:
     bool addSceneToWindow(const char* sceneName, const char* windowName) ;
 
 
-    //void addBox(const char* boxName, float boxSize1, float boxSize2, float boxSize3, const char* colorName);
-    bool addBox(const char* boxName, float boxSize1, float boxSize2, float boxSize3) ;
+    bool addBox(const char* boxName, const float boxSize1, const float boxSize2, const float boxSize3, const double* color);
+    //bool addBox(const char* boxName, float boxSize1, float boxSize2, float boxSize3) ;
 
-    bool addCapsule(const char* capsuleName, float radius, float height) ;
-    //void addCapsule(const char* capsuleName, float radius, float height, const char* colorName);
+    //bool addCapsule(const char* capsuleName, float radius, float height) ;
+    bool addCapsule(const char* capsuleName, const float radius, const float height, const double* color);
 
     bool addMesh(const char* meshName, const char* meshPath) ;
 
-    bool addCone(const char* coneName, float radius, float height) ;
-    //void addCone(const char* coneName, float radius, float height, const char* colorName);
+    //bool addCone(const char* coneName, float radius, float height) ;
+    bool addCone(const char* coneName, const float radius, const float height, const double* color);
 
-    bool addCylinder(const char* cylinderName, float radius, float height) ;
-    //void addCylinder(const char* cylinderName, float radius, float height, const char* colorName);
+    //bool addCylinder(const char* cylinderName, float radius, float height) ;
+    bool addCylinder(const char* cylinderName, const float radius, const float height, const double* color);
 
-    bool addSphere(const char* sphereName, float radius) ;
-    //void addSphere(const char* sphereName, float radius, const char* colorName);
+    //bool addSphere(const char* sphereName, float radius) ;
+    bool addSphere(const char* sphereName, const float radius, const double* color);
+
+    bool addLine(const char* lineName, const double* pos1, const double* pos2, const double* color);
 
     bool addURDF(const char* urdfName, const char* urdfPath, const char* urdfPackagePath);
 
-- 
GitLab