Merge pull request #370 from proyan/devel

[C++][Visual] Save "Material" information in geometry object

bindings/python/multibody/geometry-object.hpp

@@ -40,12 +40,18 @@ namespace se3
 			 bp::make_getter(&GeometryObject::meshScale, bp::return_value_policy<bp::return_by_value>()),
 			 bp::make_setter(&GeometryObject::meshScale),
 			 "Scaling parameter for the mesh")
+	   .add_property("meshColor",
+			 bp::make_getter(&GeometryObject::meshColor, bp::return_value_policy<bp::return_by_value>()),
+			 bp::make_setter(&GeometryObject::meshColor),
+			 "Color rgba for the mesh")
 	   .def_readwrite("name", &GeometryObject::name, "Name of the GeometryObject")
 	   .def_readwrite("parentJoint", &GeometryObject::parentJoint, "Index of the parent joint")
 	   .def_readwrite("parentFrame", &GeometryObject::parentFrame, "Index of the parent frame")
 	   .def_readwrite("placement",&GeometryObject::placement,
 		      		  "Position of geometry object in parent joint's frame")
 	   .def_readonly("meshPath", &GeometryObject::meshPath, "Absolute path to the mesh file")
+	   .def_readonly("overrideMaterial", &GeometryObject::overrideMaterial, "Boolean that tells whether material information is stored in Geometry object")
+	   .def_readonly("meshTexturePath", &GeometryObject::meshTexturePath, "Absolute path to the mesh texture file")
 	   ;
       }
 

bindings/python/scripts/robot_wrapper.py

@@ -178,9 +178,16 @@ class RobotWrapper(object):
         for visual in self.visual_model.geometryObjects :
             meshName = self.viewerNodeNames(visual) 
             meshPath = visual.meshPath
+            meshTexturePath = visual.meshTexturePath
             meshScale = visual.meshScale
+            meshColor = visual.meshColor
             if self.viewer.gui.addMesh(meshName, meshPath):
                 self.viewer.gui.setScale(meshName, npToTuple(meshScale))
+                if visual.overrideMaterial:
+                    self.viewer.gui.setColor(meshName,npToTuple(meshColor))
+                    if meshTexturePath is not '':
+                        self.viewer.gui.setTexture(meshName,meshTexturePath)
+               
 
         # Finally, refresh the layout to obtain your first rendering.
         self.viewer.gui.refresh()

src/multibody/fcl.hpp

@@ -113,9 +113,25 @@ struct GeometryObject
   /// \brief Scaling vector applied to the fcl object.
   Eigen::Vector3d meshScale;
 
+  /// \brief Decide whether to override the Material.
+  bool overrideMaterial;
+
+  /// \brief RGBA color value of the mesh.
+  Eigen::Vector4d meshColor;
+
+  /// \brief Absolute path to the mesh texture file.
+  std::string meshTexturePath;
+
+
+
   GeometryObject(const std::string & name, const FrameIndex parentF,
-                 const JointIndex parentJ, const boost::shared_ptr<fcl::CollisionGeometry> & collision,
-                 const SE3 & placement, const std::string & meshPath, const Eigen::Vector3d & meshScale)
+                 const JointIndex parentJ,
+                 const boost::shared_ptr<fcl::CollisionGeometry> & collision,
+                 const SE3 & placement, const std::string & meshPath = "",
+                 const Eigen::Vector3d & meshScale = Eigen::Vector3d::Ones(),
+                 const bool overrideMaterial = false,
+                 const Eigen::Vector4d & meshColor = Eigen::Vector4d::Zero(),
+                 const std::string & meshTexturePath = "")
                 : name(name)
                 , parentFrame(parentF)
                 , parentJoint(parentJ)
@@ -123,6 +139,9 @@ struct GeometryObject
                 , placement(placement)
                 , meshPath(meshPath)
                 , meshScale(meshScale)
+                , overrideMaterial(overrideMaterial)
+                , meshColor(meshColor)
+                , meshTexturePath(meshTexturePath)
   {}
 
   GeometryObject & operator=(const GeometryObject & other)
@@ -134,6 +153,9 @@ struct GeometryObject
     placement           = other.placement;
     meshPath            = other.meshPath;
     meshScale           = other.meshScale;
+    overrideMaterial    = other.overrideMaterial;
+    meshColor           = other.meshColor;
+    meshTexturePath     = other.meshTexturePath;
     return *this;
   }
 

src/multibody/geometry.hxx

@@ -30,7 +30,7 @@ namespace se3
   inline GeometryData::GeometryData(const GeometryModel & modelGeom)
     : oMg(modelGeom.ngeoms)
 
-#ifdef WITH_HPP_FCL   
+#ifdef WITH_HPP_FCL
     , activeCollisionPairs(modelGeom.collisionPairs.size(), true)
     , distanceRequest (true, 0, 0, fcl::GST_INDEP)
     , distanceResults(modelGeom.collisionPairs.size())

src/parsers/urdf/geometry.cpp

@@ -155,6 +155,49 @@ namespace se3
         return link->visual;
       }
 
+
+     /**
+      * @brief Get the material values from the link visual object
+      *
+      * @param[in]  Visual/Collision The Visual or the Collision object.
+      * @param[out] meshTexturePath  The absolute file path containing the texture description.
+      * @param[out] meshColor        The mesh RGBA vector.
+      * @param[in]  package_dirs     A vector containing the different directories where to search for packages
+      *
+      */
+      template<typename urdfObject>
+      inline bool getVisualMaterial(const boost::shared_ptr< urdfObject > urdf_object,std::string& meshTexturePath,
+				    Eigen::Vector4d & meshColor, const std::vector<std::string> & package_dirs);
+
+      template<>
+      inline bool getVisualMaterial< ::urdf::Collision>(const boost::shared_ptr< ::urdf::Collision >, std::string& meshTexturePath,
+							Eigen::Vector4d & meshColor, const std::vector<std::string> &)
+      {
+        meshColor.setZero();
+        meshTexturePath = "";
+        return false;
+      }
+      
+      template<>
+      inline bool getVisualMaterial< ::urdf::Visual>(const boost::shared_ptr< ::urdf::Visual > urdf_visual, std::string& meshTexturePath,
+						     Eigen::Vector4d & meshColor, const std::vector<std::string> & package_dirs)
+      {
+        meshColor.setZero();
+        meshTexturePath = "";
+        bool overrideMaterial = false;
+        if(urdf_visual->material!=NULL) {
+          overrideMaterial = true;
+          meshColor << urdf_visual->material->color.r, urdf_visual->material->color.g,
+          urdf_visual->material->color.b, urdf_visual->material->color.a;
+        if(urdf_visual->material->texture_filename!="")
+          meshTexturePath = retrieveResourcePath((urdf_visual)->material->texture_filename, package_dirs);
+        }
+        return overrideMaterial;
+      }
+
+
+
+
      /**
       * @brief Get the array of geometries attached to a link
       *
@@ -220,8 +263,12 @@ namespace se3
             if (urdf_mesh) meshPath = retrieveResourcePath(urdf_mesh->filename, package_dirs);
             
             const boost::shared_ptr<fcl::CollisionGeometry> geometry(new fcl::CollisionGeometry());
-#endif // WITH_HPP_FCL            
-            
+#endif // WITH_HPP_FCL
+
+            Eigen::Vector4d meshColor;
+            std::string meshTexturePath;
+            bool overrideMaterial = getVisualMaterial<T>((*i), meshTexturePath, meshColor, package_dirs);
+
             SE3 geomPlacement = body_placement * convertFromUrdf((*i)->origin);
             std::ostringstream geometry_object_suffix;
             geometry_object_suffix << "_" << objectId;
@@ -229,7 +276,8 @@ namespace se3
             geomModel.addGeometryObject(GeometryObject(geometry_object_name,
                                                        frame_id, model.frames[frame_id].parent, 
                                                        geometry,
-                                                       geomPlacement, meshPath, meshScale),
+                                                       geomPlacement, meshPath, meshScale,
+                                                       overrideMaterial, meshColor, meshTexturePath),
                                         model);
             ++objectId;
           }