Menu
▾
▴
personalrobots-commit
|
From: <ei...@us...> - 2009-04-01 02:22:14
|
Revision: 13206
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13206&view=rev
Author: eitanme
Date: 2009-04-01 01:32:19 +0000 (Wed, 01 Apr 2009)
Log Message:
-----------
r20001@cib: eitan | 2009-03-24 15:27:14 -0700
Finsihed implementation... now... to test
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:19995
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:20001
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:32:03 UTC (rev 13205)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-04-01 01:32:19 UTC (rev 13206)
@@ -150,10 +150,10 @@
unsigned char cost = 0;
if(distance == 0)
cost = LETHAL_OBSTACLE;
- else if(distance <= inscribed_radius_)
+ else if(distance <= cell_inscribed_radius_)
cost = INSCRIBED_INFLATED_OBSTACLE;
else {
- double factor = weight_ / (1 + pow(distance - inscribed_radius_, 2));
+ double factor = weight_ / (1 + pow(distance - cell_inscribed_radius_, 2));
cost = (unsigned char) ((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
}
return cost;
@@ -190,25 +190,42 @@
void resetMapOutsideWindow(double wx, double wy, double w_size_x, double w_size_y);
/**
+ * @brief Update the costmap with new observations
+ * @param obstacles The point clouds of obstacles to insert into the map
+ * @param clearing_observations The set of observations to use for raytracing
+ */
+ void updateWorld(double robot_x, double robot_y,
+ const std::vector<Observation>& observations, const std::vector<Observation>& clearing_observations);
+
+ /**
* @brief Insert new obstacles into the cost map
* @param obstacles The point clouds of obstacles to insert into the map
+ * @param inflation_queue The queue to place the obstacles into for inflation
*/
- void updateObstacles(const std::vector<Observation>& observations);
+ void updateObstacles(const std::vector<Observation>& observations, std::priority_queue<CellData>& inflation_queue);
/**
- * @brief Clear freespace based on any number of planar scans
- * @param clearing_scans The scans used to raytrace
+ * @brief Clear freespace based on any number of observations
+ * @param clearing_observations The observations used to raytrace
*/
- void raytraceFreespace(const std::vector<Observation>& clearing_scans);
+ void raytraceFreespace(const std::vector<Observation>& clearing_observations);
/**
- * @brief Inflate obstacles within a given window centered at a point in world coordinates
+ * @brief Clear freespace from an observation
+ * @param clearing_observation The observation used to raytrace
+ */
+ void raytraceFreespace(const Observation& clearing_observation);
+
+ /**
+ * @brief Provides support for re-inflating obstacles within a certain window (used after raytracing)
* @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)
* @param w_size_x The x size of the window in meters
* @param w_size_y The y size of the window in meters
+ * @param inflation_queue The priority queue to push items back onto for propogation
*/
- void inflateObstaclesInWindow(double wx, double wy, double w_size_x, double w_size_y);
+ void resetInflationWindow(double wx, double wy, double w_size_x, double w_size_y,
+ std::priority_queue<CellData>& inflation_queue );
/**
* @brief Accessor for the x size of the costmap in cells
@@ -224,13 +241,13 @@
/**
* @brief Accessor for the x size of the costmap in meters
- * @return The x size of the costmap
+ * @return The x size of the costmap (returns the centerpoint of the last legal cell in the map)
*/
double metersSizeX() const;
/**
* @brief Accessor for the y size of the costmap in meters
- * @return The y size of the costmap
+ * @return The y size of the costmap (returns the centerpoint of the last legal cell in the map)
*/
double metersSizeY() const;
@@ -264,7 +281,7 @@
* @return
*/
inline void enqueue(unsigned int index, unsigned int mx, unsigned int my,
- unsigned int src_x, unsigned int src_y, std::priority_queue<CellData*>& inflation_queue){
+ unsigned int src_x, unsigned int src_y, std::priority_queue<CellData>& inflation_queue){
unsigned char* marked = &markers_[index];
//set the cost of the cell being inserted
if(*marked == 0){
@@ -272,14 +289,14 @@
double distance = distanceLookup(mx, my, src_x, src_y);
//we only want to put the cell in the queue if it is within the inflation radius of the obstacle point
- if(distance > inflation_radius_)
+ if(distance > cell_inflation_radius_)
return;
//assign the cost associated with the distance from an obstacle to the cell
cost_map_[index] = costLookup(mx, my, src_x, src_y);
//push the cell data onto the queue and mark
- inflation_queue.push(new CellData(distance, index, mx, my, src_x, src_y));
+ inflation_queue.push(CellData(distance, index, mx, my, src_x, src_y));
*marked = 1;
}
}
@@ -332,10 +349,12 @@
* @brief Given a priority queue with the actual obstacles, compute the inflated costs for the costmap
* @param inflation_queue A priority queue contatining the cell data for the actual obstacles
*/
- void inflateObstacles(std::priority_queue<CellData*>& inflation_queue);
+ void inflateObstacles(std::priority_queue<CellData>& inflation_queue);
unsigned int cellDistance(double world_dist);
+ double worldDistance(unsigned int cell_dist);
+
unsigned int size_x_;
unsigned int size_y_;
double resolution_;
@@ -349,8 +368,10 @@
double raytrace_range_;
unsigned char** cached_costs_;
double** cached_distances_;
- unsigned int inscribed_radius_, circumscribed_radius_, inflation_radius_;
+ double inscribed_radius_, circumscribed_radius_, inflation_radius_;
+ unsigned int cell_inscribed_radius_, cell_circumscribed_radius_, cell_inflation_radius_;
double weight_;
+ std::priority_queue<CellData> inflation_queue_;
//functors for raytracing actions
class ClearCell {
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:32:03 UTC (rev 13205)
+++ pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-04-01 01:32:19 UTC (rev 13206)
@@ -62,17 +62,17 @@
inscribed_radius_(inscribed_radius), circumscribed_radius_(circumscribed_radius), inflation_radius_(inflation_radius),
weight_(weight){
//convert our inflations from world to cell distance
- inscribed_radius_ = cellDistance(inscribed_radius);
- circumscribed_radius_ = cellDistance(circumscribed_radius);
- inflation_radius_ = cellDistance(inflation_radius);
+ cell_inscribed_radius_ = cellDistance(inscribed_radius);
+ cell_circumscribed_radius_ = cellDistance(circumscribed_radius);
+ cell_inflation_radius_ = cellDistance(inflation_radius);
//based on the inflation radius... compute distance and cost caches
- cached_costs_ = new unsigned char*[inflation_radius_ + 1];
- cached_distances_ = new double*[inflation_radius_ + 1];
- for(unsigned int i = 0; i <= inflation_radius_; ++i){
- cached_costs_[i] = new unsigned char[inflation_radius_ + 1];
- cached_distances_[i] = new double[inflation_radius_ + 1];
- for(unsigned int j = 0; i <= inflation_radius_; ++j){
+ cached_costs_ = new unsigned char*[cell_inflation_radius_ + 1];
+ cached_distances_ = new double*[cell_inflation_radius_ + 1];
+ for(unsigned int i = 0; i <= cell_inflation_radius_; ++i){
+ cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 1];
+ cached_distances_[i] = new double[cell_inflation_radius_ + 1];
+ for(unsigned int j = 0; i <= cell_inflation_radius_; ++j){
cached_distances_[i][j] = sqrt(i*i + j*j);
cached_costs_[i][j] = computeCost(cached_distances_[i][j]);
}
@@ -81,8 +81,8 @@
if(!static_data.empty()){
ROS_ASSERT_MSG(size_x_ * size_y_ == static_data_.size(), "If you want to initialize a costmap with static data, their sizes must match.");
- //we'll need a priority queue for inflation
- std::priority_queue<CellData*> inflation_queue;
+ //make sure the inflation queue is empty at the beginning of the cycle (should always be true)
+ ROS_ASSERT_MSG(inflation_queue_.empty(), "The inflation queue must be empty at the beginning of inflation");
//initialize the costmap with static data
for(unsigned int i = 0; i < size_x_; ++i){
@@ -93,12 +93,12 @@
if(cost_map_[index] == LETHAL_OBSTACLE){
unsigned int mx, my;
indexToCells(index, mx, my);
- enqueue(index, mx, my, mx, my, inflation_queue);
+ enqueue(index, mx, my, mx, my, inflation_queue_);
}
}
}
//now... let's inflate the obstacles
- inflateObstacles(inflation_queue);
+ inflateObstacles(inflation_queue_);
//we also want to keep a copy of the current costmap as the static map
memcpy(static_map_, cost_map_, size_x_ * size_y_);
@@ -110,6 +110,11 @@
return (unsigned int) cells_dist;
}
+ double Costmap2D::worldDistance(unsigned int cell_dist){
+ double world_dist = cell_dist * resolution_;
+ return world_dist;
+ }
+
unsigned char Costmap2D::getCellCost(unsigned int mx, unsigned int my) const {
ROS_ASSERT_MSG(mx < size_x_ && my < size_y_, "You cannot get the cost of a cell that is outside the bounds of the costmap");
return cost_map_[my * size_x_ + mx];
@@ -164,42 +169,59 @@
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 = getIndex(start_x, start_y);
- unsigned int local_map_index = 0;
+ unsigned char* cost_map_cell = &cost_map_[getIndex(start_x, start_y)];
+ unsigned char* local_map_cell = local_map;
for(unsigned int y = 0; y < cell_size_y; ++y){
for(unsigned int x = 0; x < cell_size_x; ++x){
- local_map[local_map_index] = cost_map_[cost_map_index];
- local_map_index++;
- cost_map_index++;
+ *local_map_cell = *cost_map_cell;
+ local_map_cell++;
+ cost_map_cell++;
}
- local_map_index += cell_size_x;
- cost_map_index += size_x_;
+ local_map_cell += cell_size_x;
+ cost_map_cell += size_x_;
}
//now we'll reset the costmap to the static map
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 = getIndex(start_x, start_y);
- local_map_index = 0;
+ cost_map_cell = &cost_map_[getIndex(start_x, start_y)];
+ local_map_cell = local_map;
for(unsigned int y = 0; y < cell_size_y; ++y){
for(unsigned int x = 0; x < cell_size_x; ++x){
- cost_map_[cost_map_index] = local_map[local_map_index];
- local_map_index++;
- cost_map_index++;
+ *cost_map_cell = *local_map_cell;
+ local_map_cell++;
+ cost_map_cell++;
}
- local_map_index += cell_size_x;
- cost_map_index += size_x_;
+ local_map_cell += cell_size_x;
+ cost_map_cell += size_x_;
}
}
- void Costmap2D::updateObstacles(const std::vector<Observation>& observations){
+ void Costmap2D::updateWorld(double robot_x, double robot_y,
+ const vector<Observation>& observations, const vector<Observation>& clearing_observations){
//reset the markers for inflation
memset(markers_, 0, size_x_ * size_y_ * sizeof(unsigned char));
- //create a priority queue
- std::priority_queue<CellData*> inflation_queue;
+ //make sure the inflation queue is empty at the beginning of the cycle (should always be true)
+ ROS_ASSERT_MSG(inflation_queue_.empty(), "The inflation queue must be empty at the beginning of inflation");
+ //raytrace freespace
+ raytraceFreespace(clearing_observations);
+
+ //if we raytrace X meters out... we must re-inflate obstacles within the containing square of that circle
+ double inflation_window_size = 2 * (raytrace_range_ + inflation_radius_);
+
+ //reset the inflation window.. clears all costs except lethal costs and adds them to the queue for re-propagation
+ resetInflationWindow(robot_x, robot_y, inflation_window_size, inflation_window_size, inflation_queue_);
+
+ //now we also want to add the new obstacles we've received to the cost map
+ updateObstacles(observations, inflation_queue_);
+
+ inflateObstacles(inflation_queue_);
+ }
+
+ void Costmap2D::updateObstacles(const vector<Observation>& observations, priority_queue<CellData>& inflation_queue){
//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;
@@ -229,21 +251,18 @@
enqueue(index, mx, my, mx, my, inflation_queue);
}
}
- inflateObstacles(inflation_queue);
}
- void Costmap2D::inflateObstacles(priority_queue<CellData*>& inflation_queue){
- CellData* current_cell = NULL;
+ void Costmap2D::inflateObstacles(priority_queue<CellData>& inflation_queue){
while(!inflation_queue.empty()){
//get the highest priority cell and pop it off the priority queue
- current_cell = inflation_queue.top();
- inflation_queue.pop();
+ const CellData& current_cell = inflation_queue.top();
- unsigned int index = current_cell->index_;
- unsigned int mx = current_cell->x_;
- unsigned int my = current_cell->y_;
- unsigned int sx = current_cell->src_x_;
- unsigned int sy = current_cell->src_y_;
+ unsigned int index = current_cell.index_;
+ unsigned int mx = current_cell.x_;
+ unsigned int my = current_cell.y_;
+ unsigned int sx = current_cell.src_x_;
+ unsigned int sy = current_cell.src_y_;
//attempt to put the neighbors of the current cell onto the queue
if(mx > 0)
@@ -255,97 +274,155 @@
if(my < size_y_ - 1)
enqueue(index + size_x_, mx, my + 1, sx, sy, inflation_queue);
- //delete the current_cell b/c it is no longer on the queue and no longer needed
- delete current_cell;
+ //remove the current cell from the priority queue
+ inflation_queue.pop();
}
}
- void Costmap2D::raytraceFreespace(const std::vector<Observation>& clearing_scans){
+ void Costmap2D::raytraceFreespace(const std::vector<Observation>& clearing_observations){
+ for(unsigned int i = 0; i < clearing_observations.size(); ++i){
+ raytraceFreespace(clearing_observations[i]);
+ }
+ }
+
+ void Costmap2D::raytraceFreespace(const Observation& clearing_observation){
//pre-comput the squared raytrace range... saves a sqrt in an inner loop
double sq_raytrace_range = raytrace_range_ * raytrace_range_;
//create the functor that we'll use to clear cells from the costmap
ClearCell clearer(cost_map_);
- for(unsigned int i = 0; i < clearing_scans.size(); ++i){
- double ox = clearing_scans[i].origin_.x;
- double oy = clearing_scans[i].origin_.y;
- PointCloud* cloud = clearing_scans[i].cloud_;
+ double ox = clearing_observation.origin_.x;
+ double oy = clearing_observation.origin_.y;
+ PointCloud* cloud = clearing_observation.cloud_;
- //get the map coordinates of the origin of the sensor
- unsigned int x0, y0;
- if(!worldToMap(ox, oy, x0, y0))
- return;
+ //get the map coordinates of the origin of the sensor
+ unsigned int x0, y0;
+ if(!worldToMap(ox, oy, x0, y0))
+ return;
- //we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
- double end_x = origin_x_ + metersSizeX();
- double end_y = origin_y_ + metersSizeY();
+ //we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
+ double end_x = origin_x_ + metersSizeX();
+ double end_y = origin_y_ + metersSizeY();
- //for each point in the cloud, we want to trace a line from the origin and clear obstacles along it
- for(unsigned int j = 0; j < cloud->pts.size(); ++j){
- double wx = cloud->pts[j].x;
- double wy = cloud->pts[i].y;
+ //for each point in the cloud, we want to trace a line from the origin and clear obstacles along it
+ for(unsigned int i = 0; i < cloud->pts.size(); ++i){
+ double wx = cloud->pts[i].x;
+ double wy = cloud->pts[i].y;
- //we want to check that we scale the vector so that we only trace to the desired distance
- double sq_distance = (wx - ox) * (wx - ox) + (wy - oy) * (wy - oy);
- double scaling_fact = sq_raytrace_range / sq_distance;
- scaling_fact = scaling_fact > 1.0 ? 1.0 : scaling_fact;
+ //we want to check that we scale the vector so that we only trace to the desired distance
+ double sq_distance = (wx - ox) * (wx - ox) + (wy - oy) * (wy - oy);
+ double scaling_fact = sq_raytrace_range / sq_distance;
+ scaling_fact = scaling_fact > 1.0 ? 1.0 : scaling_fact;
- //now we also need to make sure that the enpoint we're raytracing
- //to isn't off the costmap and scale if necessary
- double a = wx - ox;
- double b = wy - oy;
+ //now we also need to make sure that the enpoint we're raytracing
+ //to isn't off the costmap and scale if necessary
+ double a = wx - ox;
+ double b = wy - oy;
- //the minimum value to raytrace from is the origin
- if(wx < origin_x_){
- double t = (origin_x_ - ox) / a;
- wx = ox + a * t;
- wy = oy + b * t;
- }
- if(wy < origin_y_){
- double t = (origin_y_ - oy) / b;
- wx = ox + a * t;
- wy = oy + b * t;
- }
+ //the minimum value to raytrace from is the origin
+ if(wx < origin_x_){
+ double t = (origin_x_ - ox) / a;
+ wx = ox + a * t;
+ wy = oy + b * t;
+ }
+ if(wy < origin_y_){
+ double t = (origin_y_ - oy) / b;
+ wx = ox + a * t;
+ wy = oy + b * t;
+ }
- //the maximum value to raytrace to is the end of the map
- if(wx > end_x){
- double t = (end_x - ox) / a;
- wx = ox + a * t;
- wy = oy + b * t;
- }
- if(wy > end_y){
- double t = (end_y - oy) / b;
- wx = ox + a * t;
- wy = oy + b * t;
- }
+ //the maximum value to raytrace to is the end of the map
+ if(wx > end_x){
+ double t = (end_x - ox) / a;
+ wx = ox + a * t;
+ wy = oy + b * t;
+ }
+ if(wy > end_y){
+ double t = (end_y - oy) / b;
+ wx = ox + a * t;
+ wy = oy + b * t;
+ }
- //now that the vector is scaled correctly... we'll get the map coordinates of its endpoint
- unsigned int x1, y1;
- worldToMap(wx, wy, x1, y1);
+ //now that the vector is scaled correctly... we'll get the map coordinates of its endpoint
+ unsigned int x1, y1;
+ worldToMap(wx, wy, x1, y1);
- //and finally... we can execute our trace to clear obstacles along that line
- raytraceLine(clearer, x0, y0, x1, y1);
+ //and finally... we can execute our trace to clear obstacles along that line
+ raytraceLine(clearer, x0, y0, x1, y1);
+ }
+ }
+
+ void Costmap2D::resetInflationWindow(double wx, double wy, double w_size_x, double w_size_y,
+ priority_queue<CellData>& inflation_queue){
+ //get the cell coordinates of the center point of the window
+ unsigned int mx, my;
+ if(!worldToMap(wx, wy, mx, my))
+ return;
+
+ //compute the bounds of the window
+ double start_x = wx - w_size_x / 2;
+ double start_y = wy - w_size_y / 2;
+ double end_x = start_x + w_size_x;
+ double end_y = start_y + w_size_y;
+
+ //scale the window based on the bounds of the costmap
+ start_x = start_x < origin_x_ ? origin_x_ : start_x;
+ start_y = start_y < origin_y_ ? origin_y_ : start_y;
+
+ end_x = end_x > origin_x_ + metersSizeX() ? origin_x_ + metersSizeX() : end_x;
+ end_y = end_y > origin_y_ + metersSizeY() ? origin_y_ + metersSizeY() : end_y;
+
+ //get the map coordinates of the bounds of the window
+ unsigned int map_sx, map_sy, map_ex, map_ey;
+ worldToMap(start_x, start_y, map_sx, map_sy);
+ worldToMap(end_x, end_y, map_ex, map_ey);
+
+ //we know that we want to clear all non-lethal obstacles in this window to get it ready for inflation
+ unsigned int index = getIndex(map_sx, map_sy);
+ unsigned char* current = &cost_map_[index];
+ for(unsigned int j = map_sy; j <= map_ey; ++j){
+ for(unsigned int i = map_sx; i <= map_ex; ++i){
+ //if the cell is a lethal obstacle... we'll keep it and queue it, otherwise... we'll clear it
+ if(*current == LETHAL_OBSTACLE)
+ enqueue(index, i, j, i, j, inflation_queue);
+ else
+ *current = 0;
+ current++;
+ index++;
}
+ current += size_x_;
+ index += size_x_;
}
}
- void Costmap2D::inflateObstaclesInWindow(double wx, double wy, double w_size_x, double w_size_y){
+ unsigned int Costmap2D::cellSizeX() const{
+ return size_x_;
}
- unsigned int Costmap2D::cellSizeX() const{}
+ unsigned int Costmap2D::cellSizeY() const{
+ return size_y_;
+ }
- unsigned int Costmap2D::cellSizeY() const{}
+ double Costmap2D::metersSizeX() const{
+ return (size_x_ - 1 + 0.5) * resolution_;
+ }
- double Costmap2D::metersSizeX() const{}
+ double Costmap2D::metersSizeY() const{
+ return (size_y_ - 1 + 0.5) * resolution_;
+ }
- double Costmap2D::metersSizeY() const{}
+ double Costmap2D::originX() const{
+ return origin_x_;
+ }
- double Costmap2D::originX() const{}
+ double Costmap2D::originY() const{
+ return origin_y_;
+ }
- double Costmap2D::originY() const{}
+ double Costmap2D::resolution() const{
+ return resolution_;
+ }
- double Costmap2D::resolution() const{}
-
};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-04-01 02:22:39
|
Revision: 13209
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13209&view=rev
Author: eitanme
Date: 2009-04-01 01:37:58 +0000 (Wed, 01 Apr 2009)
Log Message:
-----------
r20082@cib: eitan | 2009-03-25 10:01:33 -0700
Put scaling factor back in now that window problem is fixed
Modified Paths:
--------------
pkg/trunk/nav/trajectory_rollout/src/voxel_grid_model.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:20003
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:20082
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/nav/trajectory_rollout/src/voxel_grid_model.cpp
===================================================================
--- pkg/trunk/nav/trajectory_rollout/src/voxel_grid_model.cpp 2009-04-01 01:33:19 UTC (rev 13208)
+++ pkg/trunk/nav/trajectory_rollout/src/voxel_grid_model.cpp 2009-04-01 01:37:58 UTC (rev 13209)
@@ -233,8 +233,7 @@
double distance = dist(ox, oy, oz, wpx, wpy, wpz);
double scaling_fact = raytrace_range / distance;
- //scaling_fact = scaling_fact > 1 ? 1 : scaling_fact;
- scaling_fact = 1.0;
+ scaling_fact = scaling_fact > 1.0 ? 1.0 : scaling_fact;
wpx = scaling_fact * (wpx - ox) + ox;
wpy = scaling_fact * (wpy - oy) + oy;
wpz = scaling_fact * (wpz - oz) + oz;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-04-01 02:22:52
|
Revision: 13210
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13210&view=rev
Author: eitanme
Date: 2009-04-01 01:38:10 +0000 (Wed, 01 Apr 2009)
Log Message:
-----------
r20146@cib: eitan | 2009-03-26 12:56:13 -0700
A few bug fixes... getting much closer to working fully
Modified Paths:
--------------
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:20082
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:20146
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/src/costmap_2d.cpp
===================================================================
--- pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-04-01 01:37:58 UTC (rev 13209)
+++ pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-04-01 01:38:10 UTC (rev 13210)
@@ -139,18 +139,19 @@
double start_point_x = wx - w_size_x / 2;
double start_point_y = wy - w_size_y / 2;
+ //check start bounds
+ start_point_x = max(origin_x_, start_point_x);
+ start_point_y = max(origin_y_, start_point_y);
+
unsigned int start_x, start_y, end_x, end_y;
- //we'll do our own bounds checking for the window
- worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y);
+ worldToMap(start_point_x, start_point_y, start_x, start_y);
+
+ //we'll do our own bounds checking for the end of the window
worldToMapNoBounds(start_point_x + w_size_x, start_point_y + w_size_y, end_x, end_y);
- //check start bounds
- start_x = start_point_x < origin_x_ ? 0 : start_x;
- start_y = start_point_y < origin_y_ ? 0 : start_y;
-
//check end bounds
- end_x = end_x > size_x_ ? size_x_ : end_x;
- end_y = end_y > size_y_ ? size_y_ : end_y;
+ end_x = min(end_x, size_x_);
+ end_y = min(end_y, size_y_);
unsigned int cell_size_x = end_x - start_x;
unsigned int cell_size_y = end_y - start_y;
@@ -167,8 +168,7 @@
local_map_cell++;
cost_map_cell++;
}
- local_map_cell += cell_size_x;
- cost_map_cell += size_x_;
+ cost_map_cell += size_x_ - cell_size_x;
}
//now we'll reset the costmap to the static map
@@ -183,8 +183,7 @@
local_map_cell++;
cost_map_cell++;
}
- local_map_cell += cell_size_x;
- cost_map_cell += size_x_;
+ cost_map_cell += size_x_ - cell_size_x;
}
}
@@ -215,7 +214,9 @@
//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_);
+
+ 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_)
@@ -285,7 +286,7 @@
double ox = clearing_observation.origin_.x;
double oy = clearing_observation.origin_.y;
- PointCloud* cloud = clearing_observation.cloud_;
+ PointCloud cloud = clearing_observation.cloud_;
//get the map coordinates of the origin of the sensor
unsigned int x0, y0;
@@ -293,24 +294,28 @@
return;
//we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
- double end_x = origin_x_ + metersSizeX();
- double end_y = origin_y_ + metersSizeY();
+ double map_end_x = origin_x_ + metersSizeX();
+ double map_end_y = origin_y_ + metersSizeY();
//for each point in the cloud, we want to trace a line from the origin and clear obstacles along it
- for(unsigned int i = 0; i < cloud->pts.size(); ++i){
- double wx = cloud->pts[i].x;
- double wy = cloud->pts[i].y;
+ for(unsigned int i = 0; i < cloud.pts.size(); ++i){
+ double wx = cloud.pts[i].x;
+ double wy = cloud.pts[i].y;
//we want to check that we scale the vector so that we only trace to the desired distance
double sq_distance = (wx - ox) * (wx - ox) + (wy - oy) * (wy - oy);
double scaling_fact = sq_raytrace_range / sq_distance;
- scaling_fact = scaling_fact > 1.0 ? 1.0 : scaling_fact;
+ scaling_fact = min(1.0, scaling_fact);
- //now we also need to make sure that the enpoint we're raytracing
- //to isn't off the costmap and scale if necessary
double a = wx - ox;
double b = wy - oy;
+ wx = ox + scaling_fact * a;
+ wy = oy + scaling_fact * b;
+
+ //now we also need to make sure that the enpoint we're raytracing
+ //to isn't off the costmap and scale if necessary
+
//the minimum value to raytrace from is the origin
if(wx < origin_x_){
double t = (origin_x_ - ox) / a;
@@ -324,13 +329,13 @@
}
//the maximum value to raytrace to is the end of the map
- if(wx > end_x){
- double t = (end_x - ox) / a;
+ if(wx > map_end_x){
+ double t = (map_end_x - ox) / a;
wx = ox + a * t;
wy = oy + b * t;
}
- if(wy > end_y){
- double t = (end_y - oy) / b;
+ if(wy > map_end_y){
+ double t = (map_end_y - oy) / b;
wx = ox + a * t;
wy = oy + b * t;
}
@@ -358,11 +363,11 @@
double end_y = start_y + w_size_y;
//scale the window based on the bounds of the costmap
- start_x = start_x < origin_x_ ? origin_x_ : start_x;
- start_y = start_y < origin_y_ ? origin_y_ : start_y;
+ start_x = max(origin_x_, start_x);
+ start_y = max(origin_y_, start_y);
- end_x = end_x > origin_x_ + metersSizeX() ? origin_x_ + metersSizeX() : end_x;
- end_y = end_y > origin_y_ + metersSizeY() ? origin_y_ + metersSizeY() : end_y;
+ end_x = min(origin_x_ + metersSizeX(), end_x);
+ end_y = min(origin_y_ + metersSizeY(), end_y);
//get the map coordinates of the bounds of the window
unsigned int map_sx, map_sy, map_ex, map_ey;
@@ -382,8 +387,8 @@
current++;
index++;
}
- current += size_x_;
- index += size_x_;
+ current += size_x_ - (map_ex - map_sx) - 1;
+ index += size_x_ - (map_ex - map_sx) - 1;
}
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
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.
|
|
From: <ei...@us...> - 2009-04-01 02:23:03
|
Revision: 13208
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13208&view=rev
Author: eitanme
Date: 2009-04-01 01:33:19 +0000 (Wed, 01 Apr 2009)
Log Message:
-----------
r20003@cib: eitan | 2009-03-24 18:30:40 -0700
Fixed more bugs... now inflation works properly
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:20002
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:20003
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:32:45 UTC (rev 13207)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-04-01 01:33:19 UTC (rev 13208)
@@ -59,7 +59,7 @@
};
inline bool operator<(const CellData &a, const CellData &b){
- return a.distance_ < b.distance_;
+ return a.distance_ > b.distance_;
}
/**
@@ -114,7 +114,7 @@
* @param my The y coordinate of the cell
* @return The cost of the cell
*/
- unsigned char getCellCost(unsigned int mx, unsigned int my) const;
+ unsigned char getCost(unsigned int mx, unsigned int my) const;
/**
* @brief Convert from map coordinates to world coordinates
@@ -228,7 +228,7 @@
double distance = distanceLookup(mx, my, src_x, src_y);
//we only want to put the cell in the queue if it is within the inflation radius of the obstacle point
- if(distance > cell_inflation_radius_)
+ if(distance >= cell_inflation_radius_)
return;
//assign the cost associated with the distance from an obstacle to the cell
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:32:45 UTC (rev 13207)
+++ pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-04-01 01:33:19 UTC (rev 13208)
@@ -105,7 +105,7 @@
return (unsigned int) cells_dist;
}
- unsigned char Costmap2D::getCellCost(unsigned int mx, unsigned int my) const {
+ unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const {
ROS_ASSERT_MSG(mx < size_x_ && my < size_y_, "You cannot get the cost of a cell that is outside the bounds of the costmap");
return cost_map_[my * size_x_ + mx];
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-04-01 02:23:13
|
Revision: 13207
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13207&view=rev
Author: eitanme
Date: 2009-04-01 01:32:45 +0000 (Wed, 01 Apr 2009)
Log Message:
-----------
r20002@cib: eitan | 2009-03-24 17:02:20 -0700
Bug fix and optimization
Modified Paths:
--------------
pkg/trunk/world_models/new_costmap/CMakeLists.txt
pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h
pkg/trunk/world_models/new_costmap/manifest.xml
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:20001
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:20002
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/CMakeLists.txt
===================================================================
--- pkg/trunk/world_models/new_costmap/CMakeLists.txt 2009-04-01 01:32:19 UTC (rev 13206)
+++ pkg/trunk/world_models/new_costmap/CMakeLists.txt 2009-04-01 01:32:45 UTC (rev 13207)
@@ -8,3 +8,6 @@
rospack_add_library(new_costmap_2d src/costmap_2d.cpp)
#rospack_link_boost(costmap_2d thread)
+
+rospack_add_executable(costmap_test src/costmap_test.cpp)
+target_link_libraries(costmap_test new_costmap_2d)
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:32:19 UTC (rev 13206)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-04-01 01:32:45 UTC (rev 13207)
@@ -58,7 +58,7 @@
unsigned int src_x_, src_y_;
};
- bool operator<(const CellData &a, const CellData &b){
+ inline bool operator<(const CellData &a, const CellData &b){
return a.distance_ < b.distance_;
}
@@ -70,17 +70,6 @@
public:
/**
* @brief Constructor for a costmap
- * @param meters_size_x The x size of the map in meters
- * @param meters_size_y The y size of the map in meters
- * @param resolution The resolution of the map in meters/cell
- * @param origin_x The x origin of the map
- * @param origin_y The y origin of the map
- */
- Costmap2D(double meters_size_x, double meters_size_y,
- double resolution, double origin_x, double origin_y);
-
- /**
- * @brief Constructor for a costmap
* @param cells_size_x The x size of the map in cells
* @param cells_size_y The y size of the map in cells
* @param resolution The resolution of the map in meters/cell
@@ -103,6 +92,23 @@
const std::vector<unsigned char>& static_data, unsigned char lethal_threshold);
/**
+ * @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)
+ * @param w_size_x The x size of the window in meters
+ * @param w_size_y The y size of the window in meters
+ */
+ void resetMapOutsideWindow(double wx, double wy, double w_size_x, double w_size_y);
+
+ /**
+ * @brief Update the costmap with new observations
+ * @param obstacles The point clouds of obstacles to insert into the map
+ * @param clearing_observations The set of observations to use for raytracing
+ */
+ void updateWorld(double robot_x, double robot_y,
+ const std::vector<Observation>& observations, const std::vector<Observation>& clearing_observations);
+
+ /**
* @brief Get the cost of a cell in the costmap
* @param mx The x coordinate of the cell
* @param my The y coordinate of the cell
@@ -125,52 +131,32 @@
* @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
- * @return true if the conversion was successful (legal bounds) false otherwise
+ * @return True if the conversion was successful (legal bounds) false otherwise
*/
bool worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const;
+ /**
+ * @brief Given two map coordinates... compute the associated index
+ * @param mx The x coordinate
+ * @param my The y coordinate
+ * @return The associated index
+ */
inline unsigned int getIndex(unsigned int mx, unsigned int my) const{
return my * size_x_ + mx;
}
+ /**
+ * @brief Given an index... compute the associated map coordinates
+ * @param index The index
+ * @param mx Will be set to the x coordinate
+ * @param my Will be set to the y coordinate
+ */
inline void indexToCells(unsigned int index, unsigned int& mx, unsigned int& my) const{
my = index / size_x_;
mx = index - (my * size_x_);
}
- inline void updateCellCost(unsigned int index, unsigned char cost){
- unsigned char* cell_cost = &cost_map_[index];
- if(*cell_cost == NO_INFORMATION)
- *cell_cost = cost;
- else
- *cell_cost = std::max(cost, *cell_cost);
- }
- inline unsigned char computeCost(double distance) const {
- unsigned char cost = 0;
- if(distance == 0)
- cost = LETHAL_OBSTACLE;
- else if(distance <= cell_inscribed_radius_)
- cost = INSCRIBED_INFLATED_OBSTACLE;
- else {
- double factor = weight_ / (1 + pow(distance - cell_inscribed_radius_, 2));
- cost = (unsigned char) ((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
- }
- return cost;
- }
-
- 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];
- }
-
- 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);
- return cached_distances_[dx][dy];
- }
-
/**
* @brief Convert from map coordinates to world coordinates without checking for legal bounds
* @param wx The x world coordinate
@@ -180,54 +166,8 @@
*/
void worldToMapNoBounds(double wx, double wy, unsigned int& mx, unsigned int& my) const;
- /**
- * @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)
- * @param w_size_x The x size of the window in meters
- * @param w_size_y The y size of the window in meters
- */
- void resetMapOutsideWindow(double wx, double wy, double w_size_x, double w_size_y);
/**
- * @brief Update the costmap with new observations
- * @param obstacles The point clouds of obstacles to insert into the map
- * @param clearing_observations The set of observations to use for raytracing
- */
- void updateWorld(double robot_x, double robot_y,
- const std::vector<Observation>& observations, const std::vector<Observation>& clearing_observations);
-
- /**
- * @brief Insert new obstacles into the cost map
- * @param obstacles The point clouds of obstacles to insert into the map
- * @param inflation_queue The queue to place the obstacles into for inflation
- */
- void updateObstacles(const std::vector<Observation>& observations, std::priority_queue<CellData>& inflation_queue);
-
- /**
- * @brief Clear freespace based on any number of observations
- * @param clearing_observations The observations used to raytrace
- */
- void raytraceFreespace(const std::vector<Observation>& clearing_observations);
-
- /**
- * @brief Clear freespace from an observation
- * @param clearing_observation The observation used to raytrace
- */
- void raytraceFreespace(const Observation& clearing_observation);
-
- /**
- * @brief Provides support for re-inflating obstacles within a certain window (used after raytracing)
- * @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)
- * @param w_size_x The x size of the window in meters
- * @param w_size_y The y size of the window in meters
- * @param inflation_queue The priority queue to push items back onto for propogation
- */
- void resetInflationWindow(double wx, double wy, double w_size_x, double w_size_y,
- std::priority_queue<CellData>& inflation_queue );
-
- /**
* @brief Accessor for the x size of the costmap in cells
* @return The x size of the costmap
*/
@@ -278,7 +218,6 @@
* @param src_x The x index of the obstacle point inflation started at
* @param src_y The y index of the obstacle point inflation started at
* @param inflation_queue The priority queue to insert into
- * @return
*/
inline void enqueue(unsigned int index, unsigned int mx, unsigned int my,
unsigned int src_x, unsigned int src_y, std::priority_queue<CellData>& inflation_queue){
@@ -293,7 +232,7 @@
return;
//assign the cost associated with the distance from an obstacle to the cell
- cost_map_[index] = costLookup(mx, my, src_x, src_y);
+ updateCellCost(index, costLookup(mx, my, src_x, src_y));
//push the cell data onto the queue and mark
inflation_queue.push(CellData(distance, index, mx, my, src_x, src_y));
@@ -301,10 +240,46 @@
}
}
- inline int sign(int x){
- return x > 0 ? 1.0 : -1.0;
- }
+ /**
+ * @brief Insert new obstacles into the cost map
+ * @param obstacles The point clouds of obstacles to insert into the map
+ * @param inflation_queue The queue to place the obstacles into for inflation
+ */
+ void updateObstacles(const std::vector<Observation>& observations, std::priority_queue<CellData>& inflation_queue);
+ /**
+ * @brief Clear freespace based on any number of observations
+ * @param clearing_observations The observations used to raytrace
+ */
+ void raytraceFreespace(const std::vector<Observation>& clearing_observations);
+
+ /**
+ * @brief Clear freespace from an observation
+ * @param clearing_observation The observation used to raytrace
+ */
+ void raytraceFreespace(const Observation& clearing_observation);
+
+ /**
+ * @brief Provides support for re-inflating obstacles within a certain window (used after raytracing)
+ * @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)
+ * @param w_size_x The x size of the window in meters
+ * @param w_size_y The y size of the window in meters
+ * @param inflation_queue The priority queue to push items back onto for propogation
+ */
+ 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
+ * @param x0 The starting x coordinate
+ * @param y0 The starting y coordinate
+ * @param x1 The ending x coordinate
+ * @param y1 The ending y coordinate
+ * @return
+ */
template <class ActionType>
inline void raytraceLine(ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1){
int dx = x1 - x0;
@@ -331,6 +306,9 @@
}
+ /**
+ * @brief A 2D implementation of Bresenham's raytracing algorithm... applies an action at each step
+ */
template <class ActionType>
inline void bresenham2D(ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, int offset_b, unsigned int offset){
for(unsigned int i = 0; i < abs_da; ++i){
@@ -351,10 +329,61 @@
*/
void inflateObstacles(std::priority_queue<CellData>& inflation_queue);
+ /**
+ * @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
+ */
+ inline void updateCellCost(unsigned int index, unsigned char cost){
+ unsigned char* cell_cost = &cost_map_[index];
+ if(*cell_cost == NO_INFORMATION)
+ *cell_cost = cost;
+ else
+ *cell_cost = std::max(cost, *cell_cost);
+ }
+
+ /**
+ * @brief Given distance in the world... convert it to cells
+ * @param world_dist The world distance
+ * @return The equivalent cell distance
+ */
unsigned int cellDistance(double world_dist);
- double worldDistance(unsigned int cell_dist);
+ /**
+ * @brief Given a distance... compute a cost
+ * @param distance The distance from an obstacle in cells
+ * @return A cost value for the distance
+ */
+ inline unsigned char computeCost(double distance) const {
+ unsigned char cost = 0;
+ if(distance == 0)
+ cost = LETHAL_OBSTACLE;
+ else if(distance <= cell_inscribed_radius_)
+ cost = INSCRIBED_INFLATED_OBSTACLE;
+ else {
+ double factor = weight_ / (1 + pow(distance - cell_inscribed_radius_, 2));
+ cost = (unsigned char) ((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
+ }
+ return cost;
+ }
+ 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];
+ }
+
+ 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);
+ return cached_distances_[dx][dy];
+ }
+
+ inline int sign(int x){
+ return x > 0 ? 1.0 : -1.0;
+ }
+
+
unsigned int size_x_;
unsigned int size_y_;
double resolution_;
Modified: pkg/trunk/world_models/new_costmap/manifest.xml
===================================================================
--- pkg/trunk/world_models/new_costmap/manifest.xml 2009-04-01 01:32:19 UTC (rev 13206)
+++ pkg/trunk/world_models/new_costmap/manifest.xml 2009-04-01 01:32:45 UTC (rev 13207)
@@ -12,7 +12,8 @@
<depend package="tf" />
<depend package="robot_filter" />
<depend package="robot_srvs" />
+<depend package="costmap_2d" />
<export>
- <cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib `rosboost-cfg --lflags thread` -lnew_costmap"/>
+ <cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib `rosboost-cfg --lflags thread` -lnew_costmap_2d"/>
</export>
</package>
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:32:19 UTC (rev 13206)
+++ pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-04-01 01:32:45 UTC (rev 13207)
@@ -40,17 +40,6 @@
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) {
- //make sure that we have a legal sized cost_map
- ROS_ASSERT_MSG(meters_size_x > 0 && meters_size_y > 0, "Both the x and y dimensions of the costmap must be greater than 0.");
-
- size_x_ = (unsigned int) ceil(meters_size_x / resolution);
- size_y_ = (unsigned int) ceil(meters_size_y / resolution);
- }
-
Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y,
double resolution, double origin_x, double origin_y, double inscribed_radius,
double circumscribed_radius, double inflation_radius, double obstacle_range,
@@ -61,6 +50,12 @@
max_obstacle_height_(max_obstacle_height), raytrace_range_(raytrace_range), cached_costs_(NULL), cached_distances_(NULL),
inscribed_radius_(inscribed_radius), circumscribed_radius_(circumscribed_radius), inflation_radius_(inflation_radius),
weight_(weight){
+ //creat the cost_map, static_map, and markers
+ cost_map_ = new unsigned char[size_x_ * size_y_];
+ static_map_ = new unsigned char[size_x_ * size_y_];
+ markers_ = new unsigned char[size_x_ * size_y_];
+ memset(markers_, 0, size_x_ * size_y_ * sizeof(unsigned char));
+
//convert our inflations from world to cell distance
cell_inscribed_radius_ = cellDistance(inscribed_radius);
cell_circumscribed_radius_ = cellDistance(circumscribed_radius);
@@ -72,14 +67,14 @@
for(unsigned int i = 0; i <= cell_inflation_radius_; ++i){
cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 1];
cached_distances_[i] = new double[cell_inflation_radius_ + 1];
- for(unsigned int j = 0; i <= cell_inflation_radius_; ++j){
+ for(unsigned int j = 0; j <= cell_inflation_radius_; ++j){
cached_distances_[i][j] = sqrt(i*i + j*j);
cached_costs_[i][j] = computeCost(cached_distances_[i][j]);
}
}
if(!static_data.empty()){
- ROS_ASSERT_MSG(size_x_ * size_y_ == static_data_.size(), "If you want to initialize a costmap with static data, their sizes must match.");
+ ROS_ASSERT_MSG(size_x_ * size_y_ == static_data.size(), "If you want to initialize a costmap with static data, their sizes must match.");
//make sure the inflation queue is empty at the beginning of the cycle (should always be true)
ROS_ASSERT_MSG(inflation_queue_.empty(), "The inflation queue must be empty at the beginning of inflation");
@@ -110,11 +105,6 @@
return (unsigned int) cells_dist;
}
- double Costmap2D::worldDistance(unsigned int cell_dist){
- double world_dist = cell_dist * resolution_;
- return world_dist;
- }
-
unsigned char Costmap2D::getCellCost(unsigned int mx, unsigned int my) const {
ROS_ASSERT_MSG(mx < size_x_ && my < size_y_, "You cannot get the cost of a cell that is outside the bounds of the costmap");
return cost_map_[my * size_x_ + mx];
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-04-01 02:23:32
|
Revision: 13211
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13211&view=rev
Author: eitanme
Date: 2009-04-01 01:38:24 +0000 (Wed, 01 Apr 2009)
Log Message:
-----------
r20159@cib: eitan | 2009-03-26 19:24:01 -0700
Tested all parts of costmap functionality successfully... still need to wrap it somehow to pass the old unit tests
Modified Paths:
--------------
pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h
pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp
Added Paths:
-----------
pkg/trunk/world_models/new_costmap/src/costmap_test.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:20146
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: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
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:10 UTC (rev 13210)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-04-01 01:38:24 UTC (rev 13211)
@@ -278,10 +278,10 @@
* @param y0 The starting y coordinate
* @param x1 The ending x coordinate
* @param y1 The ending y coordinate
- * @return
+ * @param max_length The maximum desired length of the segment... allows you to not go all the way to the endpoint
*/
template <class ActionType>
- inline void raytraceLine(ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1){
+ inline void raytraceLine(ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, unsigned int max_length){
int dx = x1 - x0;
int dy = y1 - y0;
@@ -293,16 +293,20 @@
unsigned int offset = y0 * size_x_ + x0;
+ //we need to chose how much to scale our dominant dimension, based on the maximum length of the line
+ double dist = sqrt((x0 - x1) * (x0 - x1) + (y0 - y1) * (y0 - y1));
+ double scale = std::min(1.0, max_length / dist);
+
//if x is dominant
if(abs_dx >= abs_dy){
int error_y = abs_dx / 2;
- bresenham2D(at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset);
+ bresenham2D(at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx));
return;
}
//otherwise y is dominant
int error_x = abs_dy / 2;
- bresenham2D(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset);
+ bresenham2D(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy));
}
@@ -310,8 +314,9 @@
* @brief A 2D implementation of Bresenham's raytracing algorithm... applies an action at each step
*/
template <class ActionType>
- inline void bresenham2D(ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, int offset_b, unsigned int offset){
- for(unsigned int i = 0; i < abs_da; ++i){
+ inline void bresenham2D(ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, int offset_b,
+ unsigned int offset, unsigned int max_length){
+ for(unsigned int i = 0; i < std::min(max_length, abs_da); ++i){
at(offset);
offset += offset_a;
error_b += abs_db;
@@ -412,6 +417,16 @@
private:
unsigned char* cost_map_;
};
+
+ class MarkCell {
+ public:
+ MarkCell(unsigned char* cost_map) : cost_map_(cost_map) {}
+ inline void operator()(unsigned int offset){
+ cost_map_[offset] = LETHAL_OBSTACLE;
+ }
+ private:
+ unsigned char* cost_map_;
+ };
};
};
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:10 UTC (rev 13210)
+++ pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-04-01 01:38:24 UTC (rev 13211)
@@ -144,8 +144,11 @@
start_point_y = max(origin_y_, start_point_y);
unsigned int start_x, start_y, end_x, end_y;
- worldToMap(start_point_x, start_point_y, start_x, start_y);
+ //check for legality just in case
+ if(!worldToMap(start_point_x, start_point_y, start_x, start_y))
+ return;
+
//we'll do our own bounds checking for the end of the window
worldToMapNoBounds(start_point_x + w_size_x, start_point_y + w_size_y, end_x, end_y);
@@ -278,9 +281,6 @@
}
void Costmap2D::raytraceFreespace(const Observation& clearing_observation){
- //pre-comput the squared raytrace range... saves a sqrt in an inner loop
- double sq_raytrace_range = raytrace_range_ * raytrace_range_;
-
//create the functor that we'll use to clear cells from the costmap
ClearCell clearer(cost_map_);
@@ -302,50 +302,46 @@
double wx = cloud.pts[i].x;
double wy = cloud.pts[i].y;
- //we want to check that we scale the vector so that we only trace to the desired distance
- double sq_distance = (wx - ox) * (wx - ox) + (wy - oy) * (wy - oy);
- double scaling_fact = sq_raytrace_range / sq_distance;
- scaling_fact = min(1.0, scaling_fact);
-
+ //now we also need to make sure that the enpoint we're raytracing
+ //to isn't off the costmap and scale if necessary
double a = wx - ox;
double b = wy - oy;
- wx = ox + scaling_fact * a;
- wy = oy + scaling_fact * b;
-
- //now we also need to make sure that the enpoint we're raytracing
- //to isn't off the costmap and scale if necessary
-
//the minimum value to raytrace from is the origin
if(wx < origin_x_){
double t = (origin_x_ - ox) / a;
- wx = ox + a * t;
+ wx = origin_x_;
wy = oy + b * t;
}
if(wy < origin_y_){
double t = (origin_y_ - oy) / b;
wx = ox + a * t;
- wy = oy + b * t;
+ wy = origin_y_;
}
//the maximum value to raytrace to is the end of the map
if(wx > map_end_x){
double t = (map_end_x - ox) / a;
- wx = ox + a * t;
+ wx = map_end_x;
wy = oy + b * t;
}
if(wy > map_end_y){
double t = (map_end_y - oy) / b;
wx = ox + a * t;
- wy = oy + b * t;
+ wy = map_end_y;
}
//now that the vector is scaled correctly... we'll get the map coordinates of its endpoint
unsigned int x1, y1;
- worldToMap(wx, wy, x1, y1);
+ //check for legality just in case
+ if(!worldToMap(wx, wy, x1, y1))
+ continue;
+
+ unsigned int cell_raytrace_range = cellDistance(raytrace_range_);
+
//and finally... we can execute our trace to clear obstacles along that line
- raytraceLine(clearer, x0, y0, x1, y1);
+ raytraceLine(clearer, x0, y0, x1, y1, cell_raytrace_range);
}
}
@@ -371,9 +367,11 @@
//get the map coordinates of the bounds of the window
unsigned int map_sx, map_sy, map_ex, map_ey;
- worldToMap(start_x, start_y, map_sx, map_sy);
- worldToMap(end_x, end_y, map_ex, map_ey);
+ //check for legality just in case
+ if(!worldToMap(start_x, start_y, map_sx, map_sy) || !worldToMap(end_x, end_y, map_ex, map_ey))
+ return;
+
//we know that we want to clear all non-lethal obstacles in this window to get it ready for inflation
unsigned int index = getIndex(map_sx, map_sy);
unsigned char* current = &cost_map_[index];
Added: pkg/trunk/world_models/new_costmap/src/costmap_test.cpp
===================================================================
--- pkg/trunk/world_models/new_costmap/src/costmap_test.cpp (rev 0)
+++ pkg/trunk/world_models/new_costmap/src/costmap_test.cpp 2009-04-01 01:38:24 UTC (rev 13211)
@@ -0,0 +1,308 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Eitan Marder-Eppstein
+*********************************************************************/
+#include <ros/node.h>
+#include <ros/console.h>
+#include <new_costmap/costmap_2d.h>
+#include <costmap_2d/costmap_2d.h>
+#include <robot_srvs/StaticMap.h>
+#include <robot_msgs/Polyline2D.h>
+#include <map>
+#include <vector>
+
+#include <tf/transform_datatypes.h>
+#include <tf/message_notifier.h>
+#include <tf/transform_listener.h>
+
+#include <laser_scan/LaserScan.h>
+#include <laser_scan/laser_scan.h>
+
+#include <robot_msgs/PointCloud.h>
+
+// Thread suppport
+#include <boost/thread.hpp>
+
+using namespace costmap_2d;
+using namespace tf;
+using namespace robot_msgs;
+
+class CostmapTester {
+ public:
+ CostmapTester(ros::Node& ros_node) : ros_node_(ros_node), base_scan_notifier_(NULL), tf_(ros_node, true, ros::Duration(10)), observations_(1), global_frame_("map"), freq_(5) {
+ ros_node.advertise<robot_msgs::Polyline2D>("raw_obstacles", 1);
+ ros_node.advertise<robot_msgs::Polyline2D>("inflated_obstacles", 1);
+
+ base_scan_notifier_ = new MessageNotifier<laser_scan::LaserScan>(&tf_, &ros_node,
+ boost::bind(&CostmapTester::baseScanCallback, this, _1),
+ "base_scan", global_frame_, 50);
+
+ robot_srvs::StaticMap::Request map_req;
+ robot_srvs::StaticMap::Response map_resp;
+ ROS_INFO("Requesting the map...\n");
+ while(!ros::service::call("static_map", map_req, map_resp))
+ {
+ ROS_INFO("Request failed; trying again...\n");
+ usleep(1000000);
+ }
+ ROS_INFO("Received a %d X %d map at %f m/pix\n",
+ map_resp.map.width, map_resp.map.height, map_resp.map.resolution);
+
+ // We are treating cells with no information as lethal obstacles based on the input data. This is not ideal but
+ // our planner and controller do not reason about the no obstacle case
+ std::vector<unsigned char> input_data;
+ unsigned int numCells = map_resp.map.width * map_resp.map.height;
+ for(unsigned int i = 0; i < numCells; i++){
+ input_data.push_back((unsigned char) map_resp.map.data[i]);
+ }
+
+ struct timeval start, end;
+ double start_t, end_t, t_diff;
+ gettimeofday(&start, NULL);
+ new_costmap_ = new Costmap2D((unsigned int)map_resp.map.width, (unsigned int)map_resp.map.height,
+ map_resp.map.resolution, 0.0, 0.0, 0.325, 0.46, 0.55, 2.5, 2.0, 3.0, 1.0, input_data, 100);
+ gettimeofday(&end, NULL);
+ start_t = start.tv_sec + double(start.tv_usec) / 1e6;
+ end_t = end.tv_sec + double(end.tv_usec) / 1e6;
+ t_diff = end_t - start_t;
+ ROS_INFO("New map construction time: %.9f", t_diff);
+
+ gettimeofday(&start, NULL);
+ old_costmap_ = new CostMap2D((unsigned int)map_resp.map.width, (unsigned int)map_resp.map.height,
+ input_data , map_resp.map.resolution,
+ 100, 2.0, 15, 25,
+ 0.55, 0.46, 0.325, 1.0,
+ 10.0, 10.0, 10.0);
+ gettimeofday(&end, NULL);
+ start_t = start.tv_sec + double(start.tv_usec) / 1e6;
+ end_t = end.tv_sec + double(end.tv_usec) / 1e6;
+ t_diff = end_t - start_t;
+ ROS_INFO("Old map construction time: %.9f", t_diff);
+
+ //create a separate thread to publish cost data to the visualizer
+ visualizer_thread_ = new boost::thread(boost::bind(&CostmapTester::publishCostMap, this));
+ window_reset_thread_ = new boost::thread(boost::bind(&CostmapTester::resetWindow, this));
+
+ }
+
+ ~CostmapTester(){
+ delete new_costmap_;
+ delete old_costmap_;
+ delete base_scan_notifier_;
+ delete visualizer_thread_;
+ delete window_reset_thread_;
+ }
+
+ void baseScanCallback(const tf::MessageNotifier<laser_scan::LaserScan>::MessagePtr& message){
+ //project the laser into a point cloud
+ PointCloud base_cloud;
+ base_cloud.header = message->header;
+ //we want all values... even those out of range
+ projector_.projectLaser(*message, base_cloud, -1.0, true);
+ Stamped<btVector3> global_origin;
+
+ lock_.lock();
+ //we know the transform is available from the laser frame to the global frame
+ try{
+ //transform the origin for the sensor
+ Stamped<btVector3> local_origin(btVector3(0, 0, 0), base_cloud.header.stamp, base_cloud.header.frame_id);
+ tf_.transformPoint(global_frame_, local_origin, global_origin);
+ observations_[0].origin_.x = global_origin.getX();
+ observations_[0].origin_.y = global_origin.getY();
+ observations_[0].origin_.z = global_origin.getZ();
+
+ //transform the point cloud
+ tf_.transformPointCloud(global_frame_, base_cloud, observations_[0].cloud_);
+ }
+ catch(tf::TransformException& ex){
+ ROS_ERROR("TF Exception that should never happen %s", ex.what());
+ return;
+ }
+ lock_.unlock();
+ }
+
+ void spin(){
+ while(ros_node_.ok()){
+ updateMap();
+ usleep(1e6/freq_);
+ }
+ }
+
+ void updateMap(){
+ tf::Stamped<tf::Pose> robot_pose, global_pose;
+ global_pose.setIdentity();
+ robot_pose.setIdentity();
+ robot_pose.frame_id_ = "base_link";
+ robot_pose.stamp_ = ros::Time();
+ try{
+ tf_.transformPose(global_frame_, robot_pose, global_pose);
+ }
+ catch(tf::LookupException& ex) {
+ ROS_ERROR("No Transform available Error: %s\n", ex.what());
+ }
+ catch(tf::ConnectivityException& ex) {
+ ROS_ERROR("Connectivity Error: %s\n", ex.what());
+ }
+ catch(tf::ExtrapolationException& ex) {
+ ROS_ERROR("Extrapolation Error: %s\n", ex.what());
+ }
+
+ double wx = global_pose.getOrigin().x();
+ double wy = global_pose.getOrigin().y();
+
+ //in the real world... make a deep copy... but for testing I'm lazy so I'll lock around the updates
+ lock_.lock();
+ struct timeval start, end;
+ double start_t, end_t, t_diff;
+ gettimeofday(&start, NULL);
+ new_costmap_->updateWorld(wx, wy, observations_, observations_);
+ gettimeofday(&end, NULL);
+ start_t = start.tv_sec + double(start.tv_usec) / 1e6;
+ end_t = end.tv_sec + double(end.tv_usec) / 1e6;
+ t_diff = end_t - start_t;
+ ROS_INFO("Map update time: %.9f", t_diff);
+ lock_.unlock();
+ }
+
+ void resetWindow(){
+ while(ros_node_.ok()){
+ tf::Stamped<tf::Pose> robot_pose, global_pose;
+ global_pose.setIdentity();
+ robot_pose.setIdentity();
+ robot_pose.frame_id_ = "base_link";
+ robot_pose.stamp_ = ros::Time();
+ try{
+ tf_.transformPose(global_frame_, robot_pose, global_pose);
+ }
+ catch(tf::LookupException& ex) {
+ ROS_ERROR("No Transform available Error: %s\n", ex.what());
+ }
+ catch(tf::ConnectivityException& ex) {
+ ROS_ERROR("Connectivity Error: %s\n", ex.what());
+ }
+ catch(tf::ExtrapolationException& ex) {
+ ROS_ERROR("Extrapolation Error: %s\n", ex.what());
+ }
+
+ double wx = global_pose.getOrigin().x();
+ double wy = global_pose.getOrigin().y();
+ lock_.lock();
+ ROS_INFO("Resetting map outside window");
+ new_costmap_->resetMapOutsideWindow(wx, wy, 5.0, 5.0);
+ lock_.unlock();
+
+ usleep(1e6/0.2);
+ }
+ }
+
+ void publishCostMap(){
+ while(ros_node_.ok()){
+ ROS_INFO("publishing map");
+ lock_.lock();
+ std::vector< std::pair<double, double> > raw_obstacles, inflated_obstacles;
+ for(unsigned int i = 0; i<new_costmap_->cellSizeX(); i++){
+ for(unsigned int j = 0; j<new_costmap_->cellSizeY();j++){
+ double wx, wy;
+ new_costmap_->mapToWorld(i, j, wx, wy);
+ std::pair<double, double> p(wx, wy);
+
+ if(new_costmap_->getCost(i, j) == costmap_2d::LETHAL_OBSTACLE)
+ raw_obstacles.push_back(p);
+ else if(new_costmap_->getCost(i, j) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
+ inflated_obstacles.push_back(p);
+ }
+ }
+ lock_.unlock();
+
+ // First publish raw obstacles in red
+ robot_msgs::Polyline2D obstacle_msg;
+ obstacle_msg.header.frame_id = global_frame_;
+ unsigned int pointCount = raw_obstacles.size();
+ obstacle_msg.set_points_size(pointCount);
+ obstacle_msg.color.a = 0.0;
+ obstacle_msg.color.r = 1.0;
+ obstacle_msg.color.b = 0.0;
+ obstacle_msg.color.g = 0.0;
+
+ for(unsigned int i=0;i<pointCount;i++){
+ obstacle_msg.points[i].x = raw_obstacles[i].first;
+ obstacle_msg.points[i].y = raw_obstacles[i].second;
+ }
+
+ ros::Node::instance()->publish("raw_obstacles", obstacle_msg);
+
+ // Now do inflated obstacles in blue
+ pointCount = inflated_obstacles.size();
+ obstacle_msg.set_points_size(pointCount);
+ obstacle_msg.color.a = 0.0;
+ obstacle_msg.color.r = 0.0;
+ obstacle_msg.color.b = 1.0;
+ obstacle_msg.color.g = 0.0;
+
+ for(unsigned int i=0;i<pointCount;i++){
+ obstacle_msg.points[i].x = inflated_obstacles[i].first;
+ obstacle_msg.points[i].y = inflated_obstacles[i].second;
+ }
+
+ ros::Node::instance()->publish("inflated_obstacles", obstacle_msg);
+ usleep(1e6/2.0);
+ }
+ }
+
+ ros::Node& ros_node_;
+ tf::MessageNotifier<laser_scan::LaserScan>* base_scan_notifier_; ///< @brief Used to guarantee that a transform is available for base scans
+ tf::TransformListener tf_; ///< @brief Used for transforming point clouds
+ laser_scan::LaserProjection projector_; ///< @brief Used to project laser scans into point clouds
+ boost::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely
+ std::vector<Observation> observations_;
+ Costmap2D* new_costmap_;
+ CostMap2D* old_costmap_;
+ std::string global_frame_;
+ double freq_;
+ boost::thread *visualizer_thread_;
+ boost::thread *window_reset_thread_;
+
+};
+
+int main(int argc, char** argv){
+ ros::init(argc, argv);
+ ros::Node ros_node("new_costmap_tester");
+ CostmapTester tester(ros_node);
+ tester.spin();
+
+ return(0);
+
+}
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-04-01 17:24:28
|
Revision: 13231
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13231&view=rev
Author: eitanme
Date: 2009-04-01 17:24:07 +0000 (Wed, 01 Apr 2009)
Log Message:
-----------
Rolling back files changed by my awful branch
Modified Paths:
--------------
pkg/trunk/demos/plug_in/move_to_pickup.py
pkg/trunk/mapping/door_handle_detector/include/door_handle_detector/geometric_helper.h
pkg/trunk/mapping/door_handle_detector/src/geometric_helper.cpp
pkg/trunk/robot_descriptions/pr2/pr2_defs/calibration/prf_cal.xml
Added Paths:
-----------
pkg/trunk/nav/escape_controller/include/escape_controller/escape_controller_ros.h
Removed Paths:
-------------
pkg/trunk/3rdparty/svl/Makefile
pkg/trunk/3rdparty/svl/manifest.xml
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/nddl/rcs.nddl
pkg/trunk/nav/escape_controller/include/escape_controller/escape_controller.h
Deleted: pkg/trunk/3rdparty/svl/Makefile
===================================================================
--- pkg/trunk/3rdparty/svl/Makefile 2009-04-01 17:19:12 UTC (rev 13230)
+++ pkg/trunk/3rdparty/svl/Makefile 2009-04-01 17:24:07 UTC (rev 13231)
@@ -1,33 +0,0 @@
-INSTALL_DIR = opencv
-
-all: installed
-
-PKG_NAME = svl-src-1.1-1564
-TARBALL = build/$(PKG_NAME).tar.gz
-TARBALL_URL = http://ai.stanford.edu/~sgould/svl/media/$(PKG_NAME).tar.gz
-UNPACK_CMD = tar -xvzf
-SOURCE_DIR = build/$(PKG_NAME)
-
-include $(shell rospack find mk)/download_unpack_build.mk
-
-installed: wiped $(SOURCE_DIR)/unpacked
- echo "Building STAIR Vision Library..."
- # it seems that SVL does not build with the wxGTK included in ubuntu 8.04, os we are compiling this version specially for it
- (cd build && wget "http://prdownloads.sourceforge.net/wxwindows/wxGTK-2.8.7.tar.gz" && tar xzvf wxGTK-2.8.7.tar.gz && rm wxGTK-2.8.7.tar.gz && cd wxGTK-2.8.7 && mkdir buildgtk && cd buildgtk && ../configure --disable-shared --with-opengl --enable-monolithic && make)
- ln -sf ../../wxGTK-2.8.7 $(SOURCE_DIR)/external/wxGTK
- ln -sf $(shell rospack find opencv_latest)/opencv $(SOURCE_DIR)/external
- (cd $(SOURCE_DIR) && make)
- (cd build && ln -sf $(PKG_NAME) svl)
- (cd $(SOURCE_DIR)/external/lbfgs && ln -sf lbfgs.a liblbfgs.a)
- (cd $(SOURCE_DIR)/external/xmlParser && ln -sf xmlParser.a libxmlParser.a)
- touch installed
-
-wiped: Makefile
- make wipe
- touch wiped
-
-wipe:
- rm -rf build installed
-
-.PHONY : clean download
-
Deleted: pkg/trunk/3rdparty/svl/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/svl/manifest.xml 2009-04-01 17:19:12 UTC (rev 13230)
+++ pkg/trunk/3rdparty/svl/manifest.xml 2009-04-01 17:24:07 UTC (rev 13231)
@@ -1,32 +0,0 @@
-<package>
-<description brief="STAIR Vision Library">
-
- The STAIR Vision Library (SVL) (codenamed lasik) contains computer vision research code initially developed to support the STanford AI Robot project. It has since been expanded to provide a range of software infrastructure for computer vision, machine learning, and probabilistic graphical models.\
-
-</description>
-<author>Stephen Gould, Andrew Y. Ng and Daphne Koller</author>
-<license>BSD</license>
-<review status="3rdparty" notes=""/>
-<url>http://ai.stanford.edu/~sgould/svl/</url>
-<export>
- <cpp cflags="-I${prefix}/build/svl/include -I${prefix}/build/svl/external" lflags="-L${prefix}/build/svl/bin -Wl,-rpath,${prefix}/build/svl/bin -lsvlBase -lsvlDevel -lsvlML -lsvlPGM -lsvlVision ${prefix}/build/svl/external/xmlParser/xmlParser.a ${prefix}/build/svl/external/lbfgs/lbfgs.a"/>
-</export>
-
- <depend package="opencv_latest"/>
- <depend package="eigen"/>
-
- <sysdepend os="ubuntu" version="8.04-hardy" package="libgl1-mesa-dev"/>
- <sysdepend os="ubuntu" version="8.04-hardy" package="libglu1-mesa-dev"/>
- <sysdepend os="ubuntu" version="8.04-hardy" package="libjpeg-dev"/>
- <sysdepend os="ubuntu" version="8.04-hardy" package="libgtk2.0-dev"/>
- <sysdepend os="ubuntu" version="8.04-hardy" package="libpng12-dev"/>
- <sysdepend os="ubuntu" version="8.04-hardy" package="zlib1g-dev"/>
- <sysdepend os="ubuntu" version="8.04-hardy" package="libtiff4-dev"/>
- <sysdepend os="ubuntu" version="8.04-hardy" package="libjasper-dev"/>
- <sysdepend os="ubuntu" version="8.04-hardy" package="python2.5-dev"/>
- <sysdepend os="ubuntu" version="8.04-hardy" package="libavcodec-dev"/>
- <sysdepend os="ubuntu" version="8.04-hardy" package="libavformat-dev"/>
- <sysdepend os="ubuntu" version="8.04-hardy" package="ffmpeg"/>
- <sysdepend os="ubuntu" version="8.04-hardy" package="wget"/>
-</package>
-
Modified: pkg/trunk/demos/plug_in/move_to_pickup.py
===================================================================
--- pkg/trunk/demos/plug_in/move_to_pickup.py 2009-04-01 17:19:12 UTC (rev 13230)
+++ pkg/trunk/demos/plug_in/move_to_pickup.py 2009-04-01 17:24:07 UTC (rev 13231)
@@ -132,27 +132,8 @@
rospy.set_param("cartesian_trajectory_right/pose/twist/fb_rot/d", 0.0)
rospy.set_param("cartesian_trajectory_right/pose/twist/fb_rot/i_clamp", 0.2)
- rospy.set_param("cartesian_pose/root_name", 'torso_lift_link')
- rospy.set_param("cartesian_pose/tip_name", 'r_gripper_tool_frame')
- rospy.set_param("cartesian_pose/p",16.0)
- rospy.set_param("cartesian_pose/i",4.0)
- rospy.set_param("cartesian_pose/d",0.0)
- rospy.set_param("cartesian_pose/i_clamp",3.0)
-
-
- rospy.set_param("cartesian_pose/twist/ff_trans", 20.0)
- rospy.set_param("cartesian_pose/twist/ff_rot", 5.0)
- rospy.set_param("cartesian_pose/twist/fb_trans/p", 20.0)
- rospy.set_param("cartesian_pose/twist/fb_trans/i", 0.5)
- rospy.set_param("cartesian_pose/twist/fb_trans/d", 0.0 )
- rospy.set_param("cartesian_pose/twist/fb_trans/i_clamp", 1.0)
- rospy.set_param("cartesian_pose/twist/fb_rot/p", 1.5 )
- rospy.set_param("cartesian_pose/twist/fb_rot/i", 0.1)
- rospy.set_param("cartesian_pose/twist/fb_rot/d", 0.0)
- rospy.set_param("cartesian_pose/twist/fb_rot/i_clamp", 0.2)
-
if __name__ == '__main__':
-
+
rospy.wait_for_service('spawn_controller')
rospy.init_node('move_to_pickup', anonymous = True)
rospy.wait_for_service('kill_and_spawn_controllers')
@@ -228,9 +209,7 @@
print "picking up plug"
pickup()
pub.publish(Float64(0.6))
- mechanism.kill_controller('right_arm/trajectory_controller')
- mechanism.spawn_controller(xml_for_pose.read())
- controllers.append('cartesian_pose')
+
sleep(2)
#rospy.spin()
Deleted: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/nddl/rcs.nddl
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/nddl/rcs.nddl 2009-04-01 17:19:12 UTC (rev 13230)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/nddl/rcs.nddl 2009-04-01 17:24:07 UTC (rev 13231)
@@ -1,52 +0,0 @@
-/*
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- * * Neither the name of the <ORGANIZATION> nor the names of its
- * contributors may be used to endorse or promote products derived from
- * this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-// TREX base class declaration and definitions
-#include "TREX.nddl"
-
-// Exports from sub-systems
-#include "rcs.exports.nddl"
-#include "rcs.exports.doorman.nddl"
-
-/**
- * If an active token is dispatched, we start it immediately. Also, we just make behaviors execute quickly and
- * flawlessly.
- */
-Behavior::Active{
- addEq(dispatch_time, 1, start);
- eq(duration, 1);
- meets(Inactive s);
- eq(s.status, SUCCESS);
-}
-
-// Remaining behaviors for which we do not even have stubs
-ReleaseDoor release_door = new ReleaseDoor(Internal);
-OpenDoorWithoutGrasp open_door_without_grasp = new OpenDoorWithoutGrasp(Internal);
-CheckDoorway check_doorway = new CheckDoorway(Internal);
-NotifyDoorBlocked notify_door_blocked = new NotifyDoorBlocked(Internal);
Modified: pkg/trunk/mapping/door_handle_detector/include/door_handle_detector/geometric_helper.h
===================================================================
--- pkg/trunk/mapping/door_handle_detector/include/door_handle_detector/geometric_helper.h 2009-04-01 17:19:12 UTC (rev 13230)
+++ pkg/trunk/mapping/door_handle_detector/include/door_handle_detector/geometric_helper.h 2009-04-01 17:24:07 UTC (rev 13231)
@@ -195,7 +195,6 @@
void selectBestDistributionStatistics (const robot_msgs::PointCloud &points, const std::vector<int> &indices, int d_idx, std::vector<int> &inliers);
void selectBestDualDistributionStatistics (const robot_msgs::PointCloud &points, const std::vector<int> &indices, int d_idx_1, int d_idx_2, std::vector<int> &inliers);
-void selectBestDualDistributionStatistics (robot_msgs::PointCloud *points, std::vector<int> *indices, int d_idx_1, int d_idx_2, std::vector<int> &inliers);
bool checkIfClusterPerpendicular (robot_msgs::PointCloud *points, std::vector<int> *indices, robot_msgs::PointStamped *viewpoint,
std::vector<double> *coeff, double eps_angle);
Modified: pkg/trunk/mapping/door_handle_detector/src/geometric_helper.cpp
===================================================================
--- pkg/trunk/mapping/door_handle_detector/src/geometric_helper.cpp 2009-04-01 17:19:12 UTC (rev 13230)
+++ pkg/trunk/mapping/door_handle_detector/src/geometric_helper.cpp 2009-04-01 17:24:07 UTC (rev 13231)
@@ -544,10 +544,6 @@
sort (r.begin (), r.end ());
r.erase (unique (r.begin (), r.end ()), r.end ());
-
- sort (r.begin (), r.end ());
- r.erase (unique (r.begin (), r.end ()), r.end ());
-
clusters.push_back (r);
}
}
Deleted: pkg/trunk/nav/escape_controller/include/escape_controller/escape_controller.h
===================================================================
--- pkg/trunk/nav/escape_controller/include/escape_controller/escape_controller.h 2009-04-01 17:19:12 UTC (rev 13230)
+++ pkg/trunk/nav/escape_controller/include/escape_controller/escape_controller.h 2009-04-01 17:24:07 UTC (rev 13231)
@@ -1,71 +0,0 @@
-/*********************************************************************
-*
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-* Author: Eitan Marder-Eppstein
-*********************************************************************/
-#ifndef ESCAPE_CONTROLLER_ESCAPE_CONTROLLER_H_
-#define ESCAPE_CONTROLLER_ESCAPE_CONTROLLER_H_
-
-#include <string>
-#include <highlevel_controllers/highlevel_controller.hh>
-#include <escape_controllers/EscapeState.h>
-#include <escape_controllers/EscapeGoal.h>
-
-namespace escape_controller {
- class EscapeController : public HighlevelController<EscapeState, EscapeGoal> {
- public:
- EscapeController(const std::string& state_topic, const std::string& goal_topic);
-
- virtual ~EscapeController();
-
- private:
- //HighlevelController Interface
- virtual void updateGoalMsg();
- virtual void updateStateMsg();
- virtual bool makePlan();
- virtual bool goalReached();
- virtual bool dispatchCommands();
-
- void baseStateCallback();
-
- pr2_msgs::BaseControllerState base_state_msg_;
- };
-
- EscapeController::EscapeController(const string& state_topic, const string& goal_topic)
- : HighlevelController<EscapeState, EscapeGoal>("escape_controller", state_topic, goal_topic)
- {
- subscribe("/base_controller/state", base_state_msg_, &EscapeController::baseStateCallback, 1);
- }
-};
-#endif
Copied: pkg/trunk/nav/escape_controller/include/escape_controller/escape_controller_ros.h (from rev 10602, pkg/trunk/nav/escape_controller/include/escape_controller/escape_controller.h)
===================================================================
--- pkg/trunk/nav/escape_controller/include/escape_controller/escape_controller_ros.h (rev 0)
+++ pkg/trunk/nav/escape_controller/include/escape_controller/escape_controller_ros.h 2009-04-01 17:24:07 UTC (rev 13231)
@@ -0,0 +1,71 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Eitan Marder-Eppstein
+*********************************************************************/
+#ifndef ESCAPE_CONTROLLER_ESCAPE_CONTROLLER_ROS_H_
+#define ESCAPE_CONTROLLER_ESCAPE_CONTROLLER_ROS_H_
+
+#include <string>
+#include <highlevel_controllers/highlevel_controller.hh>
+#include <escape_controllers/EscapeState.h>
+#include <escape_controllers/EscapeGoal.h>
+
+namespace escape_controller {
+ class EscapeControllerROS : public HighlevelController<EscapeState, EscapeGoal> {
+ public:
+ EscapeControllerROS(const std::string& state_topic, const std::string& goal_topic);
+
+ virtual ~EscapeController();
+
+ private:
+ //HighlevelController Interface
+ virtual void updateGoalMsg();
+ virtual void updateStateMsg();
+ virtual bool makePlan();
+ virtual bool goalReached();
+ virtual bool dispatchCommands();
+
+ void baseStateCallback();
+
+ pr2_msgs::BaseControllerState base_state_msg_;
+ };
+
+ EscapeControllerROS::EscapeControllerROS(const string& state_topic, const string& goal_topic)
+ : HighlevelController<EscapeState, EscapeGoal>("escape_controller", state_topic, goal_topic)
+ {
+ subscribe("/base_controller/state", base_state_msg_, &EscapeControllerROS::baseStateCallback, 1);
+ }
+};
+#endif
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/calibration/prf_cal.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/calibration/prf_cal.xml 2009-04-01 17:19:12 UTC (rev 13230)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/calibration/prf_cal.xml 2009-04-01 17:24:07 UTC (rev 13231)
@@ -39,4 +39,4 @@
<property name="cal_head_tilt_gearing" value="0.970987" />
-</root>==== ORIGINAL VERSION robot_descriptions/pr2/pr2_defs/calibration/prf_cal.xml 123854898574114
+</root>
\ No newline at end of file
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ve...@us...> - 2009-04-01 18:05:58
|
Revision: 13239
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13239&view=rev
Author: veedee
Date: 2009-04-01 18:05:46 +0000 (Wed, 01 Apr 2009)
Log Message:
-----------
towards a faster, non-linear, Banach space :)
Modified Paths:
--------------
pkg/trunk/mapping/door_handle_detector/src/geometric_helper.cpp
pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp
pkg/trunk/mapping/door_tracker/src/door_tracker.cpp
pkg/trunk/mapping/hallway_tracker/src/hallway_tracker.cpp
pkg/trunk/mapping/planar_patch_map/src/planar_patch_map.cpp
pkg/trunk/mapping/planar_patch_map/src/planar_patch_map_omp.cpp
pkg/trunk/mapping/plug_onbase_detector/include/plug_onbase_detector/plug_onbase_detector.h
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac.h
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model.h
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_circle.h
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_cylinder.h
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_line.h
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_parallel_lines.h
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_plane.h
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_sphere.h
pkg/trunk/mapping/point_cloud_mapping/normal_estimation.launch
pkg/trunk/mapping/point_cloud_mapping/src/planar_fit.cpp
pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model.cpp
pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_circle.cpp
pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_cylinder.cpp
pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_line.cpp
pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_parallel_lines.cpp
pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_plane.cpp
pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_sphere.cpp
pkg/trunk/mapping/point_cloud_mapping/test/sample_consensus/test_circle_fit.cpp
pkg/trunk/mapping/point_cloud_mapping/test/sample_consensus/test_line_fit.cpp
pkg/trunk/mapping/point_cloud_mapping/test/sample_consensus/test_plane_fit.cpp
pkg/trunk/mapping/semantic_point_annotator/src/sac_ground_removal.cpp
pkg/trunk/mapping/semantic_point_annotator/src/sac_inc_ground_removal.cpp
pkg/trunk/mapping/semantic_point_annotator/src/semantic_point_annotator_omp.cpp
pkg/trunk/mapping/stereo_convex_patch_histogram/src/convex_patch_histogram.cpp
pkg/trunk/mapping/table_object_detector/src/table_object_detector.cpp
pkg/trunk/vision/outlet_spotting/src/outlet_spotting.cpp
Modified: pkg/trunk/mapping/door_handle_detector/src/geometric_helper.cpp
===================================================================
--- pkg/trunk/mapping/door_handle_detector/src/geometric_helper.cpp 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/door_handle_detector/src/geometric_helper.cpp 2009-04-01 18:05:46 UTC (rev 13239)
@@ -591,8 +591,8 @@
coeff.resize (0);
return (false);
}
- sac->computeCoefficients (); // Compute the model coefficients
- coeff = sac->refineCoefficients (); // Refine them using least-squares
+ sac->computeCoefficients (coeff); // Compute the model coefficients
+ sac->refineCoefficients (coeff); // Refine them using least-squares
model->selectWithinDistance (coeff, dist_thresh, inliers);
//inliers = sac->getInliers ();
@@ -809,10 +809,11 @@
model->setAxis (axis);
model->setEpsAngle (eps_angle);
+ vector<double> coeff;
// Search for the best line
if (sac->computeModel ())
{
- sac->computeCoefficients (); // Compute the model coefficients
+ sac->computeCoefficients (coeff); // Compute the model coefficients
//line_inliers = model->selectWithinDistance (sac->refineCoefficients (), dist_thresh);
line_inliers = sac->getInliers ();
}
@@ -855,9 +856,10 @@
model->setEpsAngle (eps_angle);
// Search for the best line
+ vector<double> coeff;
if (sac->computeModel ())
{
- sac->computeCoefficients (); // Compute the model coefficients
+ sac->computeCoefficients (coeff); // Compute the model coefficients
line_inliers = sac->getInliers ();
}
else
Modified: pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp
===================================================================
--- pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp 2009-04-01 18:05:46 UTC (rev 13239)
@@ -301,7 +301,9 @@
if((int) sac->getInliers().size() < sac_min_points_per_model_)
break;
inliers.push_back(sac->getInliers());
- coeff.push_back(sac->computeCoefficients());
+ vector<double> model_coefficients;
+ sac->computeCoefficients (model_coefficients);
+ coeff.push_back (model_coefficients);
//Find the edges of the line segments
cloud_geometry::statistics::getMinMax (*points, inliers.back(), minP, maxP);
Modified: pkg/trunk/mapping/door_tracker/src/door_tracker.cpp
===================================================================
--- pkg/trunk/mapping/door_tracker/src/door_tracker.cpp 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/door_tracker/src/door_tracker.cpp 2009-04-01 18:05:46 UTC (rev 13239)
@@ -377,7 +377,9 @@
if((int) sac->getInliers().size() < sac_min_points_per_model_)
break;
inliers.push_back(sac->getInliers());
- coeff.push_back(sac->computeCoefficients());
+ vector<double> model_coeff;
+ sac->computeCoefficients (model_coeff);
+ coeff.push_back (model_coeff);
//Find the edges of the line segments
cloud_geometry::statistics::getMinMax (*points, inliers.back(), minP, maxP);
Modified: pkg/trunk/mapping/hallway_tracker/src/hallway_tracker.cpp
===================================================================
--- pkg/trunk/mapping/hallway_tracker/src/hallway_tracker.cpp 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/hallway_tracker/src/hallway_tracker.cpp 2009-04-01 18:05:46 UTC (rev 13239)
@@ -227,7 +227,7 @@
// Now find the best fit parallel lines to this set of points and a corresponding set of inliers
if (sac->computeModel()) {
inliers = sac->getInliers();
- coeffs = sac->computeCoefficients();
+ sac->computeCoefficients (coeffs);
visualization(coeffs, inliers);
// Publish the result
robot_msgs::PointCloud model_cloud;
Modified: pkg/trunk/mapping/planar_patch_map/src/planar_patch_map.cpp
===================================================================
--- pkg/trunk/mapping/planar_patch_map/src/planar_patch_map.cpp 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/planar_patch_map/src/planar_patch_map.cpp 2009-04-01 18:05:46 UTC (rev 13239)
@@ -116,21 +116,25 @@
void
fitSACPlane (PointCloud *points, cloud_octree::Octree *octree, cloud_octree::Leaf* leaf, Polygon3D &poly)
{
+ double dist_thresh = 0.02;
vector<int> indices = leaf->getIndices ();
if ((int)indices.size () < sac_min_points_per_cell_)
return;
// Create and initialize the SAC model
sample_consensus::SACModelPlane *model = new sample_consensus::SACModelPlane ();
- sample_consensus::SAC *sac = new sample_consensus::RANSAC (model, 0.02);
+ sample_consensus::SAC *sac = new sample_consensus::RANSAC (model, dist_thresh);
model->setDataSet (points, indices);
+ vector<int> inliers;
+ vector<double> coeff;
// Search for the best plane
if (sac->computeModel ())
{
// Obtain the inliers and the planar model coefficients
- std::vector<int> inliers = sac->getInliers ();
- std::vector<double> coeff = sac->computeCoefficients ();
+ sac->computeCoefficients (coeff);
+ sac->refineCoefficients (coeff);
+ model->selectWithinDistance (coeff, dist_thresh, inliers);
///fprintf (stderr, "> Found a model supported by %d inliers: [%g, %g, %g, %g]\n", inliers.size (), coeff[0], coeff[1], coeff[2], coeff[3]);
// Project the inliers onto the model
Modified: pkg/trunk/mapping/planar_patch_map/src/planar_patch_map_omp.cpp
===================================================================
--- pkg/trunk/mapping/planar_patch_map/src/planar_patch_map_omp.cpp 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/planar_patch_map/src/planar_patch_map_omp.cpp 2009-04-01 18:05:46 UTC (rev 13239)
@@ -116,21 +116,25 @@
void
fitSACPlane (PointCloud *points, cloud_octree::Octree *octree, cloud_octree::Leaf* leaf, Polygon3D &poly)
{
+ double dist_thresh = 0.05;
vector<int> indices = leaf->getIndices ();
if ((int)indices.size () < sac_min_points_per_cell_)
return;
// Create and initialize the SAC model
sample_consensus::SACModelPlane *model = new sample_consensus::SACModelPlane ();
- sample_consensus::SAC *sac = new sample_consensus::RANSAC (model, 0.05);
+ sample_consensus::SAC *sac = new sample_consensus::RANSAC (model, dist_thresh);
model->setDataSet (points, indices);
+ vector<int> inliers;
+ vector<double> coeff;
// Search for the best plane
if (sac->computeModel ())
{
// Obtain the inliers and the planar model coefficients
- vector<int> inliers = sac->getInliers ();
- vector<double> coeff = sac->computeCoefficients ();
+ sac->computeCoefficients (coeff);
+ sac->refineCoefficients (coeff);
+ model->selectWithinDistance (coeff, dist_thresh, inliers);
///fprintf (stderr, "> Found a model supported by %d inliers: [%g, %g, %g, %g]\n", inliers.size (), coeff[0], coeff[1], coeff[2], coeff[3]);
// Project the inliers onto the model
Modified: pkg/trunk/mapping/plug_onbase_detector/include/plug_onbase_detector/plug_onbase_detector.h
===================================================================
--- pkg/trunk/mapping/plug_onbase_detector/include/plug_onbase_detector/plug_onbase_detector.h 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/plug_onbase_detector/include/plug_onbase_detector/plug_onbase_detector.h 2009-04-01 18:05:46 UTC (rev 13239)
@@ -212,7 +212,7 @@
// Find the base plane
vector<int> inliers;
vector<double> coeff;
- fitSACPlane (cloud_tr_, indices_in_bounds, inliers, coeff, &viewpoint_cloud_, sac_distance_threshold_, 100);
+ fitSACPlane (cloud_tr_, indices_in_bounds, inliers, coeff, viewpoint_cloud_, sac_distance_threshold_, 100);
// Remove points below the plane
for (unsigned int i = 0; i < indices_in_bounds.size (); i++)
@@ -387,7 +387,7 @@
*/
int
fitSACPlane (PointCloud &points, vector<int> indices, vector<int> &inliers, vector<double> &coeff,
- robot_msgs::PointStamped *viewpoint_cloud, double dist_thresh, int min_pts)
+ const robot_msgs::PointStamped &viewpoint_cloud, double dist_thresh, int min_pts)
{
if ((int)indices.size () < min_pts)
{
@@ -413,27 +413,12 @@
coeff.resize (0);
return (-1);
}
- sac->computeCoefficients (); // Compute the model coefficients
- coeff = sac->refineCoefficients (); // Refine them using least-squares
+ sac->computeCoefficients (coeff); // Compute the model coefficients
+ sac->refineCoefficients (coeff); // Refine them using least-squares
model->selectWithinDistance (coeff, dist_thresh, inliers);
// Flip plane normal according to the viewpoint information
- Point32 vp_m;
- vp_m.x = viewpoint_cloud->point.x - points.pts.at (inliers[0]).x;
- vp_m.y = viewpoint_cloud->point.y - points.pts.at (inliers[0]).y;
- vp_m.z = viewpoint_cloud->point.z - points.pts.at (inliers[0]).z;
-
- // Dot product between the (viewpoint - point) and the plane normal
- double cos_theta = (vp_m.x * coeff[0] + vp_m.y * coeff[1] + vp_m.z * coeff[2]);
-
- // Flip the plane normal
- if (cos_theta < 0)
- {
- for (int d = 0; d < 3; d++)
- coeff[d] *= -1;
- // Hessian form (D = nc . p_plane (centroid here) + p)
- coeff[3] = -1 * (coeff[0] * points.pts.at (inliers[0]).x + coeff[1] * points.pts.at (inliers[0]).y + coeff[2] * points.pts.at (inliers[0]).z);
- }
+ cloud_geometry::angles::flipNormalTowardsViewpoint (coeff, points.pts.at (inliers[0]), viewpoint_cloud);
//ROS_INFO ("Found a model supported by %d inliers: [%g, %g, %g, %g]\n", sac->getInliers ().size (),
// coeff[0], coeff[1], coeff[2], coeff[3]);
}
Modified: pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac.h
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac.h 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac.h 2009-04-01 18:05:46 UTC (rev 13239)
@@ -101,19 +101,21 @@
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Compute the coefficients of the model and return them. */
- virtual std::vector<double>
- computeCoefficients ()
+ virtual void
+ computeCoefficients (std::vector<double> &coefficients)
{
sac_model_->computeModelCoefficients (sac_model_->getBestModel ());
- return (sac_model_->getModelCoefficients ());
+ coefficients = sac_model_->getModelCoefficients ();
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief Use Least-Squares optimizations to refine the coefficients of the model, and return them. */
- virtual std::vector<double>
- refineCoefficients ()
+ /** \brief Use Least-Squares optimizations to refine the coefficients of the model, and return them.
+ * \param refit_coefficients the resultant recomputed coefficients after non-linear optimization
+ */
+ virtual void
+ refineCoefficients (std::vector<double> &refined_coefficients)
{
- return (sac_model_->refitModel (sac_model_->getBestInliers ()));
+ sac_model_->refitModel (sac_model_->getBestInliers (), refined_coefficients);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -138,11 +140,13 @@
/** \brief Project a set of given points (using their indices) onto the model and return their projections.
* \param indices a set of indices that represent the data that we're interested in
* \param model_coefficients the coefficients of the underlying model
+ * \param projected_points the resultant projected points
*/
- virtual robot_msgs::PointCloud
- projectPointsToModel (std::vector<int> indices, std::vector<double> model_coefficients)
+ virtual void
+ projectPointsToModel (const std::vector<int> &indices, const std::vector<double> &model_coefficients,
+ robot_msgs::PointCloud &projected_points)
{
- return (sac_model_->projectPoints (indices, model_coefficients));
+ sac_model_->projectPoints (indices, model_coefficients, projected_points);
}
Modified: pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model.h
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model.h 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model.h 2009-04-01 18:05:46 UTC (rev 13239)
@@ -61,20 +61,24 @@
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Test whether the given model coefficients are valid given the input point cloud data. Pure virtual.
- * \param model_coefficients the model coefficients that need to be tested */
+ * \param model_coefficients the model coefficients that need to be tested
+ */
virtual bool testModelCoefficients (const std::vector<double> &model_coefficients) = 0;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Check whether the given index samples can form a valid model, compute the model coefficients from
- * these samples and store them internally in model_coefficients_. Pure virtual.
- * \param indices the point indices found as possible good candidates for creating a valid model */
+ * these samples and store them internally in model_coefficients_. Pure virtual.
+ * \param indices the point indices found as possible good candidates for creating a valid model
+ */
virtual bool computeModelCoefficients (const std::vector<int> &indices) = 0;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Recompute the model coefficients using the given inlier set and return them to the user. Pure virtual.
- * @note: these are the coefficients of the model after refinement (eg. after a least-squares optimization)
- * \param inliers the data inliers found as supporting the model */
- virtual std::vector<double> refitModel (const std::vector<int> &inliers) = 0;
+ * @note: these are the coefficients of the model after refinement (eg. after a least-squares optimization)
+ * \param inliers the data inliers found as supporting the model
+ * \param refit_coefficients the resultant recomputed coefficients after non-linear optimization
+ */
+ virtual void refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients) = 0;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Compute all distances from the cloud data to a given model. Pure virtual.
@@ -95,20 +99,24 @@
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Create a new point cloud with inliers projected onto the model. Pure virtual.
- * \param inliers the data inliers that we want to project on the model
- * \param model_coefficients the coefficients of a model */
- virtual robot_msgs::PointCloud projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients) = 0;
+ * \param inliers the data inliers that we want to project on the model
+ * \param model_coefficients the coefficients of a model
+ * \param projected_points the resultant projected points
+ */
+ virtual void projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients, robot_msgs::PointCloud &projected_points) = 0;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Project inliers (in place) onto the given model. Pure virtual.
- * \param inliers the data inliers that we want to project on the model
- * \param model_coefficients the coefficients of a model */
+ * \param inliers the data inliers that we want to project on the model
+ * \param model_coefficients the coefficients of a model
+ */
virtual void projectPointsInPlace (const std::vector<int> &inliers, const std::vector<double> &model_coefficients) = 0;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Verify whether a subset of indices verifies the internal model coefficients. Pure virtual.
- * \param indices the data indices that need to be tested against the model
- * \param threshold a maximum admissible distance threshold for determining the inliers from the outliers */
+ * \param indices the data indices that need to be tested against the model
+ * \param threshold a maximum admissible distance threshold for determining the inliers from the outliers
+ */
virtual bool doSamplesVerifyModel (const std::set<int> &indices, double threshold) = 0;
@@ -150,7 +158,7 @@
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Set the best set of inliers. Used by SAC methods. Do not call this except if you know what you're doing.
* \param best_inliers the set of inliers for the best model */
- void setBestInliers (std::vector<int> best_inliers) { this->best_inliers_ = best_inliers; }
+ void setBestInliers (const std::vector<int> &best_inliers) { this->best_inliers_ = best_inliers; }
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Return the best set of inliers found so far for this model. */
Modified: pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_circle.h
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_circle.h 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_circle.h 2009-04-01 18:05:46 UTC (rev 13239)
@@ -1,5 +1,5 @@
/*
- * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
+ * Copyright (c) 2008-2009 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
*
* All rights reserved.
*
@@ -65,15 +65,17 @@
virtual bool computeModelCoefficients (const std::vector<int> &indices);
- virtual std::vector<double> refitModel (const std::vector<int> &inliers);
+ virtual void refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients);
virtual void getDistancesToModel (const std::vector<double> &model_coefficients, std::vector<double> &distances);
virtual void selectWithinDistance (const std::vector<double> &model_coefficients, double threshold, std::vector<int> &inliers);
- virtual robot_msgs::PointCloud projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients);
+ virtual void projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients, robot_msgs::PointCloud &projected_points);
virtual void projectPointsInPlace (const std::vector<int> &inliers, const std::vector<double> &model_coefficients);
virtual bool doSamplesVerifyModel (const std::set<int> &indices, double threshold);
+ static int functionToOptimize (void *p, int m, int n, const double *x, double *fvec, int iflag);
+
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Return an unique id for this model (SACMODEL_CIRCLE2D). */
virtual int getModelType () { return (SACMODEL_CIRCLE2D); }
Modified: pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_cylinder.h
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_cylinder.h 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_cylinder.h 2009-04-01 18:05:46 UTC (rev 13239)
@@ -65,15 +65,17 @@
virtual bool computeModelCoefficients (const std::vector<int> &indices);
- virtual std::vector<double> refitModel (const std::vector<int> &inliers);
+ virtual void refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients);
virtual void getDistancesToModel (const std::vector<double> &model_coefficients, std::vector<double> &distances);
virtual void selectWithinDistance (const std::vector<double> &model_coefficients, double threshold, std::vector<int> &inliers);
- virtual robot_msgs::PointCloud projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients);
+ virtual void projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients, robot_msgs::PointCloud &projected_points);
virtual void projectPointsInPlace (const std::vector<int> &inliers, const std::vector<double> &model_coefficients);
virtual bool doSamplesVerifyModel (const std::set<int> &indices, double threshold);
+ static int functionToOptimize (void *p, int m, int n, const double *x, double *fvec, int iflag);
+
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Return an unique id for this model (SACMODEL_CYLINDER). */
virtual int getModelType () { return (SACMODEL_CYLINDER); }
Modified: pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_line.h
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_line.h 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_line.h 2009-04-01 18:05:46 UTC (rev 13239)
@@ -65,11 +65,11 @@
virtual bool computeModelCoefficients (const std::vector<int> &indices);
- virtual std::vector<double> refitModel (const std::vector<int> &inliers);
+ virtual void refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients);
virtual void getDistancesToModel (const std::vector<double> &model_coefficients, std::vector<double> &distances);
virtual void selectWithinDistance (const std::vector<double> &model_coefficients, double threshold, std::vector<int> &inliers);
- virtual robot_msgs::PointCloud projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients);
+ virtual void projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients, robot_msgs::PointCloud &projected_points);
virtual void projectPointsInPlace (const std::vector<int> &inliers, const std::vector<double> &model_coefficients);
virtual bool doSamplesVerifyModel (const std::set<int> &indices, double threshold);
Modified: pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_parallel_lines.h
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_parallel_lines.h 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_parallel_lines.h 2009-04-01 18:05:46 UTC (rev 13239)
@@ -71,11 +71,11 @@
virtual bool computeModelCoefficients (const std::vector<int> &indices);
- virtual std::vector<double> refitModel (const std::vector<int> &inliers);
+ virtual void refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients);
virtual void getDistancesToModel (const std::vector<double> &model_coefficients, std::vector<double> &distances);
virtual void selectWithinDistance (const std::vector<double> &model_coefficients, double threshold, std::vector<int> &inliers);
- virtual robot_msgs::PointCloud projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients);
+ virtual void projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients, robot_msgs::PointCloud &projected_points);
virtual void projectPointsInPlace (const std::vector<int> &inliers, const std::vector<double> &model_coefficients);
virtual bool doSamplesVerifyModel (const std::set<int> &indices, double threshold);
Modified: pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_plane.h
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_plane.h 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_plane.h 2009-04-01 18:05:46 UTC (rev 13239)
@@ -65,15 +65,17 @@
virtual bool computeModelCoefficients (const std::vector<int> &indices);
- virtual std::vector<double> refitModel (const std::vector<int> &inliers);
+ virtual void refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients);
virtual void getDistancesToModel (const std::vector<double> &model_coefficients, std::vector<double> &distances);
virtual void selectWithinDistance (const std::vector<double> &model_coefficients, double threshold, std::vector<int> &inliers);
- virtual robot_msgs::PointCloud projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients);
+ virtual void...
[truncated message content] |
|
From: <ge...@us...> - 2009-04-01 23:58:42
|
Revision: 13267
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13267&view=rev
Author: gerkey
Date: 2009-04-01 23:58:32 +0000 (Wed, 01 Apr 2009)
Log Message:
-----------
Changed initialpose to be robot_msgs::PoseWithCovariance.
Modified Paths:
--------------
pkg/trunk/nav/amcl_player/CMakeLists.txt
pkg/trunk/nav/amcl_player/amcl_player.cc
pkg/trunk/nav/amcl_player/src/amcl_node.cpp
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h
pkg/trunk/nav/nav_view/src/nav_view/tools.cpp
pkg/trunk/visualization/rviz/src/rviz/tools/pose_tool.cpp
Modified: pkg/trunk/nav/amcl_player/CMakeLists.txt
===================================================================
--- pkg/trunk/nav/amcl_player/CMakeLists.txt 2009-04-01 23:57:29 UTC (rev 13266)
+++ pkg/trunk/nav/amcl_player/CMakeLists.txt 2009-04-01 23:58:32 UTC (rev 13267)
@@ -1,16 +1,11 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(amcl_player)
-set(new_code FALSE)
-if(NOT new_code)
-
rospack_add_executable(amcl_player amcl_player.cc)
target_link_libraries(amcl_player playerdrivers)
rospack_add_executable(cli cli.cpp)
-else(NOT new_code)
-
rospack_add_library(amcl_pf
src/pf/pf.c
src/pf/pf_kdtree.c
@@ -34,5 +29,3 @@
rospack_add_executable(bin/amcl
src/amcl_node.cpp)
target_link_libraries(bin/amcl amcl_sensors amcl_map amcl_pf)
-
-endif(NOT new_code)
Modified: pkg/trunk/nav/amcl_player/amcl_player.cc
===================================================================
--- pkg/trunk/nav/amcl_player/amcl_player.cc 2009-04-01 23:57:29 UTC (rev 13266)
+++ pkg/trunk/nav/amcl_player/amcl_player.cc 2009-04-01 23:58:32 UTC (rev 13267)
@@ -56,7 +56,7 @@
Subscribes to (name/type):
- @b "odom"/RobotBase2DOdom : robot's odometric pose. Only the position information is used (velocity is ignored).
- @b "scan"/LaserScan : laser scans.
-- @b "initialpose"/Pose2DFloat32: pose used to (re)initialize particle filter
+- @b "initialpose"/PoseWithCovariance: pose used to (re)initialize particle filter
Publishes to (name / type):
- @b "localizedpose"/RobotBase2DOdom : robot's localized map pose. Only the position information is set (no velocity).
@@ -100,7 +100,7 @@
#include "laser_scan/LaserScan.h"
#include "deprecated_msgs/RobotBase2DOdom.h"
#include "robot_msgs/ParticleCloud.h"
-#include "deprecated_msgs/Pose2DFloat32.h"
+#include "robot_msgs/PoseWithCovariance.h"
#include "robot_srvs/StaticMap.h"
// For transform support
@@ -148,13 +148,15 @@
robot_msgs::ParticleCloud particleCloudMsg;
deprecated_msgs::RobotBase2DOdom odomMsg;
laser_scan::LaserScan laserMsg;
- deprecated_msgs::Pose2DFloat32 initialPoseMsg;
+ robot_msgs::PoseWithCovariance initialPoseMsg;
// Message callbacks
void odomReceived();
void laserReceived();
void initialPoseReceived();
+ double getYaw(robot_msgs::Pose& p);
+
// These are the devices that amcl offers, and to which we subscribe
Driver* driver;
Device* pdevice;
@@ -877,9 +879,9 @@
void
AmclNode::initialPoseReceived()
{
- this->AmclNode::setPose(this->initialPoseMsg.x,
- this->initialPoseMsg.y,
- this->initialPoseMsg.th);
+ this->AmclNode::setPose(this->initialPoseMsg.pose.position.x,
+ this->initialPoseMsg.pose.position.y,
+ this->getYaw(this->initialPoseMsg.pose));
}
void
@@ -914,3 +916,13 @@
*/
}
+double
+AmclNode::getYaw(robot_msgs::Pose& p)
+{
+ tf::Stamped<tf::Transform> t;
+ tf::PoseMsgToTF(p, t);
+ double yaw, pitch, roll;
+ btMatrix3x3 mat = t.getBasis();
+ mat.getEulerZYX(yaw,pitch,roll);
+ return yaw;
+}
Modified: pkg/trunk/nav/amcl_player/src/amcl_node.cpp
===================================================================
--- pkg/trunk/nav/amcl_player/src/amcl_node.cpp 2009-04-01 23:57:29 UTC (rev 13266)
+++ pkg/trunk/nav/amcl_player/src/amcl_node.cpp 2009-04-01 23:58:32 UTC (rev 13267)
@@ -87,7 +87,10 @@
#include <deque>
+#include <boost/bind.hpp>
+
#include "map/map.h"
+#include "pf/pf.h"
#include "ros/assert.h"
@@ -96,14 +99,15 @@
// Messages that I need
#include "laser_scan/LaserScan.h"
-#include "deprecated_msgs/RobotBase2DOdom.h"
+#include "robot_msgs/PoseWithCovariance.h"
#include "robot_msgs/ParticleCloud.h"
-#include "deprecated_msgs/Pose2DFloat32.h"
+#include "robot_msgs/Pose.h"
#include "robot_srvs/StaticMap.h"
// For transform support
#include "tf/transform_broadcaster.h"
#include "tf/transform_listener.h"
+#include "tf/message_notifier.h"
class AmclNode
{
@@ -116,17 +120,19 @@
int setPose(double x, double y, double a);
private:
- tf::TransformBroadcaster* tf;
- tf::TransformListener* tfL;
+ tf::TransformBroadcaster* tfb_;
+ tf::TransformListener* tf_;
// incoming messages
laser_scan::LaserScan laser_msg_;
- deprecated_msgs::Pose2DFloat32 initialPoseMsg_;
+ robot_msgs::PoseWithCovariance initialPoseMsg_;
// Message callbacks
- void laserReceived();
+ void laserReceived(const tf::MessageNotifier<laser_scan::LaserScan>::MessagePtr& laser_scan);
void initialPoseReceived();
+ double getYaw(robot_msgs::Pose& p);
+
//parameter for what odom to use
std::string odom_frame_id;
@@ -137,6 +143,13 @@
bool have_laser_pose;
double laser_x, laser_y, laser_yaw;
+ tf::MessageNotifier<laser_scan::LaserScan>* laser_scan_notifer;
+
+ // Particle filter
+ pf_t *pf_;
+ int pf_min_samples_, pf_max_samples_;
+ double pf_err_, pf_z_;
+
ros::Duration cloud_pub_interval;
ros::Time last_cloud_pub_time;
@@ -174,23 +187,22 @@
AmclNode::AmclNode() :
map_(NULL),
- have_laser_pose(false)
+ have_laser_pose(false),
+ pf_(NULL)
{
// Grab params off the param server
- int max_beams, min_samples, max_samples;
+ int max_beams;
double odom_drift_xx, odom_drift_yy, odom_drift_aa, odom_drift_xa;
double d_thresh, a_thresh;
ros::Node::instance()->param("pf_laser_max_beams", max_beams, 20);
- ros::Node::instance()->param("pf_min_samples", min_samples, 500);
- ros::Node::instance()->param("pf_max_samples", max_samples, 10000);
+ ros::Node::instance()->param("pf_min_samples", pf_min_samples_, 500);
+ ros::Node::instance()->param("pf_max_samples", pf_max_samples_, 10000);
+ ros::Node::instance()->param("pf_err", pf_err_, 0.01);
+ ros::Node::instance()->param("pf_z", pf_z_, 3.0);
ros::Node::instance()->param("pf_odom_drift_xx", odom_drift_xx, 0.2);
ros::Node::instance()->param("pf_odom_drift_yy", odom_drift_yy, 0.2);
ros::Node::instance()->param("pf_odom_drift_aa", odom_drift_aa, 0.2);
ros::Node::instance()->param("pf_odom_drift_xa", odom_drift_xa, 0.2);
- odom_drift_xx = 0.5;
- odom_drift_yy = 0.5;
- odom_drift_aa = 0.5;
- odom_drift_xa = 0.5;
ros::Node::instance()->param("pf_min_d", d_thresh, 0.2);
ros::Node::instance()->param("pf_min_a", a_thresh, M_PI/6.0);
ros::Node::instance()->param("pf_odom_frame_id", odom_frame_id, std::string("odom"));
@@ -203,14 +215,27 @@
setPose(startX, startY, startTH);
cloud_pub_interval.fromSec(1.0);
- tf = new tf::TransformBroadcaster(*ros::Node::instance());
- tfL = new tf::TransformListener(*ros::Node::instance());
+ tfb_ = new tf::TransformBroadcaster(*ros::Node::instance());
+ tf_ = new tf::TransformListener(*ros::Node::instance());
map_ = requestMap();
+
+ // Create the particle filter
+ pf_ = pf_alloc(pf_min_samples_, pf_max_samples_);
+ pf_->pop_err = pf_err_;
+ pf_->pop_z = pf_z_;
- ros::Node::instance()->advertise<deprecated_msgs::RobotBase2DOdom>("localizedpose",2);
+ //ros::Node::instance()->advertise<deprecated_msgs::RobotBase2DOdom>("localizedpose",2);
+ ros::Node::instance()->advertise<robot_msgs::PoseWithCovariance>("localizedpose",2);
ros::Node::instance()->advertise<robot_msgs::ParticleCloud>("particlecloud",2);
- ros::Node::instance()->subscribe("scan", laser_msg_, &AmclNode::laserReceived,this,2);
+ //ros::Node::instance()->subscribe("scan", laser_msg_, &AmclNode::laserReceived,this,2);
+ laser_scan_notifer =
+ new tf::MessageNotifier<laser_scan::LaserScan>
+ (tf_, ros::Node::instance(),
+ boost::bind(&AmclNode::laserReceived,
+ this, _1),
+ "scan", "base_footprint",
+ 20);
ros::Node::instance()->subscribe("initialpose", initialPoseMsg_, &AmclNode::initialPoseReceived,this,2);
}
@@ -285,7 +310,7 @@
tf::Stamped<tf::Pose> odom_to_map;
try
{
- this->tfL->transformPose(odom_frame_id,tf::Stamped<tf::Pose> (btTransform(btQuaternion(pdata->pos.pa, 0, 0),
+ this->tf_->transformPose(odom_frame_id,tf::Stamped<tf::Pose> (btTransform(btQuaternion(pdata->pos.pa, 0, 0),
btVector3(pdata->pos.px, pdata->pos.py, 0.0)).inverse(),
t, "base_footprint"),odom_to_map);
}
@@ -297,7 +322,7 @@
//we want to send a transform that is good up until a tolerance time so that odom can be used
ros::Time transform_expiration;
transform_expiration.fromSec(hdr->timestamp + transform_tolerance_);
- this->tf->sendTransform(tf::Stamped<tf::Transform> (tf::Transform(tf::Quaternion( odom_to_map.getRotation() ),
+ this->tfb_->sendTransform(tf::Stamped<tf::Transform> (tf::Transform(tf::Quaternion( odom_to_map.getRotation() ),
tf::Point( odom_to_map.getOrigin() ) ).inverse(),
transform_expiration,odom_frame_id, "/map"));
@@ -500,7 +525,7 @@
tf::Stamped<btTransform> odom_pose;
try
{
- this->tfL->transformPose(odom_frame_id, ident, odom_pose);
+ this->tf_->transformPose(odom_frame_id, ident, odom_pose);
}
catch(tf::TransformException e)
{
@@ -516,8 +541,9 @@
}
void
-AmclNode::laserReceived()
+AmclNode::laserReceived(const tf::MessageNotifier<laser_scan::LaserScan>::MessagePtr& laser_scan)
{
+ puts("got scan");
#if 0
// Do we have the base->base_laser Tx yet?
if(!have_laser_pose)
@@ -528,7 +554,7 @@
tf::Stamped<btTransform> laser_pose;
try
{
- this->tfL->transformPose("base_footprint", ident, laser_pose);
+ this->tf_->transformPose("base_footprint", ident, laser_pose);
}
catch(tf::TransformException e)
{
@@ -629,10 +655,22 @@
#endif
}
+double
+AmclNode::getYaw(robot_msgs::Pose& p)
+{
+ tf::Stamped<tf::Transform> t;
+ tf::PoseMsgToTF(p, t);
+ double yaw, pitch, roll;
+ btMatrix3x3 mat = t.getBasis();
+ mat.getEulerZYX(yaw,pitch,roll);
+ return yaw;
+}
+
void
AmclNode::initialPoseReceived()
{
- setPose(initialPoseMsg_.x,
- initialPoseMsg_.y,
- initialPoseMsg_.th);
+ ROS_DEBUG("Setting pose: %.3f %.3f %.3f",
+ initialPoseMsg_.pose.position.x,
+ initialPoseMsg_.pose.position.y,
+ getYaw(initialPoseMsg_.pose));
}
Modified: pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-04-01 23:57:29 UTC (rev 13266)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-04-01 23:58:32 UTC (rev 13267)
@@ -117,7 +117,7 @@
ros_node_->param("/global_frame_id", global_frame_id_, std::string("/map"));
ros_node_->advertise<robot_msgs::Planner2DGoal>("goal", 1);
- ros_node_->advertise<deprecated_msgs::Pose2DFloat32>("initialpose", 1);
+ ros_node_->advertise<robot_msgs::PoseWithCovariance>("initialpose", 1);
ros_node_->subscribe("particlecloud", cloud_, &NavViewPanel::incomingParticleCloud, this, 1);
ros_node_->subscribe("gui_path", path_line_, &NavViewPanel::incomingGuiPath, this, 1);
ros_node_->subscribe("local_path", local_path_, &NavViewPanel::incomingLocalPath, this, 1);
Modified: pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h 2009-04-01 23:57:29 UTC (rev 13266)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h 2009-04-01 23:58:32 UTC (rev 13267)
@@ -35,7 +35,7 @@
#include "robot_msgs/ParticleCloud.h"
#include "robot_msgs/Planner2DGoal.h"
#include "robot_msgs/Polyline2D.h"
-#include "deprecated_msgs/Pose2DFloat32.h"
+#include "robot_msgs/PoseWithCovariance.h"
#include "robot_srvs/StaticMap.h"
#include <OGRE/OgreTexture.h>
Modified: pkg/trunk/nav/nav_view/src/nav_view/tools.cpp
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/tools.cpp 2009-04-01 23:57:29 UTC (rev 13266)
+++ pkg/trunk/nav/nav_view/src/nav_view/tools.cpp 2009-04-01 23:58:32 UTC (rev 13267)
@@ -37,6 +37,8 @@
#include "nav_view_panel.h"
+#include "tf/tf.h"
+
namespace nav_view
{
@@ -169,11 +171,12 @@
}
else
{
- deprecated_msgs::Pose2DFloat32 pose;
- pose.x = pos_.x;
- pose.y = pos_.y;
- pose.th = angle;
- printf( "setting pose: %.3f %.3f %.3f\n", pose.x, pose.y, pose.th );
+ robot_msgs::PoseWithCovariance pose;
+ pose.pose.position.x = pos_.x;
+ pose.pose.position.y = pos_.y;
+ tf::QuaternionTFToMsg(tf::Quaternion(angle, 0.0, 0.0),
+ pose.pose.orientation);
+ ROS_INFO( "setting pose: %.3f %.3f %.3f\n", pos_.x, pos_.y, angle );
ros_node_->publish( "initialpose", pose );
}
Modified: pkg/trunk/visualization/rviz/src/rviz/tools/pose_tool.cpp
===================================================================
--- pkg/trunk/visualization/rviz/src/rviz/tools/pose_tool.cpp 2009-04-01 23:57:29 UTC (rev 13266)
+++ pkg/trunk/visualization/rviz/src/rviz/tools/pose_tool.cpp 2009-04-01 23:58:32 UTC (rev 13267)
@@ -36,7 +36,7 @@
#include "ogre_tools/wx_ogre_render_window.h"
#include <robot_msgs/Planner2DGoal.h>
-#include <deprecated_msgs/Pose2DFloat32.h>
+#include <robot_msgs/PoseWithCovariance.h>
#include <OGRE/OgreRay.h>
#include <OGRE/OgrePlane.h>
@@ -60,7 +60,7 @@
arrow_->getSceneNode()->setVisible( false );
ros_node_->advertise<robot_msgs::Planner2DGoal>("goal", 1);
- ros_node_->advertise<deprecated_msgs::Pose2DFloat32>("initialpose", 1);
+ ros_node_->advertise<robot_msgs::PoseWithCovariance>("initialpose", 1);
}
PoseTool::~PoseTool()
@@ -161,11 +161,15 @@
}
else
{
- deprecated_msgs::Pose2DFloat32 pose;
- pose.x = robot_pos_transformed.x();
- pose.y = robot_pos_transformed.y();
- pose.th = angle;
- ROS_INFO("Setting pose: %.3f %.3f %.3f [frame=%s]", pose.x, pose.y, pose.th, fixed_frame.c_str());
+ robot_msgs::PoseWithCovariance pose;
+ pose.pose.position.x = robot_pos_transformed.x();
+ pose.pose.position.y = robot_pos_transformed.y();
+ tf::QuaternionTFToMsg(tf::Quaternion(angle, 0.0, 0.0),
+ pose.pose.orientation);
+ ROS_INFO("Setting pose: %.3f %.3f %.3f [frame=%s]",
+ robot_pos_transformed.x(),
+ robot_pos_transformed.y(),
+ angle, fixed_frame.c_str());
ros_node_->publish( "initialpose", pose );
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mar...@us...> - 2009-04-02 00:15:10
|
Revision: 13268
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13268&view=rev
Author: mariusmuja
Date: 2009-04-02 00:15:08 +0000 (Thu, 02 Apr 2009)
Log Message:
-----------
Cleaned up vision door handle detection.
Modified Paths:
--------------
pkg/trunk/util/topic_synchronizer/topic_synchronizer.h
pkg/trunk/vision/door_handle_detect/CMakeLists.txt
pkg/trunk/vision/door_handle_detect/handles.launch
pkg/trunk/vision/door_handle_detect/src/door_handle_detect.cpp
Added Paths:
-----------
pkg/trunk/vision/door_handle_detect/src/test.cpp
Modified: pkg/trunk/util/topic_synchronizer/topic_synchronizer.h
===================================================================
--- pkg/trunk/util/topic_synchronizer/topic_synchronizer.h 2009-04-01 23:58:32 UTC (rev 13267)
+++ pkg/trunk/util/topic_synchronizer/topic_synchronizer.h 2009-04-02 00:15:08 UTC (rev 13268)
@@ -313,6 +313,13 @@
{
ready_ = true;
}
+
+ void reset()
+ {
+ expected_count_ = 0;
+ count_ = 0;
+ done_ = false;
+ }
};
#endif
Modified: pkg/trunk/vision/door_handle_detect/CMakeLists.txt
===================================================================
--- pkg/trunk/vision/door_handle_detect/CMakeLists.txt 2009-04-01 23:58:32 UTC (rev 13267)
+++ pkg/trunk/vision/door_handle_detect/CMakeLists.txt 2009-04-02 00:15:08 UTC (rev 13268)
@@ -6,3 +6,5 @@
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
rospack_add_executable(door_handle_detect src/door_handle_detect.cpp)
+
+rospack_add_executable(test_service src/test.cpp)
Modified: pkg/trunk/vision/door_handle_detect/handles.launch
===================================================================
--- pkg/trunk/vision/door_handle_detect/handles.launch 2009-04-01 23:58:32 UTC (rev 13267)
+++ pkg/trunk/vision/door_handle_detect/handles.launch 2009-04-02 00:15:08 UTC (rev 13268)
@@ -24,6 +24,8 @@
<!-- <node pkg="dcam" type="stereodcam" respawn="false"/>-->
<node pkg="stereo_image_proc" type="stereoproc" respawn="false"/>
- <node pkg="door_handle_detect" name="door_handle_detect" type="door_handle_detect" respawn="false" output="screen"/>
+ <node pkg="door_handle_detect" name="door_handle_detect" type="door_handle_detect" respawn="false" output="screen">
+ <param name="display" type="bool" value="true" />
+ </node>
</launch>
Modified: pkg/trunk/vision/door_handle_detect/src/door_handle_detect.cpp
===================================================================
--- pkg/trunk/vision/door_handle_detect/src/door_handle_detect.cpp 2009-04-01 23:58:32 UTC (rev 13267)
+++ pkg/trunk/vision/door_handle_detect/src/door_handle_detect.cpp 2009-04-02 00:15:08 UTC (rev 13268)
@@ -104,317 +104,280 @@
{
public:
+
image_msgs::Image limage;
image_msgs::Image rimage;
image_msgs::Image dimage;
image_msgs::StereoInfo stinfo;
image_msgs::DisparityInfo dispinfo;
image_msgs::CamInfo rcinfo;
-
image_msgs::CvBridge lbridge;
image_msgs::CvBridge rbridge;
image_msgs::CvBridge dbridge;
+ robot_msgs::PointCloud cloud_fetch;
robot_msgs::PointCloud cloud;
- robot_msgs::Door door;
- robot_msgs::Point32 door_handle;
IplImage* left;
IplImage* right;
IplImage* disp;
IplImage* disp_clone;
- CvScalar door_plane;
- bool have_door_plane;
- bool pause;
- bool display;
-
- string destination_frame;
-
TopicSynchronizer<HandleDetector> sync;
boost::mutex cv_mutex;
+ boost::condition_variable images_ready;
tf::TransformListener *tf_;
- CvHaarClassifierCascade* cascade;
- CvMemStorage* storage;
+ // minimum height to look at (in base_link frame)
+ double min_height;
+ // maximum height to look at (in base_link frame)
+ double max_height;
+ // no. of frames to detect handle in
+ int frames_no;
+ // display stereo images ?
+ bool display;
- HandleDetector() : ros::Node("stereo_view"),
- left(NULL), right(NULL), disp(NULL), disp_clone(NULL), pause(false),
- sync(this, &HandleDetector::image_cb_all, ros::Duration().fromSec(0.05), &HandleDetector::image_cb_timeout)
- {
- tf_ = new tf::TransformListener(*this);
- param("~display", display, false);
+ CvHaarClassifierCascade* cascade;
+ CvMemStorage* storage;
- if (display) {
- cvNamedWindow("left", CV_WINDOW_AUTOSIZE);
- //cvNamedWindow("right", CV_WINDOW_AUTOSIZE);
- // cvNamedWindow("contours", CV_WINDOW_AUTOSIZE);
- cvNamedWindow("disparity", CV_WINDOW_AUTOSIZE);
- cvNamedWindow("disparity_original", CV_WINDOW_AUTOSIZE);
- }
+ HandleDetector()
+ :ros::Node("stereo_view"), left(NULL), right(NULL), disp(NULL), disp_clone(NULL), sync(this, &HandleDetector::image_cb_all, ros::Duration().fromSec(0.1), &HandleDetector::image_cb_timeout)
+ {
+ tf_ = new tf::TransformListener(*this);
+ // define node parameters
- std::list<std::string> left_list;
- left_list.push_back(std::string("stereo/left/image_rect_color"));
- left_list.push_back(std::string("stereo/left/image_rect"));
-// std::list<std::string> right_list;
-// right_list.push_back(std::string("stereo/right/image_rect_color"));
-// right_list.push_back(std::string("stereo/right/image_rect"));
+ param("~min_height", min_height, 0.7);
+ param("~max_height", max_height, 1.0);
+ param("~frames_no", frames_no, 7);
- sync.subscribe(left_list, limage, 1);
-// sync.subscribe(right_list, rimage, 1);
- sync.subscribe("stereo/disparity", dimage, 1);
- sync.subscribe("stereo/stereo_info", stinfo, 1);
- sync.subscribe("stereo/disparity_info", dispinfo, 1);
- sync.subscribe("stereo/right/cam_info", rcinfo, 1);
-
- sync.subscribe("stereo/cloud", cloud, 1);
- sync.ready();
-
- have_door_plane = false;
-
- subscribe("/door_detector/door_msg", door, &HandleDetector::door_detected,1);
-
- string handle_features_file;
- string background_features_file;
+ param("~display", display, false);
stringstream ss;
ss << getenv("ROS_ROOT") << "/../ros-pkg/vision/door_handle_detect/data/";
string path = ss.str();
-
string cascade_classifier;
- param<string>("cascade_classifier", cascade_classifier, path+"handles_data.xml");
- ROS_INFO("Loading cascade classifier\n");
- cascade = (CvHaarClassifierCascade*)cvLoad( cascade_classifier.c_str(), 0, 0, 0 );
+ param<string>("cascade_classifier", cascade_classifier, path + "handles_data.xml");
- if (!cascade) {
- ROS_ERROR("Cannot load cascade classifier\n");
+ if(display){
+ cvNamedWindow("left", CV_WINDOW_AUTOSIZE);
+ //cvNamedWindow("right", CV_WINDOW_AUTOSIZE);
+ cvNamedWindow("disparity", CV_WINDOW_AUTOSIZE);
+ cvNamedWindow("disparity_original", CV_WINDOW_AUTOSIZE);
}
- storage = cvCreateMemStorage(0);
+// subscribeStereoData();
+
+ // load a cascade classifier
+ loadClassifier(cascade_classifier);
// invalid location until we get a detection
- door_handle.x = -1;
- door_handle.y = -1;
- door_handle.z = -1;
- param<string>("destination_frame", destination_frame, "odom_combined");
- advertise<robot_msgs::PointStamped>("handle_detector/handle_location",1);
- advertise<robot_msgs::VisualizationMarker>("visualizationMarker",1);
- advertiseService("door_handle_vision_detector", &HandleDetector::detectDoorSrv, this);
- }
+// advertise<robot_msgs::PointStamped>("handle_detector/handle_location", 1);
+ advertise<robot_msgs::VisualizationMarker>("visualizationMarker", 1);
+ advertiseService("door_handle_vision_detector", &HandleDetector::detectHandleSrv, this);
+ }
- ~HandleDetector()
- {
- if (left) {
- cvReleaseImage(&left);
- }
- if (right) {
- cvReleaseImage(&right);
- }
- if (disp) {
- cvReleaseImage(&disp);
- }
- if (storage) {
- cvReleaseMemStorage(&storage);
- }
- }
+ ~HandleDetector()
+ {
+ if(left){
+ cvReleaseImage(&left);
+ }
+ if(right){
+ cvReleaseImage(&right);
+ }
+ if(disp){
+ cvReleaseImage(&disp);
+ }
+ if(storage){
+ cvReleaseMemStorage(&storage);
+ }
+ }
private:
- /////////////////////////////////////////////////
- // Analyze the disparity image that values should not be too far off from one another
- // Id -- 8 bit, 1 channel disparity image
- // R -- rectangular region of interest
- // vertical -- This is a return that tells whether something is on a wall (descending disparities) or not.
- // minDisparity -- disregard disparities less than this
- //
- double disparitySTD(IplImage *Id, CvRect &R, double& meanDisparity, double minDisparity = 0.5 )
- {
- int ws = Id->widthStep;
- unsigned char *p = (unsigned char *)(Id->imageData);
- int rx = R.x;
- int ry = R.y;
- int rw = R.width;
- int rh = R.height;
- int nchan = Id->nChannels;
- p += ws*ry + rx*nchan; //Put at start of box
- double mean = 0.0,var = 0.0;
- double val;
- int cnt = 0;
- //For vertical objects, Disparities should decrease from top to bottom, measure that
- for(int Y=0; Y<rh; ++Y)
- {
- for(int X=0; X<rw; X++, p+=nchan)
- {
- val = (double)*p;
- if(val < minDisparity)
- continue;
- mean += val;
- var += val*val;
- cnt++;
- }
- p+=ws-(rw*nchan);
- }
- if(cnt == 0) //Error condition, no disparities, return impossible variance
- {
- return 10000000.0;
- }
- //DO THE VARIANCE MATH
- mean = mean/(double)cnt;
- var = (var/(double)cnt) - mean*mean;
- meanDisparity = mean;
- return(sqrt(var));
- }
+ void loadClassifier(string cascade_classifier)
+ {
+ // subscribe("/door_detector/door_msg", door, &HandleDetector::door_detected,1);
+ ROS_INFO("Loading cascade classifier");
+ cascade = (CvHaarClassifierCascade*)(cvLoad(cascade_classifier.c_str(), 0, 0, 0));
+ if(!cascade){
+ ROS_ERROR("Cannot load cascade classifier\n");
+ }
+ storage = cvCreateMemStorage(0);
+ }
+ void subscribeStereoData()
+ {
- robot_msgs::Point disparityTo3D(CvStereoCamModel& cam_model, int x, int y, double d)
- {
- CvMat* uvd = cvCreateMat(1,3,CV_32FC1);
- cvmSet(uvd,0,0,x);
- cvmSet(uvd,0,1,y);
- cvmSet(uvd,0,2,d);
- CvMat* xyz = cvCreateMat(1,3,CV_32FC1);
- cam_model.dispToCart(uvd,xyz);
- robot_msgs::Point result;
- result.x = cvmGet(xyz,0,0);
- result.y = cvmGet(xyz,0,1);
- result.z = cvmGet(xyz,0,2);
- return result;
- }
+ sync.reset();
+ std::list<std::string> left_list;
+ left_list.push_back(std::string("stereo/left/image_rect_color"));
+ left_list.push_back(std::string("stereo/left/image_rect"));
+ sync.subscribe(left_list, limage, 1);
+ // std::list<std::string> right_list;
+ // right_list.push_back(std::string("stereo/right/image_rect_color"));
+ // right_list.push_back(std::string("stereo/right/image_rect"));
+ // sync.subscribe(right_list, rimage, 1);
- /**
- *
- * @param a
- * @param b
- * @return
- */
- double distance3D(robot_msgs::Point a, robot_msgs::Point b)
- {
- return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y)+(a.z-b.z)*(a.z-b.z));
- }
+ sync.subscribe("stereo/disparity", dimage, 1);
+ sync.subscribe("stereo/stereo_info", stinfo, 1);
+ sync.subscribe("stereo/disparity_info", dispinfo, 1);
+ sync.subscribe("stereo/right/cam_info", rcinfo, 1);
+ sync.subscribe("stereo/cloud", cloud_fetch, 1);
+ sync.ready();
+// sleep(1);
+ }
- void get_bb_dimensions(IplImage *Id, CvRect &R, double meanDisparity, double& dx, double& dy, robot_msgs::Point& center)
- {
- // initialize stereo camera model
- double Fx = rcinfo.P[0]; double Fy = rcinfo.P[5];
- double Clx = rcinfo.P[2]; double Crx = Clx;
- double Cy = rcinfo.P[6];
- double Tx = -rcinfo.P[3]/Fx;
- CvStereoCamModel cam_model(Fx,Fy,Tx,Clx,Crx,Cy,4.0/(double)dispinfo.dpp);
+ void unsubscribeStereoData()
+ {
+// unsubscribe("stereo/left/image_rect_color");
+ unsubscribe("stereo/left/image_rect");
+ unsubscribe("stereo/disparity");
+ unsubscribe("stereo/stereo_info");
+ unsubscribe("stereo/disparity_info");
+ unsubscribe("stereo/right/cam_info");
+ unsubscribe("stereo/cloud");
+ }
+ /////////////////////////////////////////////////
+ // Analyze the disparity image that values should not be too far off from one another
+ // Id -- 8 bit, 1 channel disparity image
+ // R -- rectangular region of interest
+ // vertical -- This is a return that tells whether something is on a wall (descending disparities) or not.
+ // minDisparity -- disregard disparities less than this
+ //
+ double disparitySTD(IplImage *Id, const CvRect & R, double & meanDisparity, double minDisparity = 0.5)
+ {
+ int ws = Id->widthStep;
+ unsigned char *p = (unsigned char*)(Id->imageData);
+ int rx = R.x;
+ int ry = R.y;
+ int rw = R.width;
+ int rh = R.height;
+ int nchan = Id->nChannels;
+ p += ws * ry + rx * nchan; //Put at start of box
+ double mean = 0.0, var = 0.0;
+ double val;
+ int cnt = 0;
+ //For vertical objects, Disparities should decrease from top to bottom, measure that
+ for(int Y = 0;Y < rh;++Y){
+ for(int X = 0;X < rw;X++, p += nchan){
+ val = (double)*p;
+ if(val < minDisparity)
+ continue;
- int ws = Id->widthStep;
- unsigned char *p = (unsigned char *)(Id->imageData);
- int rx = R.x;
- int ry = R.y;
- int rw = R.width;
- int rh = R.height;
- int nchan = Id->nChannels;
- p += ws*ry + rx*nchan; //Put at start of box
+ mean += val;
+ var += val * val;
+ cnt++;
+ }
+ p += ws - (rw * nchan);
+ }
- robot_msgs::Point p1 = disparityTo3D(cam_model, rx, ry, meanDisparity);
- robot_msgs::Point p2 = disparityTo3D(cam_model, rx+rw, ry, meanDisparity);
- robot_msgs::Point p3 = disparityTo3D(cam_model, rx, ry+rh, meanDisparity);
+ if(cnt == 0){
+ return 10000000.0;
+ }
+ //DO THE VARIANCE MATH
+ mean = mean / (double)cnt;
+ var = (var / (double)cnt) - mean * mean;
+ meanDisparity = mean;
+ return (sqrt(var));
+ }
- center = disparityTo3D(cam_model, rx+rw/2, ry+rh/2, meanDisparity);
+ /**
+ * \brief Transforms a disparity image pixel to real-world point
+ *
+ * @param cam_model Camera model
+ * @param x coordinate in the disparity image
+ * @param y coordinate in the disparity image
+ * @param d disparity pixel value
+ * @return point in 3D space
+ */
+ robot_msgs::Point disparityTo3D(CvStereoCamModel & cam_model, int x, int y, double d)
+ {
+ CvMat *uvd = cvCreateMat(1, 3, CV_32FC1);
+ cvmSet(uvd, 0, 0, x);
+ cvmSet(uvd, 0, 1, y);
+ cvmSet(uvd, 0, 2, d);
+ CvMat *xyz = cvCreateMat(1, 3, CV_32FC1);
+ cam_model.dispToCart(uvd, xyz);
+ robot_msgs::Point result;
+ result.x = cvmGet(xyz, 0, 0);
+ result.y = cvmGet(xyz, 0, 1);
+ result.z = cvmGet(xyz, 0, 2);
+ return result;
+ }
- dx = distance3D(p1,p2);
- dy = distance3D(p1,p3);
+ /**
+ * \brief Computes distance between two 3D points
+ *
+ * @param a
+ * @param b
+ * @return
+ */
+ double distance3D(robot_msgs::Point a, robot_msgs::Point b)
+ {
+ return sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z));
+ }
- }
+ /**
+ * \brief Computes size and center of ROI in real-world
+ *
+ * Given a disparity images and a ROI in the image this function computes the approximate real-world size
+ * and center of the ROI.
+ *
+ * This function is just an approximation, it uses the mean value of the disparity in the ROI and assumes
+ * the ROI is flat region perpendicular on the camera z axis. It could be improved by finding a dominant plane
+ * in the and using only those disparity values.
+ *
+ * @param R
+ * @param meanDisparity
+ * @param dx
+ * @param dy
+ * @param center
+ */
+ void getROIDimensions(const CvRect& r, double & dx, double & dy, robot_msgs::Point & center)
+ {
+ // initialize stereo camera model
+ double Fx = rcinfo.P[0];
+ double Fy = rcinfo.P[5];
+ double Clx = rcinfo.P[2];
+ double Crx = Clx;
+ double Cy = rcinfo.P[6];
+ double Tx = -rcinfo.P[3] / Fx;
+ CvStereoCamModel cam_model(Fx, Fy, Tx, Clx, Crx, Cy, 4.0 / (double)dispinfo.dpp);
+ double mean = 0;
+ disparitySTD(disp, r, mean);
- bool handlePossibleHere(CvRect& r)
- {
- const float nz_fraction = 0.1;
+ robot_msgs::Point p1 = disparityTo3D(cam_model, r.x, r.y, mean);
+ robot_msgs::Point p2 = disparityTo3D(cam_model, r.x + r.width, r.y, mean);
+ robot_msgs::Point p3 = disparityTo3D(cam_model, r.x, r.y + r.height, mean);
+ center = disparityTo3D(cam_model, r.x + r.width / 2, r.y + r.height / 2, mean);
+ dx = distance3D(p1, p2);
+ dy = distance3D(p1, p3);
+ }
- cvSetImageROI(disp,r);
- cvSetImageCOI(disp, 1);
- int cnt = cvCountNonZero(disp);
- cvResetImageROI(disp);
- cvSetImageCOI(disp,0);
- double mean = 0;
- disparitySTD(disp, r, mean);
-
- if (cnt<nz_fraction*r.width*r.height) {
-// ROS_INFO("Too few pixels in the disparity, discarding");
- return false;
- }
-
- robot_msgs::PointCloud pc = filterPointCloud(r);
- CvScalar plane = estimatePlaneLS(pc);
- cnt = 0;
- double sum = 0;
- double max_dist = 0;
- for (size_t i = 0; i<pc.pts.size();++i) {
- robot_msgs::Point32 p = pc.pts[i];
- double dist = fabs(plane.val[0]*p.x+plane.val[1]*p.y+plane.val[2]*p.z+plane.val[3]);
- max_dist = max(max_dist,dist);
- sum += dist;
- cnt++;
- }
- sum /= cnt;
-
- printf("Average distance to plane: %f, max: %f\n", sum, max_dist);
-
- if (max_dist>0.1 || sum<0.005) {
-// cvRectangle(left, cvPoint(r.x,r.y), cvPoint(r.x+r.width, r.y+r.height), CV_RGB(0,0,255));
- return false;
- }
-
-
- double dx, dy;
- robot_msgs::Point p;
- get_bb_dimensions(disp, r, mean, dx, dy,p);
-
- if (dx>0.25 || dy>0.15) {
- ROS_INFO("Too big, discarding");
- return false;
- }
-
- robot_msgs::PointStamped pin, pout;
- pin.header.frame_id = cloud.header.frame_id;
- pin.header.stamp = cloud.header.stamp;
- pin.point.x = p.z;
- pin.point.y = -p.x;
- pin.point.z = -p.y;
-
-
- tf_->transformPoint("base_link", pin, pout);
-
-// printf("r: (%d, %d, %d, %d), sdv: %f, dx: %f, dy: %f, x: %f, y: %f, z: %f\n", r.x, r.y, r.width, r.height, sdv, dx, dy,
-// pout.point.x, pout.point.y, pout.point.z);
-
- if (pout.point.z>0.9 || pout.point.z<0.7) {
- ROS_INFO("Too high, discarding");
- return false;
-
- }
-
-
- // publish handle location
- tf_->transformPoint(destination_frame, pin, pout);
- door_handle.x = pout.point.x;
- door_handle.y = pout.point.y;
- door_handle.z = pout.point.z;
- publish("handle_detector/handle_location", pout);
-
-
- robot_msgs::VisualizationMarker marker;
- marker.header.frame_id = destination_frame;
- marker.header.stamp = ros::Time((uint64_t)0ULL);
+ /**
+ * \brief Publishes a visualization marker for a point.
+ * @param p
+ */
+ void showHandleMarker(robot_msgs::PointStamped p)
+ {
+ robot_msgs::VisualizationMarker marker;
+ marker.header.frame_id = p.header.frame_id;
+ marker.header.stamp = ros::Time((uint64_t)(0ULL));
marker.id = 0;
marker.type = robot_msgs::VisualizationMarker::SPHERE;
marker.action = robot_msgs::VisualizationMarker::ADD;
- marker.x = pout.point.x;
- marker.y = pout.point.y;
- marker.z = pout.point.z;
+ marker.x = p.point.x;
+ marker.y = p.point.y;
+ marker.z = p.point.z;
marker.yaw = 0.0;
marker.pitch = 0.0;
marker.roll = 0.0;
@@ -425,327 +388,574 @@
marker.r = 0;
marker.g = 255;
marker.b = 0;
+ publish("visualizationMarker", marker);
+ }
- publish( "visualizationMarker", marker );
- return true;
- }
+// void showPlaneMarker(CvScalar plane, robot_msgs::PointCloud pc)
+// {
+//
+// double min_d = 10000;
+// int min_i = -1;
+// for (size_t i=0;i<pc.pts.size();++i) {
+// float dist = fabs(plane.val[0]*pc.pts[i].x+plane.val[1]*pc.pts[i].y+plane.val[2]*pc.pts[i].z+plane.val[3]);
+// if (dist<min_d) {
+// min_d = dist;
+// min_i = i;
+// }
+// }
+//
+//
+// robot_msgs::VisualizationMarker marker;
+// marker.header.frame_id = pc.header.frame_id;
+// marker.header.stamp = pc.header.stamp;
+// marker.id = 1211;
+// marker.type = robot_msgs::VisualizationMarker::SPHERE;
+// marker.action = robot_msgs::VisualizationMarker::ADD;
+// marker.x = pc.pts[min_i].x;
+// marker.y = pc.pts[min_i].y;
+// marker.z = pc.pts[min_i].z;
+// marker.yaw = 0.0;
+// marker.pitch = 0.0;
+// marker.roll = 0.0;
+// marker.xScale = 0.1;
+// marker.yScale = 0.1;
+// marker.zScale = 0.1;
+// marker.alpha = 255;
+// marker.r = 255;
+// marker.g = 0;
+// marker.b = 0;
+// publish("visualizationMarker", marker);
+//
+// printf("Show marker at: (%f,%f,%f)", marker.x, marker.y, marker.z);
+// }
- void findHandleCascade( )
- {
+ /**
+ * \brief Determine if it's possible for handle to be in a specific ROI
+ *
+ * It looks at things like, height, approximate size of ROI, how flat it is.
+ *
+ * @param r
+ * @return
+ */
+ bool handlePossibleHere(const CvRect &r)
+ {
+ const float nz_fraction = 0.1;
- IplImage *gray = cvCreateImage( cvSize(left->width,left->height), 8, 1 );
+ cvSetImageROI(disp, r);
+ cvSetImageCOI(disp, 1);
+ int cnt = cvCountNonZero(disp);
+ cvResetImageROI(disp);
+ cvSetImageCOI(disp, 0);
- cvCvtColor( left, gray, CV_BGR2GRAY );
- cvEqualizeHist( gray, gray );
- cvClearMemStorage( storage );
+ if(cnt < nz_fraction * r.width * r.height){
+ return false;
+ }
- if( cascade )
- {
- CvSeq* handles = cvHaarDetectObjects( gray, cascade, storage,
- 1.1, 2, 0
- //|CV_HAAR_FIND_BIGGEST_OBJECT
- //|CV_HAAR_DO_ROUGH_SEARCH
- //|CV_HAAR_DO_CANNY_PRUNING
- //|CV_HAAR_SCALE_IMAGE
- ,
- cvSize(10, 10) );
- for(int i = 0; i < (handles ? handles->total : 0); i++ )
- {
- CvRect* r = (CvRect*)cvGetSeqElem( handles, i );
- if (handlePossibleHere(*r)) {
- cvRectangle(left, cvPoint(r->x,r->y), cvPoint(r->x+r->width, r->y+r->height), CV_RGB(0,255,0));
- }
- else {
-// cvRectangle(left, cvPoint(r->x,r->y), cvPoint(r->x+r->width, r->y+r->height), CV_RGB(255,0,0));
- }
- }
- }
+ // compute least-squares handle plane
+ robot_msgs::PointCloud pc = filterPointCloud(r);
+ CvScalar plane = estimatePlaneLS(pc);
- cvReleaseImage( &gray );
- }
+ cnt = 0;
+ double sum = 0;
+ double max_dist = 0;
+ for(size_t i = 0;i < pc.pts.size();++i){
+ robot_msgs::Point32 p = pc.pts[i];
+ double dist = fabs(plane.val[0] * p.x + plane.val[1] * p.y + plane.val[2] * p.z + plane.val[3]);
+ max_dist = max(max_dist, dist);
+ sum += dist;
+ cnt++;
+ }
+ sum /= cnt;
+ if(max_dist > 0.1 || sum < 0.005){
+ return false;
+ }
+ double dx, dy;
+ robot_msgs::Point p;
+ getROIDimensions(r, dx, dy, p);
+ if(dx > 0.25 || dy > 0.15){
+ ROS_INFO("Too big, discarding");
+ return false;
+ }
- void image_cb_all(ros::Time t)
- {
- if (pause) return;
- cv_mutex.lock();
+ robot_msgs::PointStamped pin, pout;
+ pin.header.frame_id = cloud.header.frame_id;
+ pin.header.stamp = cloud.header.stamp;
+ pin.point.x = p.z;
+ pin.point.y = -p.x;
+ pin.point.z = -p.y;
+ try {
+ tf_->transformPoint("base_link", pin, pout);
+ }
+ catch(tf::LookupException & ex){
+ ROS_ERROR("Lookup exception: %s\n", ex.what());
+ }
+ catch(tf::ExtrapolationException & ex){
+ ROS_DEBUG("Extrapolation exception: %s\n", ex.what());
+ }
+ catch(tf::ConnectivityException & ex){
+ ROS_ERROR("Connectivity exception: %s\n", ex.what());
+ }
- if (lbridge.fromImage(limage, "bgr"))
- {
- if(left != NULL)
- cvReleaseImage(&left);
- left = cvCloneImage(lbridge.toIpl());
- }
+ if(pout.point.z > max_height || pout.point.z < min_height){
+ return false;
+ }
- if (rbridge.fromImage(rimage, "bgr"))
- {
- if(right != NULL)
- cvReleaseImage(&right);
+ printf("Handle at: (%d,%d,%d,%d)\n", r.x,r.y,r.width, r.height);
- right = cvCloneImage(rbridge.toIpl());
- }
- if (dbridge.fromImage(dimage))
- {
- if(disp != NULL)
- cvReleaseImage(&disp);
+ return true;
+ }
- disp = cvCreateImage(cvGetSize(dbridge.toIpl()), IPL_DEPTH_8U, 1);
- cvCvtScale(dbridge.toIpl(), disp, 4.0/dispinfo.dpp);
- }
+ /**
+ * \brief Start handle detection
+ */
+ void findHandleCascade(vector<CvRect> & handle_rect)
+ {
+ IplImage *gray = cvCreateImage(cvSize(left->width, left->height), 8, 1);
+ cvCvtColor(left, gray, CV_BGR2GRAY);
+ cvEqualizeHist(gray, gray);
+ cvClearMemStorage(storage);
+ if(cascade){
+ CvSeq *handles = cvHaarDetectObjects(gray, cascade, storage, 1.1, 2, 0, //|CV_HAAR_FIND_BIGGEST_OBJECT
+ //|CV_HAAR_DO_ROUGH_SEARCH
+ //|CV_HAAR_DO_CANNY_PRUNING
+ //|CV_HAAR_SCALE_IMAGE
+ cvSize(10, 10));
+ for(int i = 0;i < (handles ? handles->total : 0);i++){
+ CvRect *r = (CvRect*)cvGetSeqElem(handles, i);
- if (display) {
- cvShowImage("left", left);
- //cvShowImage("right", right);
- // cvShowImage("disparity", disp_clone);
+ if(handlePossibleHere(*r)){
+ handle_rect.push_back(*r);
+// if(display){
+// cvRectangle(left, cvPoint(r->x, r->y), cvPoint(r->x + r->width, r->y + r->height), CV_RGB(0, 255, 0));
+// }
+ }
+ else{
+ // cvRectangle(left, cvPoint(r->x, r->y), cvPoint(r->x + r->width, r->y + r->height), CV_RGB(255, 0, 0));
+ }
+ }
- cvShowImage("disparity_original", disp);
+ }
+ else {
+ ROS_ERROR("Cannot look for handle, no detector loaded");
+ }
- }
+ cvReleaseImage(&gray);
+ }
- applyPositionPrior(disp);
- if (display) {
- cvShowImage("disparity", disp);
- }
-// findEdges(left);
- findHandleCascade();
+ /**
+ * \brief Finds edges in an image
+ * @param img
+ */
+ void findEdges(IplImage *img)
+ {
+ IplImage *gray = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
+ cvCvtColor(img, gray, CV_RGB2GRAY);
+ cvCanny(gray, gray, 20, 40);
+ CvMemStorage *storage = cvCreateMemStorage(0);
+ CvSeq *lines = 0;
+ lines = cvHoughLines2(gray, storage, CV_HOUGH_PROBABILISTIC, 1, CV_PI / 360, 100, 200, 100);
+ for(int i = 0;i < lines->total;i++){
+ CvPoint *line = (CvPoint*)cvGetSeqElem(lines, i);
+ CvPoint p1 = line[0];
+ CvPoint p2 = line[1];
+ if(abs(p1.x - p2.x) > 0){
+ float min_angle = 80;
+ float slope = float(abs(p1.y - p2.y)) / abs(p1.x - p2.x);
+ float min_slope = tan(CV_PI / 2 - min_angle * CV_PI / 180);
+ if(slope < min_slope)
+ continue;
+ printf("slope: %f, min_slope: %f\n", slope, min_slope);
+ }
+ cvLine(left, p1, p2, CV_RGB(255, 0, 0), 2, CV_AA, 0);
+ }
- if (display) {
- // cvShowImage("disparity", disp);
- cvShowImage("left", left);
- }
- if (disp_clone!=NULL)
- cvReleaseImage(&disp_clone);
+ cvReleaseImage(&gray);
+ }
- cv_mutex.unlock();
+ /**
+ * \brief Checks if a point should belong to a cluster
+ *
+ * @param center
+ * @param p
+ * @return
+ */
+ bool belongsToCluster(pair<float,float> center, pair<float,float> p)
+ {
+ return fabs(center.first-p.first)<3 && fabs(center.second-p.second)<3;
+ }
-// pause = true;
- }
+ /**
+ * Helper function.
+ *
+ * @param r
+ * @return
+ */
+ pair<float,float> rectCenter(const CvRect& r)
+ {
+ return make_pair(r.x+(float)r.width/2,r.y+(float)r.height/2);
+ }
+ /**
+ * \brief Decide the handle position from several detections across multiple frames
+ *
+ * This method does some simple clustering of all the bounding boxes and picks the cluster with
+ * the most elements.
+ *
+ * @param handle_rect The handle bounding boxes
+ * @param handle Point indicating the real-world handle position
+ * @return
+ */
+ bool decideHandlePosition(vector<CvRect>& handle_rect, robot_msgs::PointStamped & handle)
+ {
+ if (handle_rect.size()==0) {
+ return false;
+ }
- void door_detected()
- {
- door_plane.val[0] = door.normal.x;
- door_plane.val[1] = door.normal.y;
- door_plane.val[2] = door.normal.z;
- door_plane.val[3] = -(door.normal.x*door.door_p1.x+door.normal.y*door.door_p1.y+door.normal.z*door.door_p1.z);
+ vector<int> cluster_size;
+ vector<pair<float,float> > centers;
+ vector<pair<float...
[truncated message content] |
|
From: <mar...@us...> - 2009-04-02 18:44:15
|
Revision: 13319
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13319&view=rev
Author: mariusmuja
Date: 2009-04-02 18:44:12 +0000 (Thu, 02 Apr 2009)
Log Message:
-----------
Moved door_handle_detect (vision door handle detection) into the door_handle_detection node.
Modified Paths:
--------------
pkg/trunk/mapping/door_handle_detector/CMakeLists.txt
pkg/trunk/mapping/door_handle_detector/manifest.xml
Added Paths:
-----------
pkg/trunk/mapping/door_handle_detector/launch/handle_detector_vision.launch
pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp
Removed Paths:
-------------
pkg/trunk/vision/door_handle_detect/CMakeLists.txt
pkg/trunk/vision/door_handle_detect/Makefile
pkg/trunk/vision/door_handle_detect/data/annotate.py
pkg/trunk/vision/door_handle_detect/data/convert.py
pkg/trunk/vision/door_handle_detect/data/handles_data.xml
pkg/trunk/vision/door_handle_detect/handles.launch
pkg/trunk/vision/door_handle_detect/manifest.xml
pkg/trunk/vision/door_handle_detect/src/door_handle_detect.cpp
pkg/trunk/vision/door_handle_detect/src/test.cpp
Modified: pkg/trunk/mapping/door_handle_detector/CMakeLists.txt
===================================================================
--- pkg/trunk/mapping/door_handle_detector/CMakeLists.txt 2009-04-02 18:43:22 UTC (rev 13318)
+++ pkg/trunk/mapping/door_handle_detector/CMakeLists.txt 2009-04-02 18:44:12 UTC (rev 13319)
@@ -42,6 +42,7 @@
rospack_add_library(executive_functions src/executive_functions.cpp)
+rospack_add_executable(handle_detector_vision src/handle_detector_vision.cpp)
Copied: pkg/trunk/mapping/door_handle_detector/launch/handle_detector_vision.launch (from rev 13318, pkg/trunk/vision/door_handle_detect/handles.launch)
===================================================================
--- pkg/trunk/mapping/door_handle_detector/launch/handle_detector_vision.launch (rev 0)
+++ pkg/trunk/mapping/door_handle_detector/launch/handle_detector_vision.launch 2009-04-02 18:44:12 UTC (rev 13319)
@@ -0,0 +1,31 @@
+
+<launch>
+ <!-- videre_mode should be one of the following:
+ disparity: Disparity and rectification done on chip.
+ Provides: Disparity and left mono image
+ disparity_raw: Disparity done on chip (debayer and rectification in software).
+ Provides: disparity and left color image.
+ none: No stereo on chip (all processing done in software).
+ Provides: all 3 images available
+ rectified: Rectification on chip
+ Provides: all 3 images available but no color
+ -->
+ <param name="stereo/videre_mode" type="str" value="none"/>
+ <param name="stereo/do_colorize" type="bool" value="True"/>
+ <param name="stereo/do_rectify" type="bool" value="True"/>
+ <param name="stereo/do_stereo" type="bool" value="True"/>
+ <param name="stereo/do_calc_points" type="bool" value="True"/>
+ <param name="stereo/do_keep_coords" type="bool" value="True"/>
+ <param name="stereo/horopter" value="128"/>
+ <param name="stereo/ndisp" value="128"/>
+ <param name="stereo/gain" type="int" value="10"/>
+ <param name="stereo/exposure" type="int" value="100"/>
+
+
+<!-- <node pkg="dcam" type="stereodcam" respawn="false"/>-->
+ <node pkg="stereo_image_proc" type="stereoproc" respawn="false"/>
+ <node pkg="door_handle_detector" name="handle_detector_vision" type="handle_detector_vision" respawn="false" output="screen">
+ <param name="display" type="bool" value="true" />
+ </node>
+</launch>
+
Modified: pkg/trunk/mapping/door_handle_detector/manifest.xml
===================================================================
--- pkg/trunk/mapping/door_handle_detector/manifest.xml 2009-04-02 18:43:22 UTC (rev 13318)
+++ pkg/trunk/mapping/door_handle_detector/manifest.xml 2009-04-02 18:44:12 UTC (rev 13319)
@@ -20,6 +20,11 @@
<depend package="point_cloud_assembler" />
<depend package="std_msgs" />
<depend package="sbpl_arm_executive" />
+ <depend package="opencv_latest"/>
+ <depend package="image_msgs"/>
+ <depend package="topic_synchronizer"/>
+ <depend package="stereo_utils"/>
+ <depend package="tf"/>
<export>
Copied: pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp (from rev 13318, pkg/trunk/vision/door_handle_detect/src/door_handle_detect.cpp)
===================================================================
--- pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp (rev 0)
+++ pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp 2009-04-02 18:44:12 UTC (rev 13319)
@@ -0,0 +1,989 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Willow Garage nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+// Author: Marius Muja
+
+#include <vector>
+#include <fstream>
+#include <sstream>
+#include <time.h>
+#include <iostream>
+#include <iomanip>
+
+
+#include "image_msgs/CvBridge.h"
+
+#include "opencv/cxcore.h"
+#include "opencv/cv.h"
+#include "opencv/highgui.h"
+
+#include "ros/node.h"
+#include "image_msgs/StereoInfo.h"
+#include "image_msgs/DisparityInfo.h"
+#include "image_msgs/CamInfo.h"
+#include "image_msgs/Image.h"
+#include "robot_msgs/PointCloud.h"
+#include "robot_msgs/Point32.h"
+#include "robot_msgs/PointStamped.h"
+#include "robot_msgs/Door.h"
+#include "robot_msgs/VisualizationMarker.h"
+#include "door_handle_detector/DoorsDetector.h"
+
+#include <string>
+
+// transform library
+#include <tf/transform_listener.h>
+
+#include "topic_synchronizer.h"
+
+#include "CvStereoCamModel.h"
+
+#include <boost/thread.hpp>
+
+using namespace std;
+
+template <typename T>
+class IndexedIplImage
+{
+public:
+ IplImage* img_;
+ T* p;
+
+ IndexedIplImage(IplImage* img) : img_(img)
+ {
+ p = (T*)img_->imageData;
+ }
+
+ operator IplImage*()
+ {
+ return img_;
+ }
+
+ T at(int x, int y, int chan = 0)
+ {
+ return *(p+y*img_->width+x*img_->nChannels+chan);
+ }
+
+ T integral_sum(const CvRect &r)
+ {
+ return at(r.x+r.width+1,r.y+r.height+1)-at(r.x+r.width+1,r.y)-at(r.x,r.y+r.height+1)+at(r.x,r.y);
+ }
+
+};
+
+class HandleDetector : public ros::Node
+{
+public:
+
+
+ image_msgs::Image limage;
+ image_msgs::Image rimage;
+ image_msgs::Image dimage;
+ image_msgs::StereoInfo stinfo;
+ image_msgs::DisparityInfo dispinfo;
+ image_msgs::CamInfo rcinfo;
+ image_msgs::CvBridge lbridge;
+ image_msgs::CvBridge rbridge;
+ image_msgs::CvBridge dbridge;
+
+ robot_msgs::PointCloud cloud_fetch;
+ robot_msgs::PointCloud cloud;
+
+ IplImage* left;
+ IplImage* right;
+ IplImage* disp;
+ IplImage* disp_clone;
+
+ TopicSynchronizer<HandleDetector> sync;
+
+ boost::mutex cv_mutex;
+ boost::condition_variable images_ready;
+
+ tf::TransformListener *tf_;
+
+
+ // minimum height to look at (in base_link frame)
+ double min_height;
+ // maximum height to look at (in base_link frame)
+ double max_height;
+ // no. of frames to detect handle in
+ int frames_no;
+ // display stereo images ?
+ bool display;
+
+
+
+ CvHaarClassifierCascade* cascade;
+ CvMemStorage* storage;
+
+ HandleDetector()
+ :ros::Node("stereo_view"), left(NULL), right(NULL), disp(NULL), disp_clone(NULL), sync(this, &HandleDetector::image_cb_all, ros::Duration().fromSec(0.1), &HandleDetector::image_cb_timeout)
+ {
+ tf_ = new tf::TransformListener(*this);
+ // define node parameters
+
+
+ param("~min_height", min_height, 0.7);
+ param("~max_height", max_height, 1.0);
+ param("~frames_no", frames_no, 7);
+
+
+ param("~display", display, false);
+ stringstream ss;
+ ss << getenv("ROS_ROOT") << "/../ros-pkg/vision/door_handle_detect/data/";
+ string path = ss.str();
+ string cascade_classifier;
+ param<string>("cascade_classifier", cascade_classifier, path + "handles_data.xml");
+
+ if(display){
+ cvNamedWindow("left", CV_WINDOW_AUTOSIZE);
+ //cvNamedWindow("right", CV_WINDOW_AUTOSIZE);
+ cvNamedWindow("disparity", CV_WINDOW_AUTOSIZE);
+ cvNamedWindow("disparity_original", CV_WINDOW_AUTOSIZE);
+ }
+
+// subscribeStereoData();
+
+ // load a cascade classifier
+ loadClassifier(cascade_classifier);
+ // invalid location until we get a detection
+
+// advertise<robot_msgs::PointStamped>("handle_detector/handle_location", 1);
+ advertise<robot_msgs::VisualizationMarker>("visualizationMarker", 1);
+ advertiseService("door_handle_vision_detector", &HandleDetector::detectHandleSrv, this);
+ }
+
+ ~HandleDetector()
+ {
+ if(left){
+ cvReleaseImage(&left);
+ }
+ if(right){
+ cvReleaseImage(&right);
+ }
+ if(disp){
+ cvReleaseImage(&disp);
+ }
+ if(storage){
+ cvReleaseMemStorage(&storage);
+ }
+ }
+
+private:
+ void loadClassifier(string cascade_classifier)
+ {
+ // subscribe("/door_detector/door_msg", door, &HandleDetector::door_detected,1);
+ ROS_INFO("Loading cascade classifier");
+ cascade = (CvHaarClassifierCascade*)(cvLoad(cascade_classifier.c_str(), 0, 0, 0));
+ if(!cascade){
+ ROS_ERROR("Cannot load cascade classifier\n");
+ }
+ storage = cvCreateMemStorage(0);
+ }
+
+ void subscribeStereoData()
+ {
+
+ sync.reset();
+ std::list<std::string> left_list;
+ left_list.push_back(std::string("stereo/left/image_rect_color"));
+ left_list.push_back(std::string("stereo/left/image_rect"));
+ sync.subscribe(left_list, limage, 1);
+
+ // std::list<std::string> right_list;
+ // right_list.push_back(std::string("stereo/right/image_rect_color"));
+ // right_list.push_back(std::string("stereo/right/image_rect"));
+ // sync.subscribe(right_list, rimage, 1);
+
+ sync.subscribe("stereo/disparity", dimage, 1);
+ sync.subscribe("stereo/stereo_info", stinfo, 1);
+ sync.subscribe("stereo/disparity_info", dispinfo, 1);
+ sync.subscribe("stereo/right/cam_info", rcinfo, 1);
+ sync.subscribe("stereo/cloud", cloud_fetch, 1);
+ sync.ready();
+// sleep(1);
+ }
+
+ void unsubscribeStereoData()
+ {
+// unsubscribe("stereo/left/image_rect_color");
+ unsubscribe("stereo/left/image_rect");
+ unsubscribe("stereo/disparity");
+ unsubscribe("stereo/stereo_info");
+ unsubscribe("stereo/disparity_info");
+ unsubscribe("stereo/right/cam_info");
+ unsubscribe("stereo/cloud");
+ }
+
+ /////////////////////////////////////////////////
+ // Analyze the disparity image that values should not be too far off from one another
+ // Id -- 8 bit, 1 channel disparity image
+ // R -- rectangular region of interest
+ // vertical -- This is a return that tells whether something is on a wall (descending disparities) or not.
+ // minDisparity -- disregard disparities less than this
+ //
+ double disparitySTD(IplImage *Id, const CvRect & R, double & meanDisparity, double minDisparity = 0.5)
+ {
+ int ws = Id->widthStep;
+ unsigned char *p = (unsigned char*)(Id->imageData);
+ int rx = R.x;
+ int ry = R.y;
+ int rw = R.width;
+ int rh = R.height;
+ int nchan = Id->nChannels;
+ p += ws * ry + rx * nchan; //Put at start of box
+ double mean = 0.0, var = 0.0;
+ double val;
+ int cnt = 0;
+ //For vertical objects, Disparities should decrease from top to bottom, measure that
+ for(int Y = 0;Y < rh;++Y){
+ for(int X = 0;X < rw;X++, p += nchan){
+ val = (double)*p;
+ if(val < minDisparity)
+ continue;
+
+ mean += val;
+ var += val * val;
+ cnt++;
+ }
+ p += ws - (rw * nchan);
+ }
+
+ if(cnt == 0){
+ return 10000000.0;
+ }
+ //DO THE VARIANCE MATH
+ mean = mean / (double)cnt;
+ var = (var / (double)cnt) - mean * mean;
+ meanDisparity = mean;
+ return (sqrt(var));
+ }
+
+ /**
+ * \brief Transforms a disparity image pixel to real-world point
+ *
+ * @param cam_model Camera model
+ * @param x coordinate in the disparity image
+ * @param y coordinate in the disparity image
+ * @param d disparity pixel value
+ * @return point in 3D space
+ */
+ robot_msgs::Point disparityTo3D(CvStereoCamModel & cam_model, int x, int y, double d)
+ {
+ CvMat *uvd = cvCreateMat(1, 3, CV_32FC1);
+ cvmSet(uvd, 0, 0, x);
+ cvmSet(uvd, 0, 1, y);
+ cvmSet(uvd, 0, 2, d);
+ CvMat *xyz = cvCreateMat(1, 3, CV_32FC1);
+ cam_model.dispToCart(uvd, xyz);
+ robot_msgs::Point result;
+ result.x = cvmGet(xyz, 0, 0);
+ result.y = cvmGet(xyz, 0, 1);
+ result.z = cvmGet(xyz, 0, 2);
+ return result;
+ }
+
+ /**
+ * \brief Computes distance between two 3D points
+ *
+ * @param a
+ * @param b
+ * @return
+ */
+ double distance3D(robot_msgs::Point a, robot_msgs::Point b)
+ {
+ return sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z));
+ }
+
+ /**
+ * \brief Computes size and center of ROI in real-world
+ *
+ * Given a disparity images and a ROI in the image this function computes the approximate real-world size
+ * and center of the ROI.
+ *
+ * This function is just an approximation, it uses the mean value of the disparity in the ROI and assumes
+ * the ROI is flat region perpendicular on the camera z axis. It could be improved by finding a dominant plane
+ * in the and using only those disparity values.
+ *
+ * @param R
+ * @param meanDisparity
+ * @param dx
+ * @param dy
+ * @param center
+ */
+ void getROIDimensions(const CvRect& r, double & dx, double & dy, robot_msgs::Point & center)
+ {
+ // initialize stereo camera model
+ double Fx = rcinfo.P[0];
+ double Fy = rcinfo.P[5];
+ double Clx = rcinfo.P[2];
+ double Crx = Clx;
+ double Cy = rcinfo.P[6];
+ double Tx = -rcinfo.P[3] / Fx;
+ CvStereoCamModel cam_model(Fx, Fy, Tx, Clx, Crx, Cy, 4.0 / (double)dispinfo.dpp);
+
+ double mean = 0;
+ disparitySTD(disp, r, mean);
+
+ robot_msgs::Point p1 = disparityTo3D(cam_model, r.x, r.y, mean);
+ robot_msgs::Point p2 = disparityTo3D(cam_model, r.x + r.width, r.y, mean);
+ robot_msgs::Point p3 = disparityTo3D(cam_model, r.x, r.y + r.height, mean);
+ center = disparityTo3D(cam_model, r.x + r.width / 2, r.y + r.height / 2, mean);
+ dx = distance3D(p1, p2);
+ dy = distance3D(p1, p3);
+ }
+
+
+ /**
+ * \brief Publishes a visualization marker for a point.
+ * @param p
+ */
+ void showHandleMarker(robot_msgs::PointStamped p)
+ {
+ robot_msgs::VisualizationMarker marker;
+ marker.header.frame_id = p.header.frame_id;
+ marker.header.stamp = ros::Time((uint64_t)(0ULL));
+ marker.id = 0;
+ marker.type = robot_msgs::VisualizationMarker::SPHERE;
+ marker.action = robot_msgs::VisualizationMarker::ADD;
+ marker.x = p.point.x;
+ marker.y = p.point.y;
+ marker.z = p.point.z;
+ marker.yaw = 0.0;
+ marker.pitch = 0.0;
+ marker.roll = 0.0;
+ marker.xScale = 0.1;
+ marker.yScale = 0.1;
+ marker.zScale = 0.1;
+ marker.alpha = 255;
+ marker.r = 0;
+ marker.g = 255;
+ marker.b = 0;
+ publish("visualizationMarker", marker);
+ }
+
+
+
+// void showPlaneMarker(CvScalar plane, robot_msgs::PointCloud pc)
+// {
+//
+// double min_d = 10000;
+// int min_i = -1;
+// for (size_t i=0;i<pc.pts.size();++i) {
+// float dist = fabs(plane.val[0]*pc.pts[i].x+plane.val[1]*pc.pts[i].y+plane.val[2]*pc.pts[i].z+plane.val[3]);
+// if (dist<min_d) {
+// min_d = dist;
+// min_i = i;
+// }
+// }
+//
+//
+// robot_msgs::VisualizationMarker marker;
+// marker.header.frame_id = pc.header.frame_id;
+// marker.header.stamp = pc.header.stamp;
+// marker.id = 1211;
+// marker.type = robot_msgs::VisualizationMarker::SPHERE;
+// marker.action = robot_msgs::VisualizationMarker::ADD;
+// marker.x = pc.pts[min_i].x;
+// marker.y = pc.pts[min_i].y;
+// marker.z = pc.pts[min_i].z;
+// marker.yaw = 0.0;
+// marker.pitch = 0.0;
+// marker.roll = 0.0;
+// marker.xScale = 0.1;
+// marker.yScale = 0.1;
+// marker.zScale = 0.1;
+// marker.alpha = 255;
+// marker.r = 255;
+// marker.g = 0;
+// marker.b = 0;
+// publish("visualizationMarker", marker);
+//
+// printf("Show marker at: (%f,%f,%f)", marker.x, marker.y, marker.z);
+// }
+
+ /**
+ * \brief Determine if it's possible for handle to be in a specific ROI
+ *
+ * It looks at things like, height, approximate size of ROI, how flat it is.
+ *
+ * @param r
+ * @return
+ */
+ bool handlePossibleHere(const CvRect &r)
+ {
+ const float nz_fraction = 0.1;
+
+ cvSetImageROI(disp, r);
+ cvSetImageCOI(disp, 1);
+ int cnt = cvCountNonZero(disp);
+ cvResetImageROI(disp);
+ cvSetImageCOI(disp, 0);
+
+ if(cnt < nz_fraction * r.width * r.height){
+ return false;
+ }
+
+ // compute least-squares handle plane
+ robot_msgs::PointCloud pc = filterPointCloud(r);
+ CvScalar plane = estimatePlaneLS(pc);
+
+ cnt = 0;
+ double sum = 0;
+ double max_dist = 0;
+ for(size_t i = 0;i < pc.pts.size();++i){
+ robot_msgs::Point32 p = pc.pts[i];
+ double dist = fabs(plane.val[0] * p.x + plane.val[1] * p.y + plane.val[2] * p.z + plane.val[3]);
+ max_dist = max(max_dist, dist);
+ sum += dist;
+ cnt++;
+ }
+ sum /= cnt;
+ if(max_dist > 0.1 || sum < 0.005){
+ return false;
+ }
+
+ double dx, dy;
+ robot_msgs::Point p;
+ getROIDimensions(r, dx, dy, p);
+ if(dx > 0.25 || dy > 0.15){
+ ROS_INFO("Too big, discarding");
+ return false;
+ }
+
+ robot_msgs::PointStamped pin, pout;
+ pin.header.frame_id = cloud.header.frame_id;
+ pin.header.stamp = cloud.header.stamp;
+ pin.point.x = p.z;
+ pin.point.y = -p.x;
+ pin.point.z = -p.y;
+ try {
+ tf_->transformPoint("base_link", pin, pout);
+ }
+ catch(tf::LookupException & ex){
+ ROS_ERROR("Lookup exception: %s\n", ex.what());
+ }
+ catch(tf::ExtrapolationException & ex){
+ ROS_DEBUG("Extrapolation exception: %s\n", ex.what());
+ }
+ catch(tf::ConnectivityException & ex){
+ ROS_ERROR("Connectivity exception: %s\n", ex.what());
+ }
+
+ if(pout.point.z > max_height || pout.point.z < min_height){
+ return false;
+ }
+
+ printf("Handle at: (%d,%d,%d,%d)\n", r.x,r.y,r.width, r.height);
+
+
+ return true;
+ }
+
+ /**
+ * \brief Start handle detection
+ */
+ void findHandleCascade(vector<CvRect> & handle_rect)
+ {
+ IplImage *gray = cvCreateImage(cvSize(left->width, left->height), 8, 1);
+ cvCvtColor(left, gray, CV_BGR2GRAY);
+ cvEqualizeHist(gray, gray);
+ cvClearMemStorage(storage);
+ if(cascade){
+ CvSeq *handles = cvHaarDetectObjects(gray, cascade, storage, 1.1, 2, 0, //|CV_HAAR_FIND_BIGGEST_OBJECT
+ //|CV_HAAR_DO_ROUGH_SEARCH
+ //|CV_HAAR_DO_CANNY_PRUNING
+ //|CV_HAAR_SCALE_IMAGE
+ cvSize(10, 10));
+ for(int i = 0;i < (handles ? handles->total : 0);i++){
+ CvRect *r = (CvRect*)cvGetSeqElem(handles, i);
+
+ if(handlePossibleHere(*r)){
+ handle_rect.push_back(*r);
+// if(display){
+// cvRectangle(left, cvPoint(r->x, r->y), cvPoint(r->x + r->width, r->y + r->height), CV_RGB(0, 255, 0));
+// }
+ }
+ else{
+ // cvRectangle(left, cvPoint(r->x, r->y), cvPoint(r->x + r->width, r->y + r->height), CV_RGB(255, 0, 0));
+ }
+ }
+
+ }
+ else {
+ ROS_ERROR("Cannot look for handle, no detector loaded");
+ }
+
+ cvReleaseImage(&gray);
+ }
+
+ /**
+ * \brief Finds edges in an image
+ * @param img
+ */
+ void findEdges(IplImage *img)
+ {
+ IplImage *gray = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
+ cvCvtColor(img, gray, CV_RGB2GRAY);
+ cvCanny(gray, gray, 20, 40);
+ CvMemStorage *storage = cvCreateMemStorage(0);
+ CvSeq *lines = 0;
+ lines = cvHoughLines2(gray, storage, CV_HOUGH_PROBABILISTIC, 1, CV_PI / 360, 100, 200, 100);
+ for(int i = 0;i < lines->total;i++){
+ CvPoint *line = (CvPoint*)cvGetSeqElem(lines, i);
+ CvPoint p1 = line[0];
+ CvPoint p2 = line[1];
+ if(abs(p1.x - p2.x) > 0){
+ float min_angle = 80;
+ float slope = float(abs(p1.y - p2.y)) / abs(p1.x - p2.x);
+ float min_slope = tan(CV_PI / 2 - min_angle * CV_PI / 180);
+ if(slope < min_slope)
+ continue;
+
+ printf("slope: %f, min_slope: %f\n", slope, min_slope);
+ }
+ cvLine(left, p1, p2, CV_RGB(255, 0, 0), 2, CV_AA, 0);
+ }
+
+ cvReleaseImage(&gray);
+ }
+
+ /**
+ * \brief Checks if a point should belong to a cluster
+ *
+ * @param center
+ * @param p
+ * @return
+ */
+ bool belongsToCluster(pair<float,float> center, pair<float,float> p)
+ {
+ return fabs(center.first-p.first)<3 && fabs(center.second-p.second)<3;
+ }
+
+
+ /**
+ * Helper function.
+ *
+ * @param r
+ * @return
+ */
+ pair<float,float> rectCenter(const CvRect& r)
+ {
+ return make_pair(r.x+(float)r.width/2,r.y+(float)r.height/2);
+ }
+
+ /**
+ * \brief Decide the handle position from several detections across multiple frames
+ *
+ * This method does some simple clustering of all the bounding boxes and picks the cluster with
+ * the most elements.
+ *
+ * @param handle_rect The handle bounding boxes
+ * @param handle Point indicating the real-world handle position
+ * @return
+ */
+ bool decideHandlePosition(vector<CvRect>& handle_rect, robot_msgs::PointStamped & handle)
+ {
+ if (handle_rect.size()==0) {
+ return false;
+ }
+
+ vector<int> cluster_size;
+ vector<pair<float,float> > centers;
+ vector<pair<float,float> > sizes;
+
+ for (size_t i=0;i<handle_rect.size();++i) {
+ CvRect r = handle_rect[i];
+ pair<float,float> crt = rectCenter(r);
+ bool found_cluster = false;
+ for (size_t j=0;j<centers.size();++j) {
+ if (belongsToCluster(centers[j],crt)) {
+ int n = cluster_size[j];
+ centers[j].first = (centers[j].first*n+crt.first)/(n+1);
+ centers[j].second = (centers[j].second*n+crt.second)/(n+1);
+ sizes[j].first = (sizes[j].first*n+r.width)/(n+1);
+ sizes[j].second = (sizes[j].second*n+r.height)/(n+1);
+ cluster_size[j]++;
+ found_cluster = true;
+ break;
+ }
+ }
+ if (!found_cluster) {
+ centers.push_back(crt);
+ sizes.push_back(make_pair(r.width,r.height));
+ cluster_size.push_back(1);
+ }
+ }
+
+ int max_ind = 0;
+ int max = cluster_size[0];
+
+ for (size_t i=0;i<cluster_size.size();++i) {
+ if (cluster_size[i]>max) {
+ max = cluster_size[i];
+ max_ind = i;
+ }
+ }
+
+ CvRect bbox;
+ bbox.x = (int) centers[max_ind].first-(int)(sizes[max_ind].first/2);
+ bbox.y = (int) centers[max_ind].second-(int)(sizes[max_ind].second/2);
+ bbox.width = (int) sizes[max_ind].first;
+ bbox.height = (int) sizes[max_ind].second;
+
+ double dx, dy;
+ robot_msgs::Point p;
+ getROIDimensions(bbox, dx, dy, p);
+
+ handle.header.frame_id = cloud.header.frame_id;
+ handle.header.stamp = cloud.header.stamp;
+ handle.point.x = p.z;
+ handle.point.y = -p.x;
+ handle.point.z = -p.y;
+
+
+ printf("Clustered Handle at: (%d,%d,%d,%d)\n", bbox.x,bbox.y,bbox.width, bbox.height);
+
+
+ if(display){
+ cvRectangle(left, cvPoint(bbox.x, bbox.y), cvPoint(bbox.x + bbox.width, bbox.y + bbox.height), CV_RGB(0, 255, 0));
+ cvShowImage("left", left);
+ }
+// showHandleMarker(handle);
+
+ return true;
+ }
+
+
+ /**
+ * \brief Runs the handle detector
+ *
+ * @param handle Position of detected handle
+ * @return True if handle was found, false otherwise.
+ */
+ bool runHandleDetector(robot_msgs::PointStamped & handle)
+ {
+ vector<CvRect> handle_rect;
+
+ // acquire cv_mutex lock
+ boost::unique_lock<boost::mutex> images_lock(cv_mutex);
+
+ for (int i=0;i<frames_no;++i) {
+
+ printf("Waiting for images\n");
+ // block until images are available to process
+ images_ready.wait(images_lock);
+
+ printf("Woke up, processing images\n");
+
+ if(display){
+ // show original disparity
+ cvShowImage("disparity_original", disp);
+ }
+ // eliminate from disparity locations that cannot contain a handle
+ applyPositionPrior();
+ // run cascade classifier
+ findHandleCascade(handle_rect);
+ if(display){
+ // show filtered disparity
+ cvShowImage("disparity", disp);
+ // show left image
+ cvShowImage("left", left);
+ }
+ }
+
+ bool found = decideHandlePosition(handle_rect, handle);
+ return found;
+ }
+
+ /**
+ * \brief Service call to detect doors
+ */
+ bool detectHandleSrv(door_handle_detector::DoorsDetector::Request & req, door_handle_detector::DoorsDetector::Response & resp)
+ {
+
+ robot_msgs::PointStamped handle;
+ subscribeStereoData();
+ bool found = runHandleDetector(handle);
+ unsubscribeStereoData();
+
+ if(!found){
+ return false;
+ }
+ robot_msgs::PointStamped handle_transformed;
+ // transform the point in the expected frame
+ try {
+ tf_->transformPoint(req.door.header.frame_id, handle, handle_transformed);
+ }
+ catch(tf::LookupException & ex){
+ ROS_ERROR("Lookup exception: %s\n", ex.what());
+ }
+ catch(tf::ExtrapolationException & ex){
+ ROS_DEBUG("Extrapolation exception: %s\n", ex.what());
+ }
+ catch(tf::ConnectivityException & ex){
+ ROS_ERROR("Connectivity exception: %s\n", ex.what());
+ }
+ if(found){
+ resp.doors.resize(1);
+ resp.doors[0] = req.door;
+ resp.doors[0].handle.x = handle.point.x;
+ resp.doors[0].handle.y = handle.point.y;
+ resp.doors[0].handle.z = handle.point.z;
+ resp.doors[0].weight = 1;
+ }else{
+ resp.doors[0].weight = -1;
+ }
+ return true;
+ }
+
+
+ /**
+ * \brief Filters a cloud point, retains only points coming from a specific region in the disparity image
+ *
+ * @param rect Region in disparity image
+ * @return Filtered point cloud
+ */
+ robot_msgs::PointCloud filterPointCloud(const CvRect & rect)
+ {
+ robot_msgs::PointCloud result;
+ result.header.frame_id = cloud.header.frame_id;
+ result.header.stamp = cloud.header.stamp;
+ int xchan = -1;
+ int ychan = -1;
+ for(size_t i = 0;i < cloud.chan.size();++i){
+ if(cloud.chan[i].name == "x"){
+ xchan = i;
+ }
+ if(cloud.chan[i].name == "y"){
+ ychan = i;
+ }
+ }
+
+ if(xchan != -1 && ychan != -1){
...
[truncated message content] |
|
From: <mar...@us...> - 2009-04-02 19:01:12
|
Revision: 13322
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13322&view=rev
Author: mariusmuja
Date: 2009-04-02 19:01:08 +0000 (Thu, 02 Apr 2009)
Log Message:
-----------
Moved stereodcam_params script into dcam package.
Modified Paths:
--------------
pkg/trunk/drivers/cam/dcam/manifest.xml
Added Paths:
-----------
pkg/trunk/drivers/cam/dcam/scripts/
pkg/trunk/drivers/cam/dcam/scripts/stereodcam_params
pkg/trunk/drivers/cam/dcam/scripts/stereodcam_params.py
pkg/trunk/drivers/cam/dcam/scripts/stereodcam_params.wxg
Removed Paths:
-------------
pkg/trunk/vision/stereodcam_params/manifest.xml
pkg/trunk/vision/stereodcam_params/stereodcam_params
pkg/trunk/vision/stereodcam_params/stereodcam_params.py
pkg/trunk/vision/stereodcam_params/stereodcam_params.wxg
pkg/trunk/vision/stereodcam_params/stereodcam_params_test.py
Modified: pkg/trunk/drivers/cam/dcam/manifest.xml
===================================================================
--- pkg/trunk/drivers/cam/dcam/manifest.xml 2009-04-02 18:51:31 UTC (rev 13321)
+++ pkg/trunk/drivers/cam/dcam/manifest.xml 2009-04-02 19:01:08 UTC (rev 13322)
@@ -15,6 +15,7 @@
<depend package="std_srvs"/>
<depend package="image_msgs"/>
<depend package="diagnostic_updater"/>
+ <depend package="rospy"/>
<sysdepend os="ubuntu" version="7.04-feisty" package="libfltk1.1-dev"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libfltk1.1-dev"/>
Copied: pkg/trunk/drivers/cam/dcam/scripts/stereodcam_params (from rev 13320, pkg/trunk/vision/stereodcam_params/stereodcam_params)
===================================================================
--- pkg/trunk/drivers/cam/dcam/scripts/stereodcam_params (rev 0)
+++ pkg/trunk/drivers/cam/dcam/scripts/stereodcam_params 2009-04-02 19:01:08 UTC (rev 13322)
@@ -0,0 +1 @@
+link stereodcam_params.py
\ No newline at end of file
Property changes on: pkg/trunk/drivers/cam/dcam/scripts/stereodcam_params
___________________________________________________________________
Added: svn:special
+ *
Copied: pkg/trunk/drivers/cam/dcam/scripts/stereodcam_params.py (from rev 13320, pkg/trunk/vision/stereodcam_params/stereodcam_params.py)
===================================================================
--- pkg/trunk/drivers/cam/dcam/scripts/stereodcam_params.py (rev 0)
+++ pkg/trunk/drivers/cam/dcam/scripts/stereodcam_params.py 2009-04-02 19:01:08 UTC (rev 13322)
@@ -0,0 +1,334 @@
+#!/usr/bin/env python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2009, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of the Willow Garage nor the names of its
+# contributors may be used to endorse or promote products derived
+# from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+# Author: Marius Muja
+
+import wx
+import wx.lib.intctrl
+import sys
+import os
+
+PKG = 'stereodcam_params' # this package name
+import roslib; roslib.load_manifest(PKG)
+from roslib.scriptutil import get_param_server, script_resolve_name
+from roslib.names import ns_join, get_ros_namespace, make_caller_id, make_global_ns, GLOBALNS
+
+import rospy
+from std_msgs.msg import Empty
+
+def _get_caller_id():
+ return make_caller_id('%s-%s'%(PKG,os.getpid()))
+
+
+def succeed(args):
+ code, msg, val = args
+ if code != 1:
+ raise RosParamException(msg)
+ return val
+
+def _get_param(param):
+ return succeed(get_param_server().getParam(_get_caller_id(), param))
+
+def _set_param(param, value):
+ succeed(get_param_server().setParam(_get_caller_id(), param, value))
+
+
+
+class IntParameterWidget(wx.Panel):
+
+ def __init__(self, parent, name, param, update_callback = None):
+
+ self.param = param
+ self.update_callback = update_callback
+
+ wx.Panel.__init__(self, parent ,-1)
+ self.label = wx.StaticText(self, -1, name)
+ self.edit = wx.lib.intctrl.IntCtrl(self, -1)
+
+ # properties
+ self.label.SetMinSize((150,-1))
+
+ # layout
+ sizer = wx.BoxSizer(wx.HORIZONTAL)
+ sizer.Add(self.label,0,0,0)
+ sizer.Add(self.edit,0,0,0)
+ self.SetSizer(sizer)
+
+ # bindings
+ self.Bind(wx.EVT_TEXT, self.onEdit, self.edit)
+
+ self._initValue()
+
+
+ def _initValue(self):
+ try:
+ value = _get_param(self.param)
+ self.edit.SetValue(value)
+ except:
+ print "Cannot read value for parameter: "+self.param
+
+
+ def _setParam(self, param, value):
+ try:
+ _set_param(param, value)
+ if self.update_callback != None:
+ self.update_callback()
+ except:
+ print "Cannot set parameter: "+self.param
+
+ def onEdit(self, event):
+ str_value = event.GetString()
+ #print str_value
+ value = int(str_value)
+ #print value
+ self._setParam(self.param, value)
+
+
+class BoolParameterWidget(wx.Panel):
+
+ def __init__(self, parent, name, param, update_callback = None):
+
+ self.param = param
+ self.update_callback = update_callback
+
+ wx.Panel.__init__(self, parent ,-1)
+ self.label = wx.StaticText(self, -1, name)
+ self.checkbox = wx.CheckBox(self, -1, "active")
+
+ # properties
+ self.label.SetMinSize((150,-1))
+
+ # layout
+ sizer = wx.BoxSizer(wx.HORIZONTAL)
+ sizer.Add(self.label,0,0,0)
+ sizer.Add(self.checkbox,0,0,0)
+ self.SetSizer(sizer)
+
+ # bindings
+ self.Bind(wx.EVT_CHECKBOX, self.onCheckbox, self.checkbox)
+
+ self._initValue()
+
+
+ def _initValue(self):
+ try:
+ value = _get_param(self.param)
+ self.checkbox.SetValue(value)
+ except:
+ print "Cannot read value for parameter: "+self.param
+
+
+ def _setParam(self, param, value):
+ try:
+ _set_param(param, value)
+ if self.update_callback != None:
+ self.update_callback()
+ except:
+ print "Cannot set parameter: "+self.param
+
+ def onCheckbox(self, event):
+ value = event.IsChecked();
+ self._setParam(self.param, value)
+
+
+class RangeParameterWidget(wx.Panel):
+
+ def __init__(self, parent, name, param, update_callback = None, has_auto = False):
+
+ self.param = param
+ self.update_callback = update_callback
+ self.has_auto = has_auto
+
+ wx.Panel.__init__(self, parent ,-1)
+ self.label = wx.StaticText(self, -1, name)
+ self.slider = wx.Slider(self, -1, 0, 0, 100)
+ self.spin = wx.SpinCtrl(self, -1, "", min=0, max=100)
+ if self.has_auto:
+ self.checkbox = wx.CheckBox(self, -1, "auto")
+
+ # properties
+ self.label.SetMinSize((150,-1))
+
+ # layout
+ sizer = wx.BoxSizer(wx.HORIZONTAL)
+ sizer.Add(self.label,0,0,0)
+ sizer.Add(self.slider,1,0,0)
+ sizer.Add(self.spin,0,0,0)
+ if self.has_auto:
+ sizer.Add(self.checkbox,0,0,0)
+ self.SetSizer(sizer)
+
+ # bindings
+ self.Bind(wx.EVT_COMMAND_SCROLL, self.onSlider, self.slider)
+ self.Bind(wx.EVT_SPINCTRL, self.onSpin, self.spin)
+ if self.has_auto:
+ self.Bind(wx.EVT_CHECKBOX, self.onCheckbox, self.checkbox)
+
+ self._initRange()
+ self._initValue()
+
+ def _initRange(self):
+ try:
+ min_value = _get_param(self.param+"_min")
+ max_value = _get_param(self.param+"_max")
+ self.slider.SetRange(min_value, max_value)
+ self.spin.SetRange(min_value, max_value)
+ except:
+ print "Cannot read range for parameter: "+self.param
+
+ def _initValue(self):
+ try:
+ value = _get_param(self.param)
+ self.slider.SetValue(value)
+ self.spin.SetValue(value)
+ except:
+ print "Cannot read value for parameter: "+self.param
+ if self.has_auto:
+ try:
+ value = _get_param(self.param+"_auto")
+ self.checkbox.SetValue(value)
+ if value:
+ self.slider.Disable()
+ self.spin.Disable()
+ else:
+ self.slider.Enable()
+ self.spin.Enable()
+ except:
+ print "Cannot tell if auto is set for parameter: "+self.param
+
+ def _setParam(self, param, value):
+ try:
+ _set_param(param, value)
+ if self.update_callback != None:
+ self.update_callback()
+ except:
+ print "Cannot set parameter: "+param
+
+ def onSlider(self, event):
+ value = event.GetPosition()
+ self.spin.SetValue(value)
+ self._setParam(self.param, value)
+
+ def onSpin(self, event):
+ value = event.GetInt()
+ self.slider.SetValue(value)
+ self._setParam(self.param, value)
+
+ def onCheckbox(self, event):
+ value = event.IsChecked();
+ self._setParam(self.param+"_auto", value)
+ if value:
+ self.slider.Disable()
+ self.spin.Disable()
+ else:
+ self.slider.Enable()
+ self.spin.Enable()
+
+
+class StereoParameters(wx.Frame):
+ def __init__(self, *args, **kwds):
+
+ kwds["style"] = wx.DEFAULT_FRAME_STYLE
+ wx.Frame.__init__(self, *args, **kwds)
+
+ self.widgets = []
+ self.widgets.append(RangeParameterWidget(self, "Exposure", "stereo/exposure", has_auto = True, update_callback = self.update_stereo_params))
+ self.widgets.append(RangeParameterWidget(self, "Gain", "stereo/gain", has_auto = True, update_callback = self.update_stereo_params))
+ self.widgets.append(RangeParameterWidget(self, "Brightness", "stereo/brightness", has_auto = True, update_callback = self.update_stereo_params))
+ self.widgets.append(BoolParameterWidget(self, "Companding", "stereo/companding", update_callback = self.update_stereo_params))
+ self.widgets.append(BoolParameterWidget(self, "HDR", "stereo/hdr", update_callback = self.update_stereo_params))
+ self.widgets.append(BoolParameterWidget(self, "Unique Check", "stereo/unique_check", update_callback = self.update_stereo_params))
+ self.widgets.append(IntParameterWidget(self, "Texture Threshold", "stereo/texture_thresh", update_callback = self.update_stereo_params))
+ self.widgets.append(IntParameterWidget(self, "Unique Threshold", "stereo/unique_thresh", update_callback = self.update_stereo_params))
+ self.widgets.append(IntParameterWidget(self, "Smoothness Threshold", "stereo/smoothness_thresh", update_callback = self.update_stereo_params))
+ self.widgets.append(IntParameterWidget(self, "Horopter", "stereo/horopter", update_callback = self.update_stereo_params))
+ self.widgets.append(IntParameterWidget(self, "Speckle Size", "stereo/speckle_size", update_callback = self.update_stereo_params))
+ self.widgets.append(IntParameterWidget(self, "Speckle Diff", "stereo/speckle_diff", update_callback = self.update_stereo_params))
+ self.widgets.append(IntParameterWidget(self, "Corr Size", "stereo/corr_size", update_callback = self.update_stereo_params))
+ self.widgets.append(IntParameterWidget(self, "Num Disp", "stereo/num_disp", update_callback = self.update_stereo_params))
+
+ self.close_button = wx.Button(self, -1, "Close")
+
+ self.__set_properties()
+ self.__do_layout()
+
+ self.Bind(wx.EVT_BUTTON, self.onClose, self.close_button)
+
+ self.check_params = rospy.Publisher('stereo/check_params', Empty)
+ rospy.init_node('stereodcam_params', anonymous=True)
+
+
+ def update_stereo_params(self):
+ self.check_params.publish(Empty())
+
+
+ def __set_properties(self):
+ self.SetTitle("Stereo Parameters")
+ self.SetSize((498, 500))
+
+ def __do_layout(self):
+ vsizer = wx.BoxSizer(wx.VERTICAL)
+ vsizer.Add((10, 10), 0, 0, 0)
+
+ for widget in self.widgets:
+ vsizer.Add(widget, 1, wx.EXPAND, 0)
+
+ vsizer.Add(self.close_button, 0, wx.ALIGN_CENTER_HORIZONTAL|wx.ALIGN_CENTER_VERTICAL, 0)
+ vsizer.Add((10, 10), 0, 0, 0)
+ hsizer = wx.BoxSizer(wx.HORIZONTAL)
+ hsizer.Add((10, 10), 0, 0, 0)
+ hsizer.Add(vsizer, 1, wx.EXPAND, 0)
+ hsizer.Add((10, 10), 0, 0, 0)
+ self.SetSizer(hsizer)
+
+ self.Layout()
+
+
+ def onClose(self, event):
+ sys.exit(0)
+
+
+
+class MyApp(wx.App):
+ def OnInit(self):
+ wx.InitAllImageHandlers()
+ frame_2 = StereoParameters(None, -1, "")
+ self.SetTopWindow(frame_2)
+ frame_2.Show()
+ return 1
+
+# end of class MyApp
+
+if __name__ == "__main__":
+ app = MyApp(0)
+ app.MainLoop()
Copied: pkg/trunk/drivers/cam/dcam/scripts/stereodcam_params.wxg (from rev 13320, pkg/trunk/vision/stereodcam_params/stereodcam_params.wxg)
===================================================================
--- pkg/trunk/drivers/cam/dcam/scripts/stereodcam_params.wxg (rev 0)
+++ pkg/trunk/drivers/cam/dcam/scripts/stereodcam_params.wxg 2009-04-02 19:01:08 UTC (rev 13322)
@@ -0,0 +1,97 @@
+<?xml version="1.0"?>
+<!-- generated by wxGlade 0.6.3 on Fri Mar 20 15:34:19 2009 -->
+
+<application path="/u/mariusm/ros/ros-pkg/vision/stereodcam_params/stereodcam_params_test.py" name="" class="MyApp" option="0" language="python" top_window="frame_1" encoding="UTF-8" use_gettext="0" overwrite="0" use_new_namespace="1" for_version="2.8" is_template="0">
+ <object class="MyFrame" name="frame_1" base="EditFrame">
+ <style>wxDEFAULT_FRAME_STYLE</style>
+ <title>frame_1</title>
+ <object class="wxBoxSizer" name="sizer_2" base="EditBoxSizer">
+ <orient>wxVERTICAL</orient>
+ <object class="sizeritem">
+ <flag>wxEXPAND</flag>
+ <border>10</border>
+ <option>1</option>
+ <object class="wxPanel" name="panel_1" base="EditPanel">
+ <style>wxTAB_TRAVERSAL</style>
+ <object class="wxBoxSizer" name="sizer_2_copy_copy_copy" base="EditBoxSizer">
+ <orient>wxHORIZONTAL</orient>
+ <object class="sizeritem">
+ <border>0</border>
+ <option>0</option>
+ <object class="wxStaticText" name="exposure_label_copy" base="EditStaticText">
+ <attribute>1</attribute>
+ <label>Exposure</label>
+ <size>100, -1</size>
+ </object>
+ </object>
+ <object class="sizeritem">
+ <border>0</border>
+ <option>0</option>
+ <object class="wxCheckBox" name="checkbox_1" base="EditCheckBox">
+ <label>auto</label>
+ <events>
+ <handler event="EVT_CHECKBOX">onCheckbox</handler>
+ </events>
+ </object>
+ </object>
+ <object class="sizeritem">
+ <border>0</border>
+ <option>1</option>
+ <object class="wxSlider" name="exposure_slider_copy" base="EditSlider">
+ <style>wxSL_HORIZONTAL</style>
+ <events>
+ <handler event="EVT_COMMAND_SCROLL">onExposureSlider</handler>
+ </events>
+ </object>
+ </object>
+ <object class="sizeritem">
+ <border>0</border>
+ <option>0</option>
+ <object class="wxSpinCtrl" name="exposure_spin_copy" base="EditSpinCtrl">
+ <events>
+ <handler event="EVT_SPINCTRL">onExposureSpin</handler>
+ </events>
+ </object>
+ </object>
+ </object>
+ </object>
+ </object>
+ <object class="sizeritem">
+ <flag>wxEXPAND</flag>
+ <border>0</border>
+ <option>1</option>
+ <object class="wxPanel" name="panel_2" base="EditPanel">
+ <style>wxTAB_TRAVERSAL</style>
+ <object class="wxStaticBoxSizer" name="sizer_3" base="EditStaticBoxSizer">
+ <orient>wxHORIZONTAL</orient>
+ <label>Test</label>
+ <object class="sizeritem">
+ <border>0</border>
+ <option>0</option>
+ <object class="wxStaticText" name="exposure_label_copy_copy" base="EditStaticText">
+ <attribute>1</attribute>
+ <label>Exposure</label>
+ <size>100, -1</size>
+ </object>
+ </object>
+ <object class="sizeritem">
+ <border>0</border>
+ <option>1</option>
+ <object class="spacer" name="spacer" base="EditSpacer">
+ <height>20</height>
+ <width>20</width>
+ </object>
+ </object>
+ <object class="sizeritem">
+ <border>0</border>
+ <option>0</option>
+ <object class="wxTextCtrl" name="text_ctrl_1" base="EditTextCtrl">
+ <size>100, -1</size>
+ </object>
+ </object>
+ </object>
+ </object>
+ </object>
+ </object>
+ </object>
+</application>
Deleted: pkg/trunk/vision/stereodcam_params/manifest.xml
===================================================================
--- pkg/trunk/vision/stereodcam_params/manifest.xml 2009-04-02 18:51:31 UTC (rev 13321)
+++ pkg/trunk/vision/stereodcam_params/manifest.xml 2009-04-02 19:01:08 UTC (rev 13322)
@@ -1,14 +0,0 @@
-<package>
- <description brief="Sets stereo camera parameters">
- This is a small GUI for setting the stereo camera parameters.
- </description>
- <author>Marius Muja</author>
- <license>BSD</license>
- <review status="unreviewed" notes=""/>
- <url>http://pr.willowgarage.com</url>
- <depend package="rospy"/>
- <depend package="rostest"/>
- <depend package="std_srvs"/>
- <depend package="std_msgs"/>
-</package>
-
Deleted: pkg/trunk/vision/stereodcam_params/stereodcam_params
===================================================================
--- pkg/trunk/vision/stereodcam_params/stereodcam_params 2009-04-02 18:51:31 UTC (rev 13321)
+++ pkg/trunk/vision/stereodcam_params/stereodcam_params 2009-04-02 19:01:08 UTC (rev 13322)
@@ -1 +0,0 @@
-link stereodcam_params.py
\ No newline at end of file
Deleted: pkg/trunk/vision/stereodcam_params/stereodcam_params.py
===================================================================
--- pkg/trunk/vision/stereodcam_params/stereodcam_params.py 2009-04-02 18:51:31 UTC (rev 13321)
+++ pkg/trunk/vision/stereodcam_params/stereodcam_params.py 2009-04-02 19:01:08 UTC (rev 13322)
@@ -1,331 +0,0 @@
-#!/usr/bin/env python
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2009, Willow Garage, Inc.
-# All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# * Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# * Redistributions in binary form must reproduce the above
-# copyright notice, this list of conditions and the following
-# disclaimer in the documentation and/or other materials provided
-# with the distribution.
-# * Neither the name of the Willow Garage nor the names of its
-# contributors may be used to endorse or promote products derived
-# from this software without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-
-
-import wx
-import wx.lib.intctrl
-import sys
-import os
-
-PKG = 'stereodcam_params' # this package name
-import roslib; roslib.load_manifest(PKG)
-from roslib.scriptutil import get_param_server, script_resolve_name
-from roslib.names import ns_join, get_ros_namespace, make_caller_id, make_global_ns, GLOBALNS
-
-import rospy
-from std_msgs.msg import Empty
-
-def _get_caller_id():
- return make_caller_id('%s-%s'%(PKG,os.getpid()))
-
-
-def succeed(args):
- code, msg, val = args
- if code != 1:
- raise RosParamException(msg)
- return val
-
-def _get_param(param):
- return succeed(get_param_server().getParam(_get_caller_id(), param))
-
-def _set_param(param, value):
- succeed(get_param_server().setParam(_get_caller_id(), param, value))
-
-
-
-class IntParameterWidget(wx.Panel):
-
- def __init__(self, parent, name, param, update_callback = None):
-
- self.param = param
- self.update_callback = update_callback
-
- wx.Panel.__init__(self, parent ,-1)
- self.label = wx.StaticText(self, -1, name)
- self.edit = wx.lib.intctrl.IntCtrl(self, -1)
-
- # properties
- self.label.SetMinSize((150,-1))
-
- # layout
- sizer = wx.BoxSizer(wx.HORIZONTAL)
- sizer.Add(self.label,0,0,0)
- sizer.Add(self.edit,0,0,0)
- self.SetSizer(sizer)
-
- # bindings
- self.Bind(wx.EVT_TEXT, self.onEdit, self.edit)
-
- self._initValue()
-
-
- def _initValue(self):
- try:
- value = _get_param(self.param)
- self.edit.SetValue(value)
- except:
- print "Cannot read value for parameter: "+self.param
-
-
- def _setParam(self, param, value):
- try:
- _set_param(param, value)
- if self.update_callback != None:
- self.update_callback()
- except:
- print "Cannot set parameter: "+self.param
-
- def onEdit(self, event):
- value = int(event.GetString())
- print value
- self._setParam(self.param, value)
-
-
-class BoolParameterWidget(wx.Panel):
-
- def __init__(self, parent, name, param, update_callback = None):
-
- self.param = param
- self.update_callback = update_callback
-
- wx.Panel.__init__(self, parent ,-1)
- self.label = wx.StaticText(self, -1, name)
- self.checkbox = wx.CheckBox(self, -1, "active")
-
- # properties
- self.label.SetMinSize((150,-1))
-
- # layout
- sizer = wx.BoxSizer(wx.HORIZONTAL)
- sizer.Add(self.label,0,0,0)
- sizer.Add(self.checkbox,0,0,0)
- self.SetSizer(sizer)
-
- # bindings
- self.Bind(wx.EVT_CHECKBOX, self.onCheckbox, self.checkbox)
-
- self._initValue()
-
-
- def _initValue(self):
- try:
- value = _get_param(self.param)
- self.checkbox.SetValue(value)
- except:
- print "Cannot read value for parameter: "+self.param
-
-
- def _setParam(self, param, value):
- try:
- _set_param(param, value)
- if self.update_callback != None:
- self.update_callback()
- except:
- print "Cannot set parameter: "+self.param
-
- def onCheckbox(self, event):
- value = event.IsChecked();
- self._setParam(self.param, value)
-
-
-class RangeParameterWidget(wx.Panel):
-
- def __init__(self, parent, name, param, update_callback = None, has_auto = False):
-
- self.param = param
- self.update_callback = update_callback
- self.has_auto = has_auto
-
- wx.Panel.__init__(self, parent ,-1)
- self.label = wx.StaticText(self, -1, name)
- self.slider = wx.Slider(self, -1, 0, 0, 100)
- self.spin = wx.SpinCtrl(self, -1, "", min=0, max=100)
- if self.has_auto:
- self.checkbox = wx.CheckBox(self, -1, "auto")
-
- # properties
- self.label.SetMinSize((150,-1))
-
- # layout
- sizer = wx.BoxSizer(wx.HORIZONTAL)
- sizer.Add(self.label,0,0,0)
- sizer.Add(self.slider,1,0,0)
- sizer.Add(self.spin,0,0,0)
- if self.has_auto:
- sizer.Add(self.checkbox,0,0,0)
- self.SetSizer(sizer)
-
- # bindings
- self.Bind(wx.EVT_COMMAND_SCROLL, self.onSlider, self.slider)
- self.Bind(wx.EVT_SPINCTRL, self.onSpin, self.spin)
- if self.has_auto:
- self.Bind(wx.EVT_CHECKBOX, self.onCheckbox, self.checkbox)
-
- self._initRange()
- self._initValue()
-
- def _initRange(self):
- try:
- min_value = _get_param(self.param+"_min")
- max_value = _get_param(self.param+"_max")
- self.slider.SetRange(min_value, max_value)
- self.spin.SetRange(min_value, max_value)
- except:
- print "Cannot read range for parameter: "+self.param
-
- def _initValue(self):
- try:
- value = _get_param(self.param)
- self.slider.SetValue(value)
- self.spin.SetValue(value)
- except:
- print "Cannot read value for parameter: "+self.param
- if self.has_auto:
- try:
- value = _get_param(self.param+"_auto")
- self.checkbox.SetValue(value)
- if value:
- self.slider.Disable()
- self.spin.Disable()
- else:
- self.slider.Enable()
- self.spin.Enable()
- except:
- print "Cannot tell if auto is set for parameter: "+self.param
-
- def _setParam(self, param, value):
- try:
- _set_param(param, value)
- if self.update_callback != None:
- self.update_callback()
- except:
- print "Cannot set parameter: "+param
-
- def onSlider(self, event):
- value = event.GetPosition()
- self.spin.SetValue(value)
- self._setParam(self.param, value)
-
- def onSpin(self, event):
- value = event.GetInt()
- self.slider.SetValue(value)
- self._setParam(self.param, value)
-
- def onCheckbox(self, event):
- value = event.IsChecked();
- self._setParam(self.param+"_auto", value)
- if value:
- self.slider.Disable()
- self.spin.Disable()
- else:
- self.slider.Enable()
- self.spin.Enable()
-
-
-class StereoParameters(wx.Frame):
- def __init__(self, *args, **kwds):
-
- kwds["style"] = wx.DEFAULT_FRAME_STYLE
- wx.Frame.__init__(self, *args, **kwds)
-
- self.widgets = []
- self.widgets.append(RangeParameterWidget(self, "Exposure", "stereo/exposure", has_auto = True, update_callback = self.update_stereo_params))
- self.widgets.append(RangeParameterWidget(self, "Gain", "stereo/gain", has_auto = True, update_callback = self...
[truncated message content] |
|
From: <ge...@us...> - 2009-04-04 00:38:15
|
Revision: 13406
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13406&view=rev
Author: gerkey
Date: 2009-04-04 00:38:12 +0000 (Sat, 04 Apr 2009)
Log Message:
-----------
Conditioned out failing tests when not on 32-bit Linux, ticket pending
Modified Paths:
--------------
pkg/trunk/demos/test_2dnav_gazebo/CMakeLists.txt
pkg/trunk/drivers/simulator/test_gazebo_plugin/CMakeLists.txt
pkg/trunk/drivers/simulator/test_pr2_collision_gazebo/CMakeLists.txt
Modified: pkg/trunk/demos/test_2dnav_gazebo/CMakeLists.txt
===================================================================
--- pkg/trunk/demos/test_2dnav_gazebo/CMakeLists.txt 2009-04-04 00:16:41 UTC (rev 13405)
+++ pkg/trunk/demos/test_2dnav_gazebo/CMakeLists.txt 2009-04-04 00:38:12 UTC (rev 13406)
@@ -2,9 +2,27 @@
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(test_2dnav_gazebo)
+# For now, the tests fail on 64-bit for some reason.
+# TODO: more thoroughly test and wrap up this 32-bit check
+include(CMakeDetermineSystem)
+if(CMAKE_SYSTEM_PROCESSOR MATCHES "i686" OR
+ CMAKE_SYSTEM_PROCESSOR MATCHES "i386" OR
+ CMAKE_SYSTEM_PROCESSOR MATCHES "unknown")
+ set(_is32bit TRUE)
+else(CMAKE_SYSTEM_PROCESSOR MATCHES "i686" OR
+ CMAKE_SYSTEM_PROCESSOR MATCHES "i386" OR
+ CMAKE_SYSTEM_PROCESSOR MATCHES "unknown")
+ set(_is32bit FALSE)
+endif(CMAKE_SYSTEM_PROCESSOR MATCHES "i686" OR
+ CMAKE_SYSTEM_PROCESSOR MATCHES "i386" OR
+ CMAKE_SYSTEM_PROCESSOR MATCHES "unknown")
+
+if(CMAKE_SYSTEM_NAME MATCHES "Linux" AND _is32bit)
+
rospack_add_rostest(test_2dnav_wg.xml)
rospack_add_rostest(test_2dnav_empty_rotation.xml)
rospack_add_rostest(test_2dnav_empty_odom.xml)
rospack_add_rostest(test_2dnav_empty_diagonal.xml)
rospack_add_rostest(test_2dnav_empty_axis.xml)
+endif(CMAKE_SYSTEM_NAME MATCHES "Linux" AND _is32bit)
Modified: pkg/trunk/drivers/simulator/test_gazebo_plugin/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/simulator/test_gazebo_plugin/CMakeLists.txt 2009-04-04 00:16:41 UTC (rev 13405)
+++ pkg/trunk/drivers/simulator/test_gazebo_plugin/CMakeLists.txt 2009-04-04 00:38:12 UTC (rev 13406)
@@ -2,5 +2,24 @@
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(test_gazebo_plugin)
+# For now, the tests fail on 64-bit for some reason.
+# TODO: more thoroughly test and wrap up this 32-bit check
+include(CMakeDetermineSystem)
+if(CMAKE_SYSTEM_PROCESSOR MATCHES "i686" OR
+ CMAKE_SYSTEM_PROCESSOR MATCHES "i386" OR
+ CMAKE_SYSTEM_PROCESSOR MATCHES "unknown")
+ set(_is32bit TRUE)
+else(CMAKE_SYSTEM_PROCESSOR MATCHES "i686" OR
+ CMAKE_SYSTEM_PROCESSOR MATCHES "i386" OR
+ CMAKE_SYSTEM_PROCESSOR MATCHES "unknown")
+ set(_is32bit FALSE)
+endif(CMAKE_SYSTEM_PROCESSOR MATCHES "i686" OR
+ CMAKE_SYSTEM_PROCESSOR MATCHES "i386" OR
+ CMAKE_SYSTEM_PROCESSOR MATCHES "unknown")
+
+if(CMAKE_SYSTEM_NAME MATCHES "Linux" AND _is32bit)
+
rospack_add_rostest(test_pendulum.xml)
+endif(CMAKE_SYSTEM_NAME MATCHES "Linux" AND _is32bit)
+
Modified: pkg/trunk/drivers/simulator/test_pr2_collision_gazebo/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_collision_gazebo/CMakeLists.txt 2009-04-04 00:16:41 UTC (rev 13405)
+++ pkg/trunk/drivers/simulator/test_pr2_collision_gazebo/CMakeLists.txt 2009-04-04 00:38:12 UTC (rev 13406)
@@ -2,5 +2,22 @@
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(test_pr2_collision_gazebo)
+# For now, the tests fail on 64-bit for some reason.
+# TODO: more thoroughly test and wrap up this 32-bit check
+include(CMakeDetermineSystem)
+if(CMAKE_SYSTEM_PROCESSOR MATCHES "i686" OR
+ CMAKE_SYSTEM_PROCESSOR MATCHES "i386" OR
+ CMAKE_SYSTEM_PROCESSOR MATCHES "unknown")
+ set(_is32bit TRUE)
+else(CMAKE_SYSTEM_PROCESSOR MATCHES "i686" OR
+ CMAKE_SYSTEM_PROCESSOR MATCHES "i386" OR
+ CMAKE_SYSTEM_PROCESSOR MATCHES "unknown")
+ set(_is32bit FALSE)
+endif(CMAKE_SYSTEM_PROCESSOR MATCHES "i686" OR
+ CMAKE_SYSTEM_PROCESSOR MATCHES "i386" OR
+ CMAKE_SYSTEM_PROCESSOR MATCHES "unknown")
+
+if(CMAKE_SYSTEM_NAME MATCHES "Linux" AND _is32bit)
rospack_add_rostest_graphical(test_slide.xml)
+endif(CMAKE_SYSTEM_NAME MATCHES "Linux" AND _is32bit)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mar...@us...> - 2009-04-07 17:38:05
|
Revision: 13529
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13529&view=rev
Author: mariusmuja
Date: 2009-04-07 17:37:55 +0000 (Tue, 07 Apr 2009)
Log Message:
-----------
Changes to outlet_spotting node and added SpotOutletAction.
Modified Paths:
--------------
pkg/trunk/highlevel/robot_actions/msg/DetectOutletState.msg
pkg/trunk/vision/outlet_spotting/CMakeLists.txt
pkg/trunk/vision/outlet_spotting/manifest.xml
pkg/trunk/vision/outlet_spotting/src/outlet_spotting.cpp
Modified: pkg/trunk/highlevel/robot_actions/msg/DetectOutletState.msg
===================================================================
--- pkg/trunk/highlevel/robot_actions/msg/DetectOutletState.msg 2009-04-07 17:37:25 UTC (rev 13528)
+++ pkg/trunk/highlevel/robot_actions/msg/DetectOutletState.msg 2009-04-07 17:37:55 UTC (rev 13529)
@@ -5,7 +5,7 @@
ActionStatus status
# Goal
-robot_msgs/Point goal
+robot_msgs/PointStamped goal
# Feedback
-robot_msgs/PoseStamped feedback
\ No newline at end of file
+robot_msgs/PoseStamped feedback
Modified: pkg/trunk/vision/outlet_spotting/CMakeLists.txt
===================================================================
--- pkg/trunk/vision/outlet_spotting/CMakeLists.txt 2009-04-07 17:37:25 UTC (rev 13528)
+++ pkg/trunk/vision/outlet_spotting/CMakeLists.txt 2009-04-07 17:37:55 UTC (rev 13529)
@@ -9,3 +9,5 @@
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
rospack_add_executable(outlet_spotting src/outlet_spotting.cpp src/outlet_tuple.cpp)
+
+rospack_add_executable(test_detect_service src/test_detect_service.cpp)
Modified: pkg/trunk/vision/outlet_spotting/manifest.xml
===================================================================
--- pkg/trunk/vision/outlet_spotting/manifest.xml 2009-04-07 17:37:25 UTC (rev 13528)
+++ pkg/trunk/vision/outlet_spotting/manifest.xml 2009-04-07 17:37:55 UTC (rev 13529)
@@ -15,4 +15,5 @@
<depend package="point_cloud_mapping"/>
<depend package="rospy"/>
<depend package="tf"/>
+ <depend package="robot_actions"/>
</package>
Modified: pkg/trunk/vision/outlet_spotting/src/outlet_spotting.cpp
===================================================================
--- pkg/trunk/vision/outlet_spotting/src/outlet_spotting.cpp 2009-04-07 17:37:25 UTC (rev 13528)
+++ pkg/trunk/vision/outlet_spotting/src/outlet_spotting.cpp 2009-04-07 17:37:55 UTC (rev 13529)
@@ -55,7 +55,7 @@
#include "image_msgs/Image.h"
#include "robot_msgs/PointCloud.h"
#include "robot_msgs/Point32.h"
-#include "robot_msgs/OutletPose.h"
+#include "robot_msgs/PoseStamped.h"
#include "robot_msgs/VisualizationMarker.h"
#include "robot_msgs/Planner2DGoal.h"
@@ -63,14 +63,18 @@
#include <point_cloud_mapping/geometry/angles.h>
#include <point_cloud_mapping/sample_consensus/sac_model_plane.h>
#include <point_cloud_mapping/sample_consensus/sac.h>
+#include <point_cloud_mapping/sample_consensus/ransac.h>
#include <point_cloud_mapping/sample_consensus/lmeds.h>
#include "topic_synchronizer.h"
#include <tf/transform_listener.h>
-
#include "CvStereoCamModel.h"
#include "outlet_tuple.h"
+#include "outlet_spotting/OutletSpotting.h"
+#include "robot_actions/action.h"
+
+
#include <boost/thread.hpp>
using namespace std;
@@ -106,6 +110,7 @@
};
+
class OutletSpotting : public ros::Node
{
public:
@@ -122,12 +127,15 @@
image_msgs::CvBridge dbridge;
robot_msgs::PointCloud cloud;
- robot_msgs::PointCloud cloud_topic;
+ robot_msgs::PointCloud cloud_fetch;
- robot_msgs::OutletPose outlet_pose;
+ robot_msgs::PointCloud laser_cloud;
+ robot_msgs::PointCloud laser_cloud_fetch;
+// robot_msgs::PoseStamped outlet_pose;
+
IplImage* left;
-// IplImage* right;''
+// IplImage* right;
IplImage* disp;
IplImage* disp_clone;
@@ -137,24 +145,25 @@
TopicSynchronizer<OutletSpotting> sync;
+ boost::mutex clound_point_mutex;
+ boost::condition_variable cloud_point_cv;
+
boost::mutex cv_mutex;
+ boost::condition_variable images_cv;
+ bool have_cloud_point_;
+
tf::TransformListener *tf_;
- ros::Time goal_time;
-
-
OutletSpotting() : ros::Node("stereo_view"),left(NULL), disp(NULL), disp_clone(NULL),
- sync(this, &OutletSpotting::image_cb_all, ros::Duration().fromSec(0.05), &OutletSpotting::image_cb_timeout)
+ sync(this, &OutletSpotting::image_cb_all, ros::Duration().fromSec(0.1), &OutletSpotting::image_cb_timeout), have_cloud_point_(false)
{
tf_ = new tf::TransformListener(*this);
-
param ("~display", display, false); // 100 points at high resolution
param ("~save_patches", save_patches, false); // 100 points at high resolution
-
if (display) {
ROS_INFO("Displaying images\n");
cvNamedWindow("left", CV_WINDOW_AUTOSIZE);
@@ -163,7 +172,28 @@
cvNamedWindow("disparity", CV_WINDOW_AUTOSIZE);
}
+ advertise<robot_msgs::VisualizationMarker>("visualizationMarker", 1);
+ advertiseService("outlet_spotting_service", &OutletSpotting::outletSpottingService, this);
+
+ }
+
+ ~OutletSpotting()
+ {
+ if (left)
+ cvReleaseImage(&left);
+// if (right)
+// cvReleaseImage(&right);
+ if (disp)
+ cvReleaseImage(&disp);
+ }
+
+private:
+
+ void subscribeToData()
+ {
+
+ sync.reset();
std::list<std::string> left_list;
left_list.push_back(std::string("stereo/left/image_rect_color"));
left_list.push_back(std::string("stereo/left/image_rect"));
@@ -179,29 +209,29 @@
sync.subscribe("stereo/disparity_info", dispinfo, 1);
sync.subscribe("stereo/right/cam_info", rcinfo, 1);
- sync.subscribe("stereo/cloud", cloud_topic, 1);
+ sync.subscribe("stereo/cloud", cloud_fetch, 1);
sync.ready();
- advertise<OutletPose>("stereo/outlet_pose",1);
- advertise<robot_msgs::VisualizationMarker>("visualizationMarker", 1);
- advertise<robot_msgs::Planner2DGoal>("goal", 1);
+ subscribe("full_cloud", laser_cloud_fetch, &OutletSpotting::laser_cloud_callback, 1);
+ }
+ void unsubscribeFromData()
+ {
+ unsubscribe("stereo/left/image_rect_color");
+ unsubscribe("stereo/left/image_rect");
+ unsubscribe("stereo/disparity");
+ unsubscribe("stereo/stereo_info");
+ unsubscribe("stereo/disparity_info");
+ unsubscribe("stereo/right/cam_info");
+ unsubscribe("stereo/cloud");
+ unsubscribe("full_cloud");
+ }
- goal_time = ros::Time::now();
- }
- ~OutletSpotting()
- {
- if (left)
- cvReleaseImage(&left);
-// if (right)
-// cvReleaseImage(&right);
- if (disp)
- cvReleaseImage(&disp);
- }
+
/////WORKING FUNCTIONS////////////////
///////////////////////////////////////////////////////////////////////////////////////////
// JUST A MODIFIED CONNECTED COMPONENTS ROUTINE
@@ -424,7 +454,7 @@
return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y)+(a.z-b.z)*(a.z-b.z));
}
- void getBBoxDimensions(IplImage *Id, CvRect &R, double meanDisparity, double& dx, double& dy, double& z)
+ void getBBoxDimensions(IplImage *Id, CvRect &R, double meanDisparity, double& dx, double& dy)
{
// initialize stereo camera model
double Fx = rcinfo.P[0]; double Fy = rcinfo.P[5];
@@ -444,8 +474,6 @@
dx = cvDist(p1,p2);
dy = cvDist(p1,p3);
-
- z = p1.z;
}
@@ -462,64 +490,6 @@
}
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief Find a plane model in a point cloud given via a set of point indices with SAmple Consensus methods
- * \param points the point cloud message
- * \param indices a pointer to a set of point cloud indices to test
- * \param inliers the resultant planar inliers
- * \param coeff the resultant plane coefficients
- * \param viewpoint_cloud a point to the pose where the data was acquired from (for normal flipping)
- * \param dist_thresh the maximum allowed distance threshold of an inlier to the model
- * \param min_pts the minimum number of points allowed as inliers for a plane model
- */
- bool
- fitSACPlane (PointCloud &points, vector<int> indices, vector<int> &inliers, vector<double> &coeff,
- const robot_msgs::Point32 &viewpoint, double dist_thresh, int min_pts)
- {
- if ((int)indices.size () < min_pts)
- {
- inliers.resize (0);
- coeff.resize (0);
- return (false);
- }
-
- // Create and initialize the SAC model
- sample_consensus::SACModelPlane *model = new sample_consensus::SACModelPlane ();
- // sample_consensus::SAC *sac = new sample_consensus::RANSAC (model, dist_thresh);
- sample_consensus::SAC *sac = new sample_consensus::LMedS (model, dist_thresh);
- sac->setMaxIterations (500);
- model->setDataSet (&points, indices);
-
- // Search for the best plane
- if (sac->computeModel ())
- {
- // Obtain the inliers and the planar model coefficients
- if ((int)sac->getInliers ().size () < min_pts)
- {
-// ROS_ERROR ("fitSACPlane: Inliers.size (%d) < sac_min_points_per_model (%d)!", sac->getInliers ().size (), min_pts);
- inliers.resize (0);
- coeff.resize (0);
- return (false);
- }
- sac->computeCoefficients (coeff); // Compute the model coefficients
- sac->refineCoefficients (coeff); // Refine them using least-squares
- model->selectWithinDistance (coeff, dist_thresh, inliers);
-
- // Flip plane normal according to the viewpoint information
- cloud_geometry::angles::flipNormalTowardsViewpoint (coeff, points.pts.at (inliers[0]), viewpoint);
- //ROS_DEBUG ("Found a model supported by %d inliers: [%g, %g, %g, %g]\n", sac->getInliers ().size (),
- // coeff[0], coeff[1], coeff[2], coeff[3]);
- }
- else
- {
- ROS_ERROR ("Could not compute a plane model.");
- return (false);
- }
- delete sac;
- delete model;
- return (true);
- }
-
/**
* \brief Finds the nearest point with non-zero disparity
* @param r
@@ -587,18 +557,18 @@
* \brief Publishes a visualization marker for a point.
* @param p
*/
- void showPointMarker(robot_msgs::PointStamped p)
+ void showMarkers(robot_msgs::PoseStamped pose)
{
robot_msgs::VisualizationMarker marker;
- marker.header.frame_id = p.header.frame_id;
+ marker.header.frame_id = pose.header.frame_id;
marker.header.stamp = ros::Time((uint64_t)(0ULL));
- marker.id = 0;
+ marker.id = 101;
marker.type = robot_msgs::VisualizationMarker::SPHERE;
marker.action = robot_msgs::VisualizationMarker::ADD;
- marker.x = p.point.x;
- marker.y = p.point.y;
- marker.z = p.point.z;
+ marker.x = pose.pose.position.x;
+ marker.y = pose.pose.position.y;
+ marker.z = pose.pose.position.z;
marker.yaw = 0.0;
marker.pitch = 0.0;
marker.roll = 0.0;
@@ -609,96 +579,232 @@
marker.r = 0;
marker.g = 255;
marker.b = 0;
+
publish("visualizationMarker", marker);
+
+
+ tf::Pose tf_pose;
+
+ tf::PoseMsgToTF(pose.pose,tf_pose);
+ tf::Point point(-1,0,0);
+ tf::Point normal = tf_pose*point;
+
+
+ marker.header.frame_id = pose.header.frame_id;
+ marker.header.stamp = ros::Time((uint64_t)(0ULL));
+ marker.id = 102;
+ marker.type = robot_msgs::VisualizationMarker::SPHERE;
+ marker.action = robot_msgs::VisualizationMarker::ADD;
+
+ marker.x = normal.x();
+ marker.y = normal.y();
+ marker.z = normal.z();
+ marker.yaw = 0.0;
+ marker.pitch = 0.0;
+ marker.roll = 0.0;
+ marker.xScale = 0.1;
+ marker.yScale = 0.1;
+ marker.zScale = 0.1;
+ marker.alpha = 255;
+ marker.r = 0;
+ marker.g = 255;
+ marker.b = 0;
+
+ publish("visualizationMarker", marker);
+ printf("Published marker.\n");
}
+ double squaredPointDistance(Point32 p1, Point p2)
+ {
+ return (p1.x-p2.x)*(p1.x-p2.x)+(p1.y-p2.y)*(p1.y-p2.y)+(p1.z-p2.z)*(p1.z-p2.z);
+ }
- bool publishOutletPose(const CvRect& r, const CvPoint& p)
+ PointCloud outletVecinity(PointCloud laser_cloud, PointStamped ps_cloud, double distance)
+ {
+ PointCloud result;
+ result.header.frame_id = laser_cloud.header.frame_id;
+ result.header.stamp = laser_cloud.header.stamp;
+
+ double d = distance*distance;
+ for (size_t i=0; i<laser_cloud.get_pts_size(); ++i) {
+ if (squaredPointDistance(laser_cloud.pts[i],ps_cloud.point)<d) {
+ result.pts.push_back(laser_cloud.pts[i]);
+ }
+ }
+
+ return result;
+ }
+
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Find a plane model in a point cloud given via a set of point indices with SAmple Consensus methods
+ * \param points the point cloud message
+ * \param indices a pointer to a set of point cloud indices to test
+ * \param inliers the resultant planar inliers
+ * \param coeff the resultant plane coefficients
+ * \param viewpoint_cloud a point to the pose where the data was acquired from (for normal flipping)
+ * \param dist_thresh the maximum allowed distance threshold of an inlier to the model
+ * \param min_pts the minimum number of points allowed as inliers for a plane model
+ */
+ bool
+ fitSACPlane (PointCloud &points, vector<int> indices, vector<int> &inliers, vector<double> &coeff,
+ const robot_msgs::PointStamped &viewpoint_cloud, double dist_thresh, int min_pts)
+ {
+ if ((int)indices.size () < min_pts)
+ {
+ inliers.resize (0);
+ coeff.resize (0);
+ return (false);
+ }
+
+ // Create and initialize the SAC model
+ sample_consensus::SACModelPlane *model = new sample_consensus::SACModelPlane ();
+ sample_consensus::SAC *sac = new sample_consensus::RANSAC (model, dist_thresh);
+ //sample_consensus::SAC *sac = new sample_consensus::LMedS (model, dist_thresh);
+ sac->setMaxIterations (500);
+ model->setDataSet (&points, indices);
+
+ // Search for the best plane
+ if (sac->computeModel ())
+ {
+ // Obtain the inliers and the planar model coefficients
+ if ((int)sac->getInliers ().size () < min_pts)
+ {
+ //ROS_ERROR ("fitSACPlane: Inliers.size (%d) < sac_min_points_per_model (%d)!", sac->getInliers ().size (), sac_min_points_per_model_);
+ inliers.resize (0);
+ coeff.resize (0);
+ return (false);
+ }
+ sac->computeCoefficients (coeff); // Compute the model coefficients
+ sac->refineCoefficients (coeff); // Refine them using least-squares
+ model->selectWithinDistance (coeff, dist_thresh, inliers);
+ //inliers = sac->getInliers ();
+
+ cloud_geometry::angles::flipNormalTowardsViewpoint (coeff, points.pts.at (inliers[0]), viewpoint_cloud);
+
+ //ROS_DEBUG ("Found a model supported by %d inliers: [%g, %g, %g, %g]\n", sac->getInliers ().size (),
+ // coeff[0], coeff[1], coeff[2], coeff[3]);
+
+ // Project the inliers onto the model
+ model->projectPointsInPlace (inliers, coeff);
+ }
+ else
+ {
+ ROS_ERROR ("Could not compute a plane model.");
+ return (false);
+ }
+ sort (inliers.begin (), inliers.end ());
+
+ delete sac;
+ delete model;
+ return (true);
+ }
+
+
+ bool getPoseStamped(const CvRect& r, const CvPoint& p, PoseStamped& pose)
{
CvPoint cp = p;
- bool found = findCenterPoint(disp, r,cp);
-
+ bool found = findCenterPoint(disp, r, cp);
if (!found) {
return false;
}
robot_msgs::PointCloud outlet_cloud = filterPointCloud(r);
-// CvScalar plane = estimatePlaneLS(outlet_cloud);
robot_msgs::Point32 center_point;
found = find3DPoint(outlet_cloud, cp, center_point);
if (!found) {
return false;
}
- outlet_pose.header.frame_id = cloud.header.frame_id;
- outlet_pose.header.stamp = cloud.header.stamp;
-
- outlet_pose.point.x = center_point.x;
- outlet_pose.point.y = center_point.y;
- outlet_pose.point.z = center_point.z;
-
-// outlet_pose.vector.x = plane.val[0];
-// outlet_pose.vector.y = plane.val[1];
-// outlet_pose.vector.z = plane.val[2];
-
- publish("stereo/outlet_pose", outlet_pose);
-
-
- // transform point in base_link
PointStamped ps_stereo;
ps_stereo.header.frame_id = cloud.header.frame_id;
ps_stereo.header.stamp = cloud.header.stamp;
-
ps_stereo.point.x = center_point.x;
ps_stereo.point.y = center_point.y;
ps_stereo.point.z = center_point.z;
- PointStamped ps_base;
+ ROS_INFO("OutletSpotter: Found outlet bbox, I'm waiting for cloud point.");
+ boost::unique_lock<boost::mutex> lock(clound_point_mutex);
+ // waiting for the cloud point
+ while (!have_cloud_point_) {
+ cloud_point_cv.wait(lock);
+ }
+
+ ROS_INFO("OutletSpotter: computing normal.");
+ // got a cloud point
+ PointStamped ps_cloud;
try {
- tf_->transformPoint("base_link", ps_stereo, ps_base);
+ tf_->transformPoint(laser_cloud.header.frame_id, ps_stereo, ps_cloud);
}
- catch(tf::LookupException & ex){
- ROS_ERROR("Lookup exception: %s\n", ex.what());
+ catch(tf::TransformException & ex){
+ ROS_ERROR("Transform exception: %s\n", ex.what());
}
- catch(tf::ExtrapolationException & ex){
- ROS_DEBUG("Extrapolation exception: %s\n", ex.what());
+ PointCloud outlet_vecinity = outletVecinity(laser_cloud, ps_cloud, 0.2);
+
+ // fit a plate in the outlet cloud
+ vector<int> indices(outlet_vecinity.pts.size());
+ for (size_t i=0;i<outlet_vecinity.get_pts_size();++i) {
+ indices[i] = i;
}
- catch(tf::ConnectivityException & ex){
- ROS_ERROR("Connectivity exception: %s\n", ex.what());
+ vector<int> inliers;
+ vector<double> coeff(4); // plane coefficients
+
+ // find viewpoint in laser frame
+ PointStamped stereo_viewpoint;
+ stereo_viewpoint.header.frame_id = cloud.header.frame_id;
+ stereo_viewpoint.header.stamp = cloud.header.stamp;
+ stereo_viewpoint.point.x = 0;
+ stereo_viewpoint.point.x = 0;
+ stereo_viewpoint.point.x = 0;
+
+ PointStamped viewpoint;
+ try {
+ tf_->transformPoint(laser_cloud.header.frame_id, stereo_viewpoint, viewpoint);
}
+ catch(tf::TransformException & ex){
+ ROS_ERROR("Transform exception: %s\n", ex.what());
+ }
- Planner2DGoal goal;
- goal.header.frame_id = ps_base.header.frame_id;
- goal.header.stamp = ps_base.header.stamp;
- goal.goal.x = ps_base.point.x;
- goal.goal.y = ps_base.point.y;
- goal.goal.th = 0; // for now
+ double dist_thresh = 0.02;
+ int min_pts = 50;
- if ((ros::Time::now()-goal_time).toSec()>2) {
- printf("Setting goal: %f, %f, %f\n", goal.goal.x, goal.goal.y, goal.goal.th);
- publish("goal", goal);
- goal_time = ros::Time::now();
+ ROS_INFO("OutletSpotter: finding wall plane");
+ if ( !fitSACPlane (outlet_vecinity, indices, inliers, coeff, viewpoint, dist_thresh, min_pts) || inliers.size()==0) {
+ ROS_ERROR ("Cannot find outlet plane in laser scan, aborting...");
+ return false;
}
-// printf("Stereo frame point: (%f,%f,%f), vector: (%f,%f,%f) \n",ps_stereo.point.x,ps_stereo.point.y,ps_stereo.point.z,
-// vec_stereo.vector.x,vec_stereo.vector.y,vec_stereo.vector.z);
-// printf("Base frame point: (%f,%f,%f), vector: (%f,%f,%f) \n",ps_base.point.x,ps_base.point.y,ps_base.point.z,
-// vec_base.vector.x,vec_base.vector.y,vec_base.vector.z);
+ // fill the outlet pose
+ pose.header.frame_id = laser_cloud.header.frame_id;
+ pose.header.stamp = laser_cloud.header.stamp;
+ btVector3 position(ps_cloud.point.x,ps_cloud.point.y,ps_cloud.point.z);
+ btVector3 normal(coeff[0],coeff[1],coeff[2]);
+ btVector3 up(0,0,1);
+ btVector3 left = normal.cross(up).normalized();
- showPointMarker(ps_base);
+ btMatrix3x3 rotation;
+ rotation[0] = -normal; // x
+ rotation[1] = left; // y
+ rotation[2] = up; // z
+ rotation = rotation.transpose();
+ btQuaternion orientation;
+ rotation.getRotation(orientation);
+ tf::Transform outlet_pose(orientation, position);
+ tf::PoseTFToMsg(outlet_pose, pose.pose);
+ ROS_INFO("OutletSpotter: finished computing outlet pose");
return true;
}
- void detectOutlet()
+ bool detectOutlet(PoseStamped& pose)
{
int Nerode = 1;
int Ndialate = 7;
@@ -734,22 +840,21 @@
if (save_patches) savePatch(left,bbs[t],"outlet_high_std");
continue;
}
- if(!vertical) {
- if (save_patches) savePatch(left,bbs[t],"outlet_not_vertical");
- continue;
- }
+// if(!vertical) {
+// if (save_patches) savePatch(left,bbs[t],"outlet_not_vertical");
+// continue;
+// }
// check bonding box real world dimensions
double dx;
double dy;
- double z;
- getBBoxDimensions(disp_clone, bbs[t], meanDisparity, dx, dy, z);
- if (dx<0.05 || dx>0.16) {
+ getBBoxDimensions(disp_clone, bbs[t], meanDisparity, dx, dy);
+ if (dx<0.05 || dx>0.26) {
if (save_patches) savePatch(left,bbs[t],"outlet_wrong_dimensions");
continue;
}
- if (dy<0.05 || dy>0.16) {
+ if (dy<0.05 || dy>0.26) {
if (save_patches) savePatch(left,bbs[t],"outlet_wrong_dimensions");
continue;
}
@@ -768,13 +873,21 @@
if (save_patches) savePatch(left,bbs[t],"outlet_match");
- publishOutletPose(bbs[t], centers[t]);
+ if (display) {
+ // draw bounding box
+ CvPoint pt2 = cvPoint(bbs[t].x+wi,bbs[t].y+hi);
+ cvRectangle(left,pt1,pt2,CV_RGB(0,255,0));
+ }
+ found = getPoseStamped(bbs[t], centers[t], pose);
- // draw bounding box for now
- CvPoint pt2 = cvPoint(bbs[t].x+wi,bbs[t].y+hi);
- cvRectangle(left,pt1,pt2,CV_RGB(0,255,0));
+ // one outlet is enough
+ if (found) {
+ return true;
+ }
+ }
- }
+ ROS_INFO("OutletSpotter: outlet not found");
+ return false;
}
@@ -843,51 +956,32 @@
return result;
}
-//
-// // returns the plane in Hessian normal form
- CvScalar estimatePlaneLS(robot_msgs::PointCloud points)
+ /**
+ * \brief Service call to spot outlets
+ */
+ bool outletSpottingService(outlet_spotting::OutletSpotting::Request & req, outlet_spotting::OutletSpotting::Response & resp)
+ {
+ ROS_INFO("OutletSpotter: service called");
+ return runOutletSpotter(req.point,resp.pose);
+ }
+
+ /**
+ *
+ */
+ void laser_cloud_callback()
{
- int cnt = points.pts.size();
- CvMat* A = cvCreateMat(cnt, 3, CV_32FC1);
+ boost::lock_guard<boost::mutex> lock(clound_point_mutex);
+ have_cloud_point_ = true;
+ ROS_INFO("OutletSpotter: received cloud point");
+ laser_cloud = laser_cloud_fetch;
- for (int i=0;i<cnt;++i) {
- robot_msgs::Point32 p = points.pts[i];
- cvmSet(A,i,0,p.x);
- cvmSet(A,i,1,p.y);
- cvmSet(A,i,2,p.z);
- }
-
- vector<float> ones(cnt,1);
- CvMat B;
- cvInitMatHeader(&B,cnt,1,CV_32FC1,&ones[0]);
- CvMat* X = cvCreateMat(3,1,CV_32FC1);
-
- int ok = cvSolve( A, &B, X,CV_SVD );
-
- CvScalar plane;
-
- if (ok) {
- float* xp = X->data.fl;
-
- float d = sqrt(xp[0]*xp[0]+xp[1]*xp[1]+xp[2]*xp[2]);
- plane.val[0] = xp[0]/d;
- plane.val[1] = xp[1]/d;
- plane.val[2] = xp[2]/d;
- plane.val[3] = -1/d;
- }
- else {
- plane.val[0] = plane.val[1] = plane.val[2] = plane.val[3] = -1;
- }
-
- cvReleaseMat(&A);
-
- return plane;
+ cloud_point_cv.notify_all();
}
void image_cb_all(ros::Time t)
{
- cv_mutex.lock();
+ boost::lock_guard<boost::mutex> lock(cv_mutex);
if (lbridge.fromImage(limage, "bgr"))
{
@@ -913,23 +1007,13 @@
cvCvtScale(dbridge.toIpl(), disp, 4.0/dispinfo.dpp);
}
- cloud = cloud_topic;
+ cloud = cloud_fetch;
- detectOutlet();
-
- if (display) {
- cvShowImage("left", left);
- //cvShowImage("right", right);
- cvShowImage("contours", disp);
- cvShowImage("disparity", disp_clone);
- }
-
if (disp_clone!=NULL)
cvReleaseImage(&disp_clone);
- cv_mutex.unlock();
-
+ images_cv.notify_all();
}
void image_cb_timeout(ros::Time t)
@@ -946,13 +1030,13 @@
if (stinfo.header.stamp != t)
printf("Timed out waiting for stereo info\n");
- if (cloud_topic.header.stamp != t)
+ if (cloud_fetch.header.stamp != t)
printf("Timed out waiting for point cloud\n");
- //Proceed to show images anyways
-// image_cb_all(t);
}
+public:
+
bool spin()
{
while (ok())
@@ -965,7 +1049,6 @@
if (key=='s')
save_patches ^= true;
-
cv_mutex.unlock();
usleep(10000);
}
@@ -973,8 +1056,105 @@
return true;
}
+
+ bool runOutletSpotter(const robot_msgs::PointStamped& request, robot_msgs::PoseStamped& pose)
+ {
+ subscribeToData();
+ boost::unique_lock<boost::mutex> images_lock(cv_mutex);
+ ROS_INFO("OutletSpotter: waiting for images");
+ images_cv.wait(images_lock);
+
+ ROS_INFO("OutletSpotter: performing detection");
+
+ bool found = detectOutlet(pose);
+
+ if (display) {
+ cvShowImage("left", left);
+ //cvShowImage("right", right);
+ cvShowImage("contours", disp);
+ cvShowImage("disparity", disp_clone);
+ }
+
+
+ unsubscribeFromData();
+
+ showMarkers(pose);
+
+ return found;
+ }
+
+
+
};
+
+
+
+
+
+class SpotOutletAction: public robot_actions::Action<robot_msgs::PointStamped, robot_msgs::PoseStamped>
+{
+public:
+ SpotOutletAction(OutletSpotting& spotter) :
+ robot_actions::Action<robot_msgs::PointStamped, robot_msgs::PoseStamped>("spot_outlet"),
+ spotter_(spotter)
+ {
+ spotter_.advertise<robot_msgs::PointStamped>("head_controller/head_track_point",10);
+ }
+ ~SpotOutletAction()
+ {
+ spotter_.unadvertise("head_controller/head_track_point");
+ }
+
+ virtual void handleActivate(const robot_msgs::PointStamped& outlet_estimate)
+ {
+ ROS_INFO("SpotOutletAction: handle activate");
+ request_preempt_ = false;
+ notifyActivated();
+
+ robot_msgs::PoseStamped outlet_pose;
+ if (!spotOutlet(outlet_estimate, outlet_pose)){
+ if (request_preempt_){
+ ROS_INFO("SpotOutletAction: Preempted");
+ notifyPreempted(outlet_pose);
+ }
+ else{
+ ROS_INFO("SpotOutletAction: Aborted");
+ notifyAborted(outlet_pose);
+ }
+ }
+ else{
+ ROS_INFO("SpotOutletAction: Succeeded");
+ notifySucceeded(outlet_pose);
+ }
+
+ }
+
+ virtual void handlePreempt()
+ {
+ request_preempt_ = true;
+ }
+
+private:
+
+ bool spotOutlet(const robot_msgs::PointStamped& outlet_estimate, robot_msgs::PoseStamped& pose)
+ {
+ // turn head to face outlet
+ spotter_.publish("head_controller/head_track_point", outlet_estimate);
+
+ return spotter_.runOutletSpotter(outlet_estimate, pose);
+
+ }
+
+ OutletSpotting& spotter_;
+ bool request_preempt_;
+
+};
+
+
+
+
+
int main(int argc, char **argv)
{
ros::init(argc, argv);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mar...@us...> - 2009-04-08 18:48:20
|
Revision: 13588
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13588&view=rev
Author: mariusmuja
Date: 2009-04-08 17:59:28 +0000 (Wed, 08 Apr 2009)
Log Message:
-----------
Minor cleanups.
Modified Paths:
--------------
pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp
pkg/trunk/vision/outlet_spotting/src/outlet_spotting.cpp
Modified: pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp
===================================================================
--- pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp 2009-04-08 17:58:50 UTC (rev 13587)
+++ pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp 2009-04-08 17:59:28 UTC (rev 13588)
@@ -208,7 +208,7 @@
void unsubscribeStereoData()
{
-// unsubscribe("stereo/left/image_rect_color");
+ unsubscribe("stereo/left/image_rect_color");
unsubscribe("stereo/left/image_rect");
unsubscribe("stereo/disparity");
unsubscribe("stereo/stereo_info");
Modified: pkg/trunk/vision/outlet_spotting/src/outlet_spotting.cpp
===================================================================
--- pkg/trunk/vision/outlet_spotting/src/outlet_spotting.cpp 2009-04-08 17:58:50 UTC (rev 13587)
+++ pkg/trunk/vision/outlet_spotting/src/outlet_spotting.cpp 2009-04-08 17:59:28 UTC (rev 13588)
@@ -165,9 +165,9 @@
{
tf_ = new tf::TransformListener(*this);
- param ("~display", display, false); // 100 points at high resolution
- param ("~save_patches", save_patches, false); // 100 points at high resolution
- param ("~frames_number", frames_number_, 7); // 100 points at high resolution
+ param ("~display", display, false);
+ param ("~save_patches", save_patches, false);
+ param ("~frames_number", frames_number_, 7);
if (display) {
ROS_INFO("Displaying images\n");
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2009-04-08 20:51:49
|
Revision: 13615
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13615&view=rev
Author: vijaypradeep
Date: 2009-04-08 20:51:43 +0000 (Wed, 08 Apr 2009)
Log Message:
-----------
Added new constructor to topic_synchronizer. Fixed include path for topic_synchronizer.h
Modified Paths:
--------------
pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp
pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp
pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp
pkg/trunk/util/topic_synchronizer/manifest.xml
pkg/trunk/vision/outlet_spotting/src/outlet_spotting.cpp
pkg/trunk/vision/people/src/face_detection.cpp
pkg/trunk/vision/people/src/stereo_face_color_tracker.cpp
pkg/trunk/vision/people/src/track_starter_gui.cpp
pkg/trunk/vision/recognition_lambertian/.build_version
pkg/trunk/vision/recognition_lambertian/src/recognition_lambertian.cpp
pkg/trunk/vision/stereo_capture/src/stereo_capture.cpp
pkg/trunk/vision/stereo_view/stereo_view.cpp
pkg/trunk/vision/stereo_view/stereo_view_pixel_info.cpp
Added Paths:
-----------
pkg/trunk/util/topic_synchronizer/CMakeLists.txt
pkg/trunk/util/topic_synchronizer/Makefile
pkg/trunk/util/topic_synchronizer/include/
pkg/trunk/util/topic_synchronizer/include/topic_synchronizer/
pkg/trunk/util/topic_synchronizer/include/topic_synchronizer/topic_synchronizer.h
Removed Paths:
-------------
pkg/trunk/util/topic_synchronizer/topic_synchronizer.h
Modified: pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp 2009-04-08 20:51:43 UTC (rev 13615)
@@ -39,7 +39,7 @@
#include "robot_msgs/MechanismState.h"
#include "robot_msgs/MocapSnapshot.h"
-#include "topic_synchronizer.h"
+#include "topic_synchronizer/topic_synchronizer.h"
#include "std_msgs/Empty.h"
#include "robot_msgs/PointCloud.h"
Modified: pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp
===================================================================
--- pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp 2009-04-08 20:51:43 UTC (rev 13615)
@@ -65,7 +65,7 @@
// transform library
#include <tf/transform_listener.h>
-#include "topic_synchronizer.h"
+#include "topic_synchronizer/topic_synchronizer.h"
#include "CvStereoCamModel.h"
Modified: pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp
===================================================================
--- pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp 2009-04-08 20:51:43 UTC (rev 13615)
@@ -59,7 +59,7 @@
#include "image_msgs/CamInfo.h"
#include "image_msgs/Image.h"
-#include "topic_synchronizer.h"
+#include "topic_synchronizer/topic_synchronizer.h"
#include "opencv/cxcore.h"
#include "opencv/cv.h"
Added: pkg/trunk/util/topic_synchronizer/CMakeLists.txt
===================================================================
--- pkg/trunk/util/topic_synchronizer/CMakeLists.txt (rev 0)
+++ pkg/trunk/util/topic_synchronizer/CMakeLists.txt 2009-04-08 20:51:43 UTC (rev 13615)
@@ -0,0 +1,5 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+rospack(topic_synchronizer)
+# genmsg()
+# gensrv()
Added: pkg/trunk/util/topic_synchronizer/Makefile
===================================================================
--- pkg/trunk/util/topic_synchronizer/Makefile (rev 0)
+++ pkg/trunk/util/topic_synchronizer/Makefile 2009-04-08 20:51:43 UTC (rev 13615)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Copied: pkg/trunk/util/topic_synchronizer/include/topic_synchronizer/topic_synchronizer.h (from rev 13608, pkg/trunk/util/topic_synchronizer/topic_synchronizer.h)
===================================================================
--- pkg/trunk/util/topic_synchronizer/include/topic_synchronizer/topic_synchronizer.h (rev 0)
+++ pkg/trunk/util/topic_synchronizer/include/topic_synchronizer/topic_synchronizer.h 2009-04-08 20:51:43 UTC (rev 13615)
@@ -0,0 +1,342 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/*! \mainpage
+ * \htmlinclude manifest.html
+ */
+
+#ifndef TOPIC_SYNCHRONIZER_HH
+#define TOPIC_SYNCHRONIZER_HH
+
+
+#include <boost/thread.hpp>
+#include "boost/date_time/posix_time/posix_time.hpp"
+
+//#include "rosthread/mutex.h"
+//#include "rosthread/condition.h"
+
+#include "ros/time.h"
+
+ //! A templated class for synchronizing incoming topics
+ /*!
+ * The Topic Synchronizer should be templated by your node, and is
+ * passed a function pointer at construction to be called every time
+ * all of your topics have arrived.
+ */
+template <class N>
+class TopicSynchronizer
+{
+
+ class UnsubscribeList
+ {
+ std::list<std::string> list_;
+ boost::mutex list_mutex_;
+ // ros::thread::mutex list_mutex_;
+
+ public:
+ UnsubscribeList(std::list<std::string>& l) : list_(l) { }
+
+ void doUnsubscribe(ros::Node* n, std::string topic)
+ {
+ list_mutex_.lock();
+ std::list<std::string>::iterator i = list_.begin();
+ while (i != list_.end() && *i != topic)
+ i++;
+
+ if (i != list_.end())
+ {
+ i++;
+
+ while (i != list_.end())
+ {
+ n->unsubscribe(*i);
+ list_.erase(i++);
+ }
+ }
+ list_mutex_.unlock();
+ }
+ };
+
+ class UnsubscribeHelper
+ {
+ UnsubscribeList* ul_;
+ std::string topic_;
+
+ public:
+
+ UnsubscribeHelper(UnsubscribeList* ul, std::string topic) : ul_(ul), topic_(topic) {}
+
+ void doUnsubscribe(ros::Node* node)
+ {
+ ul_->doUnsubscribe(node, topic_);
+ }
+ };
+
+ private:
+
+ //! A pointer to your node for calling callback
+ ros::Node* node_;
+ N* obj_ ;
+
+ //! The callback to be called if successful
+ void (N::*callback_)(ros::Time);
+
+ //! Timeout duration
+ boost::posix_time::time_duration timeout_;
+ // ros::Duration timeout_;
+
+ //! The callback to be called if timed out
+ void (N::*timeout_callback_)(ros::Time);
+
+ //! The condition variable and mutex used for synchronization
+ boost::condition_variable cond_all_;
+ boost::mutex cond_all_mutex_;
+ // ros::thread::condition cond_all_;
+
+ //! The number of expected incoming messages
+ int expected_count_;
+
+ //! The count of messages received so far
+ int count_;
+
+ //! Whether or not the given wait cycle has completed
+ bool done_;
+
+ //! Whether or not the node is ready to process data (all subscriptions are completed)
+ bool ready_;
+
+ //! The timestamp that is currently being waited for
+ ros::Time waiting_time_;
+
+ std::list< UnsubscribeList* > exclusion_lists_;
+ std::list< UnsubscribeHelper** > helper_list_;
+
+ //! The callback invoked by roscpp when any topic comes in
+ /*!
+ * \param p A void* which should point to the timestamp field of the incoming message
+ */
+ void msg_cb(void* p)
+ {
+ //Bail until we are actually ready
+ if (!ready_)
+ return;
+
+ ros::Time* time = (ros::Time*)(p);
+
+
+ boost::unique_lock<boost::mutex> lock(cond_all_mutex_);
+ // cond_all_mutex_.lock();
+
+ // If first to get message, wait for others
+ if (count_ == 0)
+ {
+ wait_for_others(time, lock);
+ return;
+ }
+
+ // If behind, skip
+ if (*time < waiting_time_)
+ {
+ // cond_all_mutex_.unlock();
+ return;
+ }
+
+ // If at time, increment count, possibly signal, and wait
+ if (*time == waiting_time_)
+ {
+ count_++;
+ if (count_ == expected_count_)
+ {
+ cond_all_.notify_all();
+ }
+
+ while (!done_ && *time == waiting_time_)
+ cond_all_.wait(lock);
+
+ // cond_all_mutex_.unlock();
+ return;
+ }
+
+ // If ahead, wakeup others, and ten wait for others
+ if (*time > waiting_time_)
+ {
+ cond_all_.notify_all();
+ wait_for_others(time, lock);
+ }
+ }
+
+ void msg_unsubothers(void* p)
+ {
+ UnsubscribeHelper** uh = (UnsubscribeHelper**)(p);
+
+ if (*uh != 0)
+ {
+ (*uh)->doUnsubscribe(node_);
+ delete (*uh);
+ (*uh) = 0;
+ }
+ }
+
+ //! The function called in a message cb to wait for other messages
+ /*!
+ * \param time The time that is being waited for
+ */
+ void wait_for_others(ros::Time* time, boost::unique_lock<boost::mutex>& lock)
+ {
+ count_ = 1;
+ done_ = false;
+
+ waiting_time_ = *time;
+ bool timed_out = false;
+
+ while (count_ < expected_count_ && *time == waiting_time_ && !timed_out)
+ if (!cond_all_.timed_wait(lock, timeout_))
+ {
+ timed_out = true;
+ if (timeout_callback_)
+ (obj_->*timeout_callback_)(*time);
+ }
+
+ if (*time == waiting_time_ && !timed_out)
+ (obj_->*callback_)(*time);
+
+ if (*time == waiting_time_)
+ {
+ done_ = true;
+ count_ = 0;
+ cond_all_.notify_all();
+ }
+ // cond_all_mutex_.unlock();
+ }
+
+ public:
+
+ //! Constructor
+ /*!
+ * The constructor for the TopicSynchronizer. Kept for backwards compatibility. Since you shouldn't
+ * be inheriting from ros::Node, this constructor is probably not the one you want to be using.
+ *
+ * \param node A pointer to your node, which happens to also be the callback object
+ * \param callback A pointer to the callback to invoke when all messages have arrived
+ * \param timeout The duration
+ * \param timeout_callback A callback which is triggered when the timeout expires
+ */
+ TopicSynchronizer(N* obj, void (N::*callback)(ros::Time), ros::Duration timeout = ros::Duration(1.0), void (N::*timeout_callback)(ros::Time) = NULL) : node_(obj), obj_(obj), callback_(callback), timeout_callback_(timeout_callback), expected_count_(0), count_(0), done_(false)
+ {
+ timeout_ = boost::posix_time::nanosec(timeout.toNSec());
+ }
+
+ //! Constructor
+ /*!
+ * The constructor for the TopicSynchronizer
+ *
+ * \param node A pointer to your node.
+ * \param obj A pointer to the callback object
+ * \param callback A pointer to the callback to invoke when all messages have arrived
+ * \param timeout The duration
+ * \param timeout_callback A callback which is triggered when the timeout expires
+ */
+ TopicSynchronizer(ros::Node* node, N* obj, void (N::*callback)(ros::Time), ros::Duration timeout = ros::Duration(1.0), void (N::*timeout_callback)(ros::Time) = NULL) : node_(node), obj_(obj), callback_(callback), timeout_callback_(timeout_callback), expected_count_(0), count_(0), done_(false)
+ {
+ timeout_ = boost::posix_time::nanosec(timeout.toNSec());
+ }
+
+ //! Destructor
+ ~TopicSynchronizer()
+ {
+ for (typename std::list<UnsubscribeList*>::iterator i = exclusion_lists_.begin();
+ i != exclusion_lists_.end();
+ i++)
+ delete (*i);
+
+ for (typename std::list<UnsubscribeHelper**>::iterator i = helper_list_.begin();
+ i != helper_list_.end();
+ i++)
+ {
+ if (**i != 0)
+ delete **i;
+ delete (*i);
+ }
+ }
+
+ //! Subscribe
+ /*!
+ * The synchronized subscribe call. Call this to subscribe for topics you want
+ * to be synchronized.
+ *
+ * \param topic_name The name of the topic to subscribe to
+ * \param msg A reference to the message that will be populated on callbacks
+ * \param queue_size The size of the incoming queue for the message
+ */
+ template <class M>
+ void subscribe(std::string topic_name, M& msg, int queue_size)
+ {
+ node_->subscribe(topic_name, msg, &TopicSynchronizer<N>::msg_cb, this, &(msg.header.stamp), queue_size);
+ expected_count_++;
+ }
+
+ template <class M>
+ void subscribe(std::list<std::string> topic_names, M& msg, int queue_size)
+ {
+ UnsubscribeList* ul = new UnsubscribeList(topic_names);
+ exclusion_lists_.push_back(ul); // so we can delete later
+
+ for (std::list<std::string>::iterator tn_iter = topic_names.begin();
+ tn_iter != topic_names.end();
+ tn_iter++)
+ {
+ UnsubscribeHelper** uh = new UnsubscribeHelper*;
+ *uh = new UnsubscribeHelper(ul, *tn_iter);
+ helper_list_.push_back(uh);
+
+ node_->subscribe(*tn_iter, msg, &TopicSynchronizer<N>::msg_unsubothers, this, uh, queue_size);
+ node_->subscribe(*tn_iter, msg, &TopicSynchronizer<N>::msg_cb, this, &(msg.header.stamp), queue_size);
+ }
+ expected_count_++;
+ }
+
+ void ready()
+ {
+ ready_ = true;
+ }
+
+ void reset()
+ {
+ expected_count_ = 0;
+ count_ = 0;
+ done_ = false;
+ }
+};
+
+#endif
Modified: pkg/trunk/util/topic_synchronizer/manifest.xml
===================================================================
--- pkg/trunk/util/topic_synchronizer/manifest.xml 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/util/topic_synchronizer/manifest.xml 2009-04-08 20:51:43 UTC (rev 13615)
@@ -6,7 +6,7 @@
<depend package="roscpp" />
<depend package="std_msgs" />
<export>
- <cpp cflags="-I${prefix} -D__STDC_LIMIT_MACROS -DBOOST_DATE_TIME_POSIX_TIME_STD_CONFIG `rosboost-cfg --cflags`" lflags="`rosboost-cfg --lflags thread`"/>
+ <cpp cflags="-I${prefix}/include -D__STDC_LIMIT_MACROS -DBOOST_DATE_TIME_POSIX_TIME_STD_CONFIG `rosboost-cfg --cflags`" lflags="`rosboost-cfg --lflags thread`"/>
</export>
<url>http://pr.willowgarage.com/wiki/topic_synchronizer</url>
</package>
Deleted: pkg/trunk/util/topic_synchronizer/topic_synchronizer.h
===================================================================
--- pkg/trunk/util/topic_synchronizer/topic_synchronizer.h 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/util/topic_synchronizer/topic_synchronizer.h 2009-04-08 20:51:43 UTC (rev 13615)
@@ -1,325 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-/*! \mainpage
- * \htmlinclude manifest.html
- */
-
-#ifndef TOPIC_SYNCHRONIZER_HH
-#define TOPIC_SYNCHRONIZER_HH
-
-
-#include <boost/thread.hpp>
-#include "boost/date_time/posix_time/posix_time.hpp"
-
-//#include "rosthread/mutex.h"
-//#include "rosthread/condition.h"
-
-#include "ros/time.h"
-
- //! A templated class for synchronizing incoming topics
- /*!
- * The Topic Synchronizer should be templated by your node, and is
- * passed a function pointer at construction to be called every time
- * all of your topics have arrived.
- */
-template <class N>
-class TopicSynchronizer
-{
-
- class UnsubscribeList
- {
- std::list<std::string> list_;
- boost::mutex list_mutex_;
- // ros::thread::mutex list_mutex_;
-
- public:
- UnsubscribeList(std::list<std::string>& l) : list_(l) { }
-
- void doUnsubscribe(ros::Node* n, std::string topic)
- {
- list_mutex_.lock();
- std::list<std::string>::iterator i = list_.begin();
- while (i != list_.end() && *i != topic)
- i++;
-
- if (i != list_.end())
- {
- i++;
-
- while (i != list_.end())
- {
- n->unsubscribe(*i);
- list_.erase(i++);
- }
- }
- list_mutex_.unlock();
- }
- };
-
- class UnsubscribeHelper
- {
- UnsubscribeList* ul_;
- std::string topic_;
-
- public:
-
- UnsubscribeHelper(UnsubscribeList* ul, std::string topic) : ul_(ul), topic_(topic) {}
-
- void doUnsubscribe(ros::Node* node)
- {
- ul_->doUnsubscribe(node, topic_);
- }
- };
-
- private:
-
- //! A pointer to your node for calling callback
- N* node_;
-
- //! The callback to be called if successful
- void (N::*callback_)(ros::Time);
-
- //! Timeout duration
- boost::posix_time::time_duration timeout_;
- // ros::Duration timeout_;
-
- //! The callback to be called if timed out
- void (N::*timeout_callback_)(ros::Time);
-
- //! The condition variable and mutex used for synchronization
- boost::condition_variable cond_all_;
- boost::mutex cond_all_mutex_;
- // ros::thread::condition cond_all_;
-
- //! The number of expected incoming messages
- int expected_count_;
-
- //! The count of messages received so far
- int count_;
-
- //! Whether or not the given wait cycle has completed
- bool done_;
-
- //! Whether or not the node is ready to process data (all subscriptions are completed)
- bool ready_;
-
- //! The timestamp that is currently being waited for
- ros::Time waiting_time_;
-
- std::list< UnsubscribeList* > exclusion_lists_;
- std::list< UnsubscribeHelper** > helper_list_;
-
- //! The callback invoked by roscpp when any topic comes in
- /*!
- * \param p A void* which should point to the timestamp field of the incoming message
- */
- void msg_cb(void* p)
- {
- //Bail until we are actually ready
- if (!ready_)
- return;
-
- ros::Time* time = (ros::Time*)(p);
-
-
- boost::unique_lock<boost::mutex> lock(cond_all_mutex_);
- // cond_all_mutex_.lock();
-
- // If first to get message, wait for others
- if (count_ == 0)
- {
- wait_for_others(time, lock);
- return;
- }
-
- // If behind, skip
- if (*time < waiting_time_)
- {
- // cond_all_mutex_.unlock();
- return;
- }
-
- // If at time, increment count, possibly signal, and wait
- if (*time == waiting_time_)
- {
- count_++;
- if (count_ == expected_count_)
- {
- cond_all_.notify_all();
- }
-
- while (!done_ && *time == waiting_time_)
- cond_all_.wait(lock);
-
- // cond_all_mutex_.unlock();
- return;
- }
-
- // If ahead, wakeup others, and ten wait for others
- if (*time > waiting_time_)
- {
- cond_all_.notify_all();
- wait_for_others(time, lock);
- }
- }
-
- void msg_unsubothers(void* p)
- {
- UnsubscribeHelper** uh = (UnsubscribeHelper**)(p);
-
- if (*uh != 0)
- {
- (*uh)->doUnsubscribe(node_);
- delete (*uh);
- (*uh) = 0;
- }
- }
-
- //! The function called in a message cb to wait for other messages
- /*!
- * \param time The time that is being waited for
- */
- void wait_for_others(ros::Time* time, boost::unique_lock<boost::mutex>& lock)
- {
- count_ = 1;
- done_ = false;
-
- waiting_time_ = *time;
- bool timed_out = false;
-
- while (count_ < expected_count_ && *time == waiting_time_ && !timed_out)
- if (!cond_all_.timed_wait(lock, timeout_))
- {
- timed_out = true;
- if (timeout_callback_)
- (*node_.*timeout_callback_)(*time);
- }
-
- if (*time == waiting_time_ && !timed_out)
- (*node_.*callback_)(*time);
-
- if (*time == waiting_time_)
- {
- done_ = true;
- count_ = 0;
- cond_all_.notify_all();
- }
- // cond_all_mutex_.unlock();
- }
-
- public:
-
- //! Constructor
- /*!
- * The constructor for the TopicSynchronizer
- *
- * \param node A pointer to your node.
- * \param callback A pointer to the callback to invoke when all messages have arrived
- * \param timeout The duration
- * \param timeout_callback A callback which is triggered when the timeout expires
- */
- TopicSynchronizer(N* node, void (N::*callback)(ros::Time), ros::Duration timeout = ros::Duration(1.0), void (N::*timeout_callback)(ros::Time) = NULL) : node_(node), callback_(callback), timeout_callback_(timeout_callback), expected_count_(0), count_(0), done_(false)
- {
- timeout_ = boost::posix_time::nanosec(timeout.toNSec());
- }
-
- //! Destructor
- ~TopicSynchronizer()
- {
- for (typename std::list<UnsubscribeList*>::iterator i = exclusion_lists_.begin();
- i != exclusion_lists_.end();
- i++)
- delete (*i);
-
- for (typename std::list<UnsubscribeHelper**>::iterator i = helper_list_.begin();
- i != helper_list_.end();
- i++)
- {
- if (**i != 0)
- delete **i;
- delete (*i);
- }
- }
-
- //! Subscribe
- /*!
- * The synchronized subscribe call. Call this to subscribe for topics you want
- * to be synchronized.
- *
- * \param topic_name The name of the topic to subscribe to
- * \param msg A reference to the message that will be populated on callbacks
- * \param queue_size The size of the incoming queue for the message
- */
- template <class M>
- void subscribe(std::string topic_name, M& msg, int queue_size)
- {
- node_->subscribe(topic_name, msg, &TopicSynchronizer<N>::msg_cb, this, &(msg.header.stamp), queue_size);
- expected_count_++;
- }
-
- template <class M>
- void subscribe(std::list<std::string> topic_names, M& msg, int queue_size)
- {
- UnsubscribeList* ul = new UnsubscribeList(topic_names);
- exclusion_lists_.push_back(ul); // so we can delete later
-
- for (std::list<std::string>::iterator tn_iter = topic_names.begin();
- tn_iter != topic_names.end();
- tn_iter++)
- {
- UnsubscribeHelper** uh = new UnsubscribeHelper*;
- *uh = new UnsubscribeHelper(ul, *tn_iter);
- helper_list_.push_back(uh);
-
- node_->subscribe(*tn_iter, msg, &TopicSynchronizer<N>::msg_unsubothers, this, uh, queue_size);
- node_->subscribe(*tn_iter, msg, &TopicSynchronizer<N>::msg_cb, this, &(msg.header.stamp), queue_size);
- }
- expected_count_++;
- }
-
- void ready()
- {
- ready_ = true;
- }
-
- void reset()
- {
- expected_count_ = 0;
- count_ = 0;
- done_ = false;
- }
-};
-
-#endif
Modified: pkg/trunk/vision/outlet_spotting/src/outlet_spotting.cpp
===================================================================
--- pkg/trunk/vision/outlet_spotting/src/outlet_spotting.cpp 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/vision/outlet_spotting/src/outlet_spotting.cpp 2009-04-08 20:51:43 UTC (rev 13615)
@@ -65,7 +65,7 @@
#include <point_cloud_mapping/sample_consensus/sac.h>
#include <point_cloud_mapping/sample_consensus/ransac.h>
#include <point_cloud_mapping/sample_consensus/lmeds.h>
-#include "topic_synchronizer.h"
+#include "topic_synchronizer/topic_synchronizer.h"
#include <tf/transform_listener.h>
#include "CvStereoCamModel.h"
Modified: pkg/trunk/vision/people/src/face_detection.cpp
===================================================================
--- pkg/trunk/vision/people/src/face_detection.cpp 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/vision/people/src/face_detection.cpp 2009-04-08 20:51:43 UTC (rev 13615)
@@ -50,7 +50,7 @@
#include "image_msgs/CvBridge.h"
#include "image_msgs/ColoredLine.h"
#include "image_msgs/ColoredLines.h"
-#include "topic_synchronizer.h"
+#include "topic_synchronizer/topic_synchronizer.h"
#include "tf/transform_listener.h"
#include "opencv/cxcore.h"
Modified: pkg/trunk/vision/people/src/stereo_face_color_tracker.cpp
===================================================================
--- pkg/trunk/vision/people/src/stereo_face_color_tracker.cpp 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/vision/people/src/stereo_face_color_tracker.cpp 2009-04-08 20:51:43 UTC (rev 13615)
@@ -49,7 +49,7 @@
#include "image_msgs/Image.h"
#include "image_msgs/CvBridge.h"
#include "image_msgs/ColoredLines.h"
-#include "topic_synchronizer.h"
+#include "topic_synchronizer/topic_synchronizer.h"
#include "tf/transform_listener.h"
#include "opencv/cxcore.h"
Modified: pkg/trunk/vision/people/src/track_starter_gui.cpp
===================================================================
--- pkg/trunk/vision/people/src/track_starter_gui.cpp 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/vision/people/src/track_starter_gui.cpp 2009-04-08 20:51:43 UTC (rev 13615)
@@ -49,7 +49,7 @@
#include "CvStereoCamModel.h"
#include <robot_msgs/PositionMeasurement.h>
#include "color_calib.h"
-#include "topic_synchronizer.h"
+#include "topic_synchronizer/topic_synchronizer.h"
#include "opencv/cxcore.h"
#include "opencv/cv.h"
Modified: pkg/trunk/vision/recognition_lambertian/.build_version
===================================================================
--- pkg/trunk/vision/recognition_lambertian/.build_version 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/vision/recognition_lambertian/.build_version 2009-04-08 20:51:43 UTC (rev 13615)
@@ -1 +1 @@
-:
+https://personalrobots.svn.sourceforge.net/svnroot/personalrobots/pkg/trunk/vision/recognition_lambertian:13607
Modified: pkg/trunk/vision/recognition_lambertian/src/recognition_lambertian.cpp
===================================================================
--- pkg/trunk/vision/recognition_lambertian/src/recognition_lambertian.cpp 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/vision/recognition_lambertian/src/recognition_lambertian.cpp 2009-04-08 20:51:43 UTC (rev 13615)
@@ -65,7 +65,7 @@
// transform library
#include <tf/transform_listener.h>
-#include "topic_synchronizer.h"
+#include "topic_synchronizer/topic_synchronizer.h"
#include "CvStereoCamModel.h"
Modified: pkg/trunk/vision/stereo_capture/src/stereo_capture.cpp
===================================================================
--- pkg/trunk/vision/stereo_capture/src/stereo_capture.cpp 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/vision/stereo_capture/src/stereo_capture.cpp 2009-04-08 20:51:43 UTC (rev 13615)
@@ -73,7 +73,7 @@
// ... //
#include "color_calib.h"
-#include "topic_synchronizer.h"
+#include "topic_synchronizer/topic_synchronizer.h"
#include <boost/thread.hpp>
//DEFINES
Modified: pkg/trunk/vision/stereo_view/stereo_view.cpp
===================================================================
--- pkg/trunk/vision/stereo_view/stereo_view.cpp 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/vision/stereo_view/stereo_view.cpp 2009-04-08 20:51:43 UTC (rev 13615)
@@ -48,7 +48,7 @@
#include "color_calib.h"
-#include "topic_synchronizer.h"
+#include "topic_synchronizer/topic_synchronizer.h"
#include <boost/thread.hpp>
Modified: pkg/trunk/vision/stereo_view/stereo_view_pixel_info.cpp
===================================================================
--- pkg/trunk/vision/stereo_view/stereo_view_pixel_info.cpp 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/vision/stereo_view/stereo_view_pixel_info.cpp 2009-04-08 20:51:43 UTC (rev 13615)
@@ -61,7 +61,7 @@
#include "color_calib.h"
-#include "topic_synchronizer.h"
+#include "topic_synchronizer/topic_synchronizer.h"
#in...
[truncated message content] |
|
From: <rph...@us...> - 2009-04-08 22:00:37
|
Revision: 13624
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13624&view=rev
Author: rphilippsen
Date: 2009-04-08 22:00:24 +0000 (Wed, 08 Apr 2009)
Log Message:
-----------
= in mpglue =
* Got rid of deprecated Pose2DFloat32, using robot_msgs/Pose now.
* Defined waypoint_s which includes a radial and an angular tolerance
for each waypoint (for planners who support that).
* SBPLEnvironment subclasses now inform the higher levels if they
support theta-planning.
* Restructed the plan conversion routines accordingly, adapted
existing planner wrappers.
= in mpbench =
* Adapted to the mpglue changes.
* Plot the robot orientation for path plans that use angle
tolerance < M_PI
* mkhtml.py now runs a comparison of non-sliding versus sliding
motion primitives for PR2
= in highlevel_controllers =
* Adapted to the mpglue changes.
* It still uses deprecated Pose2DFloat32 messages...
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
pkg/trunk/motion_planning/mpbench/data/test-sideways-sliding.xml
pkg/trunk/motion_planning/mpbench/mkhtml.py
pkg/trunk/motion_planning/mpbench/src/gfx.cpp
pkg/trunk/motion_planning/mpbench/src/gfx.h
pkg/trunk/motion_planning/mpbench/src/mpbenchmark.cpp
pkg/trunk/motion_planning/mpglue/include/mpglue/plan.h
pkg/trunk/motion_planning/mpglue/include/mpglue/sbpl_environment.h
pkg/trunk/motion_planning/mpglue/src/navfn_planner.cpp
pkg/trunk/motion_planning/mpglue/src/plan.cpp
pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp
pkg/trunk/motion_planning/mpglue/src/sbpl_planner.cpp
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-04-08 21:59:46 UTC (rev 13623)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-04-08 22:00:24 UTC (rev 13624)
@@ -615,7 +615,14 @@
sentry<MoveBase> guard(this);
if (!isValid() || plan_.size() > newPlan.size()){
plan_.clear();
- std::copy(newPlan.begin(), newPlan.end(), std::back_inserter(plan_));
+ deprecated_msgs::Pose2DFloat32 pose;
+ for (mpglue::waypoint_plan_t::const_iterator ip(newPlan.begin());
+ ip != newPlan.end(); ++ip) {
+ pose.x = ip->x;
+ pose.y = ip->y;
+ pose.th = ip->theta;
+ plan_.push_back(pose);
+ }
publishPath(true, plan_);
}
}
Modified: pkg/trunk/motion_planning/mpbench/data/test-sideways-sliding.xml
===================================================================
--- pkg/trunk/motion_planning/mpbench/data/test-sideways-sliding.xml 2009-04-08 21:59:46 UTC (rev 13623)
+++ pkg/trunk/motion_planning/mpbench/data/test-sideways-sliding.xml 2009-04-08 22:00:24 UTC (rev 13624)
@@ -40,6 +40,7 @@
<task>
<description>top / widest</description>
<goal><pose> 6.5 5.62 -1.55 </pose></goal>
+ <door> iejfrw wewoief </door>
<start><pose> 0.5 2.98 1.56 </pose></start>
</task>
</setup>
Modified: pkg/trunk/motion_planning/mpbench/mkhtml.py
===================================================================
--- pkg/trunk/motion_planning/mpbench/mkhtml.py 2009-04-08 21:59:46 UTC (rev 13623)
+++ pkg/trunk/motion_planning/mpbench/mkhtml.py 2009-04-08 22:00:24 UTC (rev 13624)
@@ -11,17 +11,19 @@
worldspec = []
worldexpl = []
-worldspec.append("pgm:204:../data/willow-clip1-r25.pgm:../data/willow-clip1-r25-a.xml")
-worldexpl.append("hallway to office")
-worldspec.append("pgm:204:../data/willow-clip1-r25.pgm:../data/willow-clip1-r25-b.xml")
-worldexpl.append("office to hallway")
+worldspec.append("xml:../data/test-sideways-sliding.xml")
+worldexpl.append("test sideways sliding")
plannerspec = []
plannerexpl = []
plannerspec.append("ad:xythetalat:../data/pr2.mprim")
-plannerexpl.append("AD planner on lattice")
+plannerexpl.append("AD planner without sideways motion")
+plannerspec.append("ad:xythetalat:../data/pr2sides.mprim")
+plannerexpl.append("AD planner with sideways motion")
plannerspec.append("ara:xythetalat:../data/pr2.mprim")
-plannerexpl.append("ARA planner on lattice")
+plannerexpl.append("ARA planner without sideways motion")
+plannerspec.append("ara:xythetalat:../data/pr2sides.mprim")
+plannerexpl.append("ARA planner with sideways motion")
robotspec = []
robotexpl = []
@@ -33,8 +35,6 @@
costmapexpl = []
costmapspec.append("sfl:25:325:460:550")
costmapexpl.append("fully inflated")
-costmapspec.append("sfl:25:325:460:460")
-costmapexpl.append("inflated to circumscribed")
costmapspec.append("sfl:25:325:325:325")
costmapexpl.append("inflated to inscribed")
costmapspec.append("sfl:25:0:0:0")
Modified: pkg/trunk/motion_planning/mpbench/src/gfx.cpp
===================================================================
--- pkg/trunk/motion_planning/mpbench/src/gfx.cpp 2009-04-08 21:59:46 UTC (rev 13623)
+++ pkg/trunk/motion_planning/mpbench/src/gfx.cpp 2009-04-08 22:00:24 UTC (rev 13624)
@@ -97,7 +97,6 @@
bool _websiteMode,
std::string const & _baseFilename,
ResultCollection const & _result,
- bool _ignorePlanTheta,
std::ostream & _logOs)
: setup(_setup),
world(*_setup.getWorld()),
@@ -109,7 +108,6 @@
baseFilename(_baseFilename),
footprint(_setup.getFootprint()),
result(_result),
- ignorePlanTheta(_ignorePlanTheta),
logOs(_logOs)
{
}
@@ -316,7 +314,8 @@
new npm::TraversabilityDrawing("costmap_dark", new CostmapWrapProxy(0),
npm::TraversabilityDrawing::MINIMAL_DARK);
- new PlanDrawing("planner", -1, -1, false);
+ static bool const detailed(true);
+ new PlanDrawing("planner", -1, -1, detailed);
view = new npm::View("planner", npm::Instance<npm::UniqueManager<npm::View> >());
// beware of weird npm::View::Configure() param order: x, y, width, height
view->Configure(v_width, 0, v_width, 1);
@@ -437,20 +436,27 @@
glColor3d(0.5, 1, 0);
glLineWidth(1);
if (plan) {
- for (wpi_t iw(plan->begin()); iw != plan->end(); iw += skip) {
+ for (wpi_t iw(plan->begin()); iw != plan->end(); /* this can segfault: iw += skip */) {
glPushMatrix();
glTranslated(iw->x, iw->y, 0);
- if (configptr->ignorePlanTheta) {
+ if (iw->ignoreTheta()) {
gluDisk(wrap_glu_quadric_instance(),
configptr->inscribedRadius,
configptr->inscribedRadius,
36, 1);
}
else {
- glRotated(180 * iw->th / M_PI, 0, 0, 1);
+ glRotated(180 * iw->theta / M_PI, 0, 0, 1);
drawFootprint();
}
glPopMatrix();
+ // quick hack to skip ahead while not missing the end of an
+ // STL container... should be done more properly.
+ for (size_t ii(0); ii < skip; ++ii) {
+ ++iw;
+ if (iw == plan->end())
+ break;
+ }
} // endfor(plan)
} // endif(plan)
} // endif(detailed)
Modified: pkg/trunk/motion_planning/mpbench/src/gfx.h
===================================================================
--- pkg/trunk/motion_planning/mpbench/src/gfx.h 2009-04-08 21:59:46 UTC (rev 13623)
+++ pkg/trunk/motion_planning/mpbench/src/gfx.h 2009-04-08 22:00:24 UTC (rev 13624)
@@ -51,7 +51,6 @@
bool websiteMode,
std::string const & baseFilename,
ResultCollection const & result,
- bool ignorePlanTheta,
std::ostream & logOs);
Setup const & setup;
@@ -64,7 +63,6 @@
std::string const baseFilename;
mpglue::footprint_t const & footprint;
ResultCollection const & result;
- bool const ignorePlanTheta;
mutable std::ostream & logOs;
};
Modified: pkg/trunk/motion_planning/mpbench/src/mpbenchmark.cpp
===================================================================
--- pkg/trunk/motion_planning/mpbench/src/mpbenchmark.cpp 2009-04-08 21:59:46 UTC (rev 13623)
+++ pkg/trunk/motion_planning/mpbench/src/mpbenchmark.cpp 2009-04-08 22:00:24 UTC (rev 13624)
@@ -126,7 +126,6 @@
websiteMode,
baseFilename(),
*result_collection,
- true, // XXXX to do: depends on 3DKIN
*logos),
"mpbench",
3, // hack: layoutID
Modified: pkg/trunk/motion_planning/mpglue/include/mpglue/plan.h
===================================================================
--- pkg/trunk/motion_planning/mpglue/include/mpglue/plan.h 2009-04-08 21:59:46 UTC (rev 13623)
+++ pkg/trunk/motion_planning/mpglue/include/mpglue/plan.h 2009-04-08 22:00:24 UTC (rev 13624)
@@ -35,18 +35,42 @@
#ifndef MPGLUE_PLAN_HPP
#define MPGLUE_PLAN_HPP
+#include <robot_msgs/Pose.h>
#include <deprecated_msgs/Pose2DFloat32.h>
#include <vector>
namespace mpglue {
typedef std::vector<int> raw_sbpl_plan_t;
- typedef std::vector<deprecated_msgs::Pose2DFloat32> waypoint_plan_t;
+ namespace deprecated {
+ typedef std::vector<deprecated_msgs::Pose2DFloat32> waypoint_plan_t;
+ }
+
class SBPLEnvironment;
class IndexTransform;
+ struct waypoint_s {
+ static double const default_dr;
+ static double const default_dtheta;
+
+ waypoint_s(double x, double y, double theta, double dr, double dtheta);
+ waypoint_s(double x, double y, double theta);
+ waypoint_s(waypoint_s const & orig);
+ waypoint_s(robot_msgs::Pose const & pose, double dr, double dtheta);
+ explicit waypoint_s(robot_msgs::Pose const & pose);
+ waypoint_s(deprecated_msgs::Pose2DFloat32 const & pose, double dr, double dtheta);
+ explicit waypoint_s(deprecated_msgs::Pose2DFloat32 const & pose);
+
+ bool ignoreTheta() const;
+
+ double x, y, theta, dr, dtheta;
+ };
+
+ typedef std::vector<waypoint_s> waypoint_plan_t;
+
+
/**
Helper class for constructing waypoint_plan_t while updating plan
statistics.
@@ -55,9 +79,101 @@
{
public:
explicit PlanConverter(waypoint_plan_t * plan);
- void addWaypoint(deprecated_msgs::Pose2DFloat32 const & waypoint);
- void addWaypoint(double px, double py, double pth);
+ PlanConverter(waypoint_plan_t * plan, double default_dr, double default_dtheta);
+ void addWaypoint(double px, double py, double pth)
+ { addWaypoint(waypoint_s(px, py, pth, default_dr, default_dtheta)); }
+
+ void addWaypoint(double px, double py, double pth, double dr, double dtheta)
+ { addWaypoint(waypoint_s(px, py, pth, dr, dtheta)); }
+
+ void addWaypoint(waypoint_s const & wp);
+
+ /**
+ Convert a plan from a raw state ID sequence (as computed by
+ SBPLPlanner subclasses) to waypoints that are
+ understandable. Optionally provides some statistics on the
+ plan.
+ */
+ static void convertSBPL(/** in: how to translate state IDs to (x, y) or (x, y, theta) */
+ SBPLEnvironment const & environment,
+ /** in: the raw plan */
+ raw_sbpl_plan_t const & raw,
+ /** in: the radial tolerance of each waypoint */
+ double dr,
+ /** in: the angular tolerance of each
+ waypoint (use M_PI to ignore theta) */
+ double dtheta,
+ /** out: the converted plan (it is just appended
+ to, not cleared for you) */
+ waypoint_plan_t * plan,
+ /** optional out: the cumulated path length */
+ double * optPlanLengthM,
+ /** optional out: the cumulated change in the path
+ tangential direction (the angle between the
+ x-axis and the delta between two successive
+ waypoints) */
+ double * optTangentChangeRad,
+ /** optional out: the cumulated change in the
+ direction of the waypoints (the delta of
+ theta values).
+ \note This only makes sense for plans that are
+ aware of the robot's heading though, i.e. dtheta < M_PI. */
+ double * optDirectionChangeRad);
+
+ /**
+ Convert a plan from an float index-pair sequence (as computed by
+ NavFn) to waypoints that are understandable. Optionally provides
+ some statistics on the plan. Have a look at
+ convertXYInterpolate() though, it takes advantage of the
+ sub-pixel resolution available in NavFn.
+ */
+ static void convertXY(/** in: how to translate map (x, y) to global (x, y) */
+ IndexTransform const & itransform,
+ /** in: array of X-coordinates (continuous grid index). */
+ float const * path_x,
+ /** in: array of Y-coordinates (continuous grid index). */
+ float const * path_y,
+ /** in: the length of path_x[] and path_y[]. */
+ int path_len,
+ /** in: the radial tolerance of each waypoint */
+ double dr,
+ /** out: the converted plan (it is just appended
+ to, not cleared for you) */
+ waypoint_plan_t * plan,
+ /** optional out: the cumulated path length */
+ double * optPlanLengthM,
+ /** optional out: the cumulated change in the path
+ tangential direction (the angle between the
+ x-axis and the delta between two successive
+ waypoints) */
+ double * optTangentChangeRad);
+
+ /**
+ Uses interpolateIndexToGlobal() for sub-pixel resolution.
+ */
+ static void convertXYInterpolate(/** in: how to translate map (x, y) to global (x, y) */
+ IndexTransform const & itransform,
+ /** in: array of X-coordinates (continuous grid index). */
+ float const * path_x,
+ /** in: array of Y-coordinates (continuous grid index). */
+ float const * path_y,
+ /** in: the length of path_x[] and path_y[]. */
+ int path_len,
+ /** in: the radial tolerance of each waypoint */
+ double dr,
+ /** out: the converted plan (it is just appended
+ to, not cleared for you) */
+ waypoint_plan_t * plan,
+ /** optional out: the cumulated path length */
+ double * optPlanLengthM,
+ /** optional out: the cumulated change in the path
+ tangential direction (the angle between the
+ x-axis and the delta between two successive
+ waypoints) */
+ double * optTangentChangeRad);
+
+ double default_dr, default_dtheta;
double plan_length, tangent_change, direction_change;
private:
@@ -66,93 +182,6 @@
double prevx_, prevy_, prevtan_, prevdir_;
};
-
- /**
- Convert a plan from a raw state ID sequence (as computed by
- SBPLPlanner subclasses) to waypoints that are
- understandable. Optionally provides some statistics on the plan.
- */
- void convertPlan(/** in: how to translate state IDs to deprecated_msgs::Pose2DFloat32 */
- SBPLEnvironment const & environment,
- /** in: the raw plan */
- raw_sbpl_plan_t const & raw,
- /** out: the converted plan (it is just appended
- to, not cleared for you) */
- waypoint_plan_t * plan,
- /** optional out: the cumulated path length */
- double * optPlanLengthM,
- /** optional out: the cumulated change in the path
- tangential direction (the angle between the
- x-axis and the delta between two successive
- waypoints) */
- double * optTangentChangeRad,
- /** optional out: the cumulated change in the
- direction of the waypoints (the delta of
- deprecated_msgs::Pose2DFloat32::th values).
- \note This only makes sense for plans that are
- aware of the robot's heading though. */
- double * optDirectionChangeRad);
-
- /**
- Convert a plan from an float index-pair sequence (as computed by
- NavFn) to waypoints that are understandable. Optionally provides
- some statistics on the plan. Have a look at
- convertPlanInterpolate() though, it takes advantage of the
- sub-pixel resolution available in NavFn.
- */
- void convertPlan(/** in: how to translate state IDs to deprecated_msgs::Pose2DFloat32 */
- IndexTransform const & itransform,
- /** in: array of X-coordinates (continuous grid index). */
- float const * path_x,
- /** in: array of Y-coordinates (continuous grid index). */
- float const * path_y,
- /** in: the length of path_x[] and path_y[]. */
- int path_len,
- /** out: the converted plan (it is just appended
- to, not cleared for you) */
- waypoint_plan_t * plan,
- /** optional out: the cumulated path length */
- double * optPlanLengthM,
- /** optional out: the cumulated change in the path
- tangential direction (the angle between the
- x-axis and the delta between two successive
- waypoints) */
- double * optTangentChangeRad,
- /** optional out: the cumulated change in the
- direction of the waypoints (the delta of
- deprecated_msgs::Pose2DFloat32::th values).
- \note This only makes sense for plans that are
- aware of the robot's heading though. */
- double * optDirectionChangeRad);
-
- /**
- Uses interpolateIndexToGlobal() for sub-pixel resolution.
- */
- void convertPlanInterpolate(/** in: how to translate state IDs to deprecated_msgs::Pose2DFloat32 */
- IndexTransform const & itransform,
- /** in: array of X-coordinates (continuous grid index). */
- float const * path_x,
- /** in: array of Y-coordinates (continuous grid index). */
- float const * path_y,
- /** in: the length of path_x[] and path_y[]. */
- int path_len,
- /** out: the converted plan (it is just appended
- to, not cleared for you) */
- waypoint_plan_t * plan,
- /** optional out: the cumulated path length */
- double * optPlanLengthM,
- /** optional out: the cumulated change in the path
- tangential direction (the angle between the
- x-axis and the delta between two successive
- waypoints) */
- double * optTangentChangeRad,
- /** optional out: the cumulated change in the
- direction of the waypoints (the delta of
- deprecated_msgs::Pose2DFloat32::th values).
- \note This only makes sense for plans that are
- aware of the robot's heading though. */
- double * optDirectionChangeRad);
-
}
#endif // MPGLUE_PLAN_HPP
Modified: pkg/trunk/motion_planning/mpglue/include/mpglue/sbpl_environment.h
===================================================================
--- pkg/trunk/motion_planning/mpglue/include/mpglue/sbpl_environment.h 2009-04-08 21:59:46 UTC (rev 13623)
+++ pkg/trunk/motion_planning/mpglue/include/mpglue/sbpl_environment.h 2009-04-08 22:00:24 UTC (rev 13624)
@@ -169,6 +169,16 @@
boost::shared_ptr<CostmapAccessor const> getCostmap() const { return cm_; }
boost::shared_ptr<IndexTransform const> getIndexTransform() const { return it_; }
+ /**
+ \return the precision of the angular information contained in
+ the environment representation. For 2D planners, this is M_PI
+ (they do not take the theta angle into account). For planners
+ that do take theta into account, the returned value should be
+ half the angular resolution, such that +/- the tolerance covers
+ the full range.
+ */
+ virtual double GetAngularTolerance() const = 0;
+
protected:
virtual bool DoUpdateCost(int ix, int iy, unsigned char newcost) = 0;
Modified: pkg/trunk/motion_planning/mpglue/src/navfn_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/mpglue/src/navfn_planner.cpp 2009-04-08 21:59:46 UTC (rev 13623)
+++ pkg/trunk/motion_planning/mpglue/src/navfn_planner.cpp 2009-04-08 22:00:24 UTC (rev 13624)
@@ -103,13 +103,30 @@
if (planner_->calcNavFnAstar()) {
plan.reset(new waypoint_plan_t());
if (interpolate_plan_)
- convertPlanInterpolate(*itransform_, planner_->getPathX(), planner_->getPathY(),
- planner_->getPathLen(), plan.get(), &stats_.plan_length,
- &stats_.plan_angle_change, 0);
+ PlanConverter::convertXYInterpolate(*itransform_,
+ planner_->getPathX(),
+ planner_->getPathY(),
+ planner_->getPathLen(),
+ // using the cell size as
+ // waypoint tolerance
+ // seems like a good
+ // heuristic
+ itransform_->getResolution(),
+ plan.get(),
+ &stats_.plan_length,
+ &stats_.plan_angle_change);
else
- convertPlan(*itransform_, planner_->getPathX(), planner_->getPathY(),
- planner_->getPathLen(), plan.get(), &stats_.plan_length,
- &stats_.plan_angle_change, 0);
+ PlanConverter::convertXY(*itransform_,
+ planner_->getPathX(),
+ planner_->getPathY(),
+ planner_->getPathLen(),
+ // using the cell size as waypoint
+ // tolerance seems like a good
+ // heuristic
+ itransform_->getResolution(),
+ plan.get(),
+ &stats_.plan_length,
+ &stats_.plan_angle_change);
}
return plan;
}
Modified: pkg/trunk/motion_planning/mpglue/src/plan.cpp
===================================================================
--- pkg/trunk/motion_planning/mpglue/src/plan.cpp 2009-04-08 21:59:46 UTC (rev 13623)
+++ pkg/trunk/motion_planning/mpglue/src/plan.cpp 2009-04-08 22:00:24 UTC (rev 13624)
@@ -38,10 +38,45 @@
namespace mpglue {
+ double const waypoint_s::default_dr(0.1);
+ double const waypoint_s::default_dtheta(M_PI);
+ waypoint_s::waypoint_s(double _x, double _y, double _theta, double _dr, double _dtheta)
+ : x(_x), y(_y), theta(_theta), dr(_dr), dtheta(_dtheta) {}
+
+ waypoint_s::waypoint_s(double _x, double _y, double _theta)
+ : x(_x), y(_y), theta(_theta), dr(default_dr), dtheta(default_dtheta) {}
+
+ waypoint_s::waypoint_s(waypoint_s const & orig)
+ : x(orig.x), y(orig.y), theta(orig.theta), dr(orig.dr), dtheta(orig.dtheta) {}
+
+ waypoint_s::waypoint_s(robot_msgs::Pose const & pose, double _dr, double _dtheta)
+ : x(pose.position.x), y(pose.position.y),
+ theta(atan2(pose.orientation.z, pose.orientation.w)),
+ dr(_dr), dtheta(_dtheta) {}
+
+ waypoint_s::waypoint_s(robot_msgs::Pose const & pose)
+ : x(pose.position.x), y(pose.position.y),
+ theta(atan2(pose.orientation.z, pose.orientation.w)),
+ dr(default_dr), dtheta(default_dtheta) {}
+
+ waypoint_s::waypoint_s(deprecated_msgs::Pose2DFloat32 const & pose, double _dr, double _dtheta)
+ : x(pose.x), y(pose.y), theta(pose.th), dr(_dr), dtheta(_dtheta) {}
+
+ waypoint_s::waypoint_s(deprecated_msgs::Pose2DFloat32 const & pose)
+ : x(pose.x), y(pose.y), theta(pose.th), dr(default_dr), dtheta(default_dtheta) {}
+
+ bool waypoint_s::ignoreTheta() const
+ {
+ return dtheta >= M_PI;
+ }
+
+
PlanConverter::
PlanConverter(waypoint_plan_t * plan)
- : plan_length(0),
+ : default_dr(waypoint_s::default_dr),
+ default_dtheta(waypoint_s::default_dtheta),
+ plan_length(0),
tangent_change(0),
direction_change(0),
plan_(plan),
@@ -54,48 +89,57 @@
}
+ PlanConverter::
+ PlanConverter(waypoint_plan_t * plan, double _default_dr, double _default_dtheta)
+ : default_dr(_default_dr),
+ default_dtheta(_default_dtheta),
+ plan_length(0),
+ tangent_change(0),
+ direction_change(0),
+ plan_(plan),
+ count_(0),
+ prevx_(0),
+ prevy_(0),
+ prevtan_(0),
+ prevdir_(0)
+ {
+ }
+
+
void PlanConverter::
- addWaypoint(deprecated_msgs::Pose2DFloat32 const & waypoint)
+ addWaypoint(waypoint_s const & wp)
{
if (0 < count_) {
- double const dx(waypoint.x - prevx_);
- double const dy(waypoint.y - prevy_);
+ double const dx(wp.x - prevx_);
+ double const dy(wp.y - prevy_);
plan_length += sqrt(pow(dx, 2) + pow(dy, 2));
- direction_change += fabs(sfl::mod2pi(waypoint.th - prevdir_));
+ direction_change += fabs(sfl::mod2pi(wp.theta - prevdir_));
double const tangent(atan2(dy, dx));
if (1 < count_) // tangent change only available after 2nd point
tangent_change += fabs(sfl::mod2pi(tangent - prevtan_));
prevtan_ = tangent;
}
- plan_->push_back(waypoint);
+ plan_->push_back(wp);
++count_;
- prevx_ = waypoint.x;
- prevy_ = waypoint.y;
- prevdir_ = waypoint.th;
+ prevx_ = wp.x;
+ prevy_ = wp.y;
+ prevdir_ = wp.theta;
}
void PlanConverter::
- addWaypoint(double px, double py, double pth)
+ convertSBPL(SBPLEnvironment const & environment,
+ raw_sbpl_plan_t const & raw,
+ double dr,
+ double dtheta,
+ waypoint_plan_t * plan,
+ double * optPlanLengthM,
+ double * optTangentChangeRad,
+ double * optDirectionChangeRad)
{
- deprecated_msgs::Pose2DFloat32 pp;
- pp.x = px;
- pp.y = py;
- pp.th = pth;
- addWaypoint(pp);
- }
-
-
- void convertPlan(SBPLEnvironment const & environment,
- raw_sbpl_plan_t const & raw,
- waypoint_plan_t * plan,
- double * optPlanLengthM,
- double * optTangentChangeRad,
- double * optDirectionChangeRad)
- {
- PlanConverter pc(plan);
+ PlanConverter pc(plan, dr, dtheta);
for (raw_sbpl_plan_t::const_iterator it(raw.begin()); it != raw.end(); ++it)
- pc.addWaypoint(environment.GetPoseFromState(*it));
+ pc.addWaypoint(waypoint_s(environment.GetPoseFromState(*it), dr, dtheta));
if (optPlanLengthM)
*optPlanLengthM = pc.plan_length;
if (optTangentChangeRad)
@@ -105,16 +149,17 @@
}
- void convertPlan(IndexTransform const & itransform,
- float const * path_x,
- float const * path_y,
- int path_len,
- waypoint_plan_t * plan,
- double * optPlanLengthM,
- double * optTangentChangeRad,
- double * optDirectionChangeRad)
+ void PlanConverter::
+ convertXY(IndexTransform const & itransform,
+ float const * path_x,
+ float const * path_y,
+ int path_len,
+ double dr,
+ waypoint_plan_t * plan,
+ double * optPlanLengthM,
+ double * optTangentChangeRad)
{
- PlanConverter pc(plan);
+ PlanConverter pc(plan, dr, M_PI);
ssize_t prev_ix(0), prev_iy(0);
for (int ii(0); ii < path_len; ++ii, ++path_x, ++path_y) {
ssize_t const ix(static_cast<ssize_t>(rint(*path_x)));
@@ -131,21 +176,20 @@
*optPlanLengthM = pc.plan_length;
if (optTangentChangeRad)
*optTangentChangeRad = pc.tangent_change;
- if (optDirectionChangeRad)
- *optDirectionChangeRad = pc.direction_change;
}
- void convertPlanInterpolate(IndexTransform const & itransform,
- float const * path_x,
- float const * path_y,
- int path_len,
- waypoint_plan_t * plan,
- double * optPlanLengthM,
- double * optTangentChangeRad,
- double * optDirectionChangeRad)
+ void PlanConverter::
+ convertXYInterpolate(IndexTransform const & itransform,
+ float const * path_x,
+ float const * path_y,
+ int path_len,
+ double dr,
+ waypoint_plan_t * plan,
+ double * optPlanLengthM,
+ double * optTangentChangeRad)
{
- PlanConverter pc(plan);
+ PlanConverter pc(plan, dr, M_PI);
double const resolution(itransform.getResolution());
for (int ii(0); ii < path_len; ++ii, ++path_x, ++path_y) {
ssize_t ix(static_cast<ssize_t>(rint(*path_x)));
@@ -160,8 +204,6 @@
*optPlanLengthM = pc.plan_length;
if (optTangentChangeRad)
*optTangentChangeRad = pc.tangent_change;
- if (optDirectionChangeRad)
- *optDirectionChangeRad = pc.direction_change;
}
}
Modified: pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp
===================================================================
--- pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp 2009-04-08 21:59:46 UTC (rev 13623)
+++ pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp 2009-04-08 22:00:24 UTC (rev 13624)
@@ -107,6 +107,8 @@
virtual deprecated_msgs::Pose2DFloat32 GetPoseFromState(int stateID) const throw(invalid_state);
virtual int GetStateFromPose(deprecated_msgs::Pose2DFloat32 const & pose) const;
+ virtual double GetAngularTolerance() const;
+
protected:
virtual bool DoUpdateCost(int ix, int iy, unsigned char newcost);
virtual StateChangeQuery const * createStateChangeQuery(std::vector<nav2dcell_t> const & changedcellsV) const;
@@ -131,10 +133,8 @@
SBPLEnvironmentDSI(boost::shared_ptr<CostmapAccessor const> cm,
boost::shared_ptr<IndexTransform const> it,
DSIsubclass * env,
- footprint_t const & footprint,
- double nominalvel_mpersecs,
- double timetoturn45degsinplace_secs)
- : SBPLEnvironment(cm, it), env_(env) {}
+ double angular_tolerance)
+ : SBPLEnvironment(cm, it), env_(env), angular_tolerance_(angular_tolerance) {}
virtual ~SBPLEnvironmentDSI()
{ delete env_; }
@@ -196,8 +196,12 @@
return env_->GetStateFromCoord(ix, iy, ith);
}
+ virtual double GetAngularTolerance() const
+ {
+ return angular_tolerance_;
+ }
+
protected:
-
/** \todo The check IsWithinMapCell() should be done inside
EnvironmentNAV3DKIN::UpdateCost() or whatever subclass we are
dealing with here. */
@@ -217,6 +221,8 @@
conceivable change the underlying DSIsubclass, which we
don't care about here. */
mutable DSIsubclass * env_;
+
+ double angular_tolerance_;
};
}
@@ -377,9 +383,8 @@
env->UpdateCost(ix, iy, cost);
}
- return new SBPLEnvironmentDSI<EnvironmentNAV3DKIN>(cm, it, env, footprint,
- nominalvel_mpersecs,
- timetoturn45degsinplace_secs);
+ static double const dtheta(M_PI / NAV3DKIN_THETADIRS);
+ return new SBPLEnvironmentDSI<EnvironmentNAV3DKIN>(cm, it, env, dtheta);
}
@@ -465,9 +470,8 @@
env->UpdateCost(ix, iy, cost);
}
- return new SBPLEnvironmentDSI<EnvironmentNAVXYTHETALAT>(cm, it, env, footprint,
- nominalvel_mpersecs,
- timetoturn45degsinplace_secs);
+ static double const dtheta(M_PI / NAVXYTHETALAT_THETADIRS);
+ return new SBPLEnvironmentDSI<EnvironmentNAVXYTHETALAT>(cm, it, env, dtheta);
}
@@ -488,13 +492,13 @@
EnvironmentNAVXYTHETADOOR * env(new EnvironmentNAVXYTHETADOOR());
if ( ! env->SetEnvParameter("cost_inscribed", cm->getInscribedCost())) {
delete env;
- throw runtime_error("mpglue::SBPLEnvironment::createXYThetaLattice():"
...
[truncated message content] |
|
From: <sf...@us...> - 2009-04-10 21:52:18
|
Revision: 13749
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13749&view=rev
Author: sfkwc
Date: 2009-04-10 21:52:13 +0000 (Fri, 10 Apr 2009)
Log Message:
-----------
cleanup of rospy API usage
Modified Paths:
--------------
pkg/trunk/demos/2dnav_pr2/pose_listen.py
pkg/trunk/demos/plug_in/fake_vision.py
pkg/trunk/demos/plug_in/outlet_pose_filter.py
pkg/trunk/demos/plug_in/plug_in.py
pkg/trunk/demos/tabletop_manipulation/scripts/executive.py
pkg/trunk/demos/tabletop_manipulation/scripts/tiltscan.py
pkg/trunk/drivers/robot/pr2/ocean_battery_driver/scripts/monitor
pkg/trunk/hardware_test/runtime_monitor/scripts/runtime_test
pkg/trunk/mechanism/mechanism_control/scripts/spawner.py
pkg/trunk/prcore/pytf/test_pytf.py
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_prototype1/motor_monitor.py
pkg/trunk/util/twitterros/nodes/rostwitter.py
pkg/trunk/vision/cv_mech_turk/src/snapper.py
pkg/trunk/vision/people/src/stereo_face_feature_tracker.py
pkg/trunk/vision/visual_odometry/nodes/camview_py.py
pkg/trunk/vision/visual_odometry/nodes/corrector.py
pkg/trunk/vision/visual_odometry/nodes/diff.py
pkg/trunk/vision/visual_odometry/nodes/vo.py
pkg/trunk/vision/vslam/nodes/roadmap.py
pkg/trunk/vision/vslam/nodes/watchmap.py
pkg/trunk/visualization/pr2_dashboard/src/pr2_dashboard/reset.py
Modified: pkg/trunk/demos/2dnav_pr2/pose_listen.py
===================================================================
--- pkg/trunk/demos/2dnav_pr2/pose_listen.py 2009-04-10 21:45:46 UTC (rev 13748)
+++ pkg/trunk/demos/2dnav_pr2/pose_listen.py 2009-04-10 21:52:13 UTC (rev 13749)
@@ -14,8 +14,8 @@
print "localized: %d.%d %.2f, %.2f, %.2f" % (data.header.stamp.secs, data.header.stamp.nsecs, data.pos.x, data.pos.y, data.pos.th)
def pose_listen():
- rospy.TopicSub("/odom", RobotBase2DOdom, odom_callback)
- rospy.TopicSub("/localizedpose", RobotBase2DOdom, localized_callback)
+ rospy.Subscriber("/odom", RobotBase2DOdom, odom_callback)
+ rospy.Subscriber("/localizedpose", RobotBase2DOdom, localized_callback)
rospy.init_node(NAME, anonymous=True)
rospy.spin()
Modified: pkg/trunk/demos/plug_in/fake_vision.py
===================================================================
--- pkg/trunk/demos/plug_in/fake_vision.py 2009-04-10 21:45:46 UTC (rev 13748)
+++ pkg/trunk/demos/plug_in/fake_vision.py 2009-04-10 21:52:13 UTC (rev 13749)
@@ -59,7 +59,7 @@
mechanism_state = Tracker('/mechanism_state', MechanismState)
def last_time():
- return rospy.rostime.get_rostime()
+ return rospy.get_rostime()
global mechanism_state
if mechanism_state.msg:
return mechanism_state.msg.header.stamp
Modified: pkg/trunk/demos/plug_in/outlet_pose_filter.py
===================================================================
--- pkg/trunk/demos/plug_in/outlet_pose_filter.py 2009-04-10 21:45:46 UTC (rev 13748)
+++ pkg/trunk/demos/plug_in/outlet_pose_filter.py 2009-04-10 21:52:13 UTC (rev 13749)
@@ -76,7 +76,7 @@
tf_msg.transforms = [pose_to_transform_stamped(outlet_msg)]
tf_msg.transforms[0].header.seq = seq
tf_msg.transforms[0].header.frame_id = 'outlet_pose'
- tf_msg.transforms[0].header.stamp = rospy.rostime.get_rostime()
+ tf_msg.transforms[0].header.stamp = rospy.get_rostime()
tf_pub.publish(tf_msg)
seq += 1
outlet_msg = track_outlet_pose.msg
Modified: pkg/trunk/demos/plug_in/plug_in.py
===================================================================
--- pkg/trunk/demos/plug_in/plug_in.py 2009-04-10 21:45:46 UTC (rev 13748)
+++ pkg/trunk/demos/plug_in/plug_in.py 2009-04-10 21:52:13 UTC (rev 13749)
@@ -105,7 +105,7 @@
if cnt % 3 == 0:
try:
set_tool_frame(track_plug_pose.msg)
- except rospy.service.ServiceException, ex:
+ except rospy.ServiceException, ex:
print "set_tool frame service went down. Reconnecting..."
rospy.wait_for_service("/%s/set_tool_frame" % CONTROLLER)
if rospy.is_shutdown(): return
Modified: pkg/trunk/demos/tabletop_manipulation/scripts/executive.py
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/scripts/executive.py 2009-04-10 21:45:46 UTC (rev 13748)
+++ pkg/trunk/demos/tabletop_manipulation/scripts/executive.py 2009-04-10 21:52:13 UTC (rev 13749)
@@ -207,10 +207,10 @@
# Set the collision_map_buffer's window size accordingly, to remember a
# fixed time window scans.
- rospy.client.set_param('collision_map_buffer/window_size',
- int(self.laser_buffer_time / (period / 2.0)))
+ rospy.set_param('collision_map_buffer/window_size',
+ int(self.laser_buffer_time / (period / 2.0)))
return resp
-
+
def getTable(self):
print 'Calling FindTable'
s = rospy.ServiceProxy('table_object_detector', FindTable)
@@ -418,7 +418,7 @@
sys.exit(0)
def doCycle(self):
- curr_time = rospy.rostime.get_time()
+ curr_time = rospy.get_time()
#make sure that all adapters have legal states
if self.legalStates():
if self.state == 'idle':
Modified: pkg/trunk/demos/tabletop_manipulation/scripts/tiltscan.py
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/scripts/tiltscan.py 2009-04-10 21:45:46 UTC (rev 13748)
+++ pkg/trunk/demos/tabletop_manipulation/scripts/tiltscan.py 2009-04-10 21:52:13 UTC (rev 13749)
@@ -60,8 +60,8 @@
if resp:
# Set the collision_map_buffer's window size accordingly, to remember a
# fixed time window scans.
- rospy.client.set_param('collision_map_buffer/window_size',
- int(self.laser_buffer_time / (period / 2.0)))
+ rospy.set_param('collision_map_buffer/window_size',
+ int(self.laser_buffer_time / (period / 2.0)))
print '[TiltScan] Waiting for a full sweep at the new speed...'
# Wait until the laser's swept through most of its period at the new
Modified: pkg/trunk/drivers/robot/pr2/ocean_battery_driver/scripts/monitor
===================================================================
--- pkg/trunk/drivers/robot/pr2/ocean_battery_driver/scripts/monitor 2009-04-10 21:45:46 UTC (rev 13748)
+++ pkg/trunk/drivers/robot/pr2/ocean_battery_driver/scripts/monitor 2009-04-10 21:52:13 UTC (rev 13749)
@@ -305,9 +305,9 @@
rospy.logfatal("Batteries have %.2f minutes estimated remaining"%average_time_remaining)
elif average_time_remaining < 5.0:
rospy.logerr("Batteries have %.2f minutes estimated remaining"%average_time_remaining)
- elif average_time_remaining < 30.0 and self.last_battery_warn + 6 < rospy.rostime.get_time():
+ elif average_time_remaining < 30.0 and self.last_battery_warn + 6 < rospy.get_time():
rospy.logwarn("Batteries have %.2f minutes estimated remaining"%average_time_remaining)
- self.last_battery_warn = rospy.rostime.get_time()
+ self.last_battery_warn = rospy.get_time()
#print "Sent Energy: %.0f (J) of %.0f (J) Power: %.2f (W)\n"%(energy, full_energy, power)
self.battery_state_pub.publish(BatteryState(None, energy, full_energy, power))
Modified: pkg/trunk/hardware_test/runtime_monitor/scripts/runtime_test
===================================================================
--- pkg/trunk/hardware_test/runtime_monitor/scripts/runtime_test 2009-04-10 21:45:46 UTC (rev 13748)
+++ pkg/trunk/hardware_test/runtime_monitor/scripts/runtime_test 2009-04-10 21:52:13 UTC (rev 13749)
@@ -45,7 +45,7 @@
latest_messages = {}
test_name = 'uninitialized'
package = 'uninitialized'
-last_runtime = rospy.rostime.get_time()
+last_runtime = rospy.get_time()
def status_to_map(status):
str_map = {}
@@ -76,11 +76,11 @@
def execute_test(args):
global last_runtime
- time_step = rospy.rostime.get_time() - last_runtime
+ time_step = rospy.get_time() - last_runtime
if 1.0 /time_step > options.max_freq:
return
else:
- last_runtime = rospy.rostime.get_time()
+ last_runtime = rospy.get_time()
test_impl, params = args
results = analyze(package, test_impl, params)
@@ -116,7 +116,7 @@
rospy.Subscriber("/diagnostics", DiagnosticMessage, callback, (test_impl, params))
while not rospy.is_shutdown():
- if rospy.rostime.get_time() - last_runtime > 1/options.min_freq:
+ if rospy.get_time() - last_runtime > 1/options.min_freq:
execute_test((test_impl, params))
time.sleep(0.5/options.min_freq)
Modified: pkg/trunk/mechanism/mechanism_control/scripts/spawner.py
===================================================================
--- pkg/trunk/mechanism/mechanism_control/scripts/spawner.py 2009-04-10 21:45:46 UTC (rev 13748)
+++ pkg/trunk/mechanism/mechanism_control/scripts/spawner.py 2009-04-10 21:52:13 UTC (rev 13749)
@@ -59,7 +59,7 @@
kill_controller(name)
rospy.logout("Succeeded in killing %s" % name)
break
- except rospy.service.ServiceException:
+ except rospy.ServiceException:
raise
rospy.logerr("ServiceException while killing %s" % name)
# We're shutdown. Now invoke rospy's handler for full cleanup.
Modified: pkg/trunk/prcore/pytf/test_pytf.py
===================================================================
--- pkg/trunk/prcore/pytf/test_pytf.py 2009-04-10 21:45:46 UTC (rev 13748)
+++ pkg/trunk/prcore/pytf/test_pytf.py 2009-04-10 21:52:13 UTC (rev 13749)
@@ -15,7 +15,7 @@
time.sleep(0.5)
for i in xrange(1,10):
print "Looping"
- current_time = rospy.rostime.get_rostime()
+ current_time = rospy.get_rostime()
if not t.can_transform("frame", "parent", current_time):
print "waiting to try again"
time.sleep(0.1)
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_prototype1/motor_monitor.py
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_prototype1/motor_monitor.py 2009-04-10 21:45:46 UTC (rev 13748)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_prototype1/motor_monitor.py 2009-04-10 21:52:13 UTC (rev 13749)
@@ -20,7 +20,7 @@
print "Resetting motors"
def listener_with_user_data():
- rospy.TopicSub("/diagnostics", DiagnosticMessage, callback)
+ rospy.Subscriber("/diagnostics", DiagnosticMessage, callback)
rospy.init_node(NAME, anonymous=True)
rospy.spin()
Modified: pkg/trunk/util/twitterros/nodes/rostwitter.py
===================================================================
--- pkg/trunk/util/twitterros/nodes/rostwitter.py 2009-04-10 21:45:46 UTC (rev 13748)
+++ pkg/trunk/util/twitterros/nodes/rostwitter.py 2009-04-10 21:52:13 UTC (rev 13749)
@@ -57,7 +57,7 @@
if twitPass == None:
twitPass = passes[twitId]
self.api = twitter.Api(username=twitId, password=twitPass)
- rospy.TopicSub('/twitter', std_msgs.msg.String, self.handle_message)
+ rospy.Subscriber('/twitter', std_msgs.msg.String, self.handle_message)
def handle_message(self, msg):
status = self.api.PostUpdate(msg.data[:140])
Modified: pkg/trunk/vision/cv_mech_turk/src/snapper.py
===================================================================
--- pkg/trunk/vision/cv_mech_turk/src/snapper.py 2009-04-10 21:45:46 UTC (rev 13748)
+++ pkg/trunk/vision/cv_mech_turk/src/snapper.py 2009-04-10 21:52:13 UTC (rev 13749)
@@ -65,7 +65,7 @@
def main(args):
s = Snapper()
rospy.init_node('snapper')
- rospy.TopicSub('/stereo/left/image_rect_color', image_msgs.msg.Image, s.handle_raw_stereo, queue_size=2, buff_size=7000000)
+ rospy.Subscriber('/stereo/left/image_rect_color', image_msgs.msg.Image, s.handle_raw_stereo, queue_size=2, buff_size=7000000)
rospy.spin()
if __name__ == '__main__':
Modified: pkg/trunk/vision/people/src/stereo_face_feature_tracker.py
===================================================================
--- pkg/trunk/vision/people/src/stereo_face_feature_tracker.py 2009-04-10 21:45:46 UTC (rev 13748)
+++ pkg/trunk/vision/people/src/stereo_face_feature_tracker.py 2009-04-10 21:52:13 UTC (rev 13749)
@@ -727,9 +727,9 @@
#people_tracker.pub = rospy.Publisher('head_controller/track_point',PointStamped)
people_tracker.pub = rospy.Publisher('/stereo_face_feature_tracker/position_measurement',PositionMeasurement)
rospy.init_node('stereo_face_feature_tracker', anonymous=True)
- #rospy.TopicSub('/head_controller/track_point',PointStamped,people_tracker.point_stamped)
- rospy.TopicSub('/videre/images',ImageArray,people_tracker.frame)
- rospy.TopicSub('/videre/cal_params',String,people_tracker.params)
+ #rospy.Subscriber('/head_controller/track_point',PointStamped,people_tracker.point_stamped)
+ rospy.Subscriber('/videre/images',ImageArray,people_tracker.frame)
+ rospy.Subscriber('/videre/cal_params',String,people_tracker.params)
rospy.spin()
Modified: pkg/trunk/vision/visual_odometry/nodes/camview_py.py
===================================================================
--- pkg/trunk/vision/visual_odometry/nodes/camview_py.py 2009-04-10 21:45:46 UTC (rev 13748)
+++ pkg/trunk/vision/visual_odometry/nodes/camview_py.py 2009-04-10 21:52:13 UTC (rev 13749)
@@ -74,9 +74,9 @@
self.mode = mode
self.library = library
- rospy.TopicSub('/videre/images', ImageArray, self.display_array)
- rospy.TopicSub('/videre/cal_params', String, self.display_params)
- rospy.TopicSub('/vo/tmo', Pose44, self.handle_corrections)
+ rospy.Subscriber('/videre/images', ImageArray, self.display_array)
+ rospy.Subscriber('/videre/cal_params', String, self.display_params)
+ rospy.Subscriber('/vo/tmo', Pose44, self.handle_corrections)
self.viz_pub = rospy.Publisher("/overlay", Lines)
self.vo_key_pub = rospy.Publisher("/vo/key", Frame)
Modified: pkg/trunk/vision/visual_odometry/nodes/corrector.py
===================================================================
--- pkg/trunk/vision/visual_odometry/nodes/corrector.py 2009-04-10 21:45:46 UTC (rev 13748)
+++ pkg/trunk/vision/visual_odometry/nodes/corrector.py 2009-04-10 21:52:13 UTC (rev 13749)
@@ -72,7 +72,7 @@
self.vo = vo
self.library = library
- rospy.TopicSub('/vo/key', Frame, self.incoming_frame)
+ rospy.Subscriber('/vo/key', Frame, self.incoming_frame)
self.pub_tmo = rospy.Publisher("/vo/tmo", Pose44)
self.frameq = Queue()
Modified: pkg/trunk/vision/visual_odometry/nodes/diff.py
===================================================================
--- pkg/trunk/vision/visual_odometry/nodes/diff.py 2009-04-10 21:45:46 UTC (rev 13748)
+++ pkg/trunk/vision/visual_odometry/nodes/diff.py 2009-04-10 21:52:13 UTC (rev 13749)
@@ -49,8 +49,8 @@
def __init__(self):
self.prev_a = 0
self.prev_b = 0
- rospy.TopicSub('/stereo/raw_stereo', image_msgs.msg.RawStereo, self.handle_a, queue_size=2, buff_size=7000000)
- rospy.TopicSub('/vo', robot_msgs.msg.VOPose, self.handle_b, queue_size=2, buff_size=7000000)
+ rospy.Subscriber('/stereo/raw_stereo', image_msgs.msg.RawStereo, self.handle_a, queue_size=2, buff_size=7000000)
+ rospy.Subscriber('/vo', robot_msgs.msg.VOPose, self.handle_b, queue_size=2, buff_size=7000000)
def handle_a(self, msg):
self.prev_a = msg.header.stamp.to_seconds()
Modified: pkg/trunk/vision/visual_odometry/nodes/vo.py
===================================================================
--- pkg/trunk/vision/visual_odometry/nodes/vo.py 2009-04-10 21:45:46 UTC (rev 13748)
+++ pkg/trunk/vision/visual_odometry/nodes/vo.py 2009-04-10 21:52:13 UTC (rev 13749)
@@ -73,7 +73,7 @@
class VO:
def __init__(self):
- rospy.TopicSub('/stereo/raw_stereo', image_msgs.msg.RawStereo, self.handle_raw_stereo, queue_size=2, buff_size=7000000)
+ rospy.Subscriber('/stereo/raw_stereo', image_msgs.msg.RawStereo, self.handle_raw_stereo, queue_size=2, buff_size=7000000)
self.pub_vo = rospy.Publisher("/vo", robot_msgs.msg.VOPose)
Modified: pkg/trunk/vision/vslam/nodes/roadmap.py
===================================================================
--- pkg/trunk/vision/vslam/nodes/roadmap.py 2009-04-10 21:45:46 UTC (rev 13748)
+++ pkg/trunk/vision/vslam/nodes/roadmap.py 2009-04-10 21:52:13 UTC (rev 13749)
@@ -74,7 +74,7 @@
time.sleep(1)
self.send_map()
- rospy.TopicSub('/stereo/raw_stereo', image_msgs.msg.RawStereo, self.handle_raw_stereo, queue_size=2, buff_size=7000000)
+ rospy.Subscriber('/stereo/raw_stereo', image_msgs.msg.RawStereo, self.handle_raw_stereo, queue_size=2, buff_size=7000000)
def send_map(self):
p = vslam.msg.Roadmap()
@@ -112,7 +112,7 @@
def __init__(self, args):
rospy.init_node('roadmap_server')
self.pub = rospy.Publisher("/roadmap", vslam.msg.Roadmap)
- rospy.TopicSub('/localizedpose', deprecated_msgs.msg.RobotBase2DOdom, self.handle_localizedpose)
+ rospy.Subscriber('/localizedpose', deprecated_msgs.msg.RobotBase2DOdom, self.handle_localizedpose)
self.nodes = []
def handle_localizedpose(self, msg):
Modified: pkg/trunk/vision/vslam/nodes/watchmap.py
===================================================================
--- pkg/trunk/vision/vslam/nodes/watchmap.py 2009-04-10 21:45:46 UTC (rev 13748)
+++ pkg/trunk/vision/vslam/nodes/watchmap.py 2009-04-10 21:52:13 UTC (rev 13749)
@@ -69,7 +69,7 @@
def main(args):
rospy.init_node('watchmap')
- rospy.TopicSub('/roadmap', vslam.msg.Roadmap, handle_roadmap)
+ rospy.Subscriber('/roadmap', vslam.msg.Roadmap, handle_roadmap)
rospy.spin()
if __name__ == '__main__':
Modified: pkg/trunk/visualization/pr2_dashboard/src/pr2_dashboard/reset.py
===================================================================
--- pkg/trunk/visualization/pr2_dashboard/src/pr2_dashboard/reset.py 2009-04-10 21:45:46 UTC (rev 13748)
+++ pkg/trunk/visualization/pr2_dashboard/src/pr2_dashboard/reset.py 2009-04-10 21:52:13 UTC (rev 13749)
@@ -36,7 +36,6 @@
import wx
from wx import xrc
import rospy
-import rospy.service
import std_srvs.srv
PKG='pr2_dashboard'
@@ -62,6 +61,6 @@
try:
reset()
- except rospy.service.ServiceException:
+ except rospy.ServiceException:
rospy.logerr('Failed to reset the motors: service call failed')
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mc...@us...> - 2009-04-14 14:11:14
|
Revision: 13808
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13808&view=rev
Author: mcgann
Date: 2009-04-14 14:11:06 +0000 (Tue, 14 Apr 2009)
Log Message:
-----------
New action API
Modified Paths:
--------------
pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_detect_door.h
pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_detect_handle.h
pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_grasp_door.h
pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_open_door.h
pkg/trunk/highlevel/doors/doors_core/src/action_detect_door.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_detect_handle.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_grasp_door.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_open_door.cpp
pkg/trunk/highlevel/doors/doors_core/test/trigger_doors_detector.cpp
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ros_action_adapter.h
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/stub_ros_container.cc
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/doors/stubs.launch
pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_fine_outlet_detect.h
pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_move_and_grasp_plug.h
pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_stow_plug.h
pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_untuck_arms.h
pkg/trunk/highlevel/plugs/plugs_core/src/action_fine_outlet_detect.cpp
pkg/trunk/highlevel/plugs/plugs_core/src/action_move_and_grasp_plug.cpp
pkg/trunk/highlevel/plugs/plugs_core/src/action_stow_plug.cpp
pkg/trunk/highlevel/plugs/plugs_core/src/action_untuck_arms.cpp
pkg/trunk/highlevel/robot_actions/include/robot_actions/action.h
pkg/trunk/highlevel/robot_actions/include/robot_actions/message_adapter.h
pkg/trunk/highlevel/robot_actions/src/action_runner.cpp
pkg/trunk/highlevel/robot_actions/test/utest.cc
pkg/trunk/highlevel/safety/safety_core/include/safety_core/action_detect_plug_on_base.h
pkg/trunk/highlevel/safety/safety_core/include/safety_core/action_tuck_arms.h
pkg/trunk/highlevel/safety/safety_core/src/action_detect_plug_on_base.cpp
pkg/trunk/highlevel/safety/safety_core/src/action_runner.cpp
pkg/trunk/highlevel/safety/safety_core/src/action_tuck_arms.cpp
pkg/trunk/mechanism/mechanism_control/src/action_mechanism_control.cpp
Modified: pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_detect_door.h
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_detect_door.h 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_detect_door.h 2009-04-14 14:11:06 UTC (rev 13808)
@@ -50,18 +50,15 @@
{
public:
DetectDoorAction(ros::Node& node);
+
~DetectDoorAction();
- virtual void handleActivate(const robot_msgs::Door& door);
- virtual void handlePreempt();
+ virtual robot_actions::ResultStatus execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback);
robot_msgs::Door tmp_result_;
-
private:
bool laserDetection(const robot_msgs::Door& door_in, robot_msgs::Door& door_out);
-
- bool request_preempt_;
tf::TransformListener tf_;
};
Modified: pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_detect_handle.h
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_detect_handle.h 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_detect_handle.h 2009-04-14 14:11:06 UTC (rev 13808)
@@ -52,16 +52,14 @@
DetectHandleAction(ros::Node& node);
~DetectHandleAction();
- virtual void handleActivate(const robot_msgs::Door& door);
- virtual void handlePreempt();
+ virtual robot_actions::ResultStatus execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback);
private:
bool laserDetection(const robot_msgs::Door& door_in, robot_msgs::Door& door_out);
bool cameraDetection(const robot_msgs::Door& door, robot_msgs::Door& door_out);
ros::Node& node_;
- bool request_preempt_;
tf::TransformListener tf_;
};
Modified: pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_grasp_door.h
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_grasp_door.h 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_grasp_door.h 2009-04-14 14:11:06 UTC (rev 13808)
@@ -58,8 +58,7 @@
GraspDoorAction(ros::Node& node);
~GraspDoorAction();
- virtual void handleActivate(const robot_msgs::Door& door);
- virtual void handlePreempt();
+ virtual robot_actions::ResultStatus execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback);
private:
@@ -67,8 +66,6 @@
double getDoorAngle(const robot_msgs::Door& door);
ros::Node& node_;
-
- bool request_preempt_;
tf::TransformListener tf_;
std_srvs::Empty::Request req_empty;
Modified: pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_open_door.h
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_open_door.h 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_open_door.h 2009-04-14 14:11:06 UTC (rev 13808)
@@ -56,15 +56,11 @@
OpenDoorAction();
~OpenDoorAction();
- virtual void handleActivate(const robot_msgs::Door& door);
- virtual void handlePreempt();
+ virtual robot_actions::ResultStatus execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback);
-
private:
ros::Node* node_;
- bool request_preempt_;
-
robot_msgs::TaskFrameFormalism tff_stop_, tff_handle_, tff_door_;
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_detect_door.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_detect_door.cpp 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_detect_door.cpp 2009-04-14 14:11:06 UTC (rev 13808)
@@ -33,7 +33,6 @@
*********************************************************************/
#include <door_handle_detector/DoorsDetectorCloud.h>
-#include <door_handle_detector/door_functions.h>
#include <point_cloud_assembler/BuildCloudAngle.h>
#include "doors_core/action_detect_door.h"
@@ -57,38 +56,33 @@
DetectDoorAction::~DetectDoorAction(){};
-
-void DetectDoorAction::handleActivate(const robot_msgs::Door& door)
+robot_actions::ResultStatus DetectDoorAction::execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback)
{
- ROS_INFO("DetectDoorAction: handle activate");
- request_preempt_ = false;
- notifyActivated();
+ ROS_INFO("DetectDoorAction: execute");
+ feedback = goal;
robot_msgs::Door result_laser;
- if (!laserDetection(door, result_laser)){
- if (request_preempt_){
+ if (!laserDetection(goal, result_laser)){
+ if (isPreemptRequested()){
ROS_ERROR("DetectDoorAction: Preempted");
- notifyPreempted(door);
+ return robot_actions::PREEMPTED;
}
else{
ROS_ERROR("DetectDoorAction: Aborted");
- notifyAborted(door);
+ return robot_actions::ABORTED;
}
}
- else{
- ROS_INFO("DetectDoorAction: Succeeded");
- notifySucceeded(result_laser);
- tmp_result_ = result_laser;
- }
+
+ ROS_INFO("DetectDoorAction: Succeeded");
+ feedback = result_laser;
+ return robot_actions::SUCCESS;
}
-
-
-
bool DetectDoorAction::laserDetection(const robot_msgs::Door& door_in, robot_msgs::Door& door_out)
{
// check where robot is relative to door
- if (request_preempt_) return false;
+ if (isPreemptRequested()) return false;
+
if (!tf_.canTransform("base_footprint", "laser_tilt_link", ros::Time(), ros::Duration().fromSec(5.0))){
ROS_ERROR("DetectDoorAction: error getting transform from 'base_footprint' to 'laser_tilt_link'");
return false;
@@ -100,11 +94,14 @@
(door_in.frame_p1.y+door_in.frame_p2.y)/2.0,
(door_in.frame_p1.z+door_in.frame_p2.z)/2.0),
ros::Time(), door_in.header.frame_id);
- if (request_preempt_) return false;
+
+ if (isPreemptRequested()) return false;
+
if (!tf_.canTransform("base_footprint", doorpoint.frame_id_, ros::Time(), ros::Duration().fromSec(5.0))){
ROS_ERROR("DetectDoorAction: error getting transform from 'base_footprint' to '%s'", doorpoint.frame_id_.c_str());
return false;
}
+
tf_.transformPoint("base_footprint", doorpoint, doorpoint);
double dist = doorpoint[0];
double door_bottom = -0.5;
@@ -112,7 +109,8 @@
ROS_INFO("DetectDoorAction: tilt laser is at height %f, and door at distance %f", laser_height, dist);
// gets a point cloud from the point_cloud_srv
- if (request_preempt_) return false;
+ if (isPreemptRequested()) return false;
+
ROS_INFO("DetectDoorAction: get a point cloud from the door");
point_cloud_assembler::BuildCloudAngle::Request req_pointcloud;
point_cloud_assembler::BuildCloudAngle::Response res_pointcloud;
@@ -125,26 +123,20 @@
}
// detect door
- if (request_preempt_) return false;
+ if (isPreemptRequested()) return false;
+
ROS_INFO("DetectDoorAction: detect the door");
door_handle_detector::DoorsDetectorCloud::Request req_doordetect;
door_handle_detector::DoorsDetectorCloud::Response res_doordetect;
req_doordetect.door = door_in;
req_doordetect.cloud = res_pointcloud.cloud;
+
if (!ros::service::call("doors_detector_cloud", req_doordetect, res_doordetect)){
ROS_ERROR("DetectDoorAction: failed to detect a door");
return false;
}
- cout << "end door detection action " << res_doordetect.doors[0] << endl;
door_out = res_doordetect.doors[0];
return true;
}
-
-
-void DetectDoorAction::handlePreempt()
-{
- request_preempt_ = true;
-};
-
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_detect_handle.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_detect_handle.cpp 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_detect_handle.cpp 2009-04-14 14:11:06 UTC (rev 13808)
@@ -64,59 +64,51 @@
-void DetectHandleAction::handleActivate(const robot_msgs::Door& door)
+robot_actions::ResultStatus DetectHandleAction::execute(const robot_msgs::Door& door, robot_msgs::Door& result)
{
- ROS_INFO("DetectHandleAction: handle activate");
- request_preempt_ = false;
- notifyActivated();
+ ROS_INFO("DetectHandleAction: execute");
cout << "begin handle detection action " << door << endl;
-
// laser detection
- robot_msgs::Door result_laser, result_camera, result;
+ robot_msgs::Door result_laser, result_camera;
if (!laserDetection(door, result_laser)){
- if (request_preempt_){
+ if (isPreemptRequested()){
ROS_INFO("DetectHandleAction: Preempted");
- notifyPreempted(door);
- return;
+ return robot_actions::PREEMPTED;
}
else{
ROS_ERROR("DetectHandleAction: Aborted laser detection");
- notifyAborted(door);
- return;
+ return robot_actions::ABORTED;
}
}
if (!door_handle_detector::transformTo(tf_, fixed_frame, result_laser, result_laser)){
ROS_ERROR ("DetectHandleAction: Could not transform laser door message from frame %s to frame %s.",
result_laser.header.frame_id.c_str (), fixed_frame.c_str ());
- notifyAborted(door);
- return;
+ return robot_actions::ABORTED;
}
cout << "result laser " << result_laser << endl;
ROS_INFO("DetectHandleAction: detected handle position transformed to '%s'", fixed_frame.c_str());
-
// camera detection
if (!cameraDetection(door, result_camera)){
- if (request_preempt_){
+ if (isPreemptRequested()){
ROS_INFO("DetectHandleAction: Preempted");
- notifyPreempted(door);
- return;
+ return robot_actions::PREEMPTED;
}
else{
ROS_ERROR("DetectHandleAction: Aborted camera detection");
- notifyAborted(door);
- return;
+ return robot_actions::ABORTED;
}
}
+
if (!door_handle_detector::transformTo(tf_, fixed_frame, result_camera, result_camera)){
ROS_ERROR ("DetectHandleAction: Could not transform camera door message from frame %s to frame %s.",
result_camera.header.frame_id.c_str (), fixed_frame.c_str ());
- notifyAborted(door);
- return;
+ return robot_actions::ABORTED;
}
+
cout << "result camera " << result_camera << endl;
ROS_INFO("DetectHandleAction: detected handle position transformed to '%s'", fixed_frame.c_str());
@@ -130,8 +122,7 @@
if (error > 0.1){
ROS_ERROR("DetectHandleAction: Aborted because error between laser and camera result is too big");
- notifyAborted(door);
- return;
+ return robot_actions::ABORTED;
}
// store handle position
@@ -149,22 +140,22 @@
cout << "result laser+camera " << result << endl;
ROS_INFO("DetectHandleAction: Succeeded");
- notifySucceeded(result);
+ return robot_actions::SUCCESS;
}
-
bool DetectHandleAction::laserDetection(const robot_msgs::Door& door_in,
robot_msgs::Door& door_out)
{
// check where robot is relative to the door
- if (request_preempt_) return false;
+ if (isPreemptRequested()) return false;
if (!tf_.canTransform("base_footprint", "laser_tilt_link", ros::Time(), ros::Duration().fromSec(5.0))){
ROS_ERROR("DetectHandleAction: could not get transform from 'base_footprint' to 'laser_tilt_link'");
return false;
}
tf::Stamped<tf::Transform> tilt_stage;
tf_.lookupTransform("base_footprint", "laser_tilt_link", ros::Time(), tilt_stage);
+ ROS_INFO("DetectHandleAction: handle activate");
double laser_height = tilt_stage.getOrigin()[2];
tf::Stamped<tf::Vector3> handlepoint(tf::Vector3((door_in.door_p1.x+door_in.door_p2.x)/2.0,
(door_in.door_p1.y+door_in.door_p2.y)/2.0,
@@ -181,7 +172,7 @@
ROS_INFO("DetectHandleAction: tilt laser is at height %f, and door at distance %f", laser_height, dist);
// gets a point cloud from the point_cloud_srv
- if (request_preempt_) return false;
+ if (isPreemptRequested()) return false;
ROS_INFO("DetectHandleAction: get a point cloud from the door");
point_cloud_assembler::BuildCloudAngle::Request req_pointcloud;
point_cloud_assembler::BuildCloudAngle::Response res_pointcloud;
@@ -194,7 +185,7 @@
}
// detect handle
- if (request_preempt_) return false;
+ if (isPreemptRequested()) return false;
ROS_INFO("DetectHandleAction: detect the handle");
door_handle_detector::DoorsDetectorCloud::Request req_handledetect;
door_handle_detector::DoorsDetectorCloud::Response res_handledetect;
@@ -209,30 +200,26 @@
return true;
}
-
-
-
bool DetectHandleAction::cameraDetection(const robot_msgs::Door& door_in,
robot_msgs::Door& door_out)
{
// make the head point towards the door
- if (request_preempt_) return false;
+ if (isPreemptRequested()) return false;
ROS_INFO("DetectHandleAction: point head towards door");
robot_msgs::PointStamped door_pnt;
door_pnt.header.frame_id = door_in.header.frame_id;
door_pnt.point.x = (door_in.door_p1.x+door_in.door_p2.x)/2.0;
door_pnt.point.y = (door_in.door_p1.y+door_in.door_p2.y)/2.0;
door_pnt.point.z = 0.9;
- cout << "head point: ("
+ cout << "door_pnt.point " << door_in.header.frame_id << " "
<< door_pnt.point.x << " "
- << door_pnt.point.y << " "
- << door_pnt.point.z << ")" << endl;
-
+ << door_pnt.point.y << " "
+ << door_pnt.point.z << endl;
node_.publish("head_controller/head_track_point", door_pnt);
ros::Duration().fromSec(2).sleep();
// detect handle
- if (request_preempt_) return false;
+ if (isPreemptRequested()) return false;
ROS_INFO("DetectHandleAction: detect the handle");
door_handle_detector::DoorsDetector::Request req_handledetect;
door_handle_detector::DoorsDetector::Response res_handledetect;
@@ -245,11 +232,3 @@
door_out = res_handledetect.doors[0];
return true;
}
-
-
-
-void DetectHandleAction::handlePreempt()
-{
- request_preempt_ = true;
-};
-
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_grasp_door.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_grasp_door.cpp 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_grasp_door.cpp 2009-04-14 14:11:06 UTC (rev 13808)
@@ -49,7 +49,6 @@
GraspDoorAction::GraspDoorAction(Node& node) :
robot_actions::Action<robot_msgs::Door, robot_msgs::Door>("grasp_handle"),
node_(node),
- request_preempt_(false),
tf_(node)
{
node_.advertise<std_msgs::Float64>("gripper_effort/set_command",10);
@@ -61,82 +60,84 @@
-void GraspDoorAction::handleActivate(const robot_msgs::Door& door)
+robot_actions::ResultStatus GraspDoorAction::execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback)
{
- notifyActivated();
-
+ feedback = goal;
+
// door needs to be in time fixed frame
- if (door.header.frame_id != fixed_frame)
- notifyAborted(door);
- Vector normal(door.normal.x, door.normal.y, door.normal.z);
- Vector handle(door.handle.x, door.handle.y, door.handle.z);
+ if (goal.header.frame_id != fixed_frame){
+ return robot_actions::ABORTED;
+ }
+
+ Vector normal(goal.normal.x, goal.normal.y, goal.normal.z);
+ Vector handle(goal.handle.x, goal.handle.y, goal.handle.z);
Stamped<Pose> gripper_pose;
gripper_pose.frame_id_ = fixed_frame;
// open the gripper
- if (request_preempt_) {notifyPreempted(door); return;}
+ if (isPreemptRequested()) {
+ return robot_actions::PREEMPTED;
+ }
+
std_msgs::Float64 gripper_msg;
gripper_msg.data = 2.0;
node_.publish("gripper_effort/set_command", gripper_msg);
// move gripper in front of door
- if (request_preempt_) {notifyPreempted(door); return;}
+ if (isPreemptRequested()) {
+ return robot_actions::PREEMPTED;
+ }
+
gripper_pose.setOrigin( Vector3(handle(0) + (normal(0) * -0.15), handle(1) + (normal(1) * -0.15), handle(2) + (normal(2) * -0.15)));
- gripper_pose.setRotation( Quaternion(getDoorAngle(door), 0, M_PI/2.0) );
+ gripper_pose.setRotation( Quaternion(getDoorAngle(goal), 0, M_PI/2.0) );
gripper_pose.stamp_ = Time::now();
PoseStampedTFToMsg(gripper_pose, req_moveto.pose);
+
if (!ros::service::call("cartesian_trajectory_right/move_to", req_moveto, res_moveto)){
- if (request_preempt_)
- notifyPreempted(door);
- else
- notifyAborted(door);
- return;
+ if (isPreemptRequested())
+ return robot_actions::PREEMPTED;
+
+ return robot_actions::ABORTED;
}
// move gripper over door handle
- if (request_preempt_) {notifyPreempted(door); return;}
+ if (isPreemptRequested()) {
+ return robot_actions::PREEMPTED;
+ }
+
gripper_pose.frame_id_ = fixed_frame;
gripper_pose.setOrigin( Vector3(handle(0) + (normal(0) * 0.2 ), handle(1) + (normal(1) * 0.2), handle(2) + (normal(2) * 0.2)));
- gripper_pose.setRotation( Quaternion(getDoorAngle(door), 0, M_PI/2.0) );
+ gripper_pose.setRotation( Quaternion(getDoorAngle(goal), 0, M_PI/2.0) );
gripper_pose.stamp_ = Time::now();
PoseStampedTFToMsg(gripper_pose, req_moveto.pose);
+
if (!ros::service::call("cartesian_trajectory_right/move_to", req_moveto, res_moveto)){
- if (request_preempt_)
- notifyPreempted(door);
- else
- notifyAborted(door);
- return;
+ if (isPreemptRequested())
+ return robot_actions::PREEMPTED;
+
+ return robot_actions::ABORTED;
}
// close the gripper
- if (request_preempt_) {notifyPreempted(door); return;}
+ if (isPreemptRequested()) {
+ return robot_actions::PREEMPTED;
+ }
+
gripper_msg.data = -2.0;
node_.publish("gripper_effort/set_command", gripper_msg);
for (unsigned int i=0; i<100; i++){
Duration().fromSec(4.0/100.0).sleep();
- if (request_preempt_) {
+ if (isPreemptRequested()) {
gripper_msg.data = 0.0;
node_.publish("gripper_effort/set_command", gripper_msg);
- notifyPreempted(door);
- return;
+ return robot_actions::PREEMPTED;
}
}
- notifySucceeded(door);
+ return robot_actions::SUCCESS;
}
-
-
-void GraspDoorAction::handlePreempt()
-{
- request_preempt_ = true;
- ros::service::call("cartesian_trajectory_right/preempt", req_empty, res_empty);
-};
-
-
-
-
double GraspDoorAction::getDoorAngle(const robot_msgs::Door& door)
{
Vector normal(door.normal.x, door.normal.y, door.normal.z);
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_open_door.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_open_door.cpp 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_open_door.cpp 2009-04-14 14:11:06 UTC (rev 13808)
@@ -48,8 +48,7 @@
OpenDoorAction::OpenDoorAction() :
robot_actions::Action<robot_msgs::Door, robot_msgs::Door>("open_door"),
- node_(ros::Node::instance()),
- request_preempt_(false)
+ node_(ros::Node::instance())
{};
@@ -59,10 +58,10 @@
-void OpenDoorAction::handleActivate(const robot_msgs::Door& door)
-{
- notifyActivated();
-
+robot_actions::ResultStatus OpenDoorAction::execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback)
+{
+ feedback = goal;
+
// stop
tff_stop_.mode.vel.x = tff_stop_.FORCE;
tff_stop_.mode.vel.y = tff_stop_.FORCE;
@@ -108,17 +107,14 @@
tff_door_.value.rot.y = 0.0;
tff_door_.value.rot.z = 0.0;
-
-
// turn handle
for (unsigned int i=0; i<100; i++){
tff_handle_.value.rot.x += -1.5/100.0;
node_->publish("cartesian_tff_right/command", tff_handle_);
Duration().fromSec(4.0/100.0).sleep();
- if (request_preempt_) {
+ if (isPreemptRequested()) {
node_->publish("cartesian_tff_right/command", tff_stop_);
- notifyPreempted(door);
- return;
+ return robot_actions::PREEMPTED;
}
}
@@ -126,23 +122,13 @@
node_->publish("cartesian_tff_right/command", tff_door_);
for (unsigned int i=0; i<500; i++){
Duration().fromSec(10.0/500.0).sleep();
- if (request_preempt_) {
+ if (isPreemptRequested()) {
node_->publish("cartesian_tff_right/command", tff_stop_);
- notifyPreempted(door);
- return;
+ return robot_actions::PREEMPTED;
}
}
// finish
node_->publish("cartesian_tff_right/command", tff_stop_);
-
- notifySucceeded(door);
+ return robot_actions::SUCCESS;
}
-
-
-
-
-void OpenDoorAction::handlePreempt()
-{
- request_preempt_ = true;
-};
Modified: pkg/trunk/highlevel/doors/doors_core/test/trigger_doors_detector.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/test/trigger_doors_detector.cpp 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/doors/doors_core/test/trigger_doors_detector.cpp 2009-04-14 14:11:06 UTC (rev 13808)
@@ -74,8 +74,9 @@
runner.connect<robot_msgs::Door, door_handle_detector::DetectDoorActionStatus, robot_msgs::Door>(handle_detector);
runner.run();
- door_detector.handleActivate(my_door_);
- handle_detector.handleActivate(door_detector.tmp_result_);
+ robot_msgs::Door feedback;
+ door_detector.execute(my_door_, feedback);
+ handle_detector.execute(door_detector.tmp_result_, feedback);
return (0);
}
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ros_action_adapter.h
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ros_action_adapter.h 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ros_action_adapter.h 2009-04-14 14:11:06 UTC (rev 13808)
@@ -6,6 +6,7 @@
#include "TokenVariable.hh"
#include "SymbolDomain.hh"
#include "StringDomain.hh"
+#include <std_msgs/Empty.h>
namespace TREX {
@@ -73,7 +74,7 @@
ROS_INFO("%sRegistering publisher for %s on topic: %s",
nameString().c_str(), timelineName.c_str(), _preempt_topic.c_str());
- m_node->registerPublisher<Goal>(_preempt_topic, QUEUE_MAX());
+ m_node->registerPublisher<std_msgs::Empty>(_preempt_topic, QUEUE_MAX());
}
Observation* getObservation(){
@@ -175,7 +176,8 @@
m_node->publishMsg<Goal>(_request_topic, goal_msg);
}
else {
- m_node->publishMsg<Goal>(_preempt_topic, goal_msg);
+ std_msgs::Empty recall_msg;
+ m_node->publishMsg<std_msgs::Empty>(_preempt_topic, recall_msg);
}
return true;
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/stub_ros_container.cc
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/stub_ros_container.cc 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/stub_ros_container.cc 2009-04-14 14:11:06 UTC (rev 13808)
@@ -39,6 +39,7 @@
* @author Conor McGann
*/
#include <executive_trex_pr2/adapters.h>
+#include <robot_actions/action.h>
#include <boost/thread.hpp>
#include <cstdlib>
@@ -48,29 +49,31 @@
*/
namespace executive_trex_pr2 {
- template <class T>
- class StubAction: public robot_actions::Action<T, T> {
+
+
+ template <class Goal, class Feedback>
+ class StubAction: public robot_actions::Action<Goal, Feedback> {
public:
- StubAction(const std::string& name): robot_actions::Action<T, T>(name) {}
+ StubAction(const std::string& name): robot_actions::Action<Goal, Feedback>(name) {}
protected:
- // Activation does all the real work
- virtual void handleActivate(const T& msg){
- // Immediate reply
- robot_actions::Action<T, T>::notifyActivated();
- _state = msg;
- notifySucceeded(msg);
+ virtual robot_actions::ResultStatus execute(const Goal& goal, Feedback& feedback){
+ ROS_DEBUG("Executing %s\n", robot_actions::Action<Goal, Feedback>::getName().c_str());
+ return robot_actions::SUCCESS;
}
+ };
- // Activation does all the real work
- virtual void handlePreempt(){
- // Immediate reply
- notifyPreempted(_state);
- }
+ template <class T> class SimpleStubAction: public robot_actions::Action<T,T> {
+ public:
+ SimpleStubAction(const std::string& name): robot_actions::Action<T, T>(name) {}
- T _state;
+ virtual robot_actions::ResultStatus execute(const T& goal, T& feedback){
+ feedback = goal;
+ ROS_DEBUG("Executing %s\n", robot_actions::Action<T, T>::getName().c_str());
+ return robot_actions::SUCCESS;
+ }
};
/**
@@ -113,31 +116,6 @@
boost::thread* _update_thread;
};
-
- template <class Goal, class Feedback>
- class StubAction1: public robot_actions::Action<Goal, Feedback> {
- public:
-
- StubAction1(const std::string& name): robot_actions::Action<Goal, Feedback>(name) {}
-
- protected:
-
- // Activation does all the real work
- virtual void handleActivate(const Goal& msg){
- // Immediate reply
- robot_actions::Action<Goal, Feedback>::notifyActivated();
- notifySucceeded(_feedback);
- }
-
- // Activation does all the real work
- virtual void handlePreempt(){
- // Immediate reply
- notifyPreempted(_feedback);
- }
-
- Feedback _feedback;
- };
-
}
@@ -177,105 +155,105 @@
/* Add action stubs for doors */
// Detect Door
- executive_trex_pr2::StubAction<robot_msgs::Door> detect_door("detect_door");
+ executive_trex_pr2::SimpleStubAction<robot_msgs::Door> detect_door("detect_door");
if (getComponentParam("/trex/enable_detect_door"))
runner.connect<robot_msgs::Door, robot_actions::DoorActionState, robot_msgs::Door>(detect_door);
// Detect Handle
- executive_trex_pr2::StubAction<robot_msgs::Door> detect_handle("detect_handle");
+ executive_trex_pr2::SimpleStubAction<robot_msgs::Door> detect_handle("detect_handle");
if (getComponentParam("/trex/enable_detect_handle"))
runner.connect<robot_msgs::Door, robot_actions::DoorActionState, robot_msgs::Door>(detect_handle);
// Grasp Handle
- executive_trex_pr2::StubAction<robot_msgs::Door> grasp_handle("grasp_handle");
+ executive_trex_pr2::SimpleStubAction<robot_msgs::Door> grasp_handle("grasp_handle");
if (getComponentParam("/trex/enable_grasp_handle"))
runner.connect<robot_msgs::Door, robot_actions::DoorActionState, robot_msgs::Door>(grasp_handle);
- executive_trex_pr2::StubAction<robot_msgs::Door> open_door("open_door");
+ executive_trex_pr2::SimpleStubAction<robot_msgs::Door> open_door("open_door");
if (getComponentParam("/trex/enable_open_door"))
runner.connect<robot_msgs::Door, robot_actions::DoorActionState, robot_msgs::Door>(open_door);
- executive_trex_pr2::StubAction<robot_msgs::Door> open_door_without_grasp("open_door_without_grasp");
+ executive_trex_pr2::SimpleStubAction<robot_msgs::Door> open_door_without_grasp("open_door_without_grasp");
if (getComponentParam("/trex/enable_open_door_without_grasp"))
runner.connect<robot_msgs::Door, robot_actions::DoorActionState, robot_msgs::Door>(open_door_without_grasp);
- executive_trex_pr2::StubAction<robot_msgs::Door> release_door("release_door");
+ executive_trex_pr2::SimpleStubAction<robot_msgs::Door> release_door("release_door");
if (getComponentParam("/trex/enable_release_door"))
runner.connect<robot_msgs::Door, robot_actions::DoorActionState, robot_msgs::Door>(release_door);
/* Action stubs for plugs */
- executive_trex_pr2::StubAction1<std_msgs::Empty, robot_msgs::PlugStow> detect_plug_on_base("detect_plug_on_base");
+ executive_trex_pr2::StubAction<std_msgs::Empty, robot_msgs::PlugStow> detect_plug_on_base("detect_plug_on_base");
if (getComponentParam("/trex/enable_detect_plug_on_base"))
runner.connect<std_msgs::Empty, robot_actions:...
[truncated message content] |
|
From: <jfa...@us...> - 2009-04-14 17:43:24
|
Revision: 13826
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13826&view=rev
Author: jfaustwg
Date: 2009-04-14 17:43:14 +0000 (Tue, 14 Apr 2009)
Log Message:
-----------
Compile fixes for Node fixes, since Node now no longer incorrectly casts to itself
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/src/joy_batt_sender.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
pkg/trunk/world_models/costmap_2d/src/costmap_node.cpp
Modified: pkg/trunk/highlevel/highlevel_controllers/src/joy_batt_sender.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/joy_batt_sender.cpp 2009-04-14 17:40:30 UTC (rev 13825)
+++ pkg/trunk/highlevel/highlevel_controllers/src/joy_batt_sender.cpp 2009-04-14 17:43:14 UTC (rev 13826)
@@ -45,8 +45,7 @@
ros::Node::instance()->param<int>("~stop_button", stop_button_, 7);
ros::Node::instance()->param<int>("~go_button", go_button_, 5);
ros::Node::instance()->param<int>("~deadman_button", deadman_button_, 4);
- robot_msgs::BatteryState bs;
- ros::Node::instance()->advertise("bogus_battery_state", bs, &JoyBattSender::sendHeartbeat, 2);
+ ros::Node::instance()->advertise<robot_msgs::BatteryState>("bogus_battery_state", boost::bind(&JoyBattSender::sendHeartbeat, this, _1), 2);
ros::Node::instance()->subscribe("joy", joy_msg_, &JoyBattSender::handleJoyMsg, this, 2);
}
@@ -108,7 +107,7 @@
node.spin();
-
+
return 0;
}
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-04-14 17:40:30 UTC (rev 13825)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-04-14 17:43:14 UTC (rev 13826)
@@ -358,7 +358,7 @@
// Might be worth eventually having a dedicated node provide this service and all
// nodes including move_base access the costmap through it, but for now leaving costmap
// in move_base for fast access
- ros::Node::instance()->advertiseService("costmap", &MoveBase::costmapCallback);
+ ros::Node::instance()->advertiseService("costmap", &MoveBase::costmapCallback, this);
// The cost map is populated with either laser scans in the case that we are unable to use a
// world model source, or point clouds if we are. We shall pick one, and will be dominated by
Modified: pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
===================================================================
--- pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp 2009-04-14 17:40:30 UTC (rev 13825)
+++ pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp 2009-04-14 17:43:14 UTC (rev 13826)
@@ -301,23 +301,17 @@
g_quit = 1;
}
-class Shutdown {
-public:
- bool shutdownService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
- {
- quitRequested(0);
- return true;
- }
-};
+bool shutdownService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
+{
+ quitRequested(0);
+ return true;
+}
-class Reset {
-public:
- bool resetMotorsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
- {
- g_reset_motors = true;
- return true;
- }
-};
+bool resetMotorsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
+{
+ g_reset_motors = true;
+ return true;
+}
void warnOnSecondary(int sig)
{
@@ -369,7 +363,7 @@
ROS_FATAL("Unable to create pid file '%s': %s", PIDFILE, strerror(errno));
goto end;
}
-
+
if ((fd = open(PIDFILE, O_RDWR)) < 0)
{
ROS_FATAL("Unable to open pid file '%s': %s", PIDFILE, strerror(errno));
@@ -522,8 +516,8 @@
// Catch if we fall back to secondary mode
signal(SIGXCPU, warnOnSecondary);
- node->advertiseService("shutdown", &Shutdown::shutdownService);
- node->advertiseService("reset_motors", &Reset::resetMotorsService);
+ node->advertiseService("shutdown", shutdownService);
+ node->advertiseService("reset_motors", resetMotorsService);
//Start thread
int rv;
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_node.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_node.cpp 2009-04-14 17:40:30 UTC (rev 13825)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_node.cpp 2009-04-14 17:43:14 UTC (rev 13826)
@@ -309,7 +309,7 @@
// Might be worth eventually having a dedicated node provide this service and all
// nodes including move_base access the costmap through it, but for now leaving costmap
// in move_base for fast access
- ros::Node::instance()->advertiseService("costmap", &CostMapNode::costmapCallback);
+ ros::Node::instance()->advertiseService("costmap", &CostMapNode::costmapCallback, this);
// The cost map is populated with either laser scans in the case that we are unable to use a
// world model source, or point clouds if we are. We shall pick one, and will be dominated by
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2009-04-15 20:23:04
|
Revision: 13883
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13883&view=rev
Author: stuglaser
Date: 2009-04-15 20:22:53 +0000 (Wed, 15 Apr 2009)
Log Message:
-----------
Better velocity saturation for the hybrid controller
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gains/arm_pose.yaml
pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gains/arm_twist.yaml
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp 2009-04-15 20:22:53 UTC (rev 13882)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp 2009-04-15 20:22:53 UTC (rev 13883)
@@ -223,14 +223,6 @@
// Computes the desired wrench from the command
- // Computes the rotational error
- KDL::Vector rot_error =
- diff(KDL::Rotation::RPY(
- mode_[3] == robot_msgs::TaskFrameFormalism::POSITION ? setpoint_[3] : 0.0,
- mode_[4] == robot_msgs::TaskFrameFormalism::POSITION ? setpoint_[4] : 0.0,
- mode_[5] == robot_msgs::TaskFrameFormalism::POSITION ? setpoint_[5] : 0.0),
- tool.M.R);
-
// Computes the filtered twist
if (use_filter_)
{
@@ -246,92 +238,74 @@
twist_meas_filtered_ = twist_meas_;
}
+ // Computes the rotational error
+ KDL::Vector rot_error =
+ diff(KDL::Rotation::RPY(
+ mode_[3] == robot_msgs::TaskFrameFormalism::POSITION ? setpoint_[3] : 0.0,
+ mode_[4] == robot_msgs::TaskFrameFormalism::POSITION ? setpoint_[4] : 0.0,
+ mode_[5] == robot_msgs::TaskFrameFormalism::POSITION ? setpoint_[5] : 0.0),
+ tool.M.R);
+
+ // Computes the desired pose
for (int i = 0; i < 6; ++i)
{
- twist_desi_[i] = 0;
- wrench_desi_[i] = 0;
- pose_desi_[i] = 0;
+ if (mode_[i] == robot_msgs::TaskFrameFormalism::POSITION)
+ pose_desi_[i] = setpoint_[i];
+ else
+ pose_desi_[i] = 0.0;
+ }
- switch(mode_[i])
+ // Computes the desired twist
+ for (int i = 0; i < 6; ++i)
+ {
+ switch (mode_[i])
{
case robot_msgs::TaskFrameFormalism::POSITION:
- pose_desi_[i] = setpoint_[i];
if (i < 3) { // Translational position
- wrench_desi_[i] = pose_pids_[i].updatePid(tool.p.p[i] - setpoint_[i], twist_meas_filtered_[i], dt);
+ twist_desi_[i] = pose_pids_[i].updatePid(tool.p.p[i] - pose_desi_[i], twist_meas_filtered_[i], dt);
}
else { // Rotational position
- wrench_desi_[i] = pose_pids_[i].updatePid(rot_error[i - 3], twist_meas_filtered_[i], dt);
+ twist_desi_[i] = pose_pids_[i].updatePid(rot_error[i - 3], twist_meas_filtered_[i], dt);
}
-
break;
case robot_msgs::TaskFrameFormalism::VELOCITY:
twist_desi_[i] = setpoint_[i];
- wrench_desi_[i] = twist_pids_[i].updatePid(tool.GetTwist()[i] - setpoint_[i], dt);
break;
- case robot_msgs::TaskFrameFormalism::FORCE:
- wrench_desi_[i] = setpoint_[i];
- break;
- default:
- abort();
}
}
- static realtime_tools::RealtimePublisher<std_msgs::Float64MultiArray> debug("/d", 2);
- bool debug_locked = debug.trylock();
- if (debug_locked)
- debug.msg_.data.clear();
-#if 0
- // Velocity saturation
+ // Limits the velocity
if (saturated_velocity_ >= 0.0)
{
- KDL::Vector &v = twist_meas_filtered_.vel;
-
- if (v.Norm() > saturated_velocity_)
+ if (twist_desi_.vel.Norm() > saturated_velocity_)
{
- double restoring = -k_saturated_velocity_ * (v.Norm() - saturated_velocity_);
- wrench_desi_.force += restoring * (v / v.Norm());
- if (debug_locked) {
- debug.msg_.data.push_back(v.Norm() - saturated_velocity_);
- debug.msg_.data.push_back(restoring);
- }
+ twist_desi_.vel = saturated_velocity_ * twist_desi_.vel / twist_desi_.vel.Norm();
}
- else if (debug_locked) {
- debug.msg_.data.push_back(0);
- debug.msg_.data.push_back(0);
- }
-
-
}
-#else
- // Velocity saturation
- if (saturated_velocity_ >= 0.0)
+ // Computes the desired wrench
+ for (int i = 0; i < 6; ++i)
{
- KDL::Vector &v = twist_meas_filtered_.vel;
-
- // Desired force along the direction of the current velocity
- double fv = dot(wrench_desi_.force, twist_meas_filtered_.vel) / v.Norm();
-
- // Max allowed force along the current velocity direction
- double fv_max = -k_saturated_velocity_ * (v.Norm() - saturated_velocity_);
-
- if (debug_locked) {
- debug.msg_.data.push_back(v.Norm());
- debug.msg_.data.push_back(v.Norm() - saturated_velocity_);
- debug.msg_.data.push_back(fv);
- debug.msg_.data.push_back(fv_max);
-
- KDL::Vector diff = (v / v.Norm()) * (fv - fv_max);
- debug.msg_.data.push_back(diff.x());
- debug.msg_.data.push_back(diff.y());
- debug.msg_.data.push_back(diff.z());
+ switch (mode_[i])
+ {
+ case robot_msgs::TaskFrameFormalism::POSITION:
+ case robot_msgs::TaskFrameFormalism::VELOCITY:
+ wrench_desi_[i] = twist_pids_[i].updatePid(twist_meas_filtered_[i] - twist_desi_[i], dt);
+ break;
+ case robot_msgs::TaskFrameFormalism::FORCE:
+ wrench_desi_[i] = setpoint_[i];
+ break;
+ default:
+ abort();
}
- if (fv > fv_max)
- debug.msg_.data.push_back(99);
- wrench_desi_.force -= (v / v.Norm()) * (fv - fv_max);
}
-#endif
+
+ static realtime_tools::RealtimePublisher<std_msgs::Float64MultiArray> debug("/d", 2);
+ bool debug_locked = debug.trylock();
if (debug_locked)
+ debug.msg_.data.clear();
+
+ if (debug_locked)
debug.unlockAndPublish();
// Transforms the wrench from the task frame to the chain root frame
@@ -381,10 +355,6 @@
setpoint_[i] = frame.p.p[i];
}
frame.M.R.GetRPY(setpoint_[3], setpoint_[4], setpoint_[5]);
-
- ROS_INFO("Starting pose: (%.2lf, %.2lf, %.2lf) (%.3lf, %.3lf, %.3lf)",
- setpoint_[0], setpoint_[1], setpoint_[2],
- setpoint_[3], setpoint_[4], setpoint_[5]);
break;
}
case robot_msgs::TaskFrameFormalism::VELOCITY:
Modified: pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gains/arm_pose.yaml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gains/arm_pose.yaml 2009-04-15 20:22:53 UTC (rev 13882)
+++ pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gains/arm_pose.yaml 2009-04-15 20:22:53 UTC (rev 13883)
@@ -1,16 +1,13 @@
fb_trans:
- p: 1500
- i: 2800
- d: 2.0
- i_clamp: 40.0
- old_p: 1200.0
- old_i: 1000.0
- old_d: 3.0
- old_i_clamp: 3.0
+ p: 60
+ old_p: 20
+ i: 80
+ d: 0
+ i_clamp: 0.2
fb_rot:
- p: 21.0
- i: 1.0
- d: 0.3
+ p: 10
+ i: 0
+ d: 0
i_clamp: 0.4
ff_trans: 0.0
ff_rot: 0.0
Modified: pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gains/arm_twist.yaml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gains/arm_twist.yaml 2009-04-15 20:22:53 UTC (rev 13882)
+++ pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gains/arm_twist.yaml 2009-04-15 20:22:53 UTC (rev 13883)
@@ -1,11 +1,11 @@
fb_trans:
- p: 20.0
- i: 0.5
+ p: 30.0
+ i: 0.0
d: 0.0
i_clamp: 1.0
fb_rot:
p: 1.5
- i: 0.1
+ i: 0.0
d: 0.0
i_clamp: 0.2
ff_trans: 20.0
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-04-16 02:48:40
|
Revision: 13931
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13931&view=rev
Author: eitanme
Date: 2009-04-16 02:48:36 +0000 (Thu, 16 Apr 2009)
Log Message:
-----------
r21187@cib: eitan | 2009-04-10 14:30:46 -0700
Making a branch for re-working the navigation stack
r21188@cib: eitan | 2009-04-10 14:32:03 -0700
Working to re-structure the trajectory controller for the new navstack
r21191@cib: eitan | 2009-04-10 14:49:06 -0700
Removed dependency on deprecated messages
r21192@cib: eitan | 2009-04-10 15:22:26 -0700
Removing old tester that is no longer needed
r21193@cib: eitan | 2009-04-10 15:53:40 -0700
Created a ros wrapper for the navfn planner
r21194@cib: eitan | 2009-04-10 17:26:29 -0700
Got it moving a bit
r21237@cib: eitan | 2009-04-13 13:50:57 -0700
Added support for specifying which observations should be used to clear space out.
r21238@cib: eitan | 2009-04-13 13:54:48 -0700
Updated documentation
r21255@cib: eitan | 2009-04-13 17:04:09 -0700
Modified to work with better cost map copies
r21256@cib: eitan | 2009-04-13 17:05:19 -0700
Better copying of costmap... sets a reference now rather than newing a pointer
r21257@cib: eitan | 2009-04-13 17:07:18 -0700
changed to work with better cost map copies
r21258@cib: eitan | 2009-04-13 17:45:58 -0700
Overloaded an assignment operator for the cost map
r21284@cib: eitan | 2009-04-14 10:36:29 -0700
Added z value to pose message, added a missing delete to action runner
r21285@cib: eitan | 2009-04-14 10:54:54 -0700
Fixed another memory leak
r21286@cib: eitan | 2009-04-14 11:37:03 -0700
Now valgrind clean
r21287@cib: eitan | 2009-04-14 11:37:48 -0700
Now valgrind clean
r21288@cib: eitan | 2009-04-14 11:38:30 -0700
build for release by default
r21333@cib: eitan | 2009-04-14 14:36:17 -0700
Works with point grid now
r21334@cib: eitan | 2009-04-14 14:36:49 -0700
Allow outside entities to get observations from the observation buffers
r21335@cib: eitan | 2009-04-14 14:38:45 -0700
Modified to work with new costmap for freespace controller
r21350@cib: eitan | 2009-04-14 16:03:59 -0700
Put in a check to see if the robot is stopped
r21352@cib: eitan | 2009-04-15 11:20:06 -0700
Fixed improper delete
r21391@cib: eitan | 2009-04-15 14:19:49 -0700
The old trajectory rollout package
r21392@cib: eitan | 2009-04-15 16:30:31 -0700
Changing to old_costmap_2d
r21393@cib: eitan | 2009-04-15 16:31:30 -0700
A compiling version of the old_costmap_2d
r21394@cib: eitan | 2009-04-15 16:36:21 -0700
Removed cpp message dir
r21395@cib: eitan | 2009-04-15 16:39:36 -0700
Removed msg/cpp from commit
r21396@cib: eitan | 2009-04-15 16:42:09 -0700
Updated to work with new action model
r21397@cib: eitan | 2009-04-15 16:50:43 -0700
Renaming new costmap to costmap_2d
r21398@cib: eitan | 2009-04-15 16:51:56 -0700
Compiling version
r21399@cib: eitan | 2009-04-15 16:53:06 -0700
Massive restructuring of naviagtion package names and directories
r21400@cib: eitan | 2009-04-15 17:12:55 -0700
Trajectory rollout is now base_local_planner
r21401@cib: eitan | 2009-04-15 17:17:55 -0700
Renamed TrajectoryController to TrajectoryPlanner
r21402@cib: eitan | 2009-04-15 17:28:11 -0700
Modified to work with new naming
r21403@cib: eitan | 2009-04-15 17:35:42 -0700
Now compiling with tests passing
r21404@cib: eitan | 2009-04-15 17:46:39 -0700
Updated to work with new costmap
r21405@cib: eitan | 2009-04-15 17:48:12 -0700
Updated to work with renaming of costmap and traj_rollout
r21406@cib: eitan | 2009-04-15 17:49:41 -0700
Remapping from goal to activate
r21407@cib: eitan | 2009-04-15 18:09:46 -0700
Update to work with new naming of old_costmap_2d
r21408@cib: eitan | 2009-04-15 18:14:52 -0700
costmap_2d has become old_costmap_2d and is now deprecated. This package should switch to the new costmap_2d.
r21462@cib: eitan | 2009-04-15 18:22:51 -0700
costmap_2d is now deprecated and named old_costmap_2d, this package should be moved to the new costmap_2d
r21463@cib: eitan | 2009-04-15 18:24:30 -0700
Removed a dependency on costmap_2d that didn't exist. Added one on angles that did
r21464@cib: eitan | 2009-04-15 18:25:44 -0700
costmap_2d is now deprecated and named old_costmap_2d, this package should be moved to the new costmap_2d
r21465@cib: eitan | 2009-04-15 18:38:03 -0700
New namespace for old_costmap_2d
r21466@cib: eitan | 2009-04-15 18:55:32 -0700
Namespace change for old_costmap_2d
r21467@cib: eitan | 2009-04-15 18:57:30 -0700
Namespace change for old_costmap_2d
r21468@cib: eitan | 2009-04-15 19:00:51 -0700
Namespace change for old_costmap_2d
r21469@cib: eitan | 2009-04-15 19:02:29 -0700
Namespace change for old_costmap_2d
r21470@cib: eitan | 2009-04-15 19:06:45 -0700
Now works with deprecated version of costmap_2d... old_costmap_2d
r21477@cib: eitan | 2009-04-15 19:13:44 -0700
Changed launch map a bit
r21478@cib: eitan | 2009-04-15 19:16:13 -0700
Changed INFO to DBUG in a few places
r21479@cib: eitan | 2009-04-15 19:24:58 -0700
Added a test_nav package
r21482@cib: eitan | 2009-04-15 19:45:08 -0700
Tyring to get merge to work
Merging new navigation stack into trunk
Modified Paths:
--------------
pkg/trunk/deprecated/highlevel_controllers/include/highlevel_controllers/move_base.hh
pkg/trunk/deprecated/highlevel_controllers/manifest.xml
pkg/trunk/deprecated/highlevel_controllers/src/move_base.cpp
pkg/trunk/deprecated/highlevel_controllers/src/move_base_door.cpp
pkg/trunk/deprecated/highlevel_controllers/src/move_base_follow.cpp
pkg/trunk/deprecated/highlevel_controllers/src/move_base_navfn.cpp
pkg/trunk/deprecated/highlevel_controllers/src/move_base_navfn_constrained.cpp
pkg/trunk/deprecated/highlevel_controllers/src/move_base_sbpl.cpp
pkg/trunk/deprecated/old_costmap_2d/CMakeLists.txt
pkg/trunk/deprecated/old_costmap_2d/manifest.xml
pkg/trunk/deprecated/old_costmap_2d/src/basic_observation_buffer.cpp
pkg/trunk/deprecated/old_costmap_2d/src/costmap_2d.cpp
pkg/trunk/deprecated/old_costmap_2d/src/costmap_2d_ros.cpp
pkg/trunk/deprecated/old_costmap_2d/src/costmap_node.cpp
pkg/trunk/deprecated/old_costmap_2d/src/costmap_standalone.cpp
pkg/trunk/deprecated/old_costmap_2d/src/observation_buffer.cpp
pkg/trunk/deprecated/old_costmap_2d/src/obstacle_map_accessor.cpp
pkg/trunk/deprecated/old_costmap_2d/src/test/benchmark.cpp
pkg/trunk/deprecated/old_costmap_2d/src/test/costmap2d_pqueue_benchmark.cpp
pkg/trunk/deprecated/old_costmap_2d/src/test/module-tests.cpp
pkg/trunk/highlevel/nav/CMakeLists.txt
pkg/trunk/highlevel/nav/manifest.xml
pkg/trunk/highlevel/nav/src/nav.cpp
pkg/trunk/highlevel/robot_actions/include/robot_actions/message_adapter.h
pkg/trunk/highlevel/robot_actions/msg/Pose2D.msg
pkg/trunk/motion_planning/mpbench/CMakeLists.txt
pkg/trunk/motion_planning/mpbench/manifest.xml
pkg/trunk/motion_planning/mpbench/src/gfx.cpp
pkg/trunk/motion_planning/mpbench/src/mpbenchmark.cpp
pkg/trunk/motion_planning/mpbench/src/setup.cpp
pkg/trunk/motion_planning/mpglue/include/mpglue/costmap.h
pkg/trunk/motion_planning/mpglue/manifest.xml
pkg/trunk/motion_planning/mpglue/src/costmap.cpp
pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp
pkg/trunk/motion_planning/navfn/CMakeLists.txt
pkg/trunk/motion_planning/navfn/manifest.xml
pkg/trunk/motion_planning/planning_research/sbpl_door_planner/manifest.xml
pkg/trunk/motion_planning/planning_research/sbpl_planner_node/include/sbpl_planner_node/sbpl_planner_node.h
pkg/trunk/motion_planning/planning_research/sbpl_planner_node/manifest.xml
pkg/trunk/motion_planning/planning_research/sbpl_planner_node/src/sbpl_planner_node.cpp
pkg/trunk/nav/base_local_planner/CMakeLists.txt
pkg/trunk/nav/base_local_planner/manifest.xml
pkg/trunk/nav/base_local_planner/scripts/indefinite_nav.py
pkg/trunk/nav/base_local_planner/src/costmap_model.cpp
pkg/trunk/nav/base_local_planner/src/map_cell.cpp
pkg/trunk/nav/base_local_planner/src/map_grid.cpp
pkg/trunk/nav/base_local_planner/src/point_grid.cpp
pkg/trunk/nav/base_local_planner/src/trajectory.cpp
pkg/trunk/nav/base_local_planner/src/voxel_grid_model.cpp
pkg/trunk/nav/base_local_planner/test/utest.cpp
pkg/trunk/world_models/new_costmap_2d/CMakeLists.txt
pkg/trunk/world_models/new_costmap_2d/costmap_demo.xml
pkg/trunk/world_models/new_costmap_2d/launch_costmap_2d_ros.xml
pkg/trunk/world_models/new_costmap_2d/launch_map.xml
pkg/trunk/world_models/new_costmap_2d/mainpage.dox
pkg/trunk/world_models/new_costmap_2d/manifest.xml
pkg/trunk/world_models/new_costmap_2d/src/costmap_2d.cpp
pkg/trunk/world_models/new_costmap_2d/src/costmap_2d_ros.cpp
pkg/trunk/world_models/new_costmap_2d/src/observation_buffer.cpp
Added Paths:
-----------
pkg/trunk/deprecated/highlevel_controllers/
pkg/trunk/deprecated/old_costmap_2d/
pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/
pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/basic_observation_buffer.h
pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/costmap2d_pqueue_benchmark.h
pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/costmap_2d.h
pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/costmap_node.h
pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/observation.h
pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/observation_buffer.h
pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/obstacle_map_accessor.h
pkg/trunk/deprecated/trajectory_rollout/
pkg/trunk/deprecated/trajectory_rollout/CMakeLists.txt
pkg/trunk/deprecated/trajectory_rollout/Makefile
pkg/trunk/deprecated/trajectory_rollout/include/
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/costmap_model.h
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/governor_node.h
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/map_cell.h
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/map_grid.h
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/planar_laser_scan.h
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/point_grid.h
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/trajectory.h
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/trajectory_controller.h
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/trajectory_controller_ros.h
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/trajectory_inc.h
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/voxel_grid_model.h
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/world_model.h
pkg/trunk/deprecated/trajectory_rollout/lib/
pkg/trunk/deprecated/trajectory_rollout/mainpage.dox
pkg/trunk/deprecated/trajectory_rollout/manifest.xml
pkg/trunk/deprecated/trajectory_rollout/msg/
pkg/trunk/deprecated/trajectory_rollout/msg/GlobalPlan.msg
pkg/trunk/deprecated/trajectory_rollout/msg/Position2DInt.msg
pkg/trunk/deprecated/trajectory_rollout/msg/ScoreMap2D.msg
pkg/trunk/deprecated/trajectory_rollout/msg/ScoreMapCell2D.msg
pkg/trunk/deprecated/trajectory_rollout/msg/WavefrontPlan.msg
pkg/trunk/deprecated/trajectory_rollout/scripts/
pkg/trunk/deprecated/trajectory_rollout/scripts/indefinite_nav.py
pkg/trunk/deprecated/trajectory_rollout/src/
pkg/trunk/deprecated/trajectory_rollout/src/costmap_model.cpp
pkg/trunk/deprecated/trajectory_rollout/src/governor_node.cpp
pkg/trunk/deprecated/trajectory_rollout/src/map_cell.cpp
pkg/trunk/deprecated/trajectory_rollout/src/map_grid.cpp
pkg/trunk/deprecated/trajectory_rollout/src/point_grid.cpp
pkg/trunk/deprecated/trajectory_rollout/src/trajectory.cpp
pkg/trunk/deprecated/trajectory_rollout/src/trajectory_controller.cpp
pkg/trunk/deprecated/trajectory_rollout/src/trajectory_controller_ros.cpp
pkg/trunk/deprecated/trajectory_rollout/src/voxel_grid_model.cpp
pkg/trunk/deprecated/trajectory_rollout/test/
pkg/trunk/deprecated/trajectory_rollout/test/utest.cpp
pkg/trunk/highlevel/nav/src/move_base_action.cpp
pkg/trunk/highlevel/test_nav/
pkg/trunk/highlevel/test_nav/config/
pkg/trunk/highlevel/test_nav/config/base_local_planner_params.xml
pkg/trunk/highlevel/test_nav/config/navfn_params.xml
pkg/trunk/highlevel/test_nav/launch_navstack.xml
pkg/trunk/highlevel/test_nav/manifest.xml
pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
pkg/trunk/nav/base_local_planner/
pkg/trunk/nav/base_local_planner/include/base_local_planner/
pkg/trunk/nav/base_local_planner/include/base_local_planner/costmap_model.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/map_cell.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/map_grid.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/planar_laser_scan.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/point_grid.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_inc.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/voxel_grid_model.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/world_model.h
pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp
pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
pkg/trunk/world_models/new_costmap_2d/
pkg/trunk/world_models/new_costmap_2d/include/costmap_2d/
pkg/trunk/world_models/new_costmap_2d/include/costmap_2d/cell_data.h
pkg/trunk/world_models/new_costmap_2d/include/costmap_2d/cost_values.h
pkg/trunk/world_models/new_costmap_2d/include/costmap_2d/costmap_2d.h
pkg/trunk/world_models/new_costmap_2d/include/costmap_2d/costmap_2d_ros.h
pkg/trunk/world_models/new_costmap_2d/include/costmap_2d/observation.h
pkg/trunk/world_models/new_costmap_2d/include/costmap_2d/observation_buffer.h
Removed Paths:
-------------
pkg/trunk/highlevel/highlevel_controllers/
pkg/trunk/nav/trajectory_rollout/
pkg/trunk/world_models/costmap_2d/
pkg/trunk/world_models/new_costmap/
Property Changed:
----------------
pkg/trunk/
pkg/trunk/world_models/new_costmap_2d/src/observation_buffer.cpp
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:20583
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:20583
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/nav_rework:21482
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/deprecated/highlevel_controllers/include/highlevel_controllers/move_base.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh 2009-04-16 01:55:13 UTC (rev 13928)
+++ pkg/trunk/deprecated/highlevel_controllers/include/highlevel_controllers/move_base.hh 2009-04-16 02:48:36 UTC (rev 13931)
@@ -39,8 +39,8 @@
#include <trajectory_rollout/trajectory_controller_ros.h>
// Costmap used for the map representation
-#include <costmap_2d/costmap_2d.h>
-#include <costmap_2d/basic_observation_buffer.h>
+#include <old_costmap_2d/costmap_2d.h>
+#include <old_costmap_2d/basic_observation_buffer.h>
//Ransac ground filter used to see small obstacles
#include <pr2_msgs/PlaneStamped.h>
@@ -103,7 +103,7 @@
* @brief Accessor for the cost map. Use mainly for initialization
* of specialized map strunture for planning
*/
- const costmap_2d::CostMapAccessor& getCostMap() const {return *global_map_accessor_;}
+ const old_costmap_2d::CostMapAccessor& getCostMap() const {return *global_map_accessor_;}
/**
* @brief A handler to be over-ridden in the derived class to handle a diff stream from the
@@ -242,18 +242,18 @@
// are looked up. If we wanted to generalize this node further, we could use a factory pattern to dynamically
// load specific derived classes. For that we would need a hand-shaking pattern to register subscribers for them
// with this node
- costmap_2d::BasicObservationBuffer* baseScanBuffer_;
- costmap_2d::BasicObservationBuffer* baseCloudBuffer_;
- costmap_2d::BasicObservationBuffer* tiltScanBuffer_;
- costmap_2d::BasicObservationBuffer* lowObstacleBuffer_;
- costmap_2d::BasicObservationBuffer* stereoCloudBuffer_;
+ old_costmap_2d::BasicObservationBuffer* baseScanBuffer_;
+ old_costmap_2d::BasicObservationBuffer* baseCloudBuffer_;
+ old_costmap_2d::BasicObservationBuffer* tiltScanBuffer_;
+ old_costmap_2d::BasicObservationBuffer* lowObstacleBuffer_;
+ old_costmap_2d::BasicObservationBuffer* stereoCloudBuffer_;
/** Should encapsulate as a controller wrapper that is not resident in the trajectory rollout package */
trajectory_rollout::TrajectoryControllerROS* controller_;
- costmap_2d::CostMap2D* costMap_; /**< The cost map mainatined incrementally from laser scans */
- costmap_2d::CostMapAccessor* global_map_accessor_; /**< Read-only access to global cost map */
- costmap_2d::CostMapAccessor* local_map_accessor_; /**< Read-only access to a window on the cost map */
+ old_costmap_2d::CostMap2D* costMap_; /**< The cost map mainatined incrementally from laser scans */
+ old_costmap_2d::CostMapAccessor* global_map_accessor_; /**< Read-only access to global cost map */
+ old_costmap_2d::CostMapAccessor* local_map_accessor_; /**< Read-only access to a window on the cost map */
robot_srvs::StaticMap::Response costmap_response_;
Modified: pkg/trunk/deprecated/highlevel_controllers/manifest.xml
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/manifest.xml 2009-04-16 01:55:13 UTC (rev 13928)
+++ pkg/trunk/deprecated/highlevel_controllers/manifest.xml 2009-04-16 02:48:36 UTC (rev 13931)
@@ -23,7 +23,7 @@
<depend package="pr2_msgs"/>
<depend package="trajectory_rollout"/>
<depend package="laser_scan" />
- <depend package="costmap_2d"/>
+ <depend package="old_costmap_2d"/>
<depend package="tf"/>
<depend package="map_server"/>
<depend package="sbpl"/>
Modified: pkg/trunk/deprecated/highlevel_controllers/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-04-16 01:55:13 UTC (rev 13928)
+++ pkg/trunk/deprecated/highlevel_controllers/src/move_base.cpp 2009-04-16 02:48:36 UTC (rev 13931)
@@ -122,7 +122,7 @@
#include <boost/thread.hpp>
#include <robot_filter/RobotFilter.h>
-using namespace costmap_2d;
+using namespace old_costmap_2d;
namespace ros {
namespace highlevel_controllers {
@@ -232,25 +232,25 @@
}
// Then allocate observation buffers
- baseScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("base_laser"), global_frame_, tf_,
+ baseScanBuffer_ = new old_costmap_2d::BasicObservationBuffer(std::string("base_laser"), global_frame_, tf_,
ros::Duration().fromSec(base_laser_keepalive),
- costmap_2d::BasicObservationBuffer::computeRefreshInterval(base_laser_update_rate),
+ old_costmap_2d::BasicObservationBuffer::computeRefreshInterval(base_laser_update_rate),
inscribedRadius, minZ_, maxZ_, filter_);
- baseCloudBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("base_laser"), global_frame_, tf_,
+ baseCloudBuffer_ = new old_costmap_2d::BasicObservationBuffer(std::string("base_laser"), global_frame_, tf_,
ros::Duration().fromSec(base_laser_keepalive),
- costmap_2d::BasicObservationBuffer::computeRefreshInterval(base_laser_update_rate),
+ old_costmap_2d::BasicObservationBuffer::computeRefreshInterval(base_laser_update_rate),
inscribedRadius, minZ_, maxZ_, filter_);
- tiltScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("laser_tilt_link"), global_frame_, tf_,
+ tiltScanBuffer_ = new old_costmap_2d::BasicObservationBuffer(std::string("laser_tilt_link"), global_frame_, tf_,
ros::Duration().fromSec(tilt_laser_keepalive),
- costmap_2d::BasicObservationBuffer::computeRefreshInterval(tilt_laser_update_rate),
+ old_costmap_2d::BasicObservationBuffer::computeRefreshInterval(tilt_laser_update_rate),
inscribedRadius, minZ_, maxZ_, filter_);
- lowObstacleBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("odom_combined"), global_frame_, tf_,
+ lowObstacleBuffer_ = new old_costmap_2d::BasicObservationBuffer(std::string("odom_combined"), global_frame_, tf_,
ros::Duration().fromSec(low_obstacle_keepalive),
- costmap_2d::BasicObservationBuffer::computeRefreshInterval(low_obstacle_update_rate),
+ old_costmap_2d::BasicObservationBuffer::computeRefreshInterval(low_obstacle_update_rate),
inscribedRadius, -10.0, maxZ_, filter_);
- stereoCloudBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("stereo_link"), global_frame_, tf_,
+ stereoCloudBuffer_ = new old_costmap_2d::BasicObservationBuffer(std::string("stereo_link"), global_frame_, tf_,
ros::Duration().fromSec(stereo_keepalive),
- costmap_2d::BasicObservationBuffer::computeRefreshInterval(stereo_update_rate),
+ old_costmap_2d::BasicObservationBuffer::computeRefreshInterval(stereo_update_rate),
inscribedRadius, minZ_, maxZ_, filter_);
@@ -809,7 +809,7 @@
ROS_DEBUG("getting observations.\n");
lock();
// Aggregate buffered observations across all sources. Must be thread safe
- std::vector<costmap_2d::Observation> observations;
+ std::vector<old_costmap_2d::Observation> observations;
baseCloudBuffer_->get_observations(observations);
//if we are not getting filtered base scans... we'll add the base scans as obstacle points
@@ -821,7 +821,7 @@
stereoCloudBuffer_->get_observations(observations);
std::vector<robot_msgs::PointCloud> points_storage(observations.size()); //needed to deep copy observations
- std::vector<costmap_2d::Observation> stored_observations(observations.size());
+ std::vector<old_costmap_2d::Observation> stored_observations(observations.size());
//we need to perform a deep copy on the observations to be thread safe
for(unsigned int i = 0; i < observations.size(); ++i){
@@ -1028,7 +1028,7 @@
lock();
// Aggregate buffered observations across all sources. Must be thread safe
- std::vector<costmap_2d::Observation> observations, raytrace_obs;
+ std::vector<old_costmap_2d::Observation> observations, raytrace_obs;
baseCloudBuffer_->get_observations(observations);
//get the observations from the base scan to use for raytracing
@@ -1044,7 +1044,7 @@
stereoCloudBuffer_->get_observations(observations);
std::vector<robot_msgs::PointCloud> points_storage(observations.size()); //needed to deep copy observations
- std::vector<costmap_2d::Observation> stored_observations(observations.size());
+ std::vector<old_costmap_2d::Observation> stored_observations(observations.size());
//we need to perform a deep copy on the observations to be thread safe
for(unsigned int i = 0; i < observations.size(); ++i){
Modified: pkg/trunk/deprecated/highlevel_controllers/src/move_base_door.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base_door.cpp 2009-04-16 01:55:13 UTC (rev 13928)
+++ pkg/trunk/deprecated/highlevel_controllers/src/move_base_door.cpp 2009-04-16 02:48:36 UTC (rev 13931)
@@ -71,7 +71,7 @@
#include <angles/angles.h>
#include <robot_msgs/Polyline2D.h>
-using namespace costmap_2d;
+using namespace old_costmap_2d;
using namespace deprecated_msgs;
namespace ros
Modified: pkg/trunk/deprecated/highlevel_controllers/src/move_base_follow.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base_follow.cpp 2009-04-16 01:55:13 UTC (rev 13928)
+++ pkg/trunk/deprecated/highlevel_controllers/src/move_base_follow.cpp 2009-04-16 02:48:36 UTC (rev 13931)
@@ -37,7 +37,7 @@
#include <highlevel_controllers/move_base.hh>
-using namespace costmap_2d;
+using namespace old_costmap_2d;
namespace ros {
namespace highlevel_controllers {
@@ -130,7 +130,7 @@
lock();
// Aggregate buffered observations across all sources. Must be thread safe
- std::vector<costmap_2d::Observation> observations;
+ std::vector<old_costmap_2d::Observation> observations;
baseScanBuffer_->get_observations(observations);
tiltScanBuffer_->get_observations(observations);
lowObstacleBuffer_->get_observations(observations);
Modified: pkg/trunk/deprecated/highlevel_controllers/src/move_base_navfn.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base_navfn.cpp 2009-04-16 01:55:13 UTC (rev 13928)
+++ pkg/trunk/deprecated/highlevel_controllers/src/move_base_navfn.cpp 2009-04-16 02:48:36 UTC (rev 13931)
@@ -66,7 +66,7 @@
#include <highlevel_controllers/move_base.hh>
#include <navfn/navfn.h>
-using namespace costmap_2d;
+using namespace old_costmap_2d;
namespace ros {
namespace highlevel_controllers {
Modified: pkg/trunk/deprecated/highlevel_controllers/src/move_base_navfn_constrained.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base_navfn_constrained.cpp 2009-04-16 01:55:13 UTC (rev 13928)
+++ pkg/trunk/deprecated/highlevel_controllers/src/move_base_navfn_constrained.cpp 2009-04-16 02:48:36 UTC (rev 13931)
@@ -68,7 +68,7 @@
#include <robot_actions/Pose2D.h>
#include <navfn/navfn.h>
-using namespace costmap_2d;
+using namespace old_costmap_2d;
using robot_msgs::Polyline2D;
using deprecated_msgs::Point2DFloat32;
using std::vector;
Modified: pkg/trunk/deprecated/highlevel_controllers/src/move_base_sbpl.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp 2009-04-16 01:55:13 UTC (rev 13928)
+++ pkg/trunk/deprecated/highlevel_controllers/src/move_base_sbpl.cpp 2009-04-16 02:48:36 UTC (rev 13931)
@@ -72,7 +72,7 @@
#include <sbpl/headers.h>
#include <err.h>
-using namespace costmap_2d;
+using namespace old_costmap_2d;
namespace {
Modified: pkg/trunk/deprecated/old_costmap_2d/CMakeLists.txt
===================================================================
--- pkg/trunk/world_models/costmap_2d/CMakeLists.txt 2009-04-16 01:55:13 UTC (rev 13928)
+++ pkg/trunk/deprecated/old_costmap_2d/CMakeLists.txt 2009-04-16 02:48:36 UTC (rev 13931)
@@ -1,27 +1,27 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
set(ROS_BUILD_TYPE Release)
-rospack(costmap_2d)
+rospack(old_costmap_2d)
genmsg()
rospack_add_boost_directories()
-rospack_add_library(costmap_2d src/costmap_2d.cpp
+rospack_add_library(old_costmap_2d src/costmap_2d.cpp
src/obstacle_map_accessor.cpp
src/observation_buffer.cpp
src/basic_observation_buffer.cpp
src/costmap_node.cpp)
-rospack_link_boost(costmap_2d thread)
+rospack_link_boost(old_costmap_2d thread)
# Test target for module tests to be included in gtest regression test harness
rospack_add_gtest(utest src/test/module-tests.cpp)
-target_link_libraries(utest costmap_2d)
+target_link_libraries(utest old_costmap_2d)
# Target for benchmarking the costmap
rospack_add_executable(benchmark src/test/benchmark.cpp )
-target_link_libraries(benchmark costmap_2d)
+target_link_libraries(benchmark old_costmap_2d)
# Target for running a standalone version of costmap
rospack_add_executable(costmap_standalone src/costmap_standalone.cpp )
-target_link_libraries(costmap_standalone costmap_2d)
+target_link_libraries(costmap_standalone old_costmap_2d)
Added: pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/basic_observation_buffer.h
===================================================================
--- pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/basic_observation_buffer.h (rev 0)
+++ pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/basic_observation_buffer.h 2009-04-16 02:48:36 UTC (rev 13931)
@@ -0,0 +1,75 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitte...
[truncated message content] |
|
From: <jfa...@us...> - 2009-04-16 17:04:14
|
Revision: 13950
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13950&view=rev
Author: jfaustwg
Date: 2009-04-16 17:04:10 +0000 (Thu, 16 Apr 2009)
Log Message:
-----------
Getting inheritors from Message ready for the __getMessageDefinition() and __s_getMessageDefinition() calls
Modified Paths:
--------------
pkg/trunk/prcore/tf/include/tf/message_notifier.h
pkg/trunk/util/message_sequencing/include/message_sequencing/time_sequencer.h
pkg/trunk/util/mux/mux.cpp
Modified: pkg/trunk/prcore/tf/include/tf/message_notifier.h
===================================================================
--- pkg/trunk/prcore/tf/include/tf/message_notifier.h 2009-04-16 16:47:57 UTC (rev 13949)
+++ pkg/trunk/prcore/tf/include/tf/message_notifier.h 2009-04-16 17:04:10 UTC (rev 13950)
@@ -143,6 +143,9 @@
*/
~MessageNotifier()
{
+ NOTIFIER_DEBUG("Successful Transforms: %d, Failed Transforms: %d, Transform messages received: %d, Messages received: %d",
+ successful_transform_count_, failed_transform_count_, transform_message_count_, incoming_message_count_);
+
unsubscribeFromMessage();
node_->unsubscribe("/tf_message", &MessageNotifier::incomingTFMessage, this);
@@ -155,9 +158,6 @@
thread_handle_.join();
clear();
-
- NOTIFIER_DEBUG("Successful Transforms: %d, Failed Transforms: %d, Transform messages received: %d, Messages received: %d",
- successful_transform_count_, failed_transform_count_, transform_message_count_, incoming_message_count_);
}
/**
Modified: pkg/trunk/util/message_sequencing/include/message_sequencing/time_sequencer.h
===================================================================
--- pkg/trunk/util/message_sequencing/include/message_sequencing/time_sequencer.h 2009-04-16 16:47:57 UTC (rev 13949)
+++ pkg/trunk/util/message_sequencing/include/message_sequencing/time_sequencer.h 2009-04-16 17:04:10 UTC (rev 13950)
@@ -196,9 +196,11 @@
inline static std::string __s_getDataType() { return M::__s_getDataType(); }
inline static std::string __s_getMD5Sum() { return M::__s_getMD5Sum(); }
+ inline static std::string __s_getMessageDefinition() { return M::__s_getMessageDefinition(); }
virtual const std::string __getDataType() const { return M::__s_getDataType(); }
virtual const std::string __getMD5Sum() const { return M::__s_getMD5Sum(); }
+ virtual const std::string __getMessageDefinition() const { return M::__s_getMessageDefinition(); }
// Topic buffer is for subscribing, not publishing
virtual uint32_t serializationLength() const { return 0; }
Modified: pkg/trunk/util/mux/mux.cpp
===================================================================
--- pkg/trunk/util/mux/mux.cpp 2009-04-16 16:47:57 UTC (rev 13949)
+++ pkg/trunk/util/mux/mux.cpp 2009-04-16 17:04:10 UTC (rev 13950)
@@ -49,8 +49,12 @@
msgBuf = NULL; msgBufAlloc = 0; }
virtual const string __getDataType() const { return string("*"); }
virtual const string __getMD5Sum() const { return string("*"); }
+ /// \todo Fill this in
+ virtual const string __getMessageDefinition() const { return string(""); }
static const string __s_getDataType() { return string("*"); }
static const string __s_getMD5Sum() { return string("*"); }
+ /// \todo Fill this in
+ static const string __s_getMessageDefinition() { return string(""); }
uint32_t serializationLength() const { return msgBufUsed; }
virtual uint8_t *serialize(uint8_t *writePtr, uint32_t) const
{
@@ -159,7 +163,7 @@
topics.push_back(argv[i]);
Mux mux(argv[1], argv[2], topics);
mux.spin();
-
+
return 0;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-04-16 17:59:15
|
Revision: 13958
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13958&view=rev
Author: eitanme
Date: 2009-04-16 17:59:05 +0000 (Thu, 16 Apr 2009)
Log Message:
-----------
Moved 2dnav_pr2 to old_2dnav_pr2 in deprecated
Added Paths:
-----------
pkg/trunk/deprecated/old_2dnav_pr2/
Removed Paths:
-------------
pkg/trunk/demos/2dnav_pr2/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
Thanks for helping keep SourceForge clean.
X