Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[5443] pkg/trunk
|
From: <mc...@us...> - 2008-10-16 20:19:22
|
Revision: 5443
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5443&view=rev
Author: mcgann
Date: 2008-10-16 20:18:27 +0000 (Thu, 16 Oct 2008)
Log Message:
-----------
Added filter for free space projection based on height of the point
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-10-16 20:15:38 UTC (rev 5442)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-10-16 20:18:27 UTC (rev 5443)
@@ -80,7 +80,8 @@
double windowLength(1.0);
unsigned char lethalObstacleThreshold(100);
unsigned char noInformation(CostMap2D::NO_INFORMATION);
- double maxZ(2.0);
+ double maxZ(2.0);
+ double freeSpaceProjectionHeight(0.5);
double inflationRadius(0.46);
double circumscribedRadius(0.46);
double inscribedRadius(0.325);
@@ -88,6 +89,7 @@
param("costmap_2d/lethal_obstacle_threshold", lethalObstacleThreshold, lethalObstacleThreshold);
param("costmap_2d/no_information_value", noInformation, noInformation);
param("costmap_2d/z_threshold", maxZ, maxZ);
+ param("costmap_2d/freespace_projection_height", freeSpaceProjectionHeight, freeSpaceProjectionHeight);
param("costmap_2d/inflation_radius", inflationRadius, inflationRadius);
param("costmap_2d/circumscribed_radius", circumscribedRadius, circumscribedRadius);
param("costmap_2d/inscribed_radius", inscribedRadius, inscribedRadius);
@@ -119,7 +121,7 @@
// Now allocate the cost map and its sliding window used by the controller
costMap_ = new CostMap2D((unsigned int)resp.map.width, (unsigned int)resp.map.height,
inputData , resp.map.resolution,
- windowLength, lethalObstacleThreshold, maxZ,
+ windowLength, lethalObstacleThreshold, maxZ, freeSpaceProjectionHeight,
inflationRadius, circumscribedRadius, inscribedRadius);
// Allocate Velocity Controller
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-10-16 20:15:38 UTC (rev 5442)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2008-10-16 20:18:27 UTC (rev 5443)
@@ -88,14 +88,16 @@
* @param window_length how long to hold onto obstacle data [sec]
* @param threshold The cost threshold where a cell is considered an obstacle
* @param maxZ gives the cut-off for points in 3D space
+ * @param freeSpaceProjectionHeight gives the upper bound for evaluating points in z for projecting free space
* @param inflationRadius the radius used to bound inflation - limit of cost propagation
* @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
*/
CostMap2D(unsigned int width, unsigned int height, const std::vector<unsigned char>& data,
double resolution, double window_length,
- unsigned char threshold, double maxZ = 0, double inflationRadius = 0,
- double circumscribedRadius = 0, double inscribedRadius = 0);
+ unsigned char threshold,
+ double maxZ = 0, double freeSpaceProjectionHeight = 0,
+ double inflationRadius = 0, double circumscribedRadius = 0, double inscribedRadius = 0);
/**
* @brief Destructor.
@@ -227,6 +229,7 @@
static const TICK WATCHDOG_LIMIT = 255; /**< The value for a reset watchdog time for observing dynamic obstacles */
const double tickLength_; /**< The duration in seconds of a tick, used to manage the watchdog timeout on obstacles. Computed from window length */
const double maxZ_; /**< Points above this will be excluded from consideration */
+ const double freeSpaceProjectionHeight_; /**< Filters points for free space projection */
const unsigned int inflationRadius_; /**< The radius in meters to propagate cost and obstacle information */
const unsigned int circumscribedRadius_;
const unsigned int inscribedRadius_;
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp 2008-10-16 20:15:38 UTC (rev 5442)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp 2008-10-16 20:18:27 UTC (rev 5443)
@@ -69,11 +69,12 @@
CostMap2D::CostMap2D(unsigned int width, unsigned int height, const std::vector<unsigned char>& data,
double resolution, double window_length, unsigned char threshold,
- double maxZ, double inflationRadius,
- double circumscribedRadius, double inscribedRadius)
+ double maxZ, double freeSpaceProjectionHeight,
+ double inflationRadius, double circumscribedRadius, double inscribedRadius)
: ObstacleMapAccessor(0, 0, width, height, resolution),
tickLength_(window_length/WATCHDOG_LIMIT),
maxZ_(maxZ),
+ freeSpaceProjectionHeight_(freeSpaceProjectionHeight),
inflationRadius_(toCellDistance(inflationRadius, (unsigned int) ceil(width * resolution), resolution)),
circumscribedRadius_(toCellDistance(circumscribedRadius, inflationRadius_, resolution)),
inscribedRadius_(toCellDistance(inscribedRadius, circumscribedRadius_, resolution)),
@@ -153,8 +154,11 @@
unsigned int ind = WC_IND(cloud.pts[i].x, cloud.pts[i].y);
queue.push(std::make_pair(0, ind));
- // Immediately update free space, which is dominated by propagated costs so they are applied afterwards
- updateFreeSpace(ind, updates);
+ // Immediately update free space, which is dominated by propagated costs so they are applied afterwards.
+ // This only applies for points with the projection height limit
+
+ if(cloud.pts[i].z <= freeSpaceProjectionHeight_)
+ updateFreeSpace(ind, updates);
}
// Propagate costs
Modified: pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc 2008-10-16 20:15:38 UTC (rev 5442)
+++ pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc 2008-10-16 20:18:27 UTC (rev 5443)
@@ -269,7 +269,7 @@
* Test inflation for both static and dynamic obstacles
*/
TEST(costmap, test7){
- CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, WINDOW_LENGTH, THRESHOLD, MAX_Z,
+ CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, WINDOW_LENGTH, THRESHOLD, MAX_Z, MAX_Z,
ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS);
@@ -354,7 +354,7 @@
* Test specific inflation scenario to ensure we do not set inflated obstacles to be raw obstacles.
*/
TEST(costmap, test8){
- CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, WINDOW_LENGTH, THRESHOLD, MAX_Z,
+ CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, WINDOW_LENGTH, THRESHOLD, MAX_Z, MAX_Z,
ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS);
std::vector<unsigned int> updates;
@@ -388,7 +388,7 @@
}
}
- CostMap2D map(GRID_WIDTH, GRID_HEIGHT, mapData, RESOLUTION, WINDOW_LENGTH, THRESHOLD, MAX_Z,
+ CostMap2D map(GRID_WIDTH, GRID_HEIGHT, mapData, RESOLUTION, WINDOW_LENGTH, THRESHOLD, MAX_Z, MAX_Z,
ROBOT_RADIUS * 3, ROBOT_RADIUS * 2, ROBOT_RADIUS);
// There should be no occupied cells
@@ -427,7 +427,7 @@
* Test for the cost map accessor
*/
TEST(costmap, test10){
- CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, WINDOW_LENGTH, THRESHOLD, MAX_Z, ROBOT_RADIUS);
+ CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, WINDOW_LENGTH, THRESHOLD, MAX_Z, MAX_Z, ROBOT_RADIUS);
// A window around a robot in the top left
CostMapAccessor ma(map, 5, 0, 0);
@@ -464,7 +464,7 @@
* Test for ray tracing free space
*/
TEST(costmap, test11){
- CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, WINDOW_LENGTH, THRESHOLD, MAX_Z, ROBOT_RADIUS);
+ CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, WINDOW_LENGTH, THRESHOLD, MAX_Z * 2, MAX_Z, ROBOT_RADIUS);
// 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 <7, 7> to be free. This is despite the fact that some of these cells are not zero cost in the
@@ -473,7 +473,7 @@
c0.set_pts_size(1);
c0.pts[0].x = 9.5;
c0.pts[0].y = 9.5;
- c0.pts[0].z = 0;
+ c0.pts[0].z = MAX_Z;
std::vector<unsigned int> updates;
map.updateDynamicObstacles(1, c0, updates);
@@ -504,6 +504,17 @@
// setting NO_INFORMATION to free space. Minor redundant cell update
map.updateDynamicObstacles(WINDOW_LENGTH + 2, 0.5, 9.5, c0, updates);
ASSERT_EQ(updates.size(), 6);
+
+ // Stale out all dynamic obstacles - then try again with point that is beyond free space projection
+ map.removeStaleObstacles(WINDOW_LENGTH * 3, updates);
+ std_msgs::PointCloudFloat32 c1;
+ c1.set_pts_size(1);
+ c1.pts[0].x = 9.5;
+ c1.pts[0].y = 9.5;
+ c1.pts[0].z = MAX_Z + 1;
+ map.updateDynamicObstacles(1, c1, updates);
+ ASSERT_EQ(updates.size(), 4); // Just obstacle cost propagation - no free space impact
+
}
int main(int argc, char** argv){
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
