Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[9539] pkg/trunk
|
From: <tpr...@us...> - 2009-01-16 17:27:29
|
Revision: 9539
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9539&view=rev
Author: tpratkanis
Date: 2009-01-16 17:27:18 +0000 (Fri, 16 Jan 2009)
Log Message:
-----------
Change the locking in highlevel controllers to make it less restrictive. Add map accessors.
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/include/HighlevelController.hh
pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh
pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp
pkg/trunk/motion_planning/sbpl_util/src/costmap_wrap.cpp
pkg/trunk/motion_planning/sbpl_util/src/costmap_wrap.h
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc
pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
pkg/trunk/world_models/costmap_2d/src/observation_buffer.cc
Modified: pkg/trunk/highlevel/highlevel_controllers/include/HighlevelController.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/HighlevelController.hh 2009-01-16 09:24:35 UTC (rev 9538)
+++ pkg/trunk/highlevel/highlevel_controllers/include/HighlevelController.hh 2009-01-16 17:27:18 UTC (rev 9539)
@@ -466,7 +466,7 @@
State state; /*!< Tracks the overall state of the controller */
double controllerCycleTime_; /*!< The duration for each control cycle */
double plannerCycleTime_; /*!< The duration for each planner cycle. */
- boost::mutex lock_; /*!< Lock for access to class members in callbacks */
+ boost::recursive_mutex lock_; /*!< Lock for access to class members in callbacks */
boost::thread* plannerThread_; /*!< Thread running the planner loop */
highlevel_controllers::Ping shutdownMsg_; /*!< For receiving shutdown from executive */
ros::Duration timeout; /*< The time limit for planning failure. */
Modified: pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2009-01-16 09:24:35 UTC (rev 9538)
+++ pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2009-01-16 17:27:18 UTC (rev 9539)
@@ -100,7 +100,7 @@
* @brief Accessor for the cost map. Use mainly for initialization
* of specialized map strunture for planning
*/
- const CostMap2D& getCostMap() const {return *costMap_;}
+ const 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
@@ -128,7 +128,7 @@
*/
void updatePlan(ompl::waypoint_plan_t const & newPlan);
- void updateCostMap(bool static_map_reset);
+ void updateCostMap();
/**
* @brief test the current plan for collisions with obstacles
@@ -239,9 +239,9 @@
VelocityController* controller_;
CostMap2D* costMap_; /**< The cost map mainatined incrementally from laser scans */
+ CostMapAccessor* global_map_accessor_; /**< Read-only access to global cost map */
+ CostMapAccessor* local_map_accessor_; /**< Read-only access to a window on the cost map */
- CostMapAccessor* ma_; /**< Sliding read-only window on the cost map */
-
tf::Stamped<tf::Pose> global_pose_; /**< The global pose in the map frame */
std_msgs::RobotBase2DOdom base_odom_; /**< Odometry in the base frame */
@@ -271,6 +271,8 @@
robot_filter::RobotFilter* filter_;
tf::MessageNotifier<std_msgs::PointCloud>* tiltLaserNotifier_;
+ //flag for reseting the costmap.
+ bool reset_cost_map_;
//ground plane extraction
ransac_ground_plane_extraction::RansacGroundPlaneExtraction ground_plane_extractor_;
Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2009-01-16 09:24:35 UTC (rev 9538)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2009-01-16 17:27:18 UTC (rev 9539)
@@ -55,12 +55,13 @@
tf_(*this, true, 10000000000ULL), // cache for 10 sec, no extrapolation
controller_(NULL),
costMap_(NULL),
- ma_(NULL),
+ local_map_accessor_(NULL),
+ global_map_accessor_(NULL),
baseLaserMaxRange_(10.0),
tiltLaserMaxRange_(10.0),
minZ_(0.02), maxZ_(2.0), robotWidth_(0.0), active_(true) , map_update_frequency_(10.0),
yaw_goal_tolerance_(0.1),
- xy_goal_tolerance_(robotWidth_ / 2)
+ xy_goal_tolerance_(robotWidth_ / 2), reset_cost_map_(false)
{
// Initialize global pose. Will be set in control loop based on actual data.
global_pose_.setIdentity();
@@ -244,7 +245,8 @@
ROS_ASSERT(mapSize <= costMap_->getWidth());
ROS_ASSERT(mapSize <= costMap_->getHeight());
- ma_ = new CostMapAccessor(*costMap_, mapSize, 0.0, 0.0);
+ global_map_accessor_ = new CostMapAccessor(*costMap_);
+ local_map_accessor_ = new CostMapAccessor(*costMap_, mapSize, 0.0, 0.0);
std_msgs::Point2DFloat32 pt;
//create a square footprint
@@ -266,7 +268,7 @@
pt.y = 0;
footprint_.push_back(pt);
- controller_ = new ros::highlevel_controllers::TrajectoryRolloutController(&tf_, *ma_,
+ controller_ = new ros::highlevel_controllers::TrajectoryRolloutController(&tf_, *local_map_accessor_,
sim_time,
sim_steps,
samples_per_dim,
@@ -327,9 +329,13 @@
if(controller_ != NULL)
delete controller_;
- if(ma_ != NULL)
- delete ma_;
+ if(local_map_accessor_ != NULL)
+ delete local_map_accessor_;
+
+ if(global_map_accessor_ != NULL)
+ delete global_map_accessor_;
+
if(costMap_ != NULL)
delete costMap_;
@@ -367,7 +373,7 @@
global_pose_.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
ROS_DEBUG("Received new position (x=%f, y=%f, th=%f)", global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), yaw);
- ma_->updateForRobotPosition(global_pose_.getOrigin().x(), global_pose_.getOrigin().y());
+ local_map_accessor_->updateForRobotPosition(global_pose_.getOrigin().x(), global_pose_.getOrigin().y());
}
@@ -376,7 +382,7 @@
*/
void MoveBase::updateGoalMsg(){
// Revert to static map on new goal. May result in oscillation, but requested by Eitan for the milestone
- updateCostMap(true);
+ reset_cost_map_ = true;
tf::Stamped<tf::Pose> goalPose, transformedGoalPose;
btQuaternion qt;
@@ -513,7 +519,7 @@
*/
void MoveBase::handlePlanningFailure(){
ROS_DEBUG("No plan found. Handling planning failure");
- updateCostMap(true);
+ reset_cost_map_ = true;
stopRobot();
}
@@ -665,9 +671,6 @@
bool planOk = checkWatchDog() && isValid();
std_msgs::BaseVel cmdVel; // Commanded velocities
- // Update the cost map window
- ma_->updateForRobotPosition(global_pose_.getOrigin().getX(), global_pose_.getOrigin().getY());
-
// if we have achieved all our waypoints but have yet to achieve the goal, then we know that we wish to accomplish our desired
// orientation
if(planOk && plan_.empty()){
@@ -758,12 +761,9 @@
/**
* @brief Utility to output local obstacles. Make the local cost map accessor. It is very cheap :-) Then
- * render the obstacles.
+ * render the obstacles. Note that the rendered window is typically larger than the local map for control
*/
void MoveBase::publishLocalCostMap() {
-
-
-
double mapSize = std::min(costMap_->getWidth()/2, costMap_->getHeight()/2);
CostMapAccessor cm(*costMap_, std::min(10.0, mapSize), global_pose_.getOrigin().x(), global_pose_.getOrigin().y());
@@ -898,26 +898,40 @@
MoveBase::footprint_t const & MoveBase::getFootprint() const{
return footprint_;
}
+
+ void MoveBase::updateCostMap() {
+ if (reset_cost_map_) {
+ costMap_->revertToStaticMap(global_pose_.getOrigin().x(), global_pose_.getOrigin().y());
+ }
- void MoveBase::updateCostMap(bool static_map_reset){
ROS_DEBUG("Starting cost map update/n");
- if(static_map_reset)
- costMap_->revertToStaticMap(global_pose_.getOrigin().x(), global_pose_.getOrigin().y());
-
- // Aggregate buffered observations across 3 sources
+
+ lock();
+ // Aggregate buffered observations across all sources. Must be thread safe
std::vector<costmap_2d::Observation> observations;
baseScanBuffer_->get_observations(observations);
tiltScanBuffer_->get_observations(observations);
lowObstacleBuffer_->get_observations(observations);
stereoCloudBuffer_->get_observations(observations);
-
+ unlock();
+
ROS_DEBUG("Applying update with %d observations/n", observations.size());
// Apply to cost map
ros::Time start = ros::Time::now();
costMap_->updateDynamicObstacles(global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), observations);
double t_diff = (ros::Time::now() - start).toSec();
+ ROS_DEBUG("Updated map in %f seconds for %d observations/n", t_diff, observations.size());
+
+ // Finally, we must extract the cost data that we have computed and:
+ // 1. Refresh the local_map_accessor for the controller
+ // 2. Refresh the global_map accessor for the planner
+ // 3. Publish the local cost map window
+ lock();
+ local_map_accessor_->refresh();
+ global_map_accessor_->refresh();
publishLocalCostMap();
- ROS_DEBUG("Updated map in %f seconds for %d observations/n", t_diff, observations.size());
+ reset_cost_map_ = false;
+ unlock();
}
/**
@@ -932,10 +946,7 @@
while (active_){
//Avoids laser race conditions.
if (isInitialized()) {
- //update the cost map without resetting to static map
- lock();
- updateCostMap(false);
- unlock();
+ updateCostMap();
}
d->sleep();
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp 2009-01-16 09:24:35 UTC (rev 9538)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp 2009-01-16 17:27:18 UTC (rev 9539)
@@ -167,7 +167,7 @@
ompl::createIndexTransformWrap(&getCostMap()), true,
0, 0, 0, 0,
CostMap2D::INSCRIBED_INFLATED_OBSTACLE);
- }
+ }
else if ("3DKIN" == environmentType) {
string const prefix("env3d/");
string obst_cost_thresh_str;
@@ -248,7 +248,7 @@
}
bool MoveBaseSBPL::isMapDataOK() {
- const CostMap2D& cm = getCostMap();
+ const CostMapAccessor& cm = getCostMap();
for(unsigned int i = 0; i<cm.getWidth(); i++){
for(unsigned int j = 0; j < cm.getHeight(); j++){
@@ -273,7 +273,7 @@
try {
// Update costs
lock();
- const CostMap2D& cm = getCostMap();
+ const CostMapAccessor& cm = getCostMap();
unsigned int x = cm.getWidth();
while(x > 0){
x--;
Modified: pkg/trunk/motion_planning/sbpl_util/src/costmap_wrap.cpp
===================================================================
--- pkg/trunk/motion_planning/sbpl_util/src/costmap_wrap.cpp 2009-01-16 09:24:35 UTC (rev 9538)
+++ pkg/trunk/motion_planning/sbpl_util/src/costmap_wrap.cpp 2009-01-16 17:27:18 UTC (rev 9539)
@@ -43,7 +43,7 @@
class cm2dCostmapWrap: public ompl::CostmapWrap {
public:
- cm2dCostmapWrap(costmap_2d::CostMap2D const * cm): cm_(cm) {}
+ cm2dCostmapWrap(costmap_2d::CostMapAccessor const * cm): cm_(cm) {}
virtual int getWSpaceObstacleCost() const { return costmap_2d::CostMap2D::INSCRIBED_INFLATED_OBSTACLE; }
virtual int getCSpaceObstacleCost() const { return costmap_2d::CostMap2D::LETHAL_OBSTACLE; }
@@ -82,13 +82,13 @@
return true;
}
- costmap_2d::CostMap2D const * cm_;
+ costmap_2d::CostMapAccessor const * cm_;
};
class cm2dTransformWrap: public ompl::IndexTransformWrap {
public:
- cm2dTransformWrap(costmap_2d::CostMap2D const * cm): cm_(cm) {}
+ cm2dTransformWrap(costmap_2d::CostMapAccessor const * cm): cm_(cm) {}
virtual void globalToIndex(double global_x, double global_y, ssize_t * index_x, ssize_t * index_y) const {
unsigned int ix, iy;
@@ -102,7 +102,7 @@
virtual double getResolution() const { return cm_->getResolution(); }
- costmap_2d::CostMap2D const * cm_;
+ costmap_2d::CostMapAccessor const * cm_;
};
@@ -173,14 +173,14 @@
namespace ompl {
- CostmapWrap * createCostmapWrap(costmap_2d::CostMap2D const * cm)
+ CostmapWrap * createCostmapWrap(costmap_2d::CostMapAccessor const * cm)
{ return new cm2dCostmapWrap(cm); }
#warning 'Using RDTravmap instead of a raw TraversabilityMap is a big performance hit!'
CostmapWrap * createCostmapWrap(sfl::RDTravmap const * rdt)
{ return new sflCostmapWrap(rdt); }
- IndexTransformWrap * createIndexTransformWrap(costmap_2d::CostMap2D const * cm)
+ IndexTransformWrap * createIndexTransformWrap(costmap_2d::CostMapAccessor const * cm)
{ return new cm2dTransformWrap(cm); }
IndexTransformWrap * createIndexTransformWrap(sfl::GridFrame const * gf)
Modified: pkg/trunk/motion_planning/sbpl_util/src/costmap_wrap.h
===================================================================
--- pkg/trunk/motion_planning/sbpl_util/src/costmap_wrap.h 2009-01-16 09:24:35 UTC (rev 9538)
+++ pkg/trunk/motion_planning/sbpl_util/src/costmap_wrap.h 2009-01-16 17:27:18 UTC (rev 9539)
@@ -41,6 +41,7 @@
namespace costmap_2d {
class CostMap2D;
+ class CostMapAccessor;
}
namespace sfl {
@@ -105,10 +106,10 @@
typedef GenericIndexTransformWrap<ssize_t> IndexTransformWrap;
- CostmapWrap * createCostmapWrap(costmap_2d::CostMap2D const * cm);
+ CostmapWrap * createCostmapWrap(costmap_2d::CostMapAccessor const * cm);
CostmapWrap * createCostmapWrap(sfl::RDTravmap const * rdt);
- IndexTransformWrap * createIndexTransformWrap(costmap_2d::CostMap2D const * cm);
+ IndexTransformWrap * createIndexTransformWrap(costmap_2d::CostMapAccessor const * cm);
IndexTransformWrap * createIndexTransformWrap(sfl::GridFrame const * gf);
}
Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2009-01-16 09:24:35 UTC (rev 9538)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2009-01-16 17:27:18 UTC (rev 9539)
@@ -316,10 +316,21 @@
CostMapAccessor(const CostMap2D& costMap, double maxSize, double pose_x, double pose_y);
/**
+ * @broef Constructor
+ * Use when we want to access the whole cost map rather than a window
+ */
+ CostMapAccessor(const CostMap2D& costMap);
+
+ /**
* @brief Set the pose for the robot. Will adjust other parameters accordingly.
*/
void updateForRobotPosition(double wx, double wy);
+ /**
+ * @brief Refresh the copied cost data structure from the source cost map
+ */
+ void refresh();
+
private:
static unsigned int computeSize(double maxSize, double resolution);
Modified: pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc 2009-01-16 09:24:35 UTC (rev 9538)
+++ pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc 2009-01-16 17:27:18 UTC (rev 9539)
@@ -99,6 +99,7 @@
Observation obs(o, map_cloud);
buffer_observation(obs);
map_cloud = NULL;
+ newData = NULL;
}
// In case we get thrown out on the second transform - clean up
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp 2009-01-16 09:24:35 UTC (rev 9538)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp 2009-01-16 17:27:18 UTC (rev 9539)
@@ -536,20 +536,32 @@
CostMapAccessor::CostMapAccessor(const CostMap2D& costMap, double maxSize, double poseX, double poseY)
: ObstacleMapAccessor(computeWX(costMap, maxSize, poseX, poseY),
- computeWY(costMap, maxSize, poseX, poseY),
- computeSize(maxSize, costMap.getResolution()),
- computeSize(maxSize, costMap.getResolution()),
- costMap.getResolution()), costMap_(costMap),
- maxSize_(maxSize){
+ computeWY(costMap, maxSize, poseX, poseY),
+ computeSize(maxSize, costMap.getResolution()),
+ computeSize(maxSize, costMap.getResolution()),
+ costMap.getResolution()),
+ costMap_(costMap), maxSize_(maxSize){
- setCircumscribedCostLowerBound(costMap.getCircumscribedCostLowerBound());
+ setCircumscribedCostLowerBound(costMap.getCircumscribedCostLowerBound());
- // Set robot position
- updateForRobotPosition(poseX, poseY);
+ // Set robot position
+ updateForRobotPosition(poseX, poseY);
- ROS_DEBUG_NAMED("costmap_2d", "Creating Local %d X %d Map\n", getWidth(), getHeight());
- }
+ ROS_DEBUG_NAMED("costmap_2d", "Creating Local %d X %d Map\n", getWidth(), getHeight());
+ }
+ CostMapAccessor::CostMapAccessor(const CostMap2D& costMap)
+ : ObstacleMapAccessor(0.0, 0.0, costMap.getWidth(), costMap.getHeight(), costMap.getResolution()),
+ costMap_(costMap), maxSize_(0.0), mx_0_(0), my_0_(0){
+
+ setCircumscribedCostLowerBound(costMap.getCircumscribedCostLowerBound());
+
+ // Set values from source
+ refresh();
+
+ ROS_DEBUG_NAMED("costmap_2d", "Creating Local %d X %d Map\n", getWidth(), getHeight());
+ }
+
void CostMapAccessor::updateForRobotPosition(double wx, double wy){
if(wx < 0 || wx > costMap_.getResolution() * costMap_.getWidth())
return;
@@ -559,11 +571,22 @@
origin_x_ = computeWX(costMap_, maxSize_, wx, wy);
origin_y_ = computeWY(costMap_, maxSize_, wx, wy);
- costMap_.WC_MC(origin_x_, origin_y_, mx_0_, my_0_);
+ costMap_.WC_MC(origin_x_, origin_y_, mx_0_, my_0_);
ROS_DEBUG( "Moving map to locate at <%f, %f> and size of %f meters for position <%f, %f>\n",
origin_x_, origin_y_, maxSize_, wx, wy);
+ refresh();
+ }
+
+
+ unsigned int CostMapAccessor::computeSize(double maxSize, double resolution){
+ unsigned int cellWidth = (unsigned int) ceil(maxSize/resolution);
+ ROS_DEBUG("Given a size of %f and a resolution of %f, we have a cell width of %d\n", maxSize, resolution, cellWidth);
+ return cellWidth;
+ }
+
+ void CostMapAccessor::refresh(){
// Now update all the cells from the cost map
for(unsigned int x = 0; x < width_; x++){
for (unsigned int y = 0; y < height_; y++){
@@ -574,14 +597,4 @@
}
}
-
- unsigned int CostMapAccessor::computeSize(double maxSize, double resolution){
- unsigned int cellWidth = (unsigned int) ceil(maxSize/resolution);
- ROS_DEBUG("Given a size of %f and a resolution of %f, we have a cell width of %d\n", maxSize, resolution, cellWidth);
- return cellWidth;
- }
-
-
-
-
}
Modified: pkg/trunk/world_models/costmap_2d/src/observation_buffer.cc
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/observation_buffer.cc 2009-01-16 09:24:35 UTC (rev 9538)
+++ pkg/trunk/world_models/costmap_2d/src/observation_buffer.cc 2009-01-16 17:27:18 UTC (rev 9539)
@@ -77,8 +77,17 @@
// observation.cloud_->header.stamp.toSec(), last_updated_.toSec());
}
- // If the duration is 0, then we just keep the latest one, so we clear out all existing observations
- while(!buffer_.empty()){
+ // Otherwise just store it and indicate success
+ buffer_.push_back(observation);
+ return true;
+ }
+
+ void ObservationBuffer::get_observations(std::vector<Observation>& observations){
+ // If the duration is 0, then we just keep the latest one, so we must have a limit of one.
+ // If the duration is non-zero, we want to delete all the observations.
+ unsigned int min_observations = (keep_alive_ == 0) ? 1 : 0;
+
+ while(buffer_.size() > min_observations){
std::list<Observation>::iterator it = buffer_.begin();
// Get the current one, and check if still alive. if so
Observation& obs = *it;
@@ -90,12 +99,6 @@
break;
}
- // Otherwise just store it and indicate success
- buffer_.push_back(observation);
- return true;
- }
-
- void ObservationBuffer::get_observations(std::vector<Observation>& observations){
// Add all remaining observations to the output
for(std::list<Observation>::const_iterator it = buffer_.begin(); it != buffer_.end(); ++it){
observations.push_back(*it);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
