/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2008, 2013, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein * David V. Lu!! *********************************************************************/ #ifndef ROBOT_COSTMAP_2D_INFLATION_LAYER_H_ #define ROBOT_COSTMAP_2D_INFLATION_LAYER_H_ #include #include #include namespace robot_costmap_2d { /** * @class CellData * @brief Storage for cell information used during obstacle inflation */ class CellData { public: /** * @brief Constructor for a CellData objects * @param i The index of the cell in the cost map * @param x The x coordinate of the cell in the cost map * @param y The y coordinate of the cell in the cost map * @param sx The x coordinate of the closest obstacle cell in the costmap * @param sy The y coordinate of the closest obstacle cell in the costmap * @return */ CellData(double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) : index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy) { } unsigned int index_; unsigned int x_, y_; unsigned int src_x_, src_y_; }; class InflationLayer : public Layer { public: InflationLayer(); virtual ~InflationLayer() { deleteKernels(); if (seen_) delete[] seen_; } virtual void onInitialize(); virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y); virtual void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); virtual bool isDiscretized() { return true; } virtual void matchSize(); virtual void reset() { onInitialize(); } /** @brief Given a distance, compute a cost. * @param distance The distance from an obstacle in cells * @return A cost value for the distance */ virtual inline unsigned char computeCost(double distance) const { unsigned char cost = 0; if (distance == 0) cost = LETHAL_OBSTACLE; else if (distance * resolution_ <= inscribed_radius_) cost = INSCRIBED_INFLATED_OBSTACLE; else { // make sure cost falls off by Euclidean distance double euclidean_distance = distance * resolution_; double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_)); cost = (unsigned char)((INSCRIBED_INFLATED_OBSTACLE - 1) * factor); } return cost; } /** * @brief Change the values of the inflation radius parameters * @param inflation_radius The new inflation radius * @param cost_scaling_factor The new weight */ void setInflationParameters(double inflation_radius, double cost_scaling_factor); protected: virtual void onFootprintChanged(); boost::recursive_mutex* inflation_access_; double resolution_; double inflation_radius_; double inscribed_radius_; double weight_; bool inflate_unknown_; private: void handleImpl(const void* data, const std::type_info& info, const std::string& source) override; /** * @brief Lookup pre-computed distances * @param mx The x coordinate of the current cell * @param my The y coordinate of the current cell * @param src_x The x coordinate of the source cell * @param src_y The y coordinate of the source cell * @return */ inline double distanceLookup(int mx, int my, int src_x, int src_y) { unsigned int dx = abs(mx - src_x); unsigned int dy = abs(my - src_y); return cached_distances_[dx][dy]; } /** * @brief Lookup pre-computed costs * @param mx The x coordinate of the current cell * @param my The y coordinate of the current cell * @param src_x The x coordinate of the source cell * @param src_y The y coordinate of the source cell * @return */ inline unsigned char costLookup(int mx, int my, int src_x, int src_y) { unsigned int dx = abs(mx - src_x); unsigned int dy = abs(my - src_y); return cached_costs_[dx][dy]; } void computeCaches(); void deleteKernels(); void inflate_area(int min_i, int min_j, int max_i, int max_j, unsigned char* master_grid); unsigned int cellDistance(double world_dist) { return layered_costmap_->getCostmap()->cellDistance(world_dist); } inline void enqueue(unsigned int index, unsigned int mx, unsigned int my, unsigned int src_x, unsigned int src_y); unsigned int cell_inflation_radius_; unsigned int cached_cell_inflation_radius_; std::map > inflation_cells_; bool* seen_; int seen_size_; unsigned char** cached_costs_; double** cached_distances_; double last_min_x_, last_min_y_, last_max_x_, last_max_y_; bool getParams(const std::string& config_file_name, robot::NodeHandle &nh); bool need_reinflation_; ///< Indicates that the entire costmap should be reinflated next time around. }; } // namespace robot_costmap_2d #endif // ROBOT_COSTMAP_2D_INFLATION_LAYER_H_