Hiep update
This commit is contained in:
197
include/robot_costmap_2d/inflation_layer.h
Executable file
197
include/robot_costmap_2d/inflation_layer.h
Executable file
@@ -0,0 +1,197 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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 <robot_costmap_2d/layer.h>
|
||||
#include <robot_costmap_2d/layered_costmap.h>
|
||||
#include <boost/thread.hpp>
|
||||
|
||||
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<double, std::vector<CellData> > 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_
|
||||
Reference in New Issue
Block a user