Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[7133] pkg/trunk
|
From: <mc...@us...> - 2008-11-21 16:12:15
|
Revision: 7133
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7133&view=rev
Author: mcgann
Date: 2008-11-21 16:12:09 +0000 (Fri, 21 Nov 2008)
Log Message:
-----------
Changed persistence policy to only revert to the static map when we receive a new goal or when no plan is found. This is trying to enable continuous plannnig but also provide some measure of hysterisis in the face of dynamic obstacles that move out of sensor range
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/test/launch_move_base.xml
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc
Modified: pkg/trunk/highlevel/highlevel_controllers/include/HighlevelController.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/HighlevelController.hh 2008-11-21 13:54:59 UTC (rev 7132)
+++ pkg/trunk/highlevel/highlevel_controllers/include/HighlevelController.hh 2008-11-21 16:12:09 UTC (rev 7133)
@@ -158,11 +158,17 @@
if(isInitialized() && isActive()){
// If the plannerCycleTime is 0 then we only call the planner when we need to
- if(plannerCycleTime_ != 0 || !isValid())
+ if(plannerCycleTime_ != 0 || !isValid()){
setValid(makePlan());
+ if(!isValid()){
+ // Could use a refined locking scheme but for now do not want to delegate that to a derived class
+ lock();
+ handlePlanningFailure();
+ unlock();
+ }
+ }
}
-
if(plannerCycleTime_ >= 0)
sleep(currentTime, std::max(plannerCycleTime_, controllerCycleTime_));
else
@@ -238,6 +244,10 @@
*/
virtual void handleActivation(){}
+ /**
+ * @brief A hook to handle the case when global planning fails
+ */
+ virtual void handlePlanningFailure(){}
/**
* @brief Aquire node level lock
Modified: pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2008-11-21 13:54:59 UTC (rev 7132)
+++ pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2008-11-21 16:12:09 UTC (rev 7133)
@@ -96,6 +96,12 @@
virtual void handleMapUpdates(const std::vector<unsigned int>& updates){}
/**
+ * @brief When planning has failed should reset the cost map to clear persistent
+ * dynamic obstacles. This is important to provide some hysterisis when interleaving planning
+ */
+ virtual void handlePlanningFailure();
+
+ /**
* @brief Overwrites the current plan with a new one. Will handle suitable publication
* @see publishPlan
*/
Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-11-21 13:54:59 UTC (rev 7132)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-11-21 16:12:09 UTC (rev 7133)
@@ -317,6 +317,10 @@
stateMsg.goal.y = goalPose.y;
stateMsg.goal.th = goalPose.yaw;
+
+ // We want to revert to the static map, and resume computations for cost map based on laser data - whenever we get a new goal
+ costMap_->revertToStaticMap();
+
ROS_DEBUG("Received new goal (x=%f, y=%f, th=%f)\n", goalMsg.goal.x, goalMsg.goal.y, goalMsg.goal.th);
}
@@ -400,6 +404,13 @@
base_odom_.unlock();
}
+ /**
+ * A lock will already be aquired here, so just revert the cost map
+ */
+ void MoveBase::handlePlanningFailure(){
+ costMap_->revertToStaticMap();
+ }
+
void MoveBase::updatePlan(const std::list<std_msgs::Pose2DFloat32>& newPlan){
lock();
@@ -788,6 +799,7 @@
ROS_DEBUG("Starting cost map update/n");
lock();
+
// Aggregate buffered observations across 3 sources
std::vector<costmap_2d::Observation> observations;
baseScanBuffer_->get_observations(observations);
Modified: pkg/trunk/highlevel/highlevel_controllers/test/launch_move_base.xml
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/test/launch_move_base.xml 2008-11-21 13:54:59 UTC (rev 7132)
+++ pkg/trunk/highlevel/highlevel_controllers/test/launch_move_base.xml 2008-11-21 16:12:09 UTC (rev 7133)
@@ -7,8 +7,8 @@
<param name="move_base/plannerType" value="ARAPlanner"/>
<param name="move_base/environmentType" value="2D"/>
<param name="move_base/controller_frequency" value="20.0"/>
- <param name="move_base/planner_frequency" value="0.0"/>
- <param name="move_base/plannerTimeLimit" value="10.0"/>
+ <param name="move_base/planner_frequency" value="1.0"/>
+ <param name="move_base/plannerTimeLimit" value="5.0"/>
<param name="/costmap_2d/base_laser_max_range" value="10.0"/>
<param name="/costmap_2d/tilt_laser_max_range" value="10.0"/>
<param name="/costmap_2d/lethal_obstacle_threshold" value="100.0"/>
Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2008-11-21 13:54:59 UTC (rev 7132)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2008-11-21 16:12:09 UTC (rev 7133)
@@ -174,6 +174,10 @@
*/
double getWeight() const {return weight_;}
+ /**
+ * @brief Will reset the cost data to the initiali propagated costs of the static map
+ */
+ void revertToStaticMap();
private:
/**
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp 2008-11-21 13:54:59 UTC (rev 7132)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp 2008-11-21 16:12:09 UTC (rev 7133)
@@ -139,6 +139,10 @@
}
+ void CostMap2D::revertToStaticMap(){
+ memcpy(costData_, staticData_, width_ * height_);
+ }
+
/**
* @brief Updated dyanmic obstacles and compute a diff. Mainly for backward compatibility. This will go away soon.
*/
@@ -187,9 +191,7 @@
{
// Revert to initial state
memset(xy_markers_, 0, width_ * height_ * sizeof(bool));
- memcpy(costData_, staticData_, width_ * height_);
-
// Now propagate free space. We iterate again over observations, process only those from an origin
// within a specific range, and a point within a certain z-range. We only want to propagate free space
// in 2D so keep point and its origin within expected range
Modified: pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc 2008-11-21 13:54:59 UTC (rev 7132)
+++ pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc 2008-11-21 16:12:09 UTC (rev 7133)
@@ -127,6 +127,7 @@
}
// Update with no hits. Should clear (revert to the static map
+ map.revertToStaticMap();
cloud.set_pts_size(0);
map.updateDynamicObstacles(0, 0, CostMap2D::toVector(cloud));
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
