Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[13194] pkg/trunk
|
From: <ei...@us...> - 2009-03-31 23:57:21
|
Revision: 13194
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13194&view=rev
Author: eitanme
Date: 2009-03-31 23:57:15 +0000 (Tue, 31 Mar 2009)
Log Message:
-----------
r19710@cib: eitan | 2009-03-20 17:34:03 -0700
More work on basic costmap structure
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:19709
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:19710
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-03-31 23:57:01 UTC (rev 13193)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-03-31 23:57:15 UTC (rev 13194)
@@ -38,9 +38,22 @@
#define COSTMAP_COSTMAP_2D_H_
#include <vector>
+#include <queue>
#include <new_costmap/observation.h>
+#include <robot_msgs/PointCloud.h>
namespace costmap_2d {
+ //for priority queue propagation
+ class CostmapCell {
+ public:
+ double priority;
+ unsigned int index;
+ };
+
+ bool operator<(const CostmapCell &a, const CostmapCell &b){
+ return a.priority < b.priority;
+ }
+
/**
* @class Costmap
* @brief A 2D costmap provides a mapping between points in the world and their associated "costs".
@@ -96,6 +109,10 @@
*/
bool worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const;
+ inline unsigned int getIndex(unsigned int mx, unsigned int my) const{
+ return my * size_x_ + mx;
+ }
+
/**
* @brief Convert from map coordinates to world coordinates without checking for legal bounds
* @param wx The x world coordinate
@@ -118,7 +135,7 @@
* @brief Insert new obstacles into the cost map
* @param obstacles The point clouds of obstacles to insert into the map
*/
- void updateObstacles(const std::vector<Observation>& obstacles);
+ void updateObstacles(const std::vector<Observation>& observations);
/**
* @brief Clear freespace based on any number of planar scans
@@ -185,6 +202,10 @@
double origin_y_;
unsigned char* static_map_;
unsigned char* cost_map_;
+ unsigned char* markers_;
+ std::priority_queue<CostmapCell> inflation_queue;
+ double sq_obstacle_range_;
+ double max_obstacle_height_;
};
};
Modified: pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp
===================================================================
--- pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-03-31 23:57:01 UTC (rev 13193)
+++ pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-03-31 23:57:15 UTC (rev 13194)
@@ -37,9 +37,11 @@
#include <new_costmap/costmap_2d.h>
using namespace std;
+using namespace robot_msgs;
namespace costmap_2d{
+
Costmap2D::Costmap2D(double meters_size_x, double meters_size_y,
double resolution, double origin_x, double origin_y) : resolution_(resolution),
origin_x_(origin_x), origin_y_(origin_y) {
@@ -108,7 +110,7 @@
unsigned char local_map[cell_size_x * cell_size_y];
//copy the local window in the costmap to the local map
- unsigned int cost_map_index = start_y * size_x_ + start_x;
+ unsigned int cost_map_index = getIndex(start_x, start_y);
unsigned int local_map_index = 0;
for(unsigned int y = 0; y < cell_size_y; ++y){
for(unsigned int x = 0; x < cell_size_x; ++x){
@@ -121,10 +123,10 @@
}
//now we'll reset the costmap to the static map
- memcpy(cost_map_, static_map_, size_x_ * size_y_);
+ memcpy(cost_map_, static_map_, size_x_ * size_y_ * sizeof(unsigned char));
//now we want to copy the local map back into the costmap
- cost_map_index = start_y * size_x_ + start_x;
+ cost_map_index = getIndex(start_x, start_y);
local_map_index = 0;
for(unsigned int y = 0; y < cell_size_y; ++y){
for(unsigned int x = 0; x < cell_size_x; ++x){
@@ -137,8 +139,46 @@
}
}
- void Costmap2D::updateObstacles(const std::vector<Observation>& obstacles){}
+ void Costmap2D::updateObstacles(const std::vector<Observation>& observations){
+ //reset the markers for inflation
+ memset(markers_, 0, size_x_ * size_y_ * sizeof(unsigned char));
+ //place the new obstacles into a priority queue... each with a priority of zero to begin with
+ for(vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it){
+ const Observation& obs = *it;
+ const PointCloud& cloud =*(obs.cloud_);
+ for(unsigned int i = 0; i < cloud.pts.size(); ++i){
+ //if the obstacle is too high or too far away from the robot we won't add it
+ if(cloud.pts[i].z > max_obstacle_height_)
+ continue;
+
+ //compute the squared distance from the hitpoint to the pointcloud's origin
+ double sq_dist = (cloud.pts[i].x - obs.origin_.x) * (cloud.pts[i].x - obs.origin_.x)
+ + (cloud.pts[i].y - obs.origin_.y) * (cloud.pts[i].y - obs.origin_.y)
+ + (cloud.pts[i].z - obs.origin_.z) * (cloud.pts[i].z - obs.origin_.z);
+
+ //if the point is far enough away... we won't consider it
+ if(sq_dist >= sq_obstacle_range_)
+ continue;
+
+ //now we need to compute the map coordinates for the observation
+ unsigned int mx, my;
+ if(!worldToMap(cloud.pts[i].x, cloud.pts[i].y, mx, my))
+ continue;
+
+ unsigned int index = getIndex(mx, my);
+
+ CostmapCell c;
+ c.priority = 0.0;
+ c.index = index;
+
+ //push the relevant cell index back onto the inflation queue and mark it
+ inflation_queue.push(c);
+ markers_[index] = 1;
+ }
+ }
+ }
+
void Costmap2D::raytraceFreespace(const std::vector<Observation>& clearing_scans){}
void Costmap2D::inflateObstaclesInWindow(double wx, double wy, double w_size_x, double w_size_y){}
@@ -156,4 +196,5 @@
double Costmap2D::originY() const{}
double Costmap2D::resolution() const{}
+
};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
