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

1738_29102025

Browse Source
This commit is contained in:
duongtd
2025-10-29 17:38:43 +07:00
parent 7c1dcfd352
commit cdb9ded893
51 changed files with 2055 additions and 1216 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
include/costmap_2d/inflation_layer.h
View File

@@ -2,14 +2,13 @@
#ifndef COSTMAP_2D_INFLATION_LAYER_H_
#define COSTMAP_2D_INFLATION_LAYER_H_
// #include <ros/ros.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/dll/alias.hpp>
#include <stdexcept>
#include <iostream>
#include <yaml-cpp/yaml.h>
namespace costmap_2d
{
@@ -46,8 +45,6 @@ public:
virtual ~InflationLayer()
{
deleteKernels();
// if (dsrv_)
// delete dsrv_;
if (seen_)
delete[] seen_;
}
@@ -155,8 +152,7 @@ private:
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 getParams();
bool need_reinflation_; ///< Indicates that the entire costmap should be reinflated next time around.
};

4
include/costmap_2d/layer.h
View File

@@ -51,7 +51,7 @@ class Layer
public:
Layer();
void initialize(LayeredCostmap* parent, std::string name, std::shared_ptr<tf2::BufferCore> *tf);
void initialize(LayeredCostmap* parent, std::string name, tf2::BufferCore *tf);
/**
* @brief This is called by the LayeredCostmap to poll this plugin as to how
@@ -140,7 +140,7 @@ protected:
bool current_;
bool enabled_;
std::string name_;
std::shared_ptr<tf2::BufferCore> *tf_;
tf2::BufferCore *tf_;
private:
std::vector<geometry_msgs::Point> footprint_spec_;

77
include/costmap_2d/obstacle_layer.h
View File

@@ -38,7 +38,6 @@
#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>
@@ -46,14 +45,14 @@
#include <nav_msgs/OccupancyGrid.h>
#include <sensor_msgs/LaserScan.h>
#include <laser_geometry/laser_geometry.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 <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
@@ -77,44 +76,43 @@ public:
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
// * @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 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 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);
// /**
// * @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
@@ -152,10 +150,10 @@ protected:
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
// 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<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
@@ -164,12 +162,11 @@ protected:
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);
bool getParams();
};
} // namespace costmap_2d

17
include/costmap_2d/static_layer.h
View File

@@ -3,12 +3,13 @@
#include <costmap_2d/costmap_layer.h>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/GenericPluginConfig.h>
#include <dynamic_reconfigure/server.h>
// #include <costmap_2d/GenericPluginConfig.h>
// #include <dynamic_reconfigure/server.h>
#include <nav_msgs/OccupancyGrid.h>
#include <map_msgs/OccupancyGridUpdate.h>
#include <message_filters/subscriber.h>
// #include <message_filters/subscriber.h>
#include <string>
#include <geometry_msgs/TransformStamped.h>
namespace costmap_2d
{
@@ -28,10 +29,10 @@ public:
virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
virtual void matchSize();
virtual void incomingMap(const nav_msgs::OccupancyGrid& new_map);
virtual void incomingUpdate(const map_msgs::OccupancyGridUpdate& update);
protected:
virtual unsigned char interpretValue(unsigned char value);
virtual void incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map);
virtual void incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr& update);
unsigned char* threshold_;
std::string base_frame_id_;
std::string global_frame_; ///< @brief The global frame for the costmap
@@ -45,7 +46,7 @@ protected:
bool use_maximum_;
bool first_map_only_; ///< @brief Store the first static map and reuse it on reinitializing
bool trinary_costmap_;
ros::Subscriber map_sub_, map_update_sub_;
// ros::Subscriber map_sub_, map_update_sub_;
unsigned char lethal_threshold_, unknown_cost_value_;
private:
@@ -55,8 +56,8 @@ private:
* map along with its size will determine what parts of the costmap's
* static map are overwritten.
*/
void reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level);
dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> *dsrv_;
// void reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level);
// dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> *dsrv_;
};
} // namespace costmap_2d