Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[8553] pkg/trunk
|
From: <tpr...@us...> - 2008-12-23 23:53:49
|
Revision: 8553
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8553&view=rev
Author: tpratkanis
Date: 2008-12-23 23:53:44 +0000 (Tue, 23 Dec 2008)
Log Message:
-----------
Huge commit to change raytracing of static obstacles in the cost map
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
pkg/trunk/highlevel/test_highlevel_controllers/test/launch_move_base.xml
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
pkg/trunk/world_models/costmap_2d/include/costmap_2d/obstacle_map_accessor.h
pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
pkg/trunk/world_models/costmap_2d/src/obstacle_map_accessor.cc
pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc
pkg/trunk/world_models/costmap_2d/testmap_compare.txt
Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-12-23 23:48:49 UTC (rev 8552)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-12-23 23:53:44 UTC (rev 8553)
@@ -165,14 +165,18 @@
}
// Now allocate the cost map and its sliding window used by the controller
- double zLB, zUB;
+ double zLB, zUB, raytraceWindow, obstacleRange, rayTraceRange;
param("/costmap_2d/zLB", zLB, 0.15);
param("/costmap_2d/zUB", zUB, 0.25);
+ param("/costmap_2d/raytrace_window", raytraceWindow, 2.5);
+ param("/costmap_2d/raytrace_range", rayTraceRange, 10.0);
+ param("/costmap_2d/obstacle_range", obstacleRange, 10.0);
costMap_ = new CostMap2D((unsigned int)resp.map.width, (unsigned int)resp.map.height,
- inputData , resp.map.resolution,
- lethalObstacleThreshold, maxZ_, zLB, zUB,
- inflationRadius, circumscribedRadius, inscribedRadius, weight);
+ inputData , resp.map.resolution,
+ lethalObstacleThreshold, maxZ_, zLB, zUB,
+ inflationRadius, circumscribedRadius, inscribedRadius, weight,
+ obstacleRange, rayTraceRange, raytraceWindow);
// Allocate Velocity Controller
double mapSize(2.0);
Modified: pkg/trunk/highlevel/test_highlevel_controllers/test/launch_move_base.xml
===================================================================
--- pkg/trunk/highlevel/test_highlevel_controllers/test/launch_move_base.xml 2008-12-23 23:48:49 UTC (rev 8552)
+++ pkg/trunk/highlevel/test_highlevel_controllers/test/launch_move_base.xml 2008-12-23 23:53:44 UTC (rev 8553)
@@ -18,6 +18,7 @@
<param name="/costmap_2d/inflation_radius" value="0.55"/>
<param name="/costmap_2d/circumscribed_radius" value="0.46"/>
<param name="/costmap_2d/inscribed_radius" value="0.325"/>
+ <param name="/costmap_2d/raytrace_window" value="4.0"/>
<param name="/costmap_2d/weight" value="0.1"/>
<!-- Forces a check that we are receiving base scans that are correctly up to date at a rate of at least 2 Hz. This is
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-23 23:48:49 UTC (rev 8552)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2008-12-23 23:53:44 UTC (rev 8553)
@@ -65,6 +65,10 @@
#include <string>
#include <queue>
+namespace std_msgs {
+ class Point2DFloat32;
+};
+
namespace costmap_2d {
typedef unsigned char TICK;
@@ -120,11 +124,13 @@
* @param circumscribedRadius the radius used to indicate objects in the circumscribed circle around the robot
* @param inscribedRadius the radius used to indicate objects in the inscribed circle around the robot
* @param weight the scaling factor in the cost function. Should be <=1. Lower values reduce the effective cost
+ * @param raytraceWindow the size of the window in which raytracing is done.
*/
CostMap2D(unsigned int width, unsigned int height, const std::vector<unsigned char>& data,
double resolution, unsigned char threshold,
double maxZ = 0.5, double zLB = 0.15, double zUB = 0.20,
- double inflationRadius = 0, double circumscribedRadius = 0, double inscribedRadius = 0, double weight = 1, double obstacleRange = 10.0, double raytraceRange = 10.0);
+ double inflationRadius = 0, double circumscribedRadius = 0, double inscribedRadius = 0, double weight = 1, double obstacleRange = 10.0, double raytraceRange = 10.0,
+ double raytraceWindow = 2.5);
/**
* @brief Destructor.
@@ -192,7 +198,6 @@
* @param name The filename to save.
*/
void saveBinary(std::string name);
-
private:
/**
@@ -273,6 +278,7 @@
const unsigned int circumscribedRadius_; /**< The radius for the circumscribed radius, in cells */
const unsigned int inscribedRadius_; /**< The radius for the inscribed radius, in cells */
const double weight_; /**< The weighting to apply to a normalized cost value */
+ const double raytraceWindow_; /**< The window in which statics are raytraced */
//used squared distance because square root computations are expensive
double sq_obstacle_range_; /** The range out to which we will consider laser hitpoints **/
@@ -285,6 +291,8 @@
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 */
+ const unsigned int raytraceCells_; /**< Raytracing cells */
+ double robotX_, robotY_; /**< The position of the robot */
};
/**
@@ -308,8 +316,6 @@
private:
- static double computeWX(const CostMap2D& costMap, double maxSize, double wx, double wy);
- static double computeWY(const CostMap2D& costMap, double maxSize, double wx, double wy);
static unsigned int computeSize(double maxSize, double resolution);
const CostMap2D& costMap_;
Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/obstacle_map_accessor.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/obstacle_map_accessor.h 2008-12-23 23:48:49 UTC (rev 8552)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/obstacle_map_accessor.h 2008-12-23 23:53:44 UTC (rev 8553)
@@ -244,6 +244,10 @@
*/
std::string toString() const;
+
+ static double computeWX(const ObstacleMapAccessor& costMap, double maxSize, double wx, double wy);
+ static double computeWY(const ObstacleMapAccessor& costMap, double maxSize, double wx, double wy);
+
protected:
/**
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp 2008-12-23 23:48:49 UTC (rev 8552)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp 2008-12-23 23:53:44 UTC (rev 8553)
@@ -72,15 +72,15 @@
CostMap2D::CostMap2D(unsigned int width, unsigned int height, const std::vector<unsigned char>& data,
double resolution, unsigned char threshold, double maxZ, double zLB, double zUB,
double inflationRadius, double circumscribedRadius, double inscribedRadius, double weight,
- double obstacleRange, double raytraceRange)
+ double obstacleRange, double raytraceRange, double raytraceWindow)
: ObstacleMapAccessor(0, 0, width, height, resolution),
- maxZ_(maxZ), zLB_(zLB), zUB_(zUB),
- inflationRadius_(toCellDistance(inflationRadius, (unsigned int) ceil(width * resolution), resolution)),
- circumscribedRadius_(toCellDistance(circumscribedRadius, inflationRadius_, resolution)),
- 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), kernelWidth_((circumscribedRadius_ * 2) + 1)
+ maxZ_(maxZ), zLB_(zLB), zUB_(zUB),
+ inflationRadius_(toCellDistance(inflationRadius, (unsigned int) ceil(width * resolution), resolution)),
+ circumscribedRadius_(toCellDistance(circumscribedRadius, inflationRadius_, resolution)),
+ inscribedRadius_(toCellDistance(inscribedRadius, circumscribedRadius_, resolution)),
+ weight_(std::max(0.0, std::min(weight, 1.0))), raytraceWindow_(raytraceWindow),
+ sq_obstacle_range_(obstacleRange * obstacleRange), sq_raytrace_range_((raytraceRange / resolution) * (raytraceRange / resolution)),
+ staticData_(NULL), xy_markers_(NULL), kernelWidth_((circumscribedRadius_ * 2) + 1), raytraceCells_ (raytraceWindow_ / resolution)
{
if(weight != weight_){
ROS_INFO("Warning - input weight %f is invalid and has been set to %f\n", weight, weight_);
@@ -96,7 +96,7 @@
for (i=0; i<=inflationRadius_; i++) {
cachedDistances[i] = new double[inflationRadius_+1];
for (j=0; j<=i; j++) {
- cachedDistances[i][j] = sqrt (pow(i, 2) + pow(j, 2));
+ cachedDistances[i][j] = sqrt(pow(i, 2) + pow(j, 2));
cachedDistances[j][i] = cachedDistances[i][j];
}
}
@@ -132,6 +132,12 @@
// Instantiate static data
memcpy(staticData_, costData_, width_ * height_);
+
+ if (raytraceCells_ + inflationRadius_ * 2 >= getWidth() || raytraceCells_ + inflationRadius_ * 2 >= getHeight()) {
+ ROS_ERROR("Raytrace area can't be larger than the costmap.");
+ ROS_ASSERT(false);
+ exit(1);
+ }
}
CostMap2D::~CostMap2D() {
@@ -225,23 +231,8 @@
// Revert to initial state
memset(xy_markers_, 0, width_ * height_ * sizeof(bool));
- // Now propagate free space. We iterate again over observations, process only those from an origin
- // within a specific range, and a point within a certain z-range. We only want to propagate free space
- // in 2D so keep point and its origin within expected range
- for(std::vector<Observation>::const_iterator it = observations.begin(); it!= observations.end(); ++it){
- const Observation& obs = *it;
- if(!in_projection_range(obs.origin_.z))
- continue;
+ robotX_ = wx; robotY_ = wy;
- const std_msgs::PointCloud& cloud = *(obs.cloud_);
- for(size_t i = 0; i < cloud.get_pts_size(); i++) {
- if(!in_projection_range(cloud.pts[i].z))
- continue;
-
- updateFreeSpace(obs.origin_, cloud.pts[i].x, cloud.pts[i].y);
- }
- }
-
// Propagation queue should be empty from completion of last propagation.
ROS_ASSERT(queue_.empty());
@@ -274,9 +265,63 @@
unsigned int mx, my;
IND_MC(ind, mx, my);
enqueue(ind, mx, my);
+
+ if(in_projection_range(cloud.pts[i].z) && in_projection_range(obs.origin_.z))
+ updateFreeSpace(obs.origin_, cloud.pts[i].x, cloud.pts[i].y);
}
}
+
+ // Now propagate free space. We iterate again over observations, process only those from an origin
+ // within a specific range, and a point within a certain z-range. We only want to propagate free space
+ // in 2D so keep point and its origin within expected range
+ /*for(std::vector<Observation>::const_iterator it = observations.begin(); it!= observations.end(); ++it){
+ const Observation& obs = *it;
+ if(!in_projection_range(obs.origin_.z))
+ continue;
+
+ const std_msgs::PointCloud& cloud = *(obs.cloud_);
+ for(size_t i = 0; i < cloud.get_pts_size(); i++) {
+ if(!in_projection_range(cloud.pts[i].z))
+ continue;
+
+ updateFreeSpace(obs.origin_, cloud.pts[i].x, cloud.pts[i].y);
+ }
+ }*/
+
+
+
+ //Clear out the window.
+ unsigned int rayStartX = 0, rayStartY = 0, rayEndX = 0, rayEndY = 0;
+ WC_MC(computeWX(*this, raytraceWindow_ + inflationRadius_ * getResolution() * 2, robotX_, robotY_),
+ computeWY(*this, raytraceWindow_ + inflationRadius_ * getResolution() * 2, robotX_, robotY_), rayStartX, rayStartY);
+ rayEndX = rayStartX + raytraceCells_ + inflationRadius_ * 2;
+ rayEndY = rayStartY + raytraceCells_ + inflationRadius_ * 2;
+
+ unsigned int clearStartX = 0, clearStartY = 0, clearEndX = 0, clearEndY = 0;
+ WC_MC(computeWX(*this, raytraceWindow_, robotX_, robotY_),
+ computeWY(*this, raytraceWindow_, robotX_, robotY_), clearStartX, clearStartY);
+ clearEndX = clearStartX + raytraceCells_;
+ clearEndY = clearStartY + raytraceCells_;
+
+
+
+ for (unsigned int x = rayStartX; x < rayEndX; x++) {
+ for (unsigned int y = rayStartY; y < rayEndY; y++) {
+ unsigned int ind = MC_IND(x, y);
+
+ if (costData_[ind] != 0 && staticData_[ind] == LETHAL_OBSTACLE && !marked(ind)) {
+ enqueue(ind, x, y);
+
+ }
+ if (x > clearStartX && x < clearEndX && y > clearStartY && y < clearEndY) {
+ costData_[ind] = 0;
+ }
+ }
+ }
+
+
+
// Propagate costs
propagateCosts();
}
@@ -310,13 +355,15 @@
void CostMap2D::saveText(std::string file) {
std::ofstream of_text(file.c_str());
+
if (of_text.fail() || !of_text) {
- printf("Failed to open file %s.\n", file.c_str());
+ ROS_INFO("Failed to open file %s.\n", file.c_str());
return;
}
for (unsigned int i = 0; i < getWidth(); i++) {
for (unsigned int j = 0; j < getHeight(); j++) {
+ of_text.width(4);
of_text << (int)(getMap()[i + j * getWidth()]) << ",";
}
of_text << std::endl;
@@ -326,7 +373,7 @@
void CostMap2D::saveBinary(std::string file) {
std::ofstream of(file.c_str(), std::ios::out|std::ios::binary);
if (of.fail() || !of) {
- printf("Failed to open file %s.\n", file.c_str());
+ ROS_INFO("Failed to open file %s.\n", file.c_str());
return;
}
@@ -334,9 +381,6 @@
of.close();
}
-
-
-
/**
* @brief It is arguable if this is the correct update rule. We are trying to avoid
* tracing holes through walls by propagating adjacent cells that are in sensor range through
@@ -435,6 +479,15 @@
numpixels = deltay; // There are more y-values than x-values
}
+
+ //Static Ray trace zone.
+ unsigned int rayStartX = 0, rayStartY = 0, rayEndX = 0, rayEndY = 0;
+ WC_MC(computeWX(*this, raytraceWindow_, robotX_, robotY_),
+ computeWY(*this, raytraceWindow_, robotX_, robotY_), rayStartX, rayStartY);
+ rayEndX = rayStartX + raytraceCells_;
+ rayEndY = rayStartY + raytraceCells_;
+
+
for (unsigned int curpixel = 0; curpixel <= numpixels; curpixel++){
unsigned int index = MC_IND(x, y);
@@ -450,14 +503,20 @@
x += xinc2; // Change the x as appropriate
y += yinc2; // Change the y as appropriate
- if(!marked(index))
+ if(!marked(index) && staticData_[index] == CostMapAccessor::LETHAL_OBSTACLE) {
costData_[index] = 0;
+ }
//we want to break out of the loop if we have raytraced far enough
double sq_cell_dist = (x - startx) * (x - startx) + (y - starty) * (y - starty);
if(sq_cell_dist >= sq_raytrace_range_)
break;
+
+ if (x < rayStartX) { return; }
+ if (x > rayEndX) { return; }
+ if (y < rayStartY) { return; }
+ if (y > rayEndY) { return; }
}
}
@@ -501,38 +560,7 @@
}
}
- double CostMapAccessor::computeWX(const CostMap2D& costMap, double maxSize, double wx, double wy){
- unsigned int mx, my;
- costMap.WC_MC(wx, wy, mx, my);
- unsigned int cellWidth = (unsigned int) (maxSize/costMap.getResolution());
- unsigned int origin_mx(0);
-
- if(mx > cellWidth/2)
- origin_mx = mx - cellWidth/2;
-
- if(origin_mx + cellWidth > costMap.getWidth())
- origin_mx = costMap.getWidth() - cellWidth - 1;
-
- return origin_mx * costMap.getResolution();
- }
-
- double CostMapAccessor::computeWY(const CostMap2D& costMap, double maxSize, double wx, double wy){
- unsigned int mx, my;
- costMap.WC_MC(wx, wy, mx, my);
-
- unsigned int cellWidth = (unsigned int) (maxSize/costMap.getResolution());
- unsigned int origin_my(0);
-
- if(my > cellWidth/2)
- origin_my = my - cellWidth/2;
-
- if(origin_my + cellWidth > costMap.getHeight())
- origin_my = costMap.getHeight() - cellWidth - 1;
-
- return origin_my * costMap.getResolution();
- }
-
unsigned int CostMapAccessor::computeSize(double maxSize, double resolution){
unsigned int cellWidth = (unsigned int) ceil(maxSize/resolution);
ROS_DEBUG("Given a size of %f and a resolution of %f, we have a cell width of %d\n", maxSize, resolution, cellWidth);
Modified: pkg/trunk/world_models/costmap_2d/src/obstacle_map_accessor.cc
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/obstacle_map_accessor.cc 2008-12-23 23:48:49 UTC (rev 8552)
+++ pkg/trunk/world_models/costmap_2d/src/obstacle_map_accessor.cc 2008-12-23 23:53:44 UTC (rev 8553)
@@ -95,4 +95,36 @@
}
return ss.str();
}
+
+ double ObstacleMapAccessor::computeWX(const ObstacleMapAccessor& costMap, double maxSize, double wx, double wy){
+ unsigned int mx, my;
+ costMap.WC_MC(wx, wy, mx, my);
+
+ unsigned int cellWidth = (unsigned int) (maxSize/costMap.getResolution());
+ unsigned int origin_mx(0);
+
+ if(mx > cellWidth/2)
+ origin_mx = mx - cellWidth/2;
+
+ if(origin_mx + cellWidth > costMap.getWidth())
+ origin_mx = costMap.getWidth() - cellWidth - 1;
+
+ return origin_mx * costMap.getResolution();
+ }
+
+ double ObstacleMapAccessor::computeWY(const ObstacleMapAccessor& costMap, double maxSize, double wx, double wy){
+ unsigned int mx, my;
+ costMap.WC_MC(wx, wy, mx, my);
+
+ unsigned int cellWidth = (unsigned int) (maxSize/costMap.getResolution());
+ unsigned int origin_my(0);
+
+ if(my > cellWidth/2)
+ origin_my = my - cellWidth/2;
+
+ if(origin_my + cellWidth > costMap.getHeight())
+ origin_my = costMap.getHeight() - cellWidth - 1;
+
+ return origin_my * costMap.getResolution();
+ }
}
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-23 23:48:49 UTC (rev 8552)
+++ pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc 2008-12-23 23:53:44 UTC (rev 8553)
@@ -86,7 +86,7 @@
}
// 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);
+ CostMap2D map(10, 10, staticMap, RESOLUTION, THRESHOLD, MAX_Z, MAX_Z, MAX_Z, 3, 3, 3, 1, 100.0, 100.0, 3.0);
// Populate the cost map with a wall around the perimeter. Free space should clear out the room.
std_msgs::PointCloud cloud;
@@ -132,33 +132,46 @@
// Update the cost map for this observation
map.updateDynamicObstacles(wx, wy, obsBuf);
- // Verify that we now have only 8 * 4 + 4 cells with lethal cost, thus provong that we have correctly cleared
+
+ // Verify that we now have only 84 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);
+ ASSERT_EQ(hitCount, 84);
- // Veriy that we have 4 free cells
+ // Veriy that we have 16 non-leathal
hitCount = 0;
for(unsigned int i=0; i < 100; i++){
- if(map.getMap()[i] == 0)
+ if(map.getMap()[i] != CostMap2D::LETHAL_OBSTACLE)
hitCount++;
}
- ASSERT_EQ(hitCount, 4);
+ ASSERT_EQ(hitCount, 16);
// 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);
- }
+
+
+ //We should retain
+ hitCount = 0;
+ for(unsigned int i=0; i <100; i++){
+ if(map.getMap()[i] == CostMap2D::LETHAL_OBSTACLE)
+ hitCount++;
}
+ ASSERT_EQ(hitCount, 84);
+
+ // Veriy that we have 16 non-leathal
+ hitCount = 0;
+ for(unsigned int i=0; i < 100; i++){
+ if(map.getMap()[i] != CostMap2D::LETHAL_OBSTACLE)
+ hitCount++;
+ }
+ ASSERT_EQ(hitCount, 16);
}
/**
@@ -337,6 +350,7 @@
std::vector<unsigned int> updates;
map.updateDynamicObstacles(cloud, updates);
+
ASSERT_EQ(updates.size(), 3);
}
@@ -575,6 +589,7 @@
c0.pts[2].z = MAX_Z;
map.updateDynamicObstacles(c0, updates);
+
ASSERT_EQ(map.getCost(3, 2), CostMap2D::INSCRIBED_INFLATED_OBSTACLE);
ASSERT_EQ(map.getCost(3, 3), CostMap2D::INSCRIBED_INFLATED_OBSTACLE);
}
@@ -660,7 +675,7 @@
*/
TEST(costmap, test11){
- CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, THRESHOLD, MAX_Z * 2, MAX_Z, MAX_Z, ROBOT_RADIUS, 0, 0, 1, 100.0, 100.0);
+ CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, THRESHOLD, MAX_Z * 2, MAX_Z, MAX_Z, ROBOT_RADIUS, 0, 0, 1, 100.0, 100.0, 7.0);
// The initial position will be <0,0> by default. So if we add an obstacle at 9,9, we would expect cells
// <0, 0> thru <8, 8> to be free
@@ -676,11 +691,13 @@
// I considered allowing the cost function to over-ride this case but we quickly find that the planner will plan through walls once it gets out of sensor range.
// Note that this will not be the case when we persist the changes to the static map more aggressively since we will retain high cost obstacle data that
// has not been ray tarced thru. If that is the case, this update count would change to 6
- ASSERT_EQ(updates.size(), 4);
+ ASSERT_EQ(updates.size(), 5);
- // all cells will have been switched to free space along the diagonal except for this inflated in the update
- for(unsigned int i=0; i < 8; i++)
- ASSERT_EQ(map.getCost(i, i), 0);
+
+ // many cells will have been switched to free space along the diagonal except for those inflated in the update
+ unsigned char test[9]= {0, 0, 0, 126, 126, 0, 0, 126, 254};
+ for(unsigned int i=0; i < 9; i++)
+ ASSERT_EQ(map.getCost(i, i), test[i]);
}
@@ -691,11 +708,11 @@
test = fopen(a.c_str(), "r");
compare = fopen(b.c_str(), "r");
if (!test) {
- printf("Could not open: %s\n", a.c_str());
+ ROS_INFO("Could not open: %s\n", a.c_str());
return false;
}
if (!compare) {
- printf("Could not open: %s\n", b.c_str());
+ ROS_INFO("Could not open: %s\n", b.c_str());
return false;
}
@@ -735,6 +752,87 @@
ASSERT_EQ(compareFiles("testmap.bin", "testmap_compare.bin"), true);
}
+
+
+
+
+
+/**
+ * Within a certian radius of the robot, the cost map most propagate obstacles. This
+ * is to avoid a case where a hit on a far obstacle clears inscribed radius around a
+ * near one.
+ */
+
+TEST(costmap, test17){
+ const unsigned char MAP_HALL_CHAR[10 * 10] = {
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 254, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 254, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 254, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ };
+ std::vector<unsigned char> MAP_HALL;
+ for (int i = 0; i < 10 * 10; i++) {
+ MAP_HALL.push_back(MAP_HALL_CHAR[i]);
+ }
+
+
+ CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_HALL, RESOLUTION,
+ THRESHOLD, MAX_Z * 2, MAX_Z, MAX_Z, ROBOT_RADIUS, 0, 0, 1, 100.0, 100.0, 7.0);
+
+
+
+ //Add a dynamic obstacle
+ vector<std_msgs::PointCloud*> cv2;
+ std_msgs::PointCloud c2;
+ cv2.push_back(&c2);
+ c2.set_pts_size(2);
+ c2.pts[0].x = 9 - 2;
+ c2.pts[0].y = 9 - 1;
+ c2.pts[0].z = 1.0;
+ c2.pts[1].x = 3.0;
+ c2.pts[1].y = 4.0;
+ c2.pts[1].z = 1.0;
+ map.updateDynamicObstacles(0.0, 0.0, cv2);
+
+
+ const unsigned char MAP_HALL_CHAR_TEST[10 * 10] = {
+ 126, 254, 126, 0, 0, 0, 0, 0, 0, 0,
+ 0, 126, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 126, 0, 0, 0, 0, 0,
+ 0, 0, 0, 126, 254, 126, 0, 0, 0, 0,
+ 0, 0, 0, 0, 126, 0, 0, 126, 0, 0,
+ 0, 0, 0, 0, 0, 0, 126, 254, 126, 0,
+ 0, 0, 0, 0, 0, 0, 0, 126, 126, 0,
+ 0, 0, 0, 0, 0, 0, 0, 126, 254, 126,
+ 0, 0, 0, 0, 0, 0, 0, 0, 126, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ };
+
+
+ for (int i = 0; i < 10 * 10; i++) {
+ ASSERT_EQ(map.getCost(i / 10, i % 10), MAP_HALL_CHAR_TEST[i]);
+ }
+
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
int main(int argc, char** argv){
for(unsigned int i = 0; i< GRID_WIDTH * GRID_HEIGHT; i++){
EMPTY_10_BY_10.push_back(0);
Modified: pkg/trunk/world_models/costmap_2d/testmap_compare.txt
===================================================================
--- pkg/trunk/world_models/costmap_2d/testmap_compare.txt 2008-12-23 23:48:49 UTC (rev 8552)
+++ pkg/trunk/world_models/costmap_2d/testmap_compare.txt 2008-12-23 23:53:44 UTC (rev 8553)
@@ -1,10 +1,10 @@
-0,0,0,0,0,70,0,0,0,0,
-0,0,0,0,0,70,0,0,0,0,
-0,0,0,0,0,0,0,126,0,0,
-0,0,0,126,126,0,126,254,126,0,
-0,0,126,254,254,126,126,254,126,0,
-0,0,0,126,126,0,126,254,126,0,
-0,0,126,126,126,0,0,126,126,126,
-0,126,254,254,254,126,0,126,254,254,
-0,126,254,254,254,126,0,126,254,254,
-0,126,254,254,254,126,0,126,254,254,
+ 0, 0, 0, 0, 0, 70, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 70, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 126, 0, 0,
+ 0, 0, 0, 126, 126, 0, 126, 254, 126, 0,
+ 0, 0, 126, 254, 254, 126, 126, 254, 126, 0,
+ 0, 0, 0, 126, 126, 0, 126, 254, 126, 0,
+ 0, 0, 126, 126, 126, 0, 0, 126, 126, 126,
+ 0, 126, 254, 254, 254, 126, 0, 126, 254, 254,
+ 0, 126, 254, 254, 254, 126, 0, 126, 254, 254,
+ 0, 126, 254, 254, 254, 126, 0, 126, 254, 254,
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
