color changes to collada_parser for rviz bug #5237 (by Kei Okada)

collada_parser/src/collada_parser.cpp

@@ -76,8 +76,11 @@ namespace urdf{
 class UnlinkFilename
 {
 public:
-    UnlinkFilename(const std::string& filename) : _filename(filename) {}
+    UnlinkFilename(const std::string& filename) : _filename(filename) {
-    virtual ~UnlinkFilename() { unlink(_filename.c_str()); }
+    }
+    virtual ~UnlinkFilename() {
+        unlink(_filename.c_str());
+    }
     std::string _filename;
 };
 static std::list<boost::shared_ptr<UnlinkFilename> > _listTempFilenames;
@@ -153,8 +156,10 @@ namespace urdf{
 
     struct USERDATA
     {
-      USERDATA() {}
+        USERDATA() {
-      USERDATA(double scale) : scale(scale) {}
+        }
+        USERDATA(double scale) : scale(scale) {
+        }
         double scale;
         boost::shared_ptr<void> p; ///< custom managed data
     };
@@ -174,7 +179,7 @@ namespace urdf{
         ///< for sphere it is radius
         ///< for cylinder, first 2 values are radius and height
         ///< for trimesh, none
-      Color emissionColor, diffuseColor, ambientColor, specularColor; ///< hints for how to color the meshes
+        Color diffuseColor, ambientColor; ///< hints for how to color the meshes
         std::vector<Vector3> vertices;
         std::vector<int> indices;
 
@@ -1041,17 +1046,13 @@ namespace urdf{
 
         std::vector<std::vector<Vector3> > vertices;
         std::vector<std::vector<int> > indices;
-      std::vector<Color> emission;
         std::vector<Color> ambients;
         std::vector<Color> diffuses;
-      std::vector<Color> specular;
         unsigned int index;
         vertices.resize(listGeomProperties.size());
         indices.resize(listGeomProperties.size());
-      emission.resize(listGeomProperties.size());
         ambients.resize(listGeomProperties.size());
         diffuses.resize(listGeomProperties.size());
-      specular.resize(listGeomProperties.size());
         index = 0;
         FOREACHC(it, listGeomProperties) {
             vertices[index].resize(it->vertices.size());
@@ -1062,10 +1063,20 @@ namespace urdf{
             for(size_t i = 0; i < it->indices.size(); ++i) {
                 indices[index][i] = it->indices[i];
             }
-	emission[index] = it->emissionColor;
             ambients[index] = it->ambientColor;
             diffuses[index] = it->diffuseColor;
-	specular[index] = it->specularColor;
+// workaround for mesh_loader.cpp:421
+// Most of are DAE files don't have ambient color defined
+            ambients[index].r *= 0.5; ambients[index].g *= 0.5; ambients[index].b *= 0.5;
+            if ( ambients[index].r == 0.0 ) {
+                ambients[index].r = 0.0001;
+            }
+            if ( ambients[index].g == 0.0 ) {
+                ambients[index].g = 0.0001;
+            }
+            if ( ambients[index].b == 0.0 ) {
+                ambients[index].b = 0.0001;
+            }
             index++;
         }
 
@@ -1099,9 +1110,6 @@ namespace urdf{
 	  <profile_COMMON>\n\
 		<technique sid=\"common\">\n\
 		  <phong>\n\
-			<emission>\n\
-			  <color>%f %f %f %f</color>\n\
-			</emission>\n\
 			<ambient>\n\
 			  <color>%f %f %f %f</color>\n\
 			</ambient>\n\
@@ -1109,27 +1117,12 @@ namespace urdf{
 			  <color>%f %f %f %f</color>\n\
 			</diffuse>\n\
 			<specular>\n\
-			  <color>%f %f %f %f</color>\n\
+			  <color>0 0 0 1</color>\n\
 			</specular>\n\
 		  </phong>\n\
 		</technique>\n\
       </profile_COMMON>\n\
-    </effect>\n")%i%emission[i].r%emission[i].g%emission[i].b%emission[i].a%ambients[i].r%ambients[i].g%ambients[i].b%ambients[i].a%diffuses[i].r%diffuses[i].g%diffuses[i].b%diffuses[i].a%specular[i].r%specular[i].g%specular[i].b%specular[i].a);
+    </effect>\n")%i%ambients[i].r%ambients[i].g%ambients[i].b%ambients[i].a%diffuses[i].r%diffuses[i].g%diffuses[i].b%diffuses[i].a);
-	std::cout << str(boost::format("\
-%d\n\
-			<emission>\n\
-			  <color>%f %f %f %f</color>\n\
-			</emission>\n\
-			<ambient>\n\
-			  <color>%f %f %f %f</color>\n\
-			</ambient>\n\
-			<diffuse>\n\
-			  <color>%f %f %f %f</color>\n\
-			</diffuse>\n\
-			<specular>\n\
-			  <color>%f %f %f %f</color>\n\
-			</specular>\n\
-")%i%emission[i].r%emission[i].g%emission[i].b%emission[i].a%ambients[i].r%ambients[i].g%ambients[i].b%ambients[i].a%diffuses[i].r%diffuses[i].g%diffuses[i].b%diffuses[i].a%specular[i].r%specular[i].g%specular[i].b%specular[i].a);
         }
         daedata << str(boost::format("\
   </library_effects>\n\
@@ -1337,13 +1330,6 @@ namespace urdf{
             if( !!peffect ) {
                 domProfile_common::domTechnique::domPhongRef pphong = daeSafeCast<domProfile_common::domTechnique::domPhong>(peffect->getDescendant(daeElement::matchType(domProfile_common::domTechnique::domPhong::ID())));
                 if( !!pphong ) {
-            if( !!pphong->getEmission() && !!pphong->getEmission()->getColor() ) {
-              domFx_color c = pphong->getEmission()->getColor()->getValue();
-              geom.emissionColor.r = c[0];
-              geom.emissionColor.g = c[1];
-              geom.emissionColor.b = c[2];
-              geom.emissionColor.a = c[3];
-            }
                     if( !!pphong->getAmbient() && !!pphong->getAmbient()->getColor() ) {
                         domFx_color c = pphong->getAmbient()->getColor()->getValue();
                         geom.ambientColor.r = c[0];
@@ -1358,13 +1344,6 @@ namespace urdf{
                         geom.diffuseColor.b = c[2];
                         geom.diffuseColor.a = c[3];
                     }
-            if( !!pphong->getSpecular() && !!pphong->getSpecular()->getColor() ) {
-              domFx_color c = pphong->getSpecular()->getColor()->getValue();
-              geom.specularColor.r = c[0];
-              geom.specularColor.g = c[1];
-              geom.specularColor.b = c[2];
-              geom.specularColor.a = c[3];
-            }
                 }
             }
         }