[Personalrobots-commit] SF.net SVN: personalrobots:[10261] pkg/trunk

[<hsu...@us...>]

Revision: 10261
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10261&view=rev
Author:   hsujohnhsu
Date:     2009-01-27 18:05:54 +0000 (Tue, 27 Jan 2009)

Log Message:
-----------
update to handle mesh as collision model.

Modified Paths:
--------------
    pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp
    pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp

Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp	2009-01-27 17:41:10 UTC (rev 10260)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp	2009-01-27 18:05:54 UTC (rev 10261)
@@ -153,6 +153,16 @@
         *sizeCount = 1;
         sizeVals[0] = static_cast<robot_desc::URDF::Link::Geometry::Sphere*>(geometry->shape)->radius;
         break;
+    case robot_desc::URDF::Link::Geometry::MESH:
+        type = "trimesh";
+        *sizeCount = 3;
+        {
+          robot_desc::URDF::Link::Geometry::Mesh* mesh = static_cast<robot_desc::URDF::Link::Geometry::Mesh*>(geometry->shape);
+          sizeVals[0] = mesh->scale[0];
+          sizeVals[1] = mesh->scale[1];
+          sizeVals[2] = mesh->scale[2];
+        }
+        break;
     default:
         *sizeCount = 0;
         printf("Unknown body type: %d in geometry '%s'\n", geometry->type, geometry->name.c_str());
@@ -210,8 +220,21 @@
             for (int j = 0 ; j < 3 ; ++j)
                 addKeyValue(geom, tagList2[j], values2str(1, link->inertial->com + j));
             
-            /* set geometry size */
-            addKeyValue(geom, "size", values2str(linkGeomSize, linkSize));
+            if (link->collision->geometry->type == robot_desc::URDF::Link::Geometry::MESH)
+            {  
+                robot_desc::URDF::Link::Geometry::Mesh* mesh = static_cast<robot_desc::URDF::Link::Geometry::Mesh*>(link->collision->geometry->shape);
+                /* set mesh size or scale */
+                addKeyValue(geom, "scale", values2str(3, mesh->scale));
+
+                /* set mesh file */
+                addKeyValue(geom, "mesh", "models/pr2/" + mesh->filename + ".mesh");
+                
+            }
+            else
+            {
+                /* set geometry size */
+                addKeyValue(geom, "size", values2str(linkGeomSize, linkSize));
+            }
             
             /* set additional data */      
             copyGazeboMap(link->collision->data, geom);

Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp	2009-01-27 17:41:10 UTC (rev 10260)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp	2009-01-27 18:05:54 UTC (rev 10261)
@@ -120,7 +120,8 @@
 std::string getGeometrySize(robot_desc::URDF::Link::Geometry* geometry, int *sizeCount, double *sizeVals)
 {
     std::string type;
-    
+
+    std::cout << " geometry type: " << geometry->type << std::endl;
     switch (geometry->type)
     {
     case robot_desc::URDF::Link::Geometry::BOX:
@@ -147,6 +148,16 @@
         *sizeCount = 1;
         sizeVals[0] = static_cast<robot_desc::URDF::Link::Geometry::Sphere*>(geometry->shape)->radius;
         break;
+    case robot_desc::URDF::Link::Geometry::MESH:
+        type = "trimesh";
+        *sizeCount = 3;
+        {
+          robot_desc::URDF::Link::Geometry::Mesh* mesh = static_cast<robot_desc::URDF::Link::Geometry::Mesh*>(geometry->shape);
+          sizeVals[0] = mesh->scale[0];
+          sizeVals[1] = mesh->scale[1];
+          sizeVals[2] = mesh->scale[2];
+        }
+        break;
     default:
         *sizeCount = 0;
         printf("Unknown body type: %d in geometry '%s'\n", geometry->type, geometry->name.c_str());
@@ -204,8 +215,21 @@
             for (int j = 0 ; j < 3 ; ++j)
                 addKeyValue(geom, tagList2[j], values2str(1, link->inertial->com + j));
             
-            /* set geometry size */
-            addKeyValue(geom, "size", values2str(linkGeomSize, linkSize));
+            if (link->collision->geometry->type == robot_desc::URDF::Link::Geometry::MESH)
+            {  
+                robot_desc::URDF::Link::Geometry::Mesh* mesh = static_cast<robot_desc::URDF::Link::Geometry::Mesh*>(link->collision->geometry->shape);
+                /* set mesh size or scale */
+                addKeyValue(geom, "scale", values2str(3, mesh->scale));
+
+                /* set mesh file */
+                addKeyValue(geom, "mesh", "models/pr2/" + mesh->filename + ".mesh");
+                
+            }
+            else
+            {
+                /* set geometry size */
+                addKeyValue(geom, "size", values2str(linkGeomSize, linkSize));
+            }
             
             /* set additional data */      
             copyGazeboMap(link->collision->data, geom);


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.