map_grid.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2017, Locus Robotics
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the copyright holder nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00035 #include <dwb_critics/map_grid.h>
00036 #include <nav_grid/coordinate_conversion.h>
00037 #include <nav_core2/exceptions.h>
00038 #include <string>
00039 #include <algorithm>
00040 
00041 namespace dwb_critics
00042 {
00043 
00044 // Customization of the CostmapQueue validCellToQueue method
00045 bool MapGridCritic::MapGridQueue::validCellToQueue(const costmap_queue::CellData& cell)
00046 {
00047   unsigned char cost = costmap_(cell.x_, cell.y_);
00048   if (cost == costmap_.LETHAL_OBSTACLE ||
00049       cost == costmap_.INSCRIBED_INFLATED_OBSTACLE ||
00050       cost == costmap_.NO_INFORMATION)
00051   {
00052     parent_.setAsObstacle(cell.x_, cell.y_);
00053     return false;
00054   }
00055 
00056   return true;
00057 }
00058 
00059 void MapGridCritic::onInit()
00060 {
00061   queue_ = std::make_shared<MapGridQueue>(*costmap_, *this);
00062 
00063   // Always set to true, but can be overriden by subclasses
00064   stop_on_failure_ = true;
00065 
00066   std::string aggro_str;
00067   critic_nh_.param("aggregation_type", aggro_str, std::string("last"));
00068   std::transform(aggro_str.begin(), aggro_str.end(), aggro_str.begin(), ::tolower);
00069   if (aggro_str == "last")
00070   {
00071     aggregationType_ = ScoreAggregationType::Last;
00072   }
00073   else if (aggro_str == "sum")
00074   {
00075     aggregationType_ = ScoreAggregationType::Sum;
00076   }
00077   else if (aggro_str == "product")
00078   {
00079     aggregationType_ = ScoreAggregationType::Product;
00080   }
00081   else
00082   {
00083     ROS_ERROR_NAMED("MapGridCritic", "aggregation_type parameter \"%s\" invalid. Using Last.", aggro_str.c_str());
00084     aggregationType_ = ScoreAggregationType::Last;
00085   }
00086 }
00087 
00088 void MapGridCritic::setAsObstacle(unsigned int x, unsigned int y)
00089 {
00090   cell_values_.setValue(x, y, obstacle_score_);
00091 }
00092 
00093 void MapGridCritic::reset()
00094 {
00095   queue_->reset();
00096   if (costmap_->getInfo() == cell_values_.getInfo())
00097   {
00098     cell_values_.reset();
00099   }
00100   else
00101   {
00102     obstacle_score_ = static_cast<double>(costmap_->getWidth() * costmap_->getHeight());
00103     unreachable_score_ = obstacle_score_ + 1.0;
00104     cell_values_.setDefaultValue(unreachable_score_);
00105     cell_values_.setInfo(costmap_->getInfo());
00106   }
00107 }
00108 
00109 void MapGridCritic::propogateManhattanDistances()
00110 {
00111   while (!queue_->isEmpty())
00112   {
00113     costmap_queue::CellData cell = queue_->getNextCell();
00114     cell_values_.setValue(cell.x_, cell.y_,
00115                           std::abs(static_cast<int>(cell.src_x_) - static_cast<int>(cell.x_))
00116                           + std::abs(static_cast<int>(cell.src_y_) - static_cast<int>(cell.y_)));
00117   }
00118 }
00119 
00120 double MapGridCritic::scoreTrajectory(const dwb_msgs::Trajectory2D& traj)
00121 {
00122   double score = 0.0;
00123   unsigned int start_index = 0;
00124   if (aggregationType_ == ScoreAggregationType::Product)
00125   {
00126     score = 1.0;
00127   }
00128   else if (aggregationType_ == ScoreAggregationType::Last && !stop_on_failure_)
00129   {
00130     start_index = traj.poses.size() - 1;
00131   }
00132   double grid_dist;
00133 
00134   for (unsigned int i = start_index; i < traj.poses.size(); ++i)
00135   {
00136     grid_dist = scorePose(traj.poses[i]);
00137     if (stop_on_failure_)
00138     {
00139       if (grid_dist == obstacle_score_)
00140       {
00141         throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Hits Obstacle.");
00142       }
00143       else if (grid_dist == unreachable_score_)
00144       {
00145         throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Hits Unreachable Area.");
00146       }
00147     }
00148 
00149     switch (aggregationType_)
00150     {
00151     case ScoreAggregationType::Last:
00152       score = grid_dist;
00153       break;
00154     case ScoreAggregationType::Sum:
00155       score += grid_dist;
00156       break;
00157     case ScoreAggregationType::Product:
00158       if (score > 0)
00159       {
00160         score *= grid_dist;
00161       }
00162       break;
00163     }
00164   }
00165 
00166   return score;
00167 }
00168 
00169 double MapGridCritic::scorePose(const geometry_msgs::Pose2D& pose)
00170 {
00171   unsigned int cell_x, cell_y;
00172   // we won't allow trajectories that go off the map... shouldn't happen that often anyways
00173   if (!worldToGridBounded(costmap_->getInfo(), pose.x, pose.y, cell_x, cell_y))
00174   {
00175     throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Goes Off Grid.");
00176   }
00177   return getScore(cell_x, cell_y);
00178 }
00179 
00180 void MapGridCritic::addCriticVisualization(sensor_msgs::PointCloud& pc)
00181 {
00182   sensor_msgs::ChannelFloat32 grid_scores;
00183   grid_scores.name = name_;
00184 
00185   const nav_core2::Costmap& costmap = *costmap_;
00186   unsigned int size_x = costmap.getWidth();
00187   unsigned int size_y = costmap.getHeight();
00188   grid_scores.values.resize(size_x * size_y);
00189   unsigned int i = 0;
00190   for (unsigned int cy = 0; cy < size_y; cy++)
00191   {
00192     for (unsigned int cx = 0; cx < size_x; cx++)
00193     {
00194       grid_scores.values[i] = getScore(cx, cy);
00195       i++;
00196     }
00197   }
00198   pc.channels.push_back(grid_scores);
00199 }
00200 
00201 }  // namespace dwb_critics


dwb_critics
Author(s): David V. Lu!!
autogenerated on Wed Jun 26 2019 20:09:47