Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[7695] pkg/trunk
|
From: <mc...@us...> - 2008-12-05 22:04:01
|
Revision: 7695
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7695&view=rev
Author: mcgann
Date: 2008-12-05 21:53:04 +0000 (Fri, 05 Dec 2008)
Log Message:
-----------
Implemeneted, tested, and integrated hybrid reset policy for cost map per trac:714
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc
Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-12-05 21:49:10 UTC (rev 7694)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-12-05 21:53:04 UTC (rev 7695)
@@ -295,7 +295,7 @@
*/
void MoveBase::updateGoalMsg(){
// Revert to static map on new goal. May result in oscillation, but requested by Eitan for the milestone
- costMap_->revertToStaticMap();
+ costMap_->revertToStaticMap(global_pose_.getOrigin().x(), global_pose_.getOrigin().y());
tf::Stamped<tf::Pose> goalPose, transformedGoalPose;
btQuaternion qt;
@@ -411,7 +411,7 @@
*/
void MoveBase::handlePlanningFailure(){
ROS_DEBUG("No plan found. Handling planning failure");
- costMap_->revertToStaticMap();
+ costMap_->revertToStaticMap(global_pose_.getOrigin().x(), global_pose_.getOrigin().y());
stopRobot();
}
Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2008-12-05 21:49:10 UTC (rev 7694)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2008-12-05 21:53:04 UTC (rev 7695)
@@ -175,9 +175,12 @@
double getWeight() const {return weight_;}
/**
- * @brief Will reset the cost data to the initiali propagated costs of the static map
+ * @brief Will reset the cost data
+ * @param wx the x position in world coordinates
+ * @param wy the y position in world coordinates
*/
- void revertToStaticMap();
+ void revertToStaticMap(double wx = 0.0, double wy = 0.0);
+
private:
/**
@@ -241,6 +244,8 @@
QUEUE queue_; /**< Used for cost propagation */
double** cachedDistances; /**< Cached distances indexed by dx, dy */
+ const unsigned int kernelWidth_; /**< The width of the kernel matrix, which will be square */
+ unsigned char* kernelData_; /**< kernel data structure for cost map updates around the robot */
};
/**
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp 2008-12-05 21:49:10 UTC (rev 7694)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp 2008-12-05 21:53:04 UTC (rev 7695)
@@ -79,7 +79,7 @@
inscribedRadius_(toCellDistance(inscribedRadius, circumscribedRadius_, resolution)),
weight_(std::max(0.0, std::min(weight, 1.0))), sq_obstacle_range_(obstacleRange * obstacleRange),
sq_raytrace_range_((raytraceRange / resolution) * (raytraceRange / resolution)),
- staticData_(NULL), xy_markers_(NULL)
+ staticData_(NULL), xy_markers_(NULL), kernelWidth_((circumscribedRadius_ * 2) + 1)
{
if(weight != weight_){
ROS_INFO("Warning - input weight %f is invalid and has been set to %f\n", weight, weight_);
@@ -90,6 +90,7 @@
xy_markers_ = new bool[width_*height_];
memset(xy_markers_, 0, width_ * height_* sizeof(bool));
+ // Set up a cache of distance values for a kernel around the robot
cachedDistances = new double*[inflationRadius_+1];
for (i=0; i<=inflationRadius_; i++) {
cachedDistances[i] = new double[inflationRadius_+1];
@@ -99,6 +100,9 @@
}
}
+ // Allocate memory for a kernel matrix to be used for map updates aruond the robot
+ kernelData_ = new unsigned char[kernelWidth_ * kernelWidth_];
+
setCircumscribedCostLowerBound(computeCost(circumscribedRadius_));
// For the first pass, just clean up the data and get the set of original obstacles.
@@ -140,17 +144,35 @@
}
- void CostMap2D::revertToStaticMap(){
- // Using the original policy since the requested change implemented below performs poorly in practice
- // as the map is accumulating free space and thus the planner does really strange things.
+ void CostMap2D::revertToStaticMap(double wx, double wy){
+ unsigned int mx, my;
+ WC_MC(wx, wy, mx, my);
+
+ // Reset the kernel. A box around the current position. Then copy the current cost data in that region
+ // We can assume the whole kernel is in the global map, because the robot is in the map and the borders of the map
+ // are walls anyway.
+ memset(kernelData_, 0, kernelWidth_ * kernelWidth_);
+ const unsigned int originX = mx - circumscribedRadius_;
+ const unsigned int originY = my - circumscribedRadius_;
+ for (unsigned int i = 0; i < kernelWidth_; i++){
+ for(unsigned int j = 0; j < kernelWidth_; j++){
+ unsigned int kernelIndex = (j * kernelWidth_) + i;
+ unsigned int costMapIndex = (originY + j) * width_ + originX + i;
+ kernelData_[kernelIndex] = costData_[costMapIndex];
+ }
+ }
+
+ // Reset the cost map to the static data
memcpy(costData_, staticData_, width_ * height_);
- /*
- // Revising the policy per Eitan's discussion with Eric where we want to revert to the most relaxed map based on our current perception
- // and the static map.
- unsigned int cellCount = width_ * height_;
- for (unsigned int i = 0; i < cellCount; i++)
- costData_[i] = std::min(costData_[i], staticData_[i]);
- */
+
+ // Now repeat the iteration over the kernel, but this time write the data back
+ for (unsigned int i = 0; i < kernelWidth_; i++){
+ for(unsigned int j = 0; j < kernelWidth_; j++){
+ unsigned int kernelIndex = (j * kernelWidth_) + i;
+ unsigned int costMapIndex = (originY + j) * width_ + originX + i;
+ costData_[costMapIndex] = std::min(costData_[costMapIndex], kernelData_[kernelIndex]);
+ }
+ }
}
/**
Modified: pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc 2008-12-05 21:49:10 UTC (rev 7694)
+++ pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc 2008-12-05 21:53:04 UTC (rev 7695)
@@ -74,6 +74,94 @@
}
/**
+ * Tests the reset method
+ */
+TEST(costmap, testResetForStaticMap){
+ // Define a static map with a large object in the center
+ std::vector<unsigned char> staticMap;
+ for(unsigned int i=0; i<10; i++){
+ for(unsigned int j=0; j<10; j++){
+ staticMap.push_back(CostMap2D::LETHAL_OBSTACLE);
+ }
+ }
+
+ // Allocate the cost map, with a inflation to 3 cells all around
+ CostMap2D map(10, 10, staticMap, RESOLUTION, THRESHOLD, MAX_Z, MAX_Z, MAX_Z, 3, 3, 3);
+
+ // Populate the cost map with a wall around the perimeter. Free space should clear out the room.
+ std_msgs::PointCloud cloud;
+ cloud.set_pts_size(40);
+
+ // Left wall
+ unsigned int ind = 0;
+ for (unsigned int i = 0; i < 10; i++){
+ // Left
+ cloud.pts[ind].x = 0;
+ cloud.pts[ind].y = i;
+ cloud.pts[ind].z = MAX_Z;
+ ind++;
+
+ // Top
+ cloud.pts[ind].x = i;
+ cloud.pts[ind].y = 0;
+ cloud.pts[ind].z = MAX_Z;
+ ind++;
+
+ // Right
+ cloud.pts[ind].x = 9;
+ cloud.pts[ind].y = i;
+ cloud.pts[ind].z = MAX_Z;
+ ind++;
+
+ // Bottom
+ cloud.pts[ind].x = i;
+ cloud.pts[ind].y = 9;
+ cloud.pts[ind].z = MAX_Z;
+ ind++;
+ }
+
+ double wx = 5.0, wy = 5.0;
+ std_msgs::Point p;
+ p.x = wx;
+ p.y = wy;
+ p.z = MAX_Z;
+ Observation obs(p, &cloud);
+ std::vector<Observation> obsBuf;
+ obsBuf.push_back(obs);
+
+ // Update the cost map for this observation
+ map.updateDynamicObstacles(wx, wy, obsBuf);
+
+ // Verify that we know have only 8 * 4 + 4 cells with lethal cost, thus provong that we have correctly cleared
+ // free space
+ unsigned int hitCount = 0;
+ for(unsigned int i=0; i <100; i++){
+ if(map.getMap()[i] == CostMap2D::LETHAL_OBSTACLE)
+ hitCount++;
+ }
+ ASSERT_EQ(hitCount, 36);
+
+ // Veriy that we have 4 free cells
+ hitCount = 0;
+ for(unsigned int i=0; i < 100; i++){
+ if(map.getMap()[i] == 0)
+ hitCount++;
+ }
+ ASSERT_EQ(hitCount, 4);
+
+ // Now if we reset the cost map, we shold retain the free space, and also retain values of INSCRIBED circle
+ // in the region of the circumscribed radius (3 cells)
+ map.revertToStaticMap(wx, wy);
+ unsigned int mx, my;
+ map.WC_MC(wx, wy, mx, my);
+ for(unsigned int x = mx - 3; x <= mx+3; x++){
+ for(unsigned int y = my -3; y <= my + 3; y++){
+ ASSERT_EQ(map.getCost(x, y) < CostMap2D::LETHAL_OBSTACLE, true);
+ }
+ }
+}
+
+/**
* Basic testing for observation buffer
*/
TEST(costmap, test15){
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
