[Personalrobots-commit] SF.net SVN: personalrobots:[19168] pkg/trunk/motion_planning/ planning_environment

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

Revision: 19168
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19168&view=rev
Author:   isucan
Date:     2009-07-18 01:42:37 +0000 (Sat, 18 Jul 2009)

Log Message:
-----------
additional padding for pointclouds (option)

Modified Paths:
--------------
    pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h
    pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h
    pkg/trunk/motion_planning/planning_environment/mainpage.dox
    pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp
    pkg/trunk/motion_planning/planning_environment/src/tools/display_planner_collision_model.cpp

Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h	2009-07-18 01:39:22 UTC (rev 19167)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h	2009-07-18 01:42:37 UTC (rev 19168)
@@ -115,7 +115,6 @@
 	/** \brief Return true if a map update has been received in the last sec seconds. If sec < 10us, this function always returns true. */
 	bool isMapUpdated(double sec) const;
 	
-
 	/** \brief Wait until a map is received */
 	void waitForMap(void) const;	
 
@@ -125,6 +124,12 @@
 	    return lastMapUpdate_;
 	}
 	
+	/** \brief Returns the padding used for pointclouds (for collision checking) */
+	double getPointCloudPadd(void) const
+	{
+	    return pointcloud_padd_;
+	}
+	
     protected:
 	
 	void setupCSM(void);
@@ -137,6 +142,7 @@
 	CollisionModels                                                *cm_;
 	collision_space::EnvironmentModel                              *collisionSpace_;
 	boost::mutex                                                    mapUpdateLock_;
+	double                                                          pointcloud_padd_;
 	
 	bool                                                            haveMap_;
 	ros::Time                                                       lastMapUpdate_;	

Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h	2009-07-18 01:39:22 UTC (rev 19167)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h	2009-07-18 01:42:37 UTC (rev 19168)
@@ -48,8 +48,7 @@
     /** \breif @b PlanningMonitor is a class which in addition to being aware
 	of a robot model, and the collision model is also aware of
 	constraints and can check the validity of states and paths.
-    */
-    
+    */    
     class PlanningMonitor : public CollisionSpaceMonitor
     {
     public:

Modified: pkg/trunk/motion_planning/planning_environment/mainpage.dox
===================================================================
--- pkg/trunk/motion_planning/planning_environment/mainpage.dox	2009-07-18 01:39:22 UTC (rev 19167)
+++ pkg/trunk/motion_planning/planning_environment/mainpage.dox	2009-07-18 01:42:37 UTC (rev 19168)
@@ -202,6 +202,8 @@
 
  - @b "~bounding_planes"/string : a sequence of plane equations specified as "a1 b1 c1 d1 a2 b2 c2 d2 ..." where each plane is defined by the equation ax+by+cz+d=0
 
+ - @b "~pointcloud_padd"/double : additional padding to be used when collision checking agains pointclouds (the padding for the robot will still be used)
+
 A robot description and its corresponding planning and collision descriptions are assumed to be loaded on the parameter server as well.
 
 Sets the following parameters on the parameter server

Modified: pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp	2009-07-18 01:39:22 UTC (rev 19167)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp	2009-07-18 01:42:37 UTC (rev 19168)
@@ -68,6 +68,8 @@
     // a list of static planes bounding the environment
     std::string planes;
     nh_.param<std::string>("~bounding_planes", planes, std::string());
+    nh_.param<double>("~pointcloud_padd", pointcloud_padd_, 0.01);
+
     std::stringstream ss(planes);
     std::vector<double> planeValues;
     if (!planes.empty())
@@ -181,7 +183,7 @@
 	    data[i4 + 1] = pso.point.y;
 	    data[i4 + 2] = pso.point.z;
 	    
-	    data[i4 + 3] = maxCoord(collisionMap->boxes[i].extents) * 0.867;
+	    data[i4 + 3] = maxCoord(collisionMap->boxes[i].extents) * 0.867 + pointcloud_padd_;
 	}
 	
 	if (err)
@@ -198,7 +200,7 @@
 	    data[i4 + 1] = collisionMap->boxes[i].center.y;
 	    data[i4 + 2] = collisionMap->boxes[i].center.z;
 	    
-	    data[i4 + 3] = maxCoord(collisionMap->boxes[i].extents) * 0.867;
+	    data[i4 + 3] = maxCoord(collisionMap->boxes[i].extents) * 0.867 + pointcloud_padd_;
 	}
     }
     

Modified: pkg/trunk/motion_planning/planning_environment/src/tools/display_planner_collision_model.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/tools/display_planner_collision_model.cpp	2009-07-18 01:39:22 UTC (rev 19167)
+++ pkg/trunk/motion_planning/planning_environment/src/tools/display_planner_collision_model.cpp	2009-07-18 01:42:37 UTC (rev 19168)
@@ -123,7 +123,7 @@
 	    const robot_msgs::Point32 &point = collisionMap->boxes[i].center;
 	    const robot_msgs::Point32 &extents = collisionMap->boxes[i].extents;
 	    sendPoint(i, nh_.getName(), point.x, point.y, point.z,
-		      std::max(std::max(extents.x, extents.y), extents.z) * 0.867,
+		      std::max(std::max(extents.x, extents.y), extents.z) * 0.867 + collisionSpaceMonitor_->getPointCloudPadd(),
 		      collisionMap->header);
 	}
     }
@@ -182,7 +182,7 @@
 
 	case mapping_msgs::Object::MESH:
 	    mk.type = visualization_msgs::Marker::LINE_LIST;
-	    mk.scale.x = mk.scale.y = mk.scale.z = 0.02;
+	    mk.scale.x = mk.scale.y = mk.scale.z = 0.01;
 	    {
 		unsigned int nt = obj.triangles.size() / 3;
 		for (unsigned int i = 0 ; i < nt ; ++i)


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