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_

6
include/costmap_2d/layer.h
View File

@@ -40,7 +40,7 @@
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/layered_costmap.h>
#include <string>
#include "tf2/buffer_core.h"
#include <tf2/buffer_core.h>
namespace costmap_2d
{
@@ -146,6 +146,10 @@ private:
std::vector<geometry_msgs::Point> footprint_spec_;
};
using PluginPtr = std::shared_ptr<Layer>;
} // namespace costmap_2d
#endif // COSTMAP_2D_LAYER_H_

177
include/costmap_2d/obstacle_layer.h Normal file
View File

@@ -0,0 +1,177 @@
/*********************************************************************
*
* 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 COSTMAP_2D_OBSTACLE_LAYER_H_
#define COSTMAP_2D_OBSTACLE_LAYER_H_
#include <ros/ros.h>
#include <costmap_2d/costmap_layer.h>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/observation_buffer.h>
#include <nav_msgs/OccupancyGrid.h>
#include <sensor_msgs/LaserScan.h>
#include <laser_geometry/laser_geometry.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <tf2_ros/message_filter.h>
#include <message_filters/subscriber.h>
#include <dynamic_reconfigure/server.h>
#include <costmap_2d/ObstaclePluginConfig.h>
#include <costmap_2d/footprint.h>
namespace costmap_2d
{
class ObstacleLayer : public CostmapLayer
{
public:
ObstacleLayer()
{
costmap_ = NULL; // this is the unsigned char* member of parent class Costmap2D.
}
virtual ~ObstacleLayer();
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 void activate();
virtual void deactivate();
virtual void reset();
/**
* @brief A callback to handle buffering LaserScan messages
* @param message The message returned from a message notifier
* @param buffer A pointer to the observation buffer to update
*/
void laserScanCallback(const sensor_msgs::LaserScanConstPtr& message,
const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
/**
* @brief A callback to handle buffering LaserScan messages which need filtering to turn Inf values into range_max.
* @param message The message returned from a message notifier
* @param buffer A pointer to the observation buffer to update
*/
void laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr& message,
const boost::shared_ptr<ObservationBuffer>& buffer);
/**
* @brief A callback to handle buffering PointCloud messages
* @param message The message returned from a message notifier
* @param buffer A pointer to the observation buffer to update
*/
void pointCloudCallback(const sensor_msgs::PointCloudConstPtr& message,
const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
/**
* @brief A callback to handle buffering PointCloud2 messages
* @param message The message returned from a message notifier
* @param buffer A pointer to the observation buffer to update
*/
void pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& message,
const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
// for testing purposes
void addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing);
void clearStaticObservations(bool marking, bool clearing);
protected:
virtual void setupDynamicReconfigure(ros::NodeHandle& nh);
/**
* @brief Get the observations used to mark space
* @param marking_observations A reference to a vector that will be populated with the observations
* @return True if all the observation buffers are current, false otherwise
*/
bool getMarkingObservations(std::vector<costmap_2d::Observation>& marking_observations) const;
/**
* @brief Get the observations used to clear space
* @param clearing_observations A reference to a vector that will be populated with the observations
* @return True if all the observation buffers are current, false otherwise
*/
bool getClearingObservations(std::vector<costmap_2d::Observation>& clearing_observations) const;
/**
* @brief Clear freespace based on one observation
* @param clearing_observation The observation used to raytrace
* @param min_x
* @param min_y
* @param max_x
* @param max_y
*/
virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
double* max_x, double* max_y);
void updateRaytraceBounds(double ox, double oy, double wx, double wy, double range, double* min_x, double* min_y,
double* max_x, double* max_y);
std::vector<geometry_msgs::Point> transformed_footprint_;
bool footprint_clearing_enabled_;
void updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y);
std::string global_frame_; ///< @brief The global frame for the costmap
double max_obstacle_height_; ///< @brief Max Obstacle Height
laser_geometry::LaserProjection projector_; ///< @brief Used to project laser scans into point clouds
std::vector<boost::shared_ptr<message_filters::SubscriberBase> > observation_subscribers_; ///< @brief Used for the observation message filters
std::vector<boost::shared_ptr<tf2_ros::MessageFilterBase> > observation_notifiers_; ///< @brief Used to make sure that transforms are available for each sensor
std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > observation_buffers_; ///< @brief Used to store observations from various sensors
std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > marking_buffers_; ///< @brief Used to store observation buffers used for marking obstacles
std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > clearing_buffers_; ///< @brief Used to store observation buffers used for clearing obstacles
// Used only for testing purposes
std::vector<costmap_2d::Observation> static_clearing_observations_, static_marking_observations_;
bool rolling_window_;
dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig> *dsrv_;
int combination_method_;
private:
void reconfigureCB(costmap_2d::ObstaclePluginConfig &config, uint32_t level);
};
} // namespace costmap_2d
#endif // COSTMAP_2D_OBSTACLE_LAYER_H_