obstacle_layer.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, 2013, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of Willow Garage, Inc. nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  * Author: Eitan Marder-Eppstein
00036  *         David V. Lu!!
00037  *********************************************************************/
00038 #include <costmap_2d/obstacle_layer.h>
00039 #include <costmap_2d/costmap_math.h>
00040 #include <pluginlib/class_list_macros.h>
00041 
00042 PLUGINLIB_EXPORT_CLASS(costmap_2d::ObstacleLayer, costmap_2d::Layer)
00043 
00044 using costmap_2d::NO_INFORMATION;
00045 using costmap_2d::LETHAL_OBSTACLE;
00046 using costmap_2d::FREE_SPACE;
00047 
00048 using costmap_2d::ObservationBuffer;
00049 using costmap_2d::Observation;
00050 
00051 namespace costmap_2d
00052 {
00053 
00054 void ObstacleLayer::onInitialize()
00055 {
00056   ros::NodeHandle nh("~/" + name_), g_nh;
00057   rolling_window_ = layered_costmap_->isRolling();
00058 
00059   bool track_unknown_space;
00060   nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown());
00061   if(track_unknown_space)
00062     default_value_ = NO_INFORMATION;
00063   else
00064     default_value_ = FREE_SPACE;
00065 
00066   ObstacleLayer::matchSize();
00067   current_ = true;
00068 
00069   global_frame_ = layered_costmap_->getGlobalFrameID();
00070   double transform_tolerance;
00071   nh.param("transform_tolerance", transform_tolerance, 0.2);
00072 
00073   std::string topics_string;
00074   //get the topics that we'll subscribe to from the parameter server
00075   nh.param("observation_sources", topics_string, std::string(""));
00076   ROS_INFO("    Subscribed to Topics: %s", topics_string.c_str());
00077 
00078   // get our tf prefix
00079   ros::NodeHandle prefix_nh;
00080   const std::string tf_prefix = tf::getPrefixParam(prefix_nh);
00081 
00082   //now we need to split the topics based on whitespace which we can use a stringstream for
00083   std::stringstream ss(topics_string);
00084 
00085   std::string source;
00086   while (ss >> source)
00087   {
00088     ros::NodeHandle source_node(nh, source);
00089 
00090     //get the parameters for the specific topic
00091     double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height;
00092     std::string topic, sensor_frame, data_type;
00093     bool inf_is_valid, clearing, marking;
00094 
00095     source_node.param("topic", topic, source);
00096     source_node.param("sensor_frame", sensor_frame, std::string(""));
00097     source_node.param("observation_persistence", observation_keep_time, 0.0);
00098     source_node.param("expected_update_rate", expected_update_rate, 0.0);
00099     source_node.param("data_type", data_type, std::string("PointCloud"));
00100     source_node.param("min_obstacle_height", min_obstacle_height, 0.0);
00101     source_node.param("max_obstacle_height", max_obstacle_height, 2.0);
00102     source_node.param("inf_is_valid", inf_is_valid, false);
00103     source_node.param("clearing", clearing, false);
00104     source_node.param("marking", marking, true);
00105 
00106     if (!sensor_frame.empty())
00107     {
00108       sensor_frame = tf::resolve(tf_prefix, sensor_frame);
00109     }
00110 
00111     if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan"))
00112     {
00113       ROS_FATAL("Only topics that use point clouds or laser scans are currently supported");
00114       throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported");
00115     }
00116 
00117     std::string raytrace_range_param_name, obstacle_range_param_name;
00118 
00119     //get the obstacle range for the sensor
00120     double obstacle_range = 2.5;
00121     if (source_node.searchParam("obstacle_range", obstacle_range_param_name))
00122     {
00123       source_node.getParam(obstacle_range_param_name, obstacle_range);
00124     }
00125 
00126     //get the raytrace range for the sensor
00127     double raytrace_range = 3.0;
00128     if (source_node.searchParam("raytrace_range", raytrace_range_param_name))
00129     {
00130       source_node.getParam(raytrace_range_param_name, raytrace_range);
00131     }
00132 
00133     ROS_DEBUG("Creating an observation buffer for source %s, topic %s, frame %s", source.c_str(), topic.c_str(),
00134               sensor_frame.c_str());
00135 
00136     //create an observation buffer
00137     observation_buffers_.push_back(
00138         boost::shared_ptr < ObservationBuffer
00139             > (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,
00140                                      max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_,
00141                                      sensor_frame, transform_tolerance)));
00142 
00143     //check if we'll add this buffer to our marking observation buffers
00144     if (marking)
00145       marking_buffers_.push_back(observation_buffers_.back());
00146 
00147     //check if we'll also add this buffer to our clearing observation buffers
00148     if (clearing)
00149       clearing_buffers_.push_back(observation_buffers_.back());
00150 
00151     ROS_DEBUG(
00152         "Created an observation buffer for source %s, topic %s, global frame: %s, expected update rate: %.2f, observation persistence: %.2f",
00153         source.c_str(), topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);
00154 
00155     //create a callback for the topic
00156     if (data_type == "LaserScan")
00157     {
00158       boost::shared_ptr < message_filters::Subscriber<sensor_msgs::LaserScan>
00159           > sub(new message_filters::Subscriber<sensor_msgs::LaserScan>(g_nh, topic, 50));
00160 
00161       boost::shared_ptr < tf::MessageFilter<sensor_msgs::LaserScan>
00162           > filter(new tf::MessageFilter<sensor_msgs::LaserScan>(*sub, *tf_, global_frame_, 50));
00163 
00164       if (inf_is_valid)
00165       {
00166         filter->registerCallback(
00167             boost::bind(&ObstacleLayer::laserScanValidInfCallback, this, _1, observation_buffers_.back()));
00168       }
00169       else
00170       {
00171         filter->registerCallback(
00172             boost::bind(&ObstacleLayer::laserScanCallback, this, _1, observation_buffers_.back()));
00173       }
00174 
00175       observation_subscribers_.push_back(sub);
00176       observation_notifiers_.push_back(filter);
00177 
00178       observation_notifiers_.back()->setTolerance(ros::Duration(0.05));
00179     }
00180     else if (data_type == "PointCloud")
00181     {
00182       boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud>
00183           > sub(new message_filters::Subscriber<sensor_msgs::PointCloud>(g_nh, topic, 50));
00184 
00185       if( inf_is_valid )
00186       {
00187        ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
00188       }
00189 
00190       boost::shared_ptr < tf::MessageFilter<sensor_msgs::PointCloud>
00191           > filter(new tf::MessageFilter<sensor_msgs::PointCloud>(*sub, *tf_, global_frame_, 50));
00192       filter->registerCallback(
00193           boost::bind(&ObstacleLayer::pointCloudCallback, this, _1, observation_buffers_.back()));
00194 
00195       observation_subscribers_.push_back(sub);
00196       observation_notifiers_.push_back(filter);
00197     }
00198     else
00199     {
00200       boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud2>
00201           > sub(new message_filters::Subscriber<sensor_msgs::PointCloud2>(g_nh, topic, 50));
00202 
00203       if( inf_is_valid )
00204       {
00205        ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
00206       }
00207 
00208       boost::shared_ptr < tf::MessageFilter<sensor_msgs::PointCloud2>
00209           > filter(new tf::MessageFilter<sensor_msgs::PointCloud2>(*sub, *tf_, global_frame_, 50));
00210       filter->registerCallback(
00211           boost::bind(&ObstacleLayer::pointCloud2Callback, this, _1, observation_buffers_.back()));
00212 
00213       observation_subscribers_.push_back(sub);
00214       observation_notifiers_.push_back(filter);
00215     }
00216 
00217     if (sensor_frame != "")
00218     {
00219       std::vector < std::string > target_frames;
00220       target_frames.push_back(global_frame_);
00221       target_frames.push_back(sensor_frame);
00222       observation_notifiers_.back()->setTargetFrames(target_frames);
00223     }
00224 
00225   }
00226 
00227   setupDynamicReconfigure(nh);
00228   footprint_layer_.initialize( layered_costmap_, name_ + "_footprint", tf_);
00229 }
00230 
00231 void ObstacleLayer::setupDynamicReconfigure(ros::NodeHandle& nh)
00232 {
00233   dsrv_ = new dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>(nh);
00234   dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>::CallbackType cb = boost::bind(
00235       &ObstacleLayer::reconfigureCB, this, _1, _2);
00236   dsrv_->setCallback(cb);
00237 }
00238 
00239 ObstacleLayer::~ObstacleLayer()
00240 {
00241     if(dsrv_)
00242         delete dsrv_;
00243 }
00244 void ObstacleLayer::reconfigureCB(costmap_2d::ObstaclePluginConfig &config, uint32_t level)
00245 {
00246   enabled_ = config.enabled;
00247   max_obstacle_height_ = config.max_obstacle_height;
00248   combination_method_ = config.combination_method;
00249 }
00250 
00251 void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScanConstPtr& message,
00252                                       const boost::shared_ptr<ObservationBuffer>& buffer)
00253 {
00254   //project the laser into a point cloud
00255   sensor_msgs::PointCloud2 cloud;
00256   cloud.header = message->header;
00257 
00258   //project the scan into a point cloud
00259   try
00260   {
00261     projector_.transformLaserScanToPointCloud(message->header.frame_id, *message, cloud, *tf_);
00262   }
00263   catch (tf::TransformException &ex)
00264   {
00265     ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", global_frame_.c_str(),
00266              ex.what());
00267     projector_.projectLaser(*message, cloud);
00268   }
00269 
00270   //buffer the point cloud
00271   buffer->lock();
00272   buffer->bufferCloud(cloud);
00273   buffer->unlock();
00274 }
00275 
00276 void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr& raw_message, 
00277                                               const boost::shared_ptr<ObservationBuffer>& buffer){
00278   // Filter positive infinities ("Inf"s) to max_range.
00279   float epsilon = 0.0001; // a tenth of a millimeter
00280   sensor_msgs::LaserScan message = *raw_message;
00281   for( size_t i = 0; i < message.ranges.size(); i++ )
00282   {
00283     float range = message.ranges[ i ];
00284     if( !std::isfinite( range ) && range > 0 )
00285     {
00286       message.ranges[ i ] = message.range_max - epsilon;
00287     }
00288   }
00289 
00290   //project the laser into a point cloud
00291   sensor_msgs::PointCloud2 cloud;
00292   cloud.header = message.header;
00293 
00294   //project the scan into a point cloud
00295   try
00296   {
00297     projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_);
00298   }
00299   catch (tf::TransformException &ex)
00300   {
00301     ROS_WARN ("High fidelity enabled, but TF returned a transform exception to frame %s: %s", global_frame_.c_str (), ex.what ());
00302     projector_.projectLaser(message, cloud);
00303   }
00304 
00305   //buffer the point cloud
00306   buffer->lock();
00307   buffer->bufferCloud(cloud);
00308   buffer->unlock();
00309 }
00310 
00311 void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloudConstPtr& message,
00312                                                const boost::shared_ptr<ObservationBuffer>& buffer)
00313 {
00314   sensor_msgs::PointCloud2 cloud2;
00315 
00316   if (!sensor_msgs::convertPointCloudToPointCloud2(*message, cloud2))
00317   {
00318     ROS_ERROR("Failed to convert a PointCloud to a PointCloud2, dropping message");
00319     return;
00320   }
00321 
00322   //buffer the point cloud
00323   buffer->lock();
00324   buffer->bufferCloud(cloud2);
00325   buffer->unlock();
00326 }
00327 
00328 void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& message,
00329                                                 const boost::shared_ptr<ObservationBuffer>& buffer)
00330 {
00331   //buffer the point cloud
00332   buffer->lock();
00333   buffer->bufferCloud(*message);
00334   buffer->unlock();
00335 }
00336 
00337 void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
00338                                           double* min_y, double* max_x, double* max_y)
00339 {
00340   if (rolling_window_)
00341     updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
00342   if (!enabled_)
00343     return;
00344   useExtraBounds(min_x, min_y, max_x, max_y);
00345 
00346   bool current = true;
00347   std::vector<Observation> observations, clearing_observations;
00348 
00349   //get the marking observations
00350   current = current && getMarkingObservations(observations);
00351 
00352   //get the clearing observations
00353   current = current && getClearingObservations(clearing_observations);
00354 
00355   //update the global current status
00356   current_ = current;
00357 
00358   //raytrace freespace
00359   for (unsigned int i = 0; i < clearing_observations.size(); ++i)
00360   {
00361     raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
00362   }
00363 
00364   //place the new obstacles into a priority queue... each with a priority of zero to begin with
00365   for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
00366   {
00367     const Observation& obs = *it;
00368 
00369     const pcl::PointCloud<pcl::PointXYZ>& cloud = *(obs.cloud_);
00370 
00371     double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
00372 
00373     for (unsigned int i = 0; i < cloud.points.size(); ++i)
00374     {
00375       double px = cloud.points[i].x, py = cloud.points[i].y, pz = cloud.points[i].z;
00376 
00377       //if the obstacle is too high or too far away from the robot we won't add it
00378       if (pz > max_obstacle_height_)
00379       {
00380         ROS_DEBUG("The point is too high");
00381         continue;
00382       }
00383 
00384       //compute the squared distance from the hitpoint to the pointcloud's origin
00385       double sq_dist = (px - obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y)
00386           + (pz - obs.origin_.z) * (pz - obs.origin_.z);
00387 
00388       //if the point is far enough away... we won't consider it
00389       if (sq_dist >= sq_obstacle_range)
00390       {
00391         ROS_DEBUG("The point is too far away");
00392         continue;
00393       }
00394 
00395       //now we need to compute the map coordinates for the observation
00396       unsigned int mx, my;
00397       if (!worldToMap(px, py, mx, my))
00398       {
00399         ROS_DEBUG("Computing map coords failed");
00400         continue;
00401       }
00402 
00403       unsigned int index = getIndex(mx, my);
00404       costmap_[index] = LETHAL_OBSTACLE;
00405       touch(px, py, min_x, min_y, max_x, max_y);
00406     }
00407   }
00408 
00409   footprint_layer_.updateBounds(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
00410 }
00411 
00412 void ObstacleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
00413 {
00414   if (!enabled_)
00415     return;
00416 
00417   // The footprint layer clears the footprint in this ObstacleLayer
00418   // before we merge this obstacle layer into the master_grid.
00419   footprint_layer_.updateCosts(*this, min_i, min_j, max_i, max_j);
00420 
00421   if(combination_method_==0)
00422     updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j);
00423   else
00424     updateWithMax(master_grid, min_i, min_j, max_i, max_j);
00425 }
00426 
00427 void ObstacleLayer::addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing)
00428 {
00429   if(marking)
00430     static_marking_observations_.push_back(obs);
00431   if(clearing)
00432     static_clearing_observations_.push_back(obs);
00433 }
00434 
00435 void ObstacleLayer::clearStaticObservations(bool marking, bool clearing){
00436   if(marking)
00437     static_marking_observations_.clear();
00438   if(clearing)
00439     static_clearing_observations_.clear();
00440 }
00441 
00442 bool ObstacleLayer::getMarkingObservations(std::vector<Observation>& marking_observations) const
00443 {
00444   bool current = true;
00445   //get the marking observations
00446   for (unsigned int i = 0; i < marking_buffers_.size(); ++i)
00447   {
00448     marking_buffers_[i]->lock();
00449     marking_buffers_[i]->getObservations(marking_observations);
00450     current = marking_buffers_[i]->isCurrent() && current;
00451     marking_buffers_[i]->unlock();
00452   }
00453   marking_observations.insert(marking_observations.end(), 
00454                               static_marking_observations_.begin(), static_marking_observations_.end());
00455   return current;
00456 }
00457 
00458 bool ObstacleLayer::getClearingObservations(std::vector<Observation>& clearing_observations) const
00459 {
00460   bool current = true;
00461   //get the clearing observations
00462   for (unsigned int i = 0; i < clearing_buffers_.size(); ++i)
00463   {
00464     clearing_buffers_[i]->lock();
00465     clearing_buffers_[i]->getObservations(clearing_observations);
00466     current = clearing_buffers_[i]->isCurrent() && current;
00467     clearing_buffers_[i]->unlock();
00468   }
00469   clearing_observations.insert(clearing_observations.end(), 
00470                               static_clearing_observations_.begin(), static_clearing_observations_.end());
00471   return current;
00472 }
00473 
00474 void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
00475                                               double* max_x, double* max_y)
00476 {
00477   double ox = clearing_observation.origin_.x;
00478   double oy = clearing_observation.origin_.y;
00479   pcl::PointCloud < pcl::PointXYZ > cloud = *(clearing_observation.cloud_);
00480 
00481   //get the map coordinates of the origin of the sensor
00482   unsigned int x0, y0;
00483   if (!worldToMap(ox, oy, x0, y0))
00484   {
00485     ROS_WARN_THROTTLE(
00486         1.0, "The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
00487         ox, oy);
00488     return;
00489   }
00490 
00491   //we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
00492   double origin_x = origin_x_, origin_y = origin_y_;
00493   double map_end_x = origin_x + size_x_ * resolution_;
00494   double map_end_y = origin_y + size_y_ * resolution_;
00495 
00496 
00497   touch(ox, oy, min_x, min_y, max_x, max_y);
00498 
00499   //for each point in the cloud, we want to trace a line from the origin and clear obstacles along it
00500   for (unsigned int i = 0; i < cloud.points.size(); ++i)
00501   {
00502     double wx = cloud.points[i].x;
00503     double wy = cloud.points[i].y;
00504 
00505     //now we also need to make sure that the enpoint we're raytracing
00506     //to isn't off the costmap and scale if necessary
00507     double a = wx - ox;
00508     double b = wy - oy;
00509 
00510     //the minimum value to raytrace from is the origin
00511     if (wx < origin_x)
00512     {
00513       double t = (origin_x - ox) / a;
00514       wx = origin_x;
00515       wy = oy + b * t;
00516     }
00517     if (wy < origin_y)
00518     {
00519       double t = (origin_y - oy) / b;
00520       wx = ox + a * t;
00521       wy = origin_y;
00522     }
00523 
00524     //the maximum value to raytrace to is the end of the map
00525     if (wx > map_end_x)
00526     {
00527       double t = (map_end_x - ox) / a;
00528       wx = map_end_x - .001;
00529       wy = oy + b * t;
00530     }
00531     if (wy > map_end_y)
00532     {
00533       double t = (map_end_y - oy) / b;
00534       wx = ox + a * t;
00535       wy = map_end_y - .001;
00536     }
00537 
00538     //now that the vector is scaled correctly... we'll get the map coordinates of its endpoint
00539     unsigned int x1, y1;
00540 
00541     //check for legality just in case
00542     if (!worldToMap(wx, wy, x1, y1))
00543       continue;
00544 
00545     unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
00546     MarkCell marker(costmap_, FREE_SPACE);
00547     //and finally... we can execute our trace to clear obstacles along that line
00548     raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range);
00549 
00550     updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
00551   }
00552 }
00553 
00554 void ObstacleLayer::activate()
00555 {
00556   //if we're stopped we need to re-subscribe to topics
00557   for (unsigned int i = 0; i < observation_subscribers_.size(); ++i)
00558   {
00559     if (observation_subscribers_[i] != NULL)
00560       observation_subscribers_[i]->subscribe();
00561   }
00562 
00563   for (unsigned int i = 0; i < observation_buffers_.size(); ++i)
00564   {
00565     if (observation_buffers_[i])
00566       observation_buffers_[i]->resetLastUpdated();
00567   }
00568 }
00569 void ObstacleLayer::deactivate()
00570 {
00571   for (unsigned int i = 0; i < observation_subscribers_.size(); ++i)
00572   {
00573     if (observation_subscribers_[i] != NULL)
00574       observation_subscribers_[i]->unsubscribe();
00575   }
00576 }
00577 
00578 void ObstacleLayer::updateRaytraceBounds(double ox, double oy, double wx, double wy, double range, double* min_x, double* min_y,
00579                                          double* max_x, double* max_y)
00580 {
00581   double dx = wx-ox, dy = wy-oy;
00582   double full_distance = hypot(dx, dy);
00583   double scale = std::min(1.0, range / full_distance);
00584   double ex = ox + dx * scale, ey = oy + dy * scale;
00585   touch(ex, ey, min_x, min_y, max_x, max_y);
00586 }
00587 
00588 void ObstacleLayer::reset()
00589 {
00590     deactivate();
00591     resetMaps();
00592     current_ = true;
00593     activate();
00594 }
00595 
00596 void ObstacleLayer::onFootprintChanged()
00597 {
00598   footprint_layer_.onFootprintChanged();
00599 }
00600 
00601 }  // namespace costmap_2d


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Thu Aug 27 2015 14:06:55