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

[<is...@us...>]

Revision: 18264
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18264&view=rev
Author:   isucan
Date:     2009-07-03 05:44:47 +0000 (Fri, 03 Jul 2009)

Log Message:
-----------
using resource location

Modified Paths:
--------------
    pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
    pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
    pkg/trunk/util/geometric_shapes/src/bodies.cpp
    pkg/trunk/util/robot_self_filter/self_filter.launch

Modified: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h	2009-07-03 05:06:26 UTC (rev 18263)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h	2009-07-03 05:44:47 UTC (rev 18264)
@@ -336,7 +336,7 @@
 	    const double* computeTransform(const double *params, int groupID);
 
 	    /** \brief Extract the information needed by the joint given the URDF description */
-	    void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
+	    void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot, const robot_desc::URDF &model);
 	    
 	};
 	

Modified: pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic.cpp	2009-07-03 05:06:26 UTC (rev 18263)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic.cpp	2009-07-03 05:44:47 UTC (rev 18264)
@@ -227,7 +227,7 @@
     robot->stateBounds.push_back(limit[1]);
 }
 
-void planning_models::KinematicModel::Link::extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot)
+void planning_models::KinematicModel::Link::extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot, const robot_desc::URDF &model)
 {
     /* compute the geometry for this link */
     switch (urdfLink->collision->geometry->type)
@@ -259,9 +259,10 @@
 	break;
     case robot_desc::URDF::Link::Geometry::MESH:
 	{
-	    std::string filename = static_cast<const robot_desc::URDF::Link::Geometry::Mesh*>(urdfLink->collision->geometry->shape)->filename;
+	    std::string filename = model.getResourceLocation() + "/" + static_cast<const robot_desc::URDF::Link::Geometry::Mesh*>(urdfLink->collision->geometry->shape)->filename;
 	    if (filename.rfind(".stl") == std::string::npos)
 		filename += ".stl";
+	    std::cout << "Loading '" << filename << "'" << std::endl;	    
 	    shapes::Mesh *mesh = shapes::create_mesh_from_binary_stl(filename.c_str());
 	    shape              = mesh;
 	}
@@ -572,7 +573,9 @@
     link->owner  = robot;
     robot->links.push_back(link);
     
-    link->extractInformation(urdfLink, robot);
+    link->extractInformation(urdfLink, robot, model);
+    if (link->shape == NULL)
+	m_msg.error("Unable to construct shape for link '%s'", link->name.c_str());
     
     for (unsigned int i = 0 ; i < urdfLink->children.size() ; ++i)
     {

Modified: pkg/trunk/util/geometric_shapes/src/bodies.cpp
===================================================================
--- pkg/trunk/util/geometric_shapes/src/bodies.cpp	2009-07-03 05:06:26 UTC (rev 18263)
+++ pkg/trunk/util/geometric_shapes/src/bodies.cpp	2009-07-03 05:44:47 UTC (rev 18264)
@@ -43,24 +43,25 @@
 {
     Body *body = NULL;
     
-    switch (shape->type)
-    {
-    case shapes::Shape::BOX:
-	body = new bodies::Box(shape);
-	break;
-    case shapes::Shape::SPHERE:
-	body = new bodies::Sphere(shape);
-	break;
-    case shapes::Shape::CYLINDER:
-	body = new bodies::Cylinder(shape);
-	break;
-    case shapes::Shape::MESH:
-	body = new bodies::ConvexMesh(shape);
-	break;
-    default:
-	std::cerr << "Creating body from shape: Unknown shape type" << shape->type << std::endl;
-	break;
-    }
+    if (shape)
+	switch (shape->type)
+	{
+	case shapes::Shape::BOX:
+	    body = new bodies::Box(shape);
+	    break;
+	case shapes::Shape::SPHERE:
+	    body = new bodies::Sphere(shape);
+	    break;
+	case shapes::Shape::CYLINDER:
+	    body = new bodies::Cylinder(shape);
+	    break;
+	case shapes::Shape::MESH:
+	    body = new bodies::ConvexMesh(shape);
+	    break;
+	default:
+	    std::cerr << "Creating body from shape: Unknown shape type" << shape->type << std::endl;
+	    break;
+	}
     
     return body;
 }
@@ -315,7 +316,7 @@
 		}
 		
 		if (behindPlane > 0)
-		    std::cerr << "Approximate plane: " << behindPlane << " of " << m_vertices.size() << " points are behind the plane";
+		    std::cerr << "Approximate plane: " << behindPlane << " of " << m_vertices.size() << " points are behind the plane" << std::endl;
 		
 		m_planes.push_back(planeEquation);
 	    }

Modified: pkg/trunk/util/robot_self_filter/self_filter.launch
===================================================================
--- pkg/trunk/util/robot_self_filter/self_filter.launch	2009-07-03 05:06:26 UTC (rev 18263)
+++ pkg/trunk/util/robot_self_filter/self_filter.launch	2009-07-03 05:44:47 UTC (rev 18264)
@@ -1,6 +1,6 @@
 <launch> 
    <include file="$(find pr2_defs)/pr2_planning_environment.launch" />
-   <node pkg="new_robot_self_filter" type="self_filter" respawn="true" output="screen">
+   <node pkg="robot_self_filter" type="self_filter" respawn="true" output="screen">
      <remap from="robot_description" to="robotdesc/pr2" />
      <remap from="cloud_in" to="tilt_scan_cloud" />
      <remap from="cloud_out" to="tilt_scan_cloud_filtered" />


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