DuongTD/costmap_2d
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

1720_28102025

Browse Source
This commit is contained in:
duongtd
2025-10-28 17:21:02 +07:00
parent 5d8981eab6
commit 7c1dcfd352
31 changed files with 663 additions and 445 deletions
Download Patch File Download Diff File Expand all files Collapse all files

373
include/costmap_2d/inflation_layer.h
View File

@@ -3,265 +3,164 @@
#define COSTMAP_2D_INFLATION_LAYER_H_
// #include <ros/ros.h>
// #include <costmap_2d/layer.h>
// #include <costmap_2d/layered_costmap.h>
#include <costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h>
// #include <costmap_2d/InflationPluginConfig.h>
// #include <dynamic_reconfigure/server.h>
// #include <boost/thread.hpp>
#include <boost/thread.hpp>
#include <boost/dll/alias.hpp>
// namespace 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();
namespace 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_;
};
// virtual ~InflationLayer()
// {
// deleteKernels();
// if (dsrv_)
// delete dsrv_;
// if (seen_)
// delete[] seen_;
// }
class InflationLayer : public Layer
{
public:
InflationLayer();
// 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(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 ~InflationLayer()
{
deleteKernels();
// if (dsrv_)
// delete dsrv_;
if (seen_)
delete[] seen_;
}
// virtual void reset() { onInitialize(); }
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(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();
// /** @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;
// }
virtual void reset() { onInitialize(); }
// /**
// * @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);
/** @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;
}
// protected:
// virtual void onFootprintChanged();
// boost::recursive_mutex* inflation_access_;
/**
* @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);
// double resolution_;
// double inflation_radius_;
// double inscribed_radius_;
// double weight_;
// bool inflate_unknown_;
protected:
virtual void onFootprintChanged();
boost::recursive_mutex* inflation_access_;
// private:
// /**
// * @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];
// }
double resolution_;
double inflation_radius_;
double inscribed_radius_;
double weight_;
bool inflate_unknown_;
// /**
// * @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];
// }
private:
/**
* @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];
}
// void computeCaches();
// void deleteKernels();
// void inflate_area(int min_i, int min_j, int max_i, int max_j, unsigned char* master_grid);
/**
* @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];
}
// unsigned int cellDistance(double world_dist)
// {
// return layered_costmap_->getCostmap()->cellDistance(world_dist);
// }
void computeCaches();
void deleteKernels();
void inflate_area(int min_i, int min_j, int max_i, int max_j, unsigned char* master_grid);
// inline void enqueue(unsigned int index, unsigned int mx, unsigned int my,
// unsigned int src_x, unsigned int src_y);
unsigned int cellDistance(double world_dist)
{
return layered_costmap_->getCostmap()->cellDistance(world_dist);
}
// unsigned int cell_inflation_radius_;
// unsigned int cached_cell_inflation_radius_;
// std::map<double, std::vector<CellData> > inflation_cells_;
inline void enqueue(unsigned int index, unsigned int mx, unsigned int my,
unsigned int src_x, unsigned int src_y);
// bool* seen_;
// int seen_size_;
unsigned int cell_inflation_radius_;
unsigned int cached_cell_inflation_radius_;
std::map<double, std::vector<CellData> > inflation_cells_;
// unsigned char** cached_costs_;
// double** cached_distances_;
// double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
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_;
// dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig> *dsrv_;
// void reconfigureCB(costmap_2d::InflationPluginConfig &config, uint32_t level);
// bool need_reinflation_; ///< Indicates that the entire costmap should be reinflated next time around.
// };
bool need_reinflation_; ///< Indicates that the entire costmap should be reinflated next time around.
};
// } // namespace costmap_2d
// #endif // COSTMAP_2D_INFLATION_LAYER_H_
// /////////////////////////////////////////
// #ifndef INFLATION_LAYER_H_
// #define INFLATION_LAYER_H_
// #include "share.h"
// #include <cmath>
// #include <map>
// #include <vector>
// #include <boost/thread/recursive_mutex.hpp>
// /**
// * @brief Lưu thông tin 1 cell khi thực hiện inflation
// */
// class CellData
// {
// public:
// 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_;
// };
// /**
// * @brief Lớp thực hiện "inflation" cho costmap (mở rộng vùng vật cản)
// */
// class InflationLayer : public Layer
// {
// public:
// InflationLayer();
// virtual ~InflationLayer();
// 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(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(); }
// 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:
// inline double distanceLookup(int mx, int my, int src_x, int src_y);
// inline unsigned char costLookup(int mx, int my, int src_x, int src_y);
// inline unsigned char computeCost(double distance) const;
// void computeCaches();
// void deleteKernels();
// void inflate_area(int min_i, int min_j, int max_i, int max_j, unsigned char* master_grid);
// 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 need_reinflation_;
// };
#endif // INFLATION_LAYER_H_
} // namespace costmap_2d
#endif // COSTMAP_2D_INFLATION_LAYER_H_