Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[6881] pkg/trunk
|
From: <mc...@us...> - 2008-11-18 14:29:18
|
Revision: 6881
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6881&view=rev
Author: mcgann
Date: 2008-11-18 14:29:12 +0000 (Tue, 18 Nov 2008)
Log Message:
-----------
Added observation buffers for multiple sources
Modified Paths:
--------------
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/highlevel/highlevel_controllers/test/launch_move_base.xml
pkg/trunk/highlevel/highlevel_controllers/test/launch_stage_5cm.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/MoveBase.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2008-11-18 14:25:33 UTC (rev 6880)
+++ pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2008-11-18 14:29:12 UTC (rev 6881)
@@ -63,6 +63,38 @@
namespace ros {
namespace highlevel_controllers {
+ /**
+ * @brief Extend base class to handle buffering until a transform is available, and to support locking for mult-threaded
+ * access
+ */
+ class ObservationBuffer: public costmap_2d::ObservationBuffer {
+ public:
+ ObservationBuffer(const std::string& frame_id, rosTFClient& tf, ros::Duration keepAlive, double robotRadius, double minZ, double maxZ);
+
+ void buffer_cloud(const std_msgs::PointCloud& local_cloud);
+
+ virtual void get_observations(std::vector<Observation>& observations);
+
+ private:
+
+ /**
+ * @brief Test if point in the square footprint of the robot
+ */
+ bool inFootprint(double x, double y) const;
+
+ /**
+ * @brief Provide a filtered set of points based on the extraction of the robot footprint and the
+ * filter based on the min and max z values
+ */
+ std_msgs::PointCloud * extractFootprintAndGround(const std_msgs::PointCloud& baseFrameCloud) const;
+
+ const std::string frame_id_;
+ rosTFClient& tf_;
+ std::deque<std_msgs::PointCloud> point_clouds_; /**< Buffer point clouds until a transform is available */
+ ros::thread::mutex buffer_mutex_;
+ const double robotRadius_, minZ_, maxZ_; /**< Constraints for filtering points */
+ };
+
class MoveBase : public HighlevelController<std_msgs::Planner2DState, std_msgs::Planner2DGoal> {
public:
@@ -149,22 +181,9 @@
*/
void odomCallback();
- void bufferData(const std_msgs::PointCloud& local_cloud);
-
- /**
- * @brief Process point cloud data
- * @todo migrate to costmap
- */
- void processData();
-
void updateGlobalPose();
/**
- * @brief Helper method to update the costmap and conduct other book-keeping
- */
- void updateDynamicObstacles(const std::vector<std_msgs::PointCloud*>& clouds);
-
- /**
* @brief Issue zero velocity commands
*/
void stopRobot();
@@ -190,17 +209,6 @@
bool withinDistance(double x1, double y1, double th1, double x2, double y2, double th2) const ;
/**
- * @brief Test if point in the square footprint of the robot
- */
- bool inFootprint(double x, double y) const;
-
- /**
- * @brief Provide a filtered set of points based on the extraction of the robot footprint and the
- * filter based on the min and max z values
- */
- std_msgs::PointCloud * extractFootprintAndGround(const std_msgs::PointCloud& baseFrameCloud) const;
-
- /**
* @brief Watchdog Handling
*/
void petTheWatchDog(const ros::Time& t);
@@ -220,6 +228,11 @@
rosTFClient tf_; /**< Used to do transforms */
+ // Observation Buffers
+ ObservationBuffer* baseScanBuffer_;
+ ObservationBuffer* tiltScanBuffer_;
+ ObservationBuffer* stereoCloudBuffer_;
+
/** Should encapsulate as a controller wrapper that is not resident in the trajectory rollout package */
VelocityController* controller_;
@@ -237,9 +250,6 @@
std::list<std_msgs::Pose2DFloat32> plan_; /**< The 2D plan in grid co-ordinates of the cost map */
- std::deque<std_msgs::PointCloud> point_clouds_; /**< Buffer point clouds until a transform is available */
- ros::thread::mutex bufferMutex_;
-
// Filter parameters
double minZ_;
double maxZ_;
Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-11-18 14:25:33 UTC (rev 6880)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-11-18 14:29:12 UTC (rev 6881)
@@ -46,14 +46,153 @@
namespace ros {
namespace highlevel_controllers {
+ ObservationBuffer::ObservationBuffer(const std::string& frame_id, rosTFClient& tf, ros::Duration keepAlive,
+ double robotRadius, double minZ, double maxZ)
+ : costmap_2d::ObservationBuffer(keepAlive), frame_id_(frame_id), tf_(tf), robotRadius_(robotRadius), minZ_(minZ), maxZ_(maxZ){}
+
+ void ObservationBuffer::buffer_cloud(const std_msgs::PointCloud& local_cloud)
+ {
+ static const ros::Duration max_transform_delay(10, 0); // max time we will wait for a transform before chucking out the data
+ point_clouds_.push_back(local_cloud);
+
+ std_msgs::PointCloud * newData = NULL;
+ std_msgs::PointCloud * map_cloud = NULL;
+
+ while(!point_clouds_.empty()){
+ const std_msgs::PointCloud& point_cloud = point_clouds_.front();
+ std_msgs::Point origin;
+
+ // Throw out point clouds that are old.
+ if((last_updated_ - point_cloud.header.stamp) > max_transform_delay){
+ ROS_INFO("Discarding stale point cloud\n");
+ point_clouds_.pop_front();
+ continue;
+ }
+
+ libTF::TFPoint map_origin;
+ std_msgs::PointCloud base_cloud;
+
+ /* Transform to the base frame */
+ try
+ {
+ // First we want the origin for the sensor
+ libTF::TFPoint local_origin;
+ local_origin.x = 0;
+ local_origin.y = 0;
+ local_origin.z = 0;
+ local_origin.time = point_cloud.header.stamp.toNSec();
+ local_origin.frame = frame_id_;
+ map_origin = tf_.transformPoint("map", local_origin);
+
+ tf_.transformPointCloud("base", base_cloud, point_cloud);
+ newData = extractFootprintAndGround(base_cloud);
+ map_cloud = new std_msgs::PointCloud();
+ tf_.transformPointCloud("map", *map_cloud, *newData);
+
+ ROS_INFO("Buffering cloud for %s at origin <%f, %f, %f>\n", frame_id_.c_str(), map_origin.x, map_origin.y, map_origin.z);
+ }
+ catch(libTF::TransformReference::LookupException& ex)
+ {
+ ROS_ERROR("Lookup exception: %s\n", ex.what());
+ break;
+ }
+ catch(libTF::TransformReference::ExtrapolateException& ex)
+ {
+ ROS_DEBUG("No transform available yet - have to try later: %s . Buffer size is %d\n", ex.what(), point_clouds_.size());
+ break;
+ }
+ catch(libTF::TransformReference::ConnectivityException& ex)
+ {
+ ROS_ERROR("Connectivity exception: %s\n", ex.what());
+ break;
+ }
+ catch(...)
+ {
+ ROS_ERROR("Exception in point cloud computation\n");
+ break;
+ }
+
+ point_clouds_.pop_front();
+
+ if (newData != NULL){
+ delete newData;
+ newData = NULL;
+ }
+
+ // Get the point from whihc we ray trace
+ std_msgs::Point o;
+ o.x = map_origin.x;
+ o.y = map_origin.y;
+ o.z = map_origin.z;
+
+ // Allocate and buffer the observation
+ Observation obs(o, map_cloud);
+ buffer_observation(obs);
+ map_cloud = NULL;
+ }
+
+ // In case we get thrown out on the second transform - clean up
+ if (newData != NULL){
+ delete newData;
+ newData = NULL;
+ }
+
+ if(map_cloud != NULL){
+ delete map_cloud;
+ map_cloud = NULL;
+ } }
+
+ void ObservationBuffer::get_observations(std::vector<costmap_2d::Observation>& observations){
+ costmap_2d::ObservationBuffer::get_observations(observations);
+ }
+
+ /**
+ * The point is in the footprint if its x and y values are in the range [0 robotWidth] in
+ * the base frame.
+ */
+ bool ObservationBuffer::inFootprint(double x, double y) const {
+ bool result = fabs(x) <= robotRadius_ && fabs(y) <= robotRadius_;
+
+ if(result){
+ ROS_DEBUG("Discarding point <%f, %f> in footprint\n", x, y);
+ }
+
+ return result;
+ }
+
+ std_msgs::PointCloud * ObservationBuffer::extractFootprintAndGround(const std_msgs::PointCloud& baseFrameCloud) const {
+ std_msgs::PointCloud *copy = new std_msgs::PointCloud();
+ copy->header = baseFrameCloud.header;
+
+ unsigned int n = baseFrameCloud.get_pts_size();
+ unsigned int j = 0;
+ copy->set_pts_size(n);
+
+ for (unsigned int k = 0 ; k < n ; ++k){
+ bool ok = baseFrameCloud.pts[k].z > minZ_ && baseFrameCloud.pts[k].z < maxZ_;
+ if (ok && !inFootprint(baseFrameCloud.pts[k].x, baseFrameCloud.pts[k].y)){
+ copy->pts[j++] = baseFrameCloud.pts[k];
+ }
+ else {
+ ROS_DEBUG("Discarding <%f, %f, %f>\n", baseFrameCloud.pts[k].x, baseFrameCloud.pts[k].y, baseFrameCloud.pts[k].z);
+ }
+ }
+
+ copy->set_pts_size(j);
+
+ ROS_DEBUG("Filter discarded %d points (%d left) \n", n - j, j);
+
+ return copy;
+ }
+
MoveBase::MoveBase()
: HighlevelController<std_msgs::Planner2DState, std_msgs::Planner2DGoal>("move_base", "state", "goal"),
- tf_(*this, true, 10000000000ULL, 0ULL), // cache for 10 sec, no extrapolation
- controller_(NULL),
- costMap_(NULL),
- ma_(NULL),
- baseLaserMaxRange_(10.0),
- tiltLaserMaxRange_(10.0),
+ tf_(*this, true, 10000000000ULL, 0ULL), // cache for 10 sec, no extrapolation
+ controller_(NULL),
+ costMap_(NULL),
+ ma_(NULL),
+ baseLaserMaxRange_(10.0),
+ tiltLaserMaxRange_(10.0),
minZ_(0.03), maxZ_(2.0), robotWidth_(0.0), active_(true) , map_update_frequency_(10.0)
{
// Initialize global pose. Will be set in control loop based on actual data.
@@ -66,7 +205,7 @@
base_odom_.vel.y = 0;
base_odom_.vel.th = 0;
- // Initialize state message parameters that are unsused
+ // Initialize state message parameters that are unused
stateMsg.waypoint.x = 0.0;
stateMsg.waypoint.y = 0.0;
stateMsg.waypoint.th = 0.0;
@@ -104,6 +243,11 @@
robotWidth_ = inscribedRadius * 2;
+ // Allocate observation buffers
+ baseScanBuffer_ = new ObservationBuffer(std::string("base_laser"), tf_, ros::Duration(0, 0), inscribedRadius, minZ_, maxZ_);
+ tiltScanBuffer_ = new ObservationBuffer(std::string("tilt_laser"), tf_, ros::Duration(1, 0), inscribedRadius, minZ_, maxZ_);
+ stereoCloudBuffer_ = new ObservationBuffer(std::string("stereo"), tf_, ros::Duration(0, 0), inscribedRadius, minZ_, maxZ_);
+
// get map via RPC
std_srvs::StaticMap::request req;
std_srvs::StaticMap::response resp;
@@ -128,7 +272,7 @@
// Now allocate the cost map and its sliding window used by the controller
costMap_ = new CostMap2D((unsigned int)resp.map.width, (unsigned int)resp.map.height,
inputData , resp.map.resolution,
- lethalObstacleThreshold, maxZ_, freeSpaceProjectionHeight,
+ lethalObstacleThreshold, maxZ_, 0.0, freeSpaceProjectionHeight,
inflationRadius, circumscribedRadius, inscribedRadius, weight);
// Allocate Velocity Controller
@@ -204,7 +348,7 @@
// Advertize message to publish velocity cmds
advertise<std_msgs::BaseVel>("cmd_vel", 1);
- //Advertize message to publis local goal for head to track
+ //Advertize message to publish local goal for head to track
advertise<std_msgs::PointStamped>("head_controller/head_track_point", 1);
// The cost map is populated with either laser scans in the case that we are unable to use a
@@ -238,6 +382,9 @@
if(costMap_ != NULL)
delete costMap_;
+ delete baseScanBuffer_;
+ delete tiltScanBuffer_;
+ delete stereoCloudBuffer_;
}
void MoveBase::updateGlobalPose(){
@@ -309,13 +456,16 @@
void MoveBase::baseScanCallback()
{
- ROS_INFO("Base Scan Callback");
+ ROS_DEBUG("Base Scan Callback");
// Project laser into point cloud
std_msgs::PointCloud local_cloud;
local_cloud.header = baseScanMsg_.header;
projector_.projectLaser(baseScanMsg_, local_cloud, baseLaserMaxRange_);
- ROS_INFO("Projected");
- bufferData(local_cloud);
+ petTheWatchDog(local_cloud.header.stamp);
+ ROS_DEBUG("Projected");
+ lock();
+ baseScanBuffer_->buffer_cloud(local_cloud);
+ unlock();
}
void MoveBase::tiltScanCallback()
@@ -324,144 +474,19 @@
std_msgs::PointCloud local_cloud;
local_cloud.header = tiltScanMsg_.header;
projector_.projectLaser(tiltScanMsg_, local_cloud, tiltLaserMaxRange_);
- bufferData(local_cloud);
+ lock();
+ tiltScanBuffer_->buffer_cloud(local_cloud);
+ unlock();
}
void MoveBase::stereoCloudCallback()
{
- bufferData(stereoCloudMsg_);
+ lock();
+ stereoCloudBuffer_->buffer_cloud(stereoCloudMsg_);
+ unlock();
}
- void MoveBase::bufferData(const std_msgs::PointCloud& local_cloud)
- {
- bufferMutex_.lock();
- point_clouds_.push_back(local_cloud);
- petTheWatchDog(local_cloud.header.stamp);
- bufferMutex_.unlock();
- }
-
- void MoveBase::processData()
- {
- // Setting timing values to
- const ros::Duration maxDuration(10, 0);
-
- bufferMutex_.lock();
-
- std_msgs::PointCloud * newData = NULL;
- std_msgs::PointCloud * map_cloud = NULL;
- std::vector<std_msgs::PointCloud*> pending_updates;
-
- while(!point_clouds_.empty()){
- const std_msgs::PointCloud& point_cloud = point_clouds_.front();
-
- // Throw out point clouds that are old.
- if(last_time_stamp_ - point_cloud.header.stamp > maxDuration){
- ROS_INFO("Discarding stale point cloud\n");
- point_clouds_.pop_front();
- continue;
- }
-
- std_msgs::PointCloud base_cloud;
-
- /* Transform to the base frame */
- try
- {
- tf_.transformPointCloud("base", base_cloud, point_cloud);
- newData = extractFootprintAndGround(base_cloud);
- map_cloud = new std_msgs::PointCloud();
- tf_.transformPointCloud("map", *map_cloud, *newData);
- }
- catch(libTF::TransformReference::LookupException& ex)
- {
- ROS_ERROR("Lookup exception: %s\n", ex.what());
- break;
- }
- catch(libTF::TransformReference::ExtrapolateException& ex)
- {
- ROS_DEBUG("No transform available yet - have to try later: %s . Buffer size is %d\n", ex.what(), point_clouds_.size());
- break;
- }
- catch(libTF::TransformReference::ConnectivityException& ex)
- {
- ROS_ERROR("Connectivity exception: %s\n", ex.what());
- break;
- }
- catch(...)
- {
- ROS_ERROR("Exception in point cloud computation\n");
- break;
- }
-
- point_clouds_.pop_front();
-
- if (newData != NULL){
- delete newData;
- newData = NULL;
- }
-
- pending_updates.push_back(map_cloud);
- map_cloud = NULL;
- }
-
- // In case we get thrown out on the second transform - clean up
- if (newData != NULL){
- delete newData;
- newData = NULL;
- }
-
- if(map_cloud != NULL){
- delete map_cloud;
- map_cloud = NULL;
- }
-
- updateDynamicObstacles(pending_updates);
-
- // Now clean up retrieved clouds
- for(std::vector<std_msgs::PointCloud*>::const_iterator it = pending_updates.begin(); it != pending_updates.end(); ++it)
- delete *it;
-
- bufferMutex_.unlock();
- }
-
/**
- * The point is in the footprint if its x and y values are in the range [0 robotWidth] in
- * the base frame.
- */
- bool MoveBase::inFootprint(double x, double y) const {
- bool result = fabs(x) <= robotWidth_/2 && fabs(y) <= robotWidth_/2;
-
- if(result){
- ROS_DEBUG("Discarding point <%f, %f> in footprint\n", x, y);
- }
- return result;
- }
-
- std_msgs::PointCloud * MoveBase::extractFootprintAndGround(const std_msgs::PointCloud& baseFrameCloud) const {
- std_msgs::PointCloud *copy = new std_msgs::PointCloud();
- copy->header = baseFrameCloud.header;
-
- unsigned int n = baseFrameCloud.get_pts_size();
- unsigned int j = 0;
- copy->set_pts_size(n);
-
- for (unsigned int k = 0 ; k < n ; ++k){
- bool ok = baseFrameCloud.pts[k].z > minZ_ && baseFrameCloud.pts[k].z < maxZ_;
- if (ok && !inFootprint(baseFrameCloud.pts[k].x, baseFrameCloud.pts[k].y)){
- copy->pts[j++] = baseFrameCloud.pts[k];
- }
- else {
- ROS_DEBUG("Discarding <%f, %f, %f>\n", baseFrameCloud.pts[k].x, baseFrameCloud.pts[k].y, baseFrameCloud.pts[k].z);
- }
- }
-
- copy->set_pts_size(j);
-
- ROS_DEBUG("Filter discarded %d points (%d left) \n", n - j, j);
-
- return copy;
- }
-
- /**
* The odomMsg_ will be updates and we will do the transform to update the odom in the base frame
*/
void MoveBase::odomCallback(){
@@ -674,7 +699,6 @@
// The plan is bogus if it is empty
if(planOk && plan_.empty()){
planOk = false;
- ROS_ASSERT(inCollision());
ROS_DEBUG("No path points in local window.\n");
}
@@ -694,11 +718,12 @@
ROS_DEBUG("Velocity Controller could not find a valid trajectory.\n");
planOk = false;
}
+
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_DEBUG("Cycle Time: %.3f\n", t_diff);
+ ROS_INFO("Cycle Time: %.3f\n", t_diff);
if(!planOk){
// Zero out the velocities
@@ -858,7 +883,7 @@
}
void MoveBase::stopRobot(){
- ROS_INFO("Stopping the robot now!\n");
+ ROS_DEBUG("Stopping the robot now!\n");
std_msgs::BaseVel cmdVel; // Commanded velocities
cmdVel.vx = 0.0;
cmdVel.vy = 0.0;
@@ -870,41 +895,48 @@
stopRobot();
}
- void MoveBase::updateDynamicObstacles(const std::vector<std_msgs::PointCloud*>& clouds){
- //Avoids laser race conditions.
- if (!isInitialized()) {
- return;
- }
-
- std::vector<unsigned int> updates;
- lock();
- struct timeval curr;
- gettimeofday(&curr,NULL);
- double curr_t, last_t, t_diff;
- curr_t = curr.tv_sec + curr.tv_usec / 1e6;
- costMap_->updateDynamicObstacles(global_pose_.x, global_pose_.y, clouds, updates);
- gettimeofday(&curr,NULL);
- last_t = curr.tv_sec + curr.tv_usec / 1e6;
- t_diff = last_t - curr_t;
- handleMapUpdates(updates);
- publishLocalCostMap();
- unlock();
- ROS_INFO("Updated map in %f seconds/n", t_diff);
- }
-
MoveBase::footprint_t const & MoveBase::getFootprint() const{
return footprint_;
}
-
+ /**
+ * Each update loop will query all observations and aggregate them and then apply
+ * a batch update to the cost map
+ */
void MoveBase::mapUpdateLoop()
{
ros::Duration *d = new ros::Duration(1.0/map_update_frequency_);
while (active_){
- processData();
+ //Avoids laser race conditions.
+ if (isInitialized()) {
+
+ ROS_INFO("Starting cost map update/n");
+ lock();
+ // Aggregate buffered observations across 3 sources
+ std::vector<costmap_2d::Observation> observations;
+ baseScanBuffer_->get_observations(observations);
+ tiltScanBuffer_->get_observations(observations);
+ stereoCloudBuffer_->get_observations(observations);
+
+ ROS_INFO("Applying update with %d observations/n", observations.size());
+ // Apply to cost map
+ struct timeval curr;
+ gettimeofday(&curr,NULL);
+ double curr_t, last_t, t_diff;
+ curr_t = curr.tv_sec + curr.tv_usec / 1e6;
+ costMap_->updateDynamicObstacles(global_pose_.x, global_pose_.y, observations);
+ gettimeofday(&curr,NULL);
+ last_t = curr.tv_sec + curr.tv_usec / 1e6;
+ t_diff = last_t - curr_t;
+ publishLocalCostMap();
+ unlock();
+ ROS_INFO("Updated map in %f seconds for %d observations/n", t_diff, observations.size());
+ }
+
d->sleep();
}
+
delete d;
}
}
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp 2008-11-18 14:25:33 UTC (rev 6880)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp 2008-11-18 14:29:12 UTC (rev 6881)
@@ -106,12 +106,6 @@
protected:
/**
- * @brief Called during update of cost map. Will just buffer and handle in batch.
- * @see applyMapUpdates
- */
- virtual void handleMapUpdates(const std::vector<unsigned int>& updates);
-
- /**
* @brief Builds a plan from current state to goal state
*/
virtual bool makePlan();
@@ -248,20 +242,6 @@
delete pMgr_;
}
- /**
- * @brief This is called during a cost map update. Will insert new updates, possibly overwriting prior values
- */
- void MoveBaseSBPL::handleMapUpdates(const std::vector<unsigned int>& updates){
-
- const CostMap2D& cm = getCostMap();
-
- for(std::vector<unsigned int>::const_iterator it = updates.begin(); it != updates.end(); ++it){
- unsigned int x, y; // Cell coordinates
- cm.IND_MC(*it, x, y);
- env_->UpdateCost(x, y, (unsigned char) cm.getCost(x, y));
- }
- }
-
bool MoveBaseSBPL::isMapDataOK() {
const CostMap2D& cm = getCostMap();
@@ -287,8 +267,20 @@
ompl::SBPLPlannerStatistics::entry & statsEntry(pStat_.top());
try {
+ // Update costs
+ lock();
const CostMap2D& cm = getCostMap();
-
+ unsigned int x = cm.getWidth();
+ while(x > 0){
+ x--;
+ unsigned int y = cm.getHeight();
+ while(y > 0){
+ y--;
+ env_->UpdateCost(x, y, (unsigned char) cm.getCost(x, y));
+ }
+ }
+ unlock();
+
// Copy out start and goal states to minimize locking requirement. Lock was not previously required because the
// planner and controller were running on the same thread and the only contention was for cost map updates on call
// backs. Note that cost map queries here are const methods that merely do co-ordinate transformations, so we do not need
@@ -369,7 +361,7 @@
prevth = th;
#warning 'add the cumulation of delta(waypoint.th) now that we can have 3D plans'
}
-
+
plan.push_back(waypoint);
}
// probably we should add the delta from the last theta to
Modified: pkg/trunk/highlevel/highlevel_controllers/test/launch_move_base.xml
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/test/launch_move_base.xml 2008-11-18 14:25:33 UTC (rev 6880)
+++ pkg/trunk/highlevel/highlevel_controllers/test/launch_move_base.xml 2008-11-18 14:29:12 UTC (rev 6881)
@@ -1,7 +1,6 @@
<launch>
<group name="wg">
- <!--param name="costmap_2d/dynamic_obstacle_window" value="255.0"/-->
<!-- for the moment use e.g.: ./shparam set move_base/plannerType string ADPlanner -->
<param name="move_base/environmentList" value="2D,3DKIN"/> <!-- future extension -->
<param name="move_base/plannerList" value="ADPlanner,ARAPlanner"/> <!-- future extension -->
@@ -10,15 +9,16 @@
<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"/>
- <node pkg="highlevel_controllers" type="move_base_sbpl" args="" respawn="false" />
-
- <!-- Set parameters for mailing -->
- <param name="recharge/email_addresses" value="mc...@wi..."/>
- <param name="recharge/subject_plugin" value="Robot Needs To Be Plugged In"/>
- <param name="recharge/subject_unplug" value="Robot Needs To Be Unplugged"/>
- <param name="recharge/body_plugin" value="Hello, could you please plug me in?\nThanks, PR2"/>
- <param name="recharge/body_unplug" value="Hello, could you please unplug me?\nThanks, PR2"/>
- <param name="recharge/mail_client" value="mailx -s"/>
- <node pkg="highlevel_controllers" type="recharge_controller" args="battery_state:=bogus_battery_state" respawn="false" />
+ <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"/>
+ <param name="/costmap_2d/no_information_value" value="255"/>
+ <param name="/costmap_2d/z_threshold" value="2.0"/>
+ <param name="/costmap_2d/freespace_projection_height" value="2.0"/>
+ <param name="/costmap_2d/inflation_radius" value="0.55"/>
+ <param name="/costmap_2d/circumscribed_radius" value="0.46"/>
+ <param name="/costmap_2d/inscribed_radius" value="0.325"/>
+ <param name="/costmap_2d/weight" value="0.1"/>
+ <node pkg="highlevel_controllers" type="move_base_sbpl" args="base_scan:=base_scan tilt_scan:=tilt_scan" respawn="false" />
</group>
</launch>
Modified: pkg/trunk/highlevel/highlevel_controllers/test/launch_stage_5cm.xml
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/test/launch_stage_5cm.xml 2008-11-18 14:25:33 UTC (rev 6880)
+++ pkg/trunk/highlevel/highlevel_controllers/test/launch_stage_5cm.xml 2008-11-18 14:29:12 UTC (rev 6881)
@@ -11,17 +11,8 @@
<node pkg="fake_localization" type="fake_localization" respawn="false" />
<node pkg="nav_view" type="nav_view" respawn="false"/>
- <!--param name="costmap_2d/dynamic_obstacle_window" value="255.0"/-->
- <!-- for the moment use e.g.: ./shparam set move_base/plannerType string ADPlanner -->
- <param name="move_base/environmentList" value="2D,3DKIN"/> <!-- future extension -->
- <param name="move_base/plannerList" value="ADPlanner,ARAPlanner"/> <!-- future extension -->
- <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="costmap_2d/weight" value="0.1"/>
- <node pkg="highlevel_controllers" type="move_base_sbpl" args="scan:=base_scan" respawn="false" />
+ <!-- Now launch controller node required -->
+ <include file="$(find highlevel_controllers)/test/launch_move_base.xml"/>
<!-- Set parameters for mailing -->
<param name="recharge/email_addresses" value="mc...@wi..."/>
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-18 14:25:33 UTC (rev 6880)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2008-11-18 14:29:12 UTC (rev 6881)
@@ -56,6 +56,8 @@
// Base class
#include "obstacle_map_accessor.h"
+#include <std_msgs/Point.h>
+#include <std_msgs/PointCloud.h>
//c++
#include <list>
@@ -63,12 +65,8 @@
#include <map>
#include <set>
#include <string>
-#include <map>
#include <queue>
-// For point clouds <Could Make it a template>
-#include "std_msgs/PointCloud.h"
-
namespace costmap_2d {
typedef unsigned char TICK;
@@ -98,6 +96,50 @@
typedef std::priority_queue< QueueElement*, std::vector<QueueElement*>, QueueElementComparator > QUEUE;
+ /**
+ * @brief Stores an observation in terms of a point cloud and the origin of the source
+ * @note Tried to make members and constructor arguments const but the compiler would not accept the default
+ * assignment operator for vector insertion!
+ */
+ class Observation {
+ public:
+ // Structors
+ Observation(std_msgs::Point& p, std_msgs::PointCloud* cloud): origin_(p), cloud_(cloud) {}
+ Observation(const Observation& org): origin_(org.origin_), cloud_(org.cloud_){}
+
+ std_msgs::Point origin_;
+ std_msgs::PointCloud* cloud_;
+ };
+
+ /**
+ * @brief Base class for buffering observations with a time to live property
+ * The inputs are in the map frame
+ */
+ class ObservationBuffer {
+ public:
+ ObservationBuffer(ros::Duration keep_alive):keep_alive_(keep_alive){}
+
+ virtual ~ObservationBuffer();
+
+ /**
+ * @brief Buffer a current observation
+ * @return true if succeded, false if not (which might occur if there was no transform available for example)
+ */
+ bool buffer_observation(const Observation& observation);
+
+ /**
+ * @brief Queries for current observations. Any observations that are no longer
+ * current will be deleted. Will append observations to the input vector
+ */
+ virtual void get_observations(std::vector<Observation>& observations);
+
+ protected:
+ std::list<Observation> buffer_;
+ const ros::Duration keep_alive_;
+ ros::Time last_updated_;
+ };
+
+
class CostMap2D: public ObstacleMapAccessor
{
public:
@@ -126,7 +168,7 @@
*/
CostMap2D(unsigned int width, unsigned int height, const std::vector<unsigned char>& data,
double resolution, unsigned char threshold,
- double maxZ = 0, double freeSpaceProjectionHeight = 0,
+ double maxZ = 0.5, double zLB = 0.10, double zUB = 0.15,
double inflationRadius = 0, double circumscribedRadius = 0, double inscribedRadius = 0, double weight = 1);
/**
@@ -152,12 +194,22 @@
std::vector<unsigned int>& updates);
/**
* @brief Updates dynamic obstacles
+ * @param wx The current x position
+ * @param wy The current y position
* @param clouds holds projected scan data
*/
void updateDynamicObstacles(double wx, double wy,
const std::vector<std_msgs::PointCloud*>& clouds);
/**
+ * @brief Updates dynamic obstacles based on a buffer of observations
+ * @param wx The current x position
+ * @param wy The current y position
+ * @param observations The collection of observations from all data sources
+ */
+ void updateDynamicObstacles(double wx, double wy, const std::vector<Observation>& observations);
+
+ /**
* @brief Get pointer into the obstacle map (which contains both static
* and dynamic obstacles)
*/
@@ -213,10 +265,10 @@
/**
* @brief Utility to push free space inferred from a laser point hit via ray-tracing
- * @param ind the cell index of the obstacle detected
- * @param updates the buffer for updated cells as a result
+ * @param The origin from which to trace out
+ * @param The target in map co-ordinates to trace to
*/
- void updateFreeSpace(unsigned int ind);
+ void updateFreeSpace(const std_msgs::Point& origin, double wx, double wy);
/**
* @brief Simple test for a cell having been marked during current propaagtion
@@ -241,8 +293,11 @@
void updateCellCost(unsigned int ind, unsigned char cost);
+ bool in_projection_range(double z){return z >= zLB_ && z <= zUB_;}
+
const double maxZ_; /**< Points above this will be excluded from consideration */
- const double freeSpaceProjectionHeight_; /**< Filters points for free space projection */
+ const double zLB_; /**< Filters points for free space projection */
+ const double zUB_; /**< Filters points for free space projection */
const unsigned int inflationRadius_; /**< The radius in cells to propagate cost and obstacle information */
const unsigned int circumscribedRadius_; /**< The radius for the circumscribed radius, in cells */
const unsigned int inscribedRadius_; /**< The radius for the inscribed radius, in cells */
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp 2008-11-18 14:25:33 UTC (rev 6880)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp 2008-11-18 14:29:12 UTC (rev 6881)
@@ -68,13 +68,54 @@
return (unsigned int) a;
}
+ // Just clean up outstanding observations
+ ObservationBuffer::~ObservationBuffer(){
+ while(!buffer_.empty()){
+ std::list<Observation>::iterator it = buffer_.begin();
+ const std_msgs::PointCloud* cloud = it->cloud_;
+ delete cloud;
+ buffer_.erase(it);
+ }
+ }
+
+ // Only works if the observation is in the map frame - test for it. It should be transformed before
+ // we enque it
+ bool ObservationBuffer::buffer_observation(const Observation& observation){
+ last_updated_ = observation.cloud_->header.stamp;
+
+ if(observation.cloud_->header.frame_id != "map")
+ return false;
+
+ // If the duration is 0, then we just keep the latest one, so we clear out all existing observations
+ while(!buffer_.empty()){
+ std::list<Observation>::iterator it = buffer_.begin();
+ // Get the current one, and check if still alive. if so
+ Observation& obs = *it;
+ if((last_updated_ - obs.cloud_->header.stamp) > keep_alive_){
+ delete obs.cloud_;
+ buffer_.erase(it);
+ }
+ else
+ 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);
+ }
+ }
+
CostMap2D::CostMap2D(unsigned int width, unsigned int height, const std::vector<unsigned char>& data,
- double resolution, unsigned char threshold,
- double maxZ, double freeSpaceProjectionHeight,
+ double resolution, unsigned char threshold, double maxZ, double zLB, double zUB,
double inflationRadius, double circumscribedRadius, double inscribedRadius, double weight)
: ObstacleMapAccessor(0, 0, width, height, resolution),
- maxZ_(maxZ),
- freeSpaceProjectionHeight_(freeSpaceProjectionHeight),
+ maxZ_(maxZ), zLB_(zLB), zUB_(zUB),
inflationRadius_(toCellDistance(inflationRadius, (unsigned int) ceil(width * resolution), resolution)),
circumscribedRadius_(toCellDistance(circumscribedRadius, inflationRadius_, resolution)),
inscribedRadius_(toCellDistance(inscribedRadius, circumscribedRadius_, resolution)),
@@ -169,6 +210,24 @@
void CostMap2D::updateDynamicObstacles(double wx, double wy,
const std::vector<std_msgs::PointCloud*>& clouds)
{
+ std_msgs::Point origin;
+ origin.x = wx;
+ origin.y = wy;
+ origin.z = zLB_;
+ std::vector<Observation> observations;
+ for(std::vector<std_msgs::PointCloud*>::const_iterator it = clouds.begin(); it != clouds.end(); ++it){
+ Observation obs(origin, *it);
+ observations.push_back(obs);
+ }
+
+ updateDynamicObstacles(wx, wy, observations);
+ }
+
+ /**
+ * @brief Update the cost map based on aggregate observations
+ */
+ void CostMap2D::updateDynamicObstacles(double wx, double wy, const std::vector<Observation>& observations)
+ {
// Update current grid position
WC_MC(wx, wy, mx_, my_);
@@ -176,42 +235,53 @@
memset(xy_markers_, 0, width_ * height_);
memcpy(costData_, staticData_, width_ * height_);
+ // Propagation queue should be empty from completion of last propagation.
ROS_ASSERT(queue_.empty());
- std::vector<unsigned int> F; // The set of points for free space projection
- for(std::vector<std_msgs::PointCloud*>::const_iterator it = clouds.begin(); it != clouds.end(); ++it){
- const std_msgs::PointCloud& cloud = *(*it);
+
+ // First we propagate costs. For this we iterate over observations, and process the internal point clouds
+ for(std::vector<Observation>::const_iterator it = observations.begin(); it!= observations.end(); ++it){
+ const Observation& obs = *it;
+ const std_msgs::PointCloud& cloud = *(obs.cloud_);
for(size_t i = 0; i < cloud.get_pts_size(); i++) {
- // Filter out points too high (can use for free space propagation?)
- if(cloud.pts[i].z > maxZ_)
- continue;
+ // Filter out points too high (can use for free space propagation?)
+ if(cloud.pts[i].z > maxZ_)
+ continue;
- // Queue cell for cost propagation
- unsigned int ind = WC_IND(cloud.pts[i].x, cloud.pts[i].y);
+ // Queue cell for cost propagation
+ unsigned int ind = WC_IND(cloud.pts[i].x, cloud.pts[i].y);
- // If we have already processed the cell for this point, skip it
- if(marked(ind))
- continue;
+ // If we have already processed the cell for this point, skip it
+ if(marked(ind))
+ continue;
- // Buffer for cost propagation. This will mark the cell
- unsigned int mx, my;
- IND_MC(ind, mx, my);
- enqueue(ind, mx, my);
-
- // Buffer for free space projection (Need a ctest based on the origin)
- if(cloud.pts[i].z <= freeSpaceProjectionHeight_)
- F.push_back(ind);
+ // Buffer for cost propagation. This will mark the cell
+ unsigned int mx, my;
+ IND_MC(ind, mx, my);
+ enqueue(ind, mx, my);
}
}
// Propagate costs
propagateCosts();
- // Propagate free space
- for(std::vector<unsigned int>::const_iterator it = F.begin(); it != F.end(); ++it)
- updateFreeSpace(*it);
+ // 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
+ for(std::vector<Observation>::const_iterator it = observations.begin(); it!= observations.end(); ++it){
+ const Observation& obs = *it;
+ if(!in_projection_range(obs.origin_.z))
+ continue;
+
+ const std_msgs::PointCloud& cloud = *(obs.cloud_);
+ for(size_t i = 0; i < cloud.get_pts_size(); i++) {
+ if(!in_projection_range(cloud.pts[i].z))
+ continue;
+
+ updateFreeSpace(obs.origin_, cloud.pts[i].x, cloud.pts[i].y);
+ }
+ }
}
-
const unsigned char* CostMap2D::getMap() const {
return costData_;
}
@@ -339,15 +409,13 @@
* Utilizes Eitan's implementation of Bresenhams ray tracing algorithm to iterate over the cells between the
* current position (mx_, my_).
*/
- void CostMap2D::updateFreeSpace(unsigned int ind){
- unsigned int x1, y1;
- IND_MC(ind, x1, y1);
+ void CostMap2D::updateFreeSpace(const std_msgs::Point& origin, double wx, double wy){
+ unsigned int x, y, x1, y1;
+ WC_MC(origin.x, origin.y, x, y);
+ WC_MC(wx, wy, x1, y1);
+ unsigned int deltax = abs(x1 - x); // The difference between the x's
+ unsigned int deltay = abs(y1 - y); // The difference between the y's
- unsigned int deltax = abs(x1 - mx_); // The difference between the x's
- unsigned int deltay = abs(y1 - my_); // The difference between the y's
- unsigned int x = mx_; // Start x off at the first pixel
- unsigned int y = my_; // Start y off at the first pixel
-
unsigned int xinc1, xinc2, yinc1, yinc2;
unsigned int den, num, numadd, numpixels;
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-18 14:25:33 UTC (rev 6880)
+++ pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc 2008-11-18 14:29:12 UTC (rev 6881)
@@ -72,12 +72,19 @@
return false;
}
+/**
+ * Basic testing for observation buffer
+ */
+TEST(costmap, test15){
+ ros::Duration keep_alive(10, 0);
+ ObservationBuffer buffer(keep_alive);
+}
/**
* Test for the cost function correctness with a larger range and different values
*/
TEST(costmap, test14){
- CostMap2D map(100, 100, EMPTY_100_BY_100, RESOLUTION, THRESHOLD, MAX_Z, MAX_Z,
+ CostMap2D map(100, 100, EMPTY_100_BY_100, RESOLUTION, THRESHOLD, MAX_Z, MAX_Z, MAX_Z,
ROBOT_RADIUS * 10.5, ROBOT_RADIUS * 8.0, ROBOT_RADIUS * 5.0, 0.5);
// Verify that the circumscribed cost lower bound is as expected: based on the cost function.
@@ -89,7 +96,7 @@
cloud.set_pts_size(1);
cloud.pts[0].x = 50;
cloud.pts[0].y = 50;
- cloud.pts[0].z = 1;
+ cloud.pts[0].z = MAX_Z;
map.updateDynamicObstacles(0, 0, CostMap2D::toVector(cloud));
@@ -152,7 +159,7 @@
*/
TEST(costmap, test12){
// Start with an empty map
- CostMap2D map(GRID_WIDTH, GRID_HEIGHT, EMPTY_10_BY_10, RESOLUTION, THRESHOLD, MAX_Z * 2, MAX_Z,
+ CostMap2D map(GRID_WIDTH, GRID_HEIGHT, EMPTY_10_BY_10, RESOLUTION, THRESHOLD, MAX_Z * 2, MAX_Z, MAX_Z,
ROBOT_RADIUS*3, ROBOT_RADIUS * 2, ROBOT_RADIUS);
@@ -161,13 +168,13 @@
cloud.set_pts_size(3);
cloud.pts[0].x = 3;
cloud.pts[0].y = 3;
- cloud.pts[0].z = 0;
+ cloud.pts[0].z = MAX_Z;
cloud.pts[1].x = 5;
cloud.pts[1].y = 5;
- cloud.pts[1].z = 0;
+ cloud.pts[1].z = MAX_Z;
cloud.pts[2].x = 7;
cloud.pts[2].y = 7;
- cloud.pts[2].z = 0;
+ cloud.pts[2].z = MAX_Z;
std::vector<unsigned int> updates;
map.updateDynamicObstacles(cloud, updates);
@@ -180,13 +187,14 @@
* Test for ray tracing free space
*/
TEST(costmap, test0){
- CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, THRESHOLD, MAX_Z, MAX_Z,
+ CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, THRESHOLD, MAX_Z, MAX_Z, MAX_Z,
ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS);
// Add a point cloud and verify its insertion. There should be only one new one
std_msgs::PointCloud cloud;
cloud.set_pts_size(1);
cloud.pts[0].x = 0;
cloud.pts[0].y = 0;
+ cloud.pts[0].z = MAX_Z;
std::vector<unsigned int> updates;
map.updateDynamicObstacles(cloud, updates);
@@ -303,7 +311,7 @@
* Make sure we ignore points outside of our z threshold
*/
TEST(costmap, test6){
- CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, THRESHOLD, MAX_Z);
+ CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, THRESHOLD, MAX_Z, MAX_Z, MAX_Z);
// A point cloud with 2 points falling in a cell with a non-lethal cost
std_msgs::PointCloud c0;
@@ -324,7 +332,7 @@
* Test inflation for both static and dynamic obstacles
*/
TEST(costmap, test7){
- CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, THRESHOLD, MAX_Z, MAX_Z,
+ CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, THRESHOLD, MAX_Z, MAX_Z, MAX_Z,
ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS);
@@ -404,7 +412,7 @@
* Test specific inflation scenario to ensure we do not set inflated obstacles to be raw obstacles.
*/
TEST(costmap, test8){
- CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, THRESHOLD, MAX_Z, MAX_Z,
+ CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, THRESHOLD, MAX_Z, MAX_Z, MAX_Z,
ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS);
std::vector<unsigned int> updates;
@@ -414,13 +422,13 @@
c0.set_pts_size(3);
c0.pts[0].x = 1;
c0.pts[0].y = 1;
- c0.pts[0].z = 0;
+ c0.pts[0].z = MAX_Z;
c0.pts[1].x = 1;
c0.pts[1].y = 2;
- c0.pts[1].z = 0;
+ c0.pts[1].z = MAX_Z;
c0.pts[2].x = 2;
c0.pts[2].y = 2;
- c0.pts[2].z = 0;
+ c0.pts[2].z = MAX_Z;
map.updateDynamicObstacles(c0, updates);
ASSERT_EQ(map.getCost(3, 2), CostMap2D::INSCRIBED_INFLATED_OBSTACLE);
@@ -438,7 +446,7 @@
}
}
- CostMap2D map(GRID_WIDTH, GRID_HEIGHT, mapData, RESOLUTION, THRESHOLD, MAX_Z, MAX_Z,
+ CostMap2D map(GRID_WIDTH, GRID_HEIGHT, mapData, RESOLUTION, THRESHOLD, MAX_Z, MAX_Z, MAX_Z,
ROBOT_RADIUS * 3, ROBOT_RADIUS * 2, ROBOT_RADIUS);
// There should be no occupied cells
@@ -451,7 +459,7 @@
c0.set_pts_size(1);
c0.pts[0].x = 5;
c0.pts[0].y = 5;
- c0.pts[0].z = 0;
+ c0.pts[0].z = MAX_Z;
std::vector<unsigned int> updates;
map.updateDynamicObstacles(c0, updates);
@@ -507,7 +515,7 @@
* Test for ray tracing free space
*/
TEST(costmap, test11){
- CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, THRESHOLD, MAX_Z * 2, MAX_Z, ROBOT_RADIUS);
+ CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, THRESHOLD, MAX_Z * 2, MAX_Z, MAX_Z, ROBOT_RADIUS);
// The initial position will be <0,0> by default. So if we add an obstacle at 9,9, we would expect cells
// <0, 0> thru <7, 7> to be free.
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
