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

update file voxel_layer

Browse Source
This commit is contained in:
duongtd
2025-11-15 17:36:52 +07:00
parent bd98bf4e08
commit 49a72383c8
23 changed files with 977 additions and 253 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
include/costmap_2d/costmap_2d_robot.h
View File

@@ -47,7 +47,7 @@
#include <geometry_msgs/PolygonStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <dynamic_reconfigure/server.h>
#include <pluginlib/class_loader.hpp>
// #include <pluginlib/class_loader.hpp>
#include <tf2/LinearMath/Transform.h>
// class SuperValue : public XmlRpc::XmlRpcValue
@@ -71,7 +71,7 @@ namespace costmap_2d
/** @brief A ROS wrapper for a 2D Costmap. Handles subscribing to
* topics that provide observations about obstacles in either the form
* of PointCloud or LaserScan messages. */
class Costmap2DRobot
class Costmap2DROS
{
public:
/**
@@ -79,8 +79,8 @@ public:
* @param name The name for this costmap
* @param tf A reference to a TransformListener
*/
Costmap2DRobot(const std::string &name, tf2::BufferCore& tf);
~Costmap2DRobot();
Costmap2DROS(const std::string &name, tf2_ros::Buffer& tf);
~Costmap2DROS();
/**
* @brief Subscribes to sensor topics if necessary and starts costmap
@@ -255,28 +255,28 @@ private:
void warnForOldParameters(ros::NodeHandle& nh);
void checkOldParam(ros::NodeHandle& nh, const std::string &param_name);
void copyParentParameters(const std::string& plugin_name, const std::string& plugin_type, ros::NodeHandle& nh);
// void reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level);
void reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level);
void movementCB(const ros::TimerEvent &event);
void mapUpdateLoop(double frequency);
bool map_update_thread_shutdown_;
bool stop_updates_, initialized_, stopped_;
boost::thread* map_update_thread_; ///< @brief A thread for updating the map
// ros::Time last_publish_;
// ros::Duration publish_cycle;
// pluginlib::ClassLoader<costmap_2d::Layer> plugin_loader_;
// Costmap2DPublisher* publisher_;
// dynamic_reconfigure::Server<costmap_2d::Costmap2DConfig> *dsrv_;
ros::Time last_publish_;
ros::Duration publish_cycle;
pluginlib::ClassLoader<costmap_2d::Layer> plugin_loader_;
Costmap2DPublisher* publisher_;
dynamic_reconfigure::Server<costmap_2d::Costmap2DConfig> *dsrv_;
boost::recursive_mutex configuration_mutex_;
// ros::Subscriber footprint_sub_;
// ros::Publisher footprint_pub_;
ros::Subscriber footprint_sub_;
ros::Publisher footprint_pub_;
std::vector<geometry_msgs::Point> unpadded_footprint_;
std::vector<geometry_msgs::Point> padded_footprint_;
float footprint_padding_;
// costmap_2d::Costmap2DConfig old_config_;
costmap_2d::Costmap2DConfig old_config_;
};
// class Costmap2DRobot
// class Costmap2DROS
} // namespace costmap_2d
#endif // COSTMAP_2D_COSTMAP_2D_ROS_H

17
include/costmap_2d/costmap_layer.h
View File

@@ -71,22 +71,7 @@ public:
*/
void addExtraBounds(double mx0, double my0, double mx1, double my1);
template<typename T>
void dataCallBack(const T& value, const std::string& topic) {
handle(value, topic);
}
protected:
// Hàm template public, dùng để gửi dữ liệu
template<typename T>
void handle(const T& data, const std::string& topic) {
handleImpl(static_cast<const void*>(&data), typeid(T), topic);
}
// Hàm ảo duy nhất mà plugin sẽ override
virtual void handleImpl(const void* data,
const std::type_info& type,
const std::string& topic) = 0;
/*
* Updates the master_grid within the specified
* bounding box using this layer's values.
@@ -162,7 +147,5 @@ private:
double extra_min_x_, extra_max_x_, extra_min_y_, extra_max_y_;
};
using PluginCostmapLayerPtr = std::shared_ptr<CostmapLayer>;
} // namespace costmap_2d
#endif // COSTMAP_2D_COSTMAP_LAYER_H_

10
include/costmap_2d/inflation_layer.h
View File

@@ -43,9 +43,16 @@ public:
virtual ~InflationLayer()
{
// ✅ FIX: Delete mutex được tạo bằng new (phải delete trước để tránh lock trong destructor)
if (inflation_access_)
delete inflation_access_;
inflation_access_ = nullptr;
// Delete kernels và seen array
deleteKernels();
if (seen_)
delete[] seen_;
seen_ = nullptr;
}
virtual void onInitialize();
@@ -98,6 +105,9 @@ protected:
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

19
include/costmap_2d/layer.h
View File

@@ -41,6 +41,8 @@
#include <costmap_2d/layered_costmap.h>
#include <string>
#include <tf2/buffer_core.h>
#include <costmap_2d/layered_costmap.h>
#include <boost/shared_ptr.hpp>
namespace costmap_2d
{
@@ -129,7 +131,22 @@ public:
* notified of changes to the robot's footprint. */
virtual void onFootprintChanged() {}
template<typename T>
void dataCallBack(const T& value, const std::string& topic) {
handle(value, topic);
}
protected:
// Hàm template public, dùng để gửi dữ liệu
template<typename T>
void handle(const T& data, const std::string& topic) {
handleImpl(static_cast<const void*>(&data), typeid(T), topic);
}
// Hàm ảo duy nhất mà plugin sẽ override
virtual void handleImpl(const void* data,
const std::type_info& type,
const std::string& topic) = 0;
/** @brief This is called at the end of initialize(). Override to
* implement subclass-specific initialization.
*
@@ -146,8 +163,6 @@ private:
std::vector<geometry_msgs::Point> footprint_spec_;
};
using PluginLayerPtr = std::shared_ptr<Layer>;
} // namespace costmap_2d
#endif // COSTMAP_2D_LAYER_H_

12
include/costmap_2d/layered_costmap.h
View File

@@ -1,17 +1,19 @@
#ifndef COSTMAP_2D_LAYERED_COSTMAP_H_
#define COSTMAP_2D_LAYERED_COSTMAP_H_
#include <costmap_2d/cost_values.h> // Chứa các hằng số giá trị chi phí (ví dụ: FREE_SPACE, LETHAL_OBSTACLE, NO_INFORMATION)
#include <costmap_2d/layer.h> // Lớp cơ sở cho các lớp (layer) con: static layer, obstacle layer, inflation layer...
#include <costmap_2d/cost_values.h> // Chứa các hằng số giá trị chi phí (ví dụ: FREE_SPACE, LETHAL_OBSTACLE, NO_INFORMATION)
#include <costmap_2d/costmap_2d.h> // Định nghĩa lớp Costmap2D (bản đồ 2D cơ bản)
#include <vector>
#include <string>
#include <boost/dll/import.hpp>
namespace costmap_2d
{
class Layer; // Khai báo trước để dùng trong vector plugin
using PluginLayerPtr = std::shared_ptr<Layer>;
/**
* @class LayeredCostmap
* @brief Lớp này quản lý nhiều "layer" (tầng bản đồ) khác nhau và kết hợp chúng thành một bản đồ chi phí tổng hợp.
@@ -102,7 +104,7 @@ public:
* @brief Trả về danh sách các plugin (layer) đã được nạp.
* @return vector các shared_ptr<Layer>.
*/
std::vector<boost::shared_ptr<Layer> >* getPlugins()
std::vector<costmap_2d::PluginLayerPtr>* getPlugins()
{
return &plugins_;
}
@@ -111,7 +113,7 @@ public:
* @brief Thêm một plugin (layer) mới vào bản đồ.
* @param plugin: con trỏ thông minh đến lớp layer.
*/
void addPlugin(boost::shared_ptr<Layer> plugin)
void addPlugin(costmap_2d::PluginLayerPtr plugin)
{
plugins_.push_back(plugin);
}
@@ -179,7 +181,7 @@ private:
double minx_, miny_, maxx_, maxy_; ///< Giới hạn vùng bản đồ được cập nhật (tọa độ thế giới)
unsigned int bx0_, bxn_, by0_, byn_; ///< Giới hạn vùng bản đồ được cập nhật (tọa độ lưới)
std::vector<boost::shared_ptr<Layer> > plugins_; ///< Danh sách các layer plugin (ví dụ: StaticLayer, ObstacleLayer, InflationLayer)
std::vector<costmap_2d::PluginLayerPtr> plugins_; ///< Danh sách các layer plugin (ví dụ: StaticLayer, ObstacleLayer, InflationLayer)
bool initialized_; ///< Đã được khởi tạo hoàn toàn hay chưa
bool size_locked_; ///< Có cho phép thay đổi kích thước bản đồ sau này không

2
include/costmap_2d/obstacle_layer.h
View File

@@ -149,8 +149,6 @@ private:
bool getParams();
};
using PluginObstacleLayerPtr = std::shared_ptr<ObstacleLayer>;
} // namespace costmap_2d
#endif // COSTMAP_2D_OBSTACLE_LAYER_H_

2
include/costmap_2d/static_layer.h
View File

@@ -64,8 +64,6 @@ private:
bool getParams();
};
using PluginStaticLayerPtr = std::shared_ptr<StaticLayer>;
} // namespace costmap_2d
#endif // COSTMAP_2D_STATIC_LAYER_H_

2
include/costmap_2d/voxel_layer.h
View File

@@ -53,7 +53,7 @@
// #include <dynamic_reconfigure/server.h>
// #include <costmap_2d/VoxelPluginConfig.h>
#include <costmap_2d/obstacle_layer.h>
// #include <voxel_grid/voxel_grid.h>
#include <voxel_grid/voxel_grid.h>
namespace costmap_2d
{