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

hiep sua ten file

Browse Source
This commit is contained in:
HiepLM
2025-12-30 09:56:35 +07:00
parent 2c3d7d586d
commit 4246453ae6
14 changed files with 97 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
include/costmap_2d/directional_layer.h
View File

@@ -2,7 +2,7 @@
#define COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
#include <costmap_2d/static_layer.h>
#include <nav_msgs/Path.h>
#include <robot_nav_msgs/Path.h>
namespace costmap_2d
{
@@ -15,20 +15,20 @@ namespace costmap_2d
void resetMap();
private:
void incomingMap(const nav_msgs::OccupancyGrid &new_map);
bool laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const nav_msgs::Path path);
void incomingMap(const robot_nav_msgs::OccupancyGrid &new_map);
bool laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const robot_nav_msgs::Path path);
unsigned char laneFilter(unsigned int x, unsigned int y, double yaw_robot);
void laneFilter(std::vector<unsigned int> x, std::vector<unsigned int> y, std::vector<double> yaw_robot);
void laneFilter(std::vector<std::pair<unsigned int, double>> x, std::vector<std::pair<unsigned int, double>> y, std::vector<double> yaw_robot);
bool inSkipErea(double start_x, double start_y, double end_x, double end_y, double skip_distance);
void convertToMap(unsigned char *costmap, nav_msgs::OccupancyGrid &dir_map, double occ_th, double free_th);
void convertToMap(unsigned char *costmap, robot_nav_msgs::OccupancyGrid &dir_map, double occ_th, double free_th);
bool getRobotPose(double &x, double &y, double &yaw);
std::vector<std::array<uint16_t, 2>> directional_areas_;
double pose_x_, pose_y_, pose_yaw_;
double distance_skip_ = 1.0;
// ros::Publisher lane_mask_pub_;
nav_msgs::OccupancyGrid new_map_;
robot_nav_msgs::OccupancyGrid new_map_;
};
}

16
include/costmap_2d/observation.h
View File

@@ -33,7 +33,7 @@
#define COSTMAP_2D_OBSERVATION_H_
#include <robot_geometry_msgs/Point.h>
#include <sensor_msgs/PointCloud2.h>
#include <robot_sensor_msgs/PointCloud2.h>
namespace costmap_2d
{
@@ -50,7 +50,7 @@ public:
* @brief Creates an empty observation
*/
Observation() :
cloud_(new sensor_msgs::PointCloud2()), obstacle_range_(0.0), raytrace_range_(0.0)
cloud_(new robot_sensor_msgs::PointCloud2()), obstacle_range_(0.0), raytrace_range_(0.0)
{
}
@@ -66,9 +66,9 @@ public:
* @param obstacle_range The range out to which an observation should be able to insert obstacles
* @param raytrace_range The range out to which an observation should be able to clear via raytracing
*/
Observation(robot_geometry_msgs::Point& origin, const sensor_msgs::PointCloud2 &cloud,
Observation(robot_geometry_msgs::Point& origin, const robot_sensor_msgs::PointCloud2 &cloud,
double obstacle_range, double raytrace_range) :
origin_(origin), cloud_(new sensor_msgs::PointCloud2(cloud)),
origin_(origin), cloud_(new robot_sensor_msgs::PointCloud2(cloud)),
obstacle_range_(obstacle_range), raytrace_range_(raytrace_range)
{
}
@@ -78,7 +78,7 @@ public:
* @param obs The observation to copy
*/
Observation(const Observation& obs) :
origin_(obs.origin_), cloud_(new sensor_msgs::PointCloud2(*(obs.cloud_))),
origin_(obs.origin_), cloud_(new robot_sensor_msgs::PointCloud2(*(obs.cloud_))),
obstacle_range_(obs.obstacle_range_), raytrace_range_(obs.raytrace_range_)
{
}
@@ -88,13 +88,13 @@ public:
* @param cloud The point cloud of the observation
* @param obstacle_range The range out to which an observation should be able to insert obstacles
*/
Observation(const sensor_msgs::PointCloud2 &cloud, double obstacle_range) :
cloud_(new sensor_msgs::PointCloud2(cloud)), obstacle_range_(obstacle_range), raytrace_range_(0.0)
Observation(const robot_sensor_msgs::PointCloud2 &cloud, double obstacle_range) :
cloud_(new robot_sensor_msgs::PointCloud2(cloud)), obstacle_range_(obstacle_range), raytrace_range_(0.0)
{
}
robot_geometry_msgs::Point origin_;
sensor_msgs::PointCloud2* cloud_;
robot_sensor_msgs::PointCloud2* cloud_;
double obstacle_range_, raytrace_range_;
};

4
include/costmap_2d/observation_buffer.h
View File

@@ -44,7 +44,7 @@
#include <costmap_2d/observation.h>
#include <tf3/buffer_core.h>
#include <sensor_msgs/PointCloud2.h>
#include <robot_sensor_msgs/PointCloud2.h>
// Thread support
#include <boost/thread.hpp>
@@ -96,7 +96,7 @@ public:
* <b>Note: The burden is on the user to make sure the transform is available... ie they should use a MessageNotifier</b>
* @param cloud The cloud to be buffered
*/
void bufferCloud(const sensor_msgs::PointCloud2& cloud);
void bufferCloud(const robot_sensor_msgs::PointCloud2& cloud);
/**
* @brief Pushes copies of all current observations onto the end of the vector passed in

18
include/costmap_2d/obstacle_layer.h
View File

@@ -43,13 +43,13 @@
#include <costmap_2d/observation_buffer.h>
#include <costmap_2d/footprint.h>
#include <nav_msgs/OccupancyGrid.h>
#include <robot_nav_msgs/OccupancyGrid.h>
#include <sensor_msgs/LaserScan.h>
#include <robot_sensor_msgs/LaserScan.h>
#include <laser_geometry/laser_geometry.hpp>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <robot_sensor_msgs/PointCloud.h>
#include <robot_sensor_msgs/PointCloud2.h>
#include <robot_sensor_msgs/point_cloud_conversion.h>
namespace costmap_2d
{
@@ -86,7 +86,7 @@ protected:
* @param message The message returned from a message notifier
* @param buffer A pointer to the observation buffer to update
*/
void laserScanCallback(const sensor_msgs::LaserScan& message,
void laserScanCallback(const robot_sensor_msgs::LaserScan& message,
const boost::shared_ptr<ObservationBuffer>& buffer);
/**
@@ -94,7 +94,7 @@ protected:
* @param message The message returned from a message notifier
* @param buffer A pointer to the observation buffer to update
*/
void laserScanValidInfCallback(const sensor_msgs::LaserScan& message,
void laserScanValidInfCallback(const robot_sensor_msgs::LaserScan& message,
const boost::shared_ptr<ObservationBuffer>& buffer);
/**
@@ -102,7 +102,7 @@ protected:
* @param message The message returned from a message notifier
* @param buffer A pointer to the observation buffer to update
*/
void pointCloudCallback(const sensor_msgs::PointCloud& message,
void pointCloudCallback(const robot_sensor_msgs::PointCloud& message,
const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
/**
@@ -110,7 +110,7 @@ protected:
* @param message The message returned from a message notifier
* @param buffer A pointer to the observation buffer to update
*/
void pointCloud2Callback(const sensor_msgs::PointCloud2& message,
void pointCloud2Callback(const robot_sensor_msgs::PointCloud2& message,
const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
/**

8
include/costmap_2d/static_layer.h
View File

@@ -41,8 +41,8 @@
#include <costmap_2d/costmap_layer.h>
#include <costmap_2d/layered_costmap.h>
#include <nav_msgs/OccupancyGrid.h>
#include <map_msgs/OccupancyGridUpdate.h>
#include <robot_nav_msgs/OccupancyGrid.h>
#include <robot_map_msgs/OccupancyGridUpdate.h>
#include <string>
namespace costmap_2d
@@ -68,8 +68,8 @@ protected:
const std::type_info& type,
const std::string& topic) override;
virtual unsigned char interpretValue(unsigned char value);
virtual void incomingMap(const nav_msgs::OccupancyGrid& new_map);
virtual void incomingUpdate(const map_msgs::OccupancyGridUpdate& update);
virtual void incomingMap(const robot_nav_msgs::OccupancyGrid& new_map);
virtual void incomingUpdate(const robot_map_msgs::OccupancyGridUpdate& update);
unsigned char* threshold_;
std::string base_frame_id_;
std::string global_frame_; ///< @brief The global frame for the costmap

12
include/costmap_2d/testing_helper.h
View File

@@ -7,7 +7,7 @@
#include <costmap_2d/obstacle_layer.h>
#include <costmap_2d/inflation_layer.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h>
const double MAX_Z(1.0);
@@ -66,13 +66,13 @@ costmap_2d::ObstacleLayer* addObstacleLayer(costmap_2d::LayeredCostmap& layers,
void addObservation(costmap_2d::ObstacleLayer* olayer, double x, double y, double z = 0.0,
double ox = 0.0, double oy = 0.0, double oz = MAX_Z){
sensor_msgs::PointCloud2 cloud;
sensor_msgs::PointCloud2Modifier modifier(cloud);
robot_sensor_msgs::PointCloud2 cloud;
robot_sensor_msgs::PointCloud2Modifier modifier(cloud);
modifier.setPointCloud2FieldsByString(1, "xyz");
modifier.resize(1);
sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
robot_sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
robot_sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
robot_sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
*iter_x = x;
*iter_y = y;
*iter_z = z;

4
include/costmap_2d/voxel_grid.h
View File

@@ -10,7 +10,7 @@
#define VOXEL_GRID_COSTMAP_2D_H
#include <vector>
#include <std_msgs/Header.h>
#include <robot_std_msgs/Header.h>
#include <robot_geometry_msgs/Point32.h>
#include <robot_geometry_msgs/Vector3.h>
@@ -18,7 +18,7 @@ namespace costmap_2d
{
struct VoxelGrid
{
std_msgs::Header header;
robot_std_msgs::Header header;
std::vector<uint32_t> data;
robot_geometry_msgs::Point32 origin;
robot_geometry_msgs::Vector3 resolutions;

12
include/costmap_2d/voxel_layer.h
View File

@@ -42,12 +42,12 @@
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/observation_buffer.h>
#include <costmap_2d/voxel_grid.h>
#include <nav_msgs/OccupancyGrid.h>
#include <sensor_msgs/LaserScan.h>
#include <robot_nav_msgs/OccupancyGrid.h>
#include <robot_sensor_msgs/LaserScan.h>
#include <laser_geometry/laser_geometry.hpp>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <robot_sensor_msgs/PointCloud.h>
#include <robot_sensor_msgs/PointCloud2.h>
#include <robot_sensor_msgs/point_cloud_conversion.h>
#include <costmap_2d/obstacle_layer.h>
#include <voxel_grid/voxel_grid.h>
@@ -93,7 +93,7 @@ private:
voxel_grid::VoxelGrid voxel_grid_;
double z_resolution_, origin_z_;
unsigned int unknown_threshold_, mark_threshold_, size_z_;
sensor_msgs::PointCloud clearing_endpoints_;
robot_sensor_msgs::PointCloud clearing_endpoints_;
inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz)
{