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

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

Revision: 16865
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16865&view=rev
Author:   isucan
Date:     2009-06-09 17:35:35 +0000 (Tue, 09 Jun 2009)

Log Message:
-----------
properly dealing with frames for collision maps

Modified Paths:
--------------
    pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
    pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
    pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp

Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h	2009-06-09 17:34:31 UTC (rev 16864)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h	2009-06-09 17:35:35 UTC (rev 16865)
@@ -68,7 +68,8 @@
 	    }
 	    
 	    bool        hasParam(const std::string &param);
-	    std::string getParam(const std::string &param);
+	    std::string getParamString(const std::string &param, const std::string &def = "");
+	    double      getParamDouble(const std::string &param, double def);
 	    
 	private:
 	    

Modified: pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp	2009-06-09 17:34:31 UTC (rev 16864)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp	2009-06-09 17:35:35 UTC (rev 16865)
@@ -35,6 +35,7 @@
 /** \author Ioan Sucan */
 
 #include "planning_environment/collision_space_monitor.h"
+#include <robot_msgs/PointStamped.h>
 
 namespace planning_environment
 {
@@ -73,23 +74,55 @@
 
 void planning_environment::CollisionSpaceMonitor::collisionMapCallback(const robot_msgs::CollisionMapConstPtr &collisionMap)
 {
-    unsigned int n = collisionMap->get_boxes_size();
-    ROS_DEBUG("Received %u points (collision map)", n);
+    int n = collisionMap->get_boxes_size();
+    ROS_DEBUG("Received %d points (collision map)", n);
     
     if (onBeforeMapUpdate_ != NULL)
 	onBeforeMapUpdate_(collisionMap);
+
+    // we want to make sure the frame the robot model is kept in is the same as the frame of the collisionMap
+    bool transform = !frame_id_.empty() && collisionMap->header.frame_id != frame_id_;
     
     ros::WallTime startTime = ros::WallTime::now();
     double *data = new double[4 * n];	
-    for (unsigned int i = 0 ; i < n ; ++i)
+
+    if (transform)
     {
-	unsigned int i4 = i * 4;	    
-	data[i4    ] = collisionMap->boxes[i].center.x;
-	data[i4 + 1] = collisionMap->boxes[i].center.y;
-	data[i4 + 2] = collisionMap->boxes[i].center.z;
+	std::string target = frame_id_;
 	
-	data[i4 + 3] = radiusOfBox(collisionMap->boxes[i].extents);
+#pragma omp parallel for
+	for (int i = 0 ; i < n ; ++i)
+	{
+	    int i4 = i * 4;
+	    robot_msgs::PointStamped psi;
+	    psi.header  = collisionMap->header;
+	    psi.point.x = collisionMap->boxes[i].center.x;
+	    psi.point.y = collisionMap->boxes[i].center.y;
+	    psi.point.z = collisionMap->boxes[i].center.z;
+
+	    robot_msgs::PointStamped pso;
+	    tf_.transformPoint(target, psi, pso);
+	    
+	    data[i4    ] = pso.point.x;
+	    data[i4 + 1] = pso.point.y;
+	    data[i4 + 2] = pso.point.z;
+	    
+	    data[i4 + 3] = radiusOfBox(collisionMap->boxes[i].extents);
+	}
     }
+    else
+    {
+#pragma omp parallel for
+	for (int i = 0 ; i < n ; ++i)
+	{
+	    int i4 = i * 4;
+	    data[i4    ] = collisionMap->boxes[i].center.x;
+	    data[i4 + 1] = collisionMap->boxes[i].center.y;
+	    data[i4 + 2] = collisionMap->boxes[i].center.z;
+	    
+	    data[i4 + 3] = radiusOfBox(collisionMap->boxes[i].extents);
+	}
+    }
     
     collisionSpace_->lock();
     collisionSpace_->clearObstacles();

Modified: pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp	2009-06-09 17:34:31 UTC (rev 16864)
+++ pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp	2009-06-09 17:35:35 UTC (rev 16865)
@@ -37,6 +37,8 @@
 #include "planning_environment/robot_models.h"
 #include <planning_models/output.h>
 #include <ros/console.h>
+
+#include <boost/algorithm/string.hpp>
 #include <sstream>
 
 // make sure messages from planning_environments & collision_space go to ROS console
@@ -162,13 +164,21 @@
     return nh_.hasParam(description_ + "_planning/planner_configs/" + config_ + "/" + param);
 }
 
-std::string planning_environment::RobotModels::PlannerConfig::getParam(const std::string &param)
+std::string planning_environment::RobotModels::PlannerConfig::getParamString(const std::string &param, const std::string& def)
 {
     std::string value;
-    nh_.param(description_ + "_planning/planner_configs/" + config_ + "/" + param, value, std::string(""));
+    nh_.param(description_ + "_planning/planner_configs/" + config_ + "/" + param, value, def);
+    boost::trim(value);
     return value;
 }
 
+double planning_environment::RobotModels::PlannerConfig::getParamDouble(const std::string &param, double def)
+{
+    double value;
+    nh_.param(description_ + "_planning/planner_configs/" + config_ + "/" + param, value, def);
+    return value;
+}
+
 std::vector< boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> > planning_environment::RobotModels::getGroupPlannersConfig(const std::string &group)
 {
     std::string configs_list;


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