Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[13212] pkg/trunk
|
From: <ei...@us...> - 2009-04-01 02:22:53
|
Revision: 13212
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13212&view=rev
Author: eitanme
Date: 2009-04-01 01:38:39 +0000 (Wed, 01 Apr 2009)
Log Message:
-----------
r20160@cib: eitan | 2009-03-26 19:57:13 -0700
Added a proper destrcutor to the cost map so it cleans up nicely
Modified Paths:
--------------
pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h
pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:20159
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:20160
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Modified: pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h
===================================================================
--- pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-04-01 01:38:24 UTC (rev 13211)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-04-01 01:38:39 UTC (rev 13212)
@@ -92,6 +92,11 @@
const std::vector<unsigned char>& static_data, unsigned char lethal_threshold);
/**
+ * @brief Destructor
+ */
+ ~Costmap2D();
+
+ /**
* @brief Revert to the static map outside of a specified window centered at a world coordinate
* @param wx The x coordinate of the center point of the window in world space (meters)
* @param wy The y coordinate of the center point of the window in world space (meters)
@@ -156,18 +161,7 @@
mx = index - (my * size_x_);
}
-
/**
- * @brief Convert from map coordinates to world coordinates without checking for legal bounds
- * @param wx The x world coordinate
- * @param wy The y world coordinate
- * @param mx Will be set to the associated map x coordinate
- * @param my Will be set to the associated map y coordinate
- */
- void worldToMapNoBounds(double wx, double wy, unsigned int& mx, unsigned int& my) const;
-
-
- /**
* @brief Accessor for the x size of the costmap in cells
* @return The x size of the costmap
*/
@@ -270,7 +264,6 @@
void resetInflationWindow(double wx, double wy, double w_size_x, double w_size_y,
std::priority_queue<CellData>& inflation_queue );
-
/**
* @brief Raytrace a line and apply some action at each step
* @param at The action to take... a functor
@@ -335,6 +328,15 @@
void inflateObstacles(std::priority_queue<CellData>& inflation_queue);
/**
+ * @brief Convert from map coordinates to world coordinates without checking for legal bounds
+ * @param wx The x world coordinate
+ * @param wy The y world coordinate
+ * @param mx Will be set to the associated map x coordinate
+ * @param my Will be set to the associated map y coordinate
+ */
+ void worldToMapNoBounds(double wx, double wy, unsigned int& mx, unsigned int& my) const;
+
+ /**
* @brief Takes the max of existing cost and the new cost... keeps static map obstacles from being overridden prematurely
* @param index The index od the cell to assign a cost to
* @param cost The cost
@@ -372,12 +374,28 @@
return cost;
}
+ /**
+ * @brief Lookup pre-computed costs
+ * @param mx The x coordinate of the current cell
+ * @param my The y coordinate of the current cell
+ * @param src_x The x coordinate of the source cell
+ * @param src_y The y coordinate of the source cell
+ * @return
+ */
inline char costLookup(int mx, int my, int src_x, int src_y){
unsigned int dx = abs(mx - src_x);
unsigned int dy = abs(my - src_y);
return cached_costs_[dx][dy];
}
+ /**
+ * @brief Lookup pre-computed distances
+ * @param mx The x coordinate of the current cell
+ * @param my The y coordinate of the current cell
+ * @param src_x The x coordinate of the source cell
+ * @param src_y The y coordinate of the source cell
+ * @return
+ */
inline double distanceLookup(int mx, int my, int src_x, int src_y){
unsigned int dx = abs(mx - src_x);
unsigned int dy = abs(my - src_y);
Modified: pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp
===================================================================
--- pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-04-01 01:38:24 UTC (rev 13211)
+++ pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-04-01 01:38:39 UTC (rev 13212)
@@ -100,6 +100,24 @@
}
}
+ Costmap2D::~Costmap2D(){
+ if(cost_map_ != NULL) delete[] cost_map_;
+ if(static_map_ != NULL) delete[] static_map_;
+ if(markers_ != NULL) delete[] markers_;
+
+ if(cached_distances_ != NULL){
+ for(unsigned int i = 0; i <= cell_inflation_radius_; ++i){
+ if(cached_distances_[i] != NULL) delete[] cached_distances_[i];
+ }
+ }
+
+ if(cached_costs_ != NULL){
+ for(unsigned int i = 0; i <= cell_inflation_radius_; ++i){
+ if(cached_costs_[i] != NULL) delete[] cached_costs_[i];
+ }
+ }
+ }
+
unsigned int Costmap2D::cellDistance(double world_dist){
double cells_dist = max(0.0, ceil(world_dist / resolution_));
return (unsigned int) cells_dist;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
