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

fix core dumped err when loadplugin

Browse Source
This commit is contained in:
duongtd
2025-12-01 17:34:23 +07:00
parent 2439f2a71d
commit 11ff1baa79
34 changed files with 1177 additions and 760 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
include/costmap_2d/array_parser.h Normal file → Executable file
View File

0
include/costmap_2d/cost_values.h Normal file → Executable file
View File

311
include/costmap_2d/costmap_2d.h Normal file → Executable file
View File

@@ -1,103 +1,172 @@
#ifndef COSTMAP_2D_COSTMAP_2D_H_ // Bảo vệ tránh include file nhiều lần
/*********************************************************************
*
* 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_COSTMAP_2D_H_
#define COSTMAP_2D_COSTMAP_2D_H_
#include <vector> // Dùng cho std::vector
#include <queue> // Dùng trong các thuật toán quét ô (flood fill, BFS)
#include <boost/thread.hpp> // Dùng mutex để đồng bộ truy cập giữa các thread (multi-thread safe)
#include <vector>
#include <queue>
#include <geometry_msgs/Point.h>
#include <boost/thread.hpp>
namespace costmap_2d // Mọi thứ nằm trong namespace costmap_2d
namespace costmap_2d
{
// Cấu trúc tiện dụng để lưu trữ cặp tọa độ (x, y) trên bản đồ (dạng ô)
// convenient for storing x/y point pairs
struct MapLocation
{
unsigned int x; // Chỉ số hàng (hoặc cột) theo trục X trong costmap
unsigned int y; // Chỉ số hàng (hoặc cột) theo trục Y trong costmap
unsigned int x;
unsigned int y;
};
/**
* @class Costmap2D
* @brief Lớp này đại diện cho bản đồ chi phí 2D (costmap)
*
* Nó ánh xạ giữa không gian thế giới (tọa độ thực, mét) và lưới ô (cells),
* mỗi ô chứa "cost" – tức là chi phí để robot đi qua ô đó (ví dụ: 0 là free, 255 là obstacle).
* @brief A 2D costmap provides a mapping between points in the world and their associated "costs".
*/
class Costmap2D
{
friend class CostmapTester; // Cho phép lớp test (gtest) truy cập vào các thành viên private
friend class CostmapTester; // Need this for gtest to work correctly
public:
/**
* Constructor khởi tạo bản đồ costmap
* @param cells_size_x: số ô theo chiều X
* @param cells_size_y: số ô theo chiều Y
* @param resolution: kích thước 1 ô (mét / ô)
* @param origin_x: gốc bản đồ (tọa độ thế giới) theo trục X
* @param origin_y: gốc bản đồ (tọa độ thế giới) theo trục Y
* @param default_value: giá trị mặc định của mỗi ô (thường là 0 = free)
* @brief Constructor for a costmap
* @param cells_size_x The x size of the map in cells
* @param cells_size_y The y size of the map in cells
* @param resolution The resolution of the map in meters/cell
* @param origin_x The x origin of the map
* @param origin_y The y origin of the map
* @param default_value Default Value
*/
Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
double origin_x, double origin_y, unsigned char default_value = 0);
/**
* Constructor sao chép (copy constructor)
* Dùng để tạo bản sao nhanh của một bản đồ costmap khác.
* @brief Copy constructor for a costmap, creates a copy efficiently
* @param map The costmap to copy
*/
Costmap2D(const Costmap2D& map);
/**
* Toán tử gán (=) được overload để sao chép dữ liệu từ bản đồ khác.
* @brief Overloaded assignment operator
* @param map The costmap to copy
* @return A reference to the map after the copy has finished
*/
Costmap2D& operator=(const Costmap2D& map);
/**
* Tạo bản sao từ một vùng (window) của bản đồ khác.
* Dùng khi ta chỉ muốn sao chép một phần nhỏ của bản đồ lớn.
* @brief Turn this costmap into a copy of a window of a costmap passed in
* @param map The costmap to copy
* @param win_origin_x The x origin (lower left corner) for the window to copy, in meters
* @param win_origin_y The y origin (lower left corner) for the window to copy, in meters
* @param win_size_x The x size of the window, in meters
* @param win_size_y The y size of the window, in meters
*/
bool copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y, double win_size_x,
double win_size_y);
/// Constructor mặc định (không khởi tạo kích thước)
/**
* @brief Default constructor
*/
Costmap2D();
/// Destructor
/**
* @brief Destructor
*/
virtual ~Costmap2D();
/**
* Trả về giá trị chi phí (cost) của một ô (mx, my)
* @brief Get the cost of a cell in the costmap
* @param mx The x coordinate of the cell
* @param my The y coordinate of the cell
* @return The cost of the cell
*/
unsigned char getCost(unsigned int mx, unsigned int my) const;
/**
* Đặt giá trị cost cho một ô (mx, my)
* @brief Set the cost of a cell in the costmap
* @param mx The x coordinate of the cell
* @param my The y coordinate of the cell
* @param cost The cost to set the cell to
*/
void setCost(unsigned int mx, unsigned int my, unsigned char cost);
/**
* Chuyển đổi tọa độ map (ô) → tọa độ thế giới (mét)
* Dùng để vẽ, debug hoặc xác định vị trí thực tế của một ô.
* @brief Convert from map coordinates to world coordinates
* @param mx The x map coordinate
* @param my The y map coordinate
* @param wx Will be set to the associated world x coordinate
* @param wy Will be set to the associated world y coordinate
*/
void mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const;
/**
* Chuyển đổi tọa độ thế giới → tọa độ map (ô)
* Nếu tọa độ nằm ngoài bản đồ → trả về false
* @brief Convert from world coordinates to map coordinates
* @param wx The x world coordinate
* @param wy The y world coordinate
* @param mx Will be set to the associated map x coordinate
* @param my Will be set to the associated map y coordinate
* @return True if the conversion was successful (legal bounds) false otherwise
*/
bool worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const;
/**
* Chuyển đổi mà không kiểm tra biên hợp lệ (có thể nằm ngoài bản đồ)
* @brief Convert from world coordinates to map coordinates without checking for legal bounds
* @param wx The x world coordinate
* @param wy The y world coordinate
* @param mx Will be set to the associated map x coordinate
* @param my Will be set to the associated map y coordinate
* @note The returned map coordinates <b>are not guaranteed to lie within the map.</b>
*/
void worldToMapNoBounds(double wx, double wy, int& mx, int& my) const;
/**
* Chuyển đổi nhưng ép kết quả nằm trong biên của bản đồ (clamped)
* @brief Convert from world coordinates to map coordinates, constraining results to legal bounds.
* @param wx The x world coordinate
* @param wy The y world coordinate
* @param mx Will be set to the associated map x coordinate
* @param my Will be set to the associated map y coordinate
* @note The returned map coordinates are guaranteed to lie within the map.
*/
void worldToMapEnforceBounds(double wx, double wy, int& mx, int& my) const;
/**
* Tính chỉ số 1D của ô từ cặp tọa độ (mx, my)
* Công thức: index = my * size_x_ + mx
* @brief Given two map coordinates... compute the associated index
* @param mx The x coordinate
* @param my The y coordinate
* @return The associated index
*/
inline unsigned int getIndex(unsigned int mx, unsigned int my) const
{
@@ -105,7 +174,10 @@ public:
}
/**
* Chuyển ngược lại: từ chỉ số 1D → cặp tọa độ (mx, my)
* @brief Given an index... compute the associated map coordinates
* @param index The index
* @param mx Will be set to the x coordinate
* @param my Will be set to the y coordinate
*/
inline void indexToCells(unsigned int index, unsigned int& mx, unsigned int& my) const
{
@@ -114,103 +186,111 @@ public:
}
/**
* Trả về con trỏ đến mảng dữ liệu costmap (unsigned char array)
* @brief Will return a pointer to the underlying unsigned char array used as the costmap
* @return A pointer to the underlying unsigned char array storing cost values
*/
unsigned char* getCharMap() const;
/**
* Trả về số ô theo trục X
* @brief Accessor for the x size of the costmap in cells
* @return The x size of the costmap
*/
unsigned int getSizeInCellsX() const;
/**
* Trả về số ô theo trục Y
* @brief Accessor for the y size of the costmap in cells
* @return The y size of the costmap
*/
unsigned int getSizeInCellsY() const;
/**
* Trả về kích thước bản đồ theo mét (theo X)
* @brief Accessor for the x size of the costmap in meters
* @return The x size of the costmap (returns the centerpoint of the last legal cell in the map)
*/
double getSizeInMetersX() const;
/**
* Trả về kích thước bản đồ theo mét (theo Y)
* @brief Accessor for the y size of the costmap in meters
* @return The y size of the costmap (returns the centerpoint of the last legal cell in the map)
*/
double getSizeInMetersY() const;
/**
* Trả về tọa độ gốc bản đồ theo trục X (tọa độ thế giới)
* @brief Accessor for the x origin of the costmap
* @return The x origin of the costmap
*/
double getOriginX() const;
/**
* Trả về tọa độ gốc bản đồ theo trục Y (tọa độ thế giới)
* @brief Accessor for the y origin of the costmap
* @return The y origin of the costmap
*/
double getOriginY() const;
/**
* Trả về độ phân giải (mét / ô)
* @brief Accessor for the resolution of the costmap
* @return The resolution of the costmap
*/
double getResolution() const;
/// Đặt giá trị mặc định (cost khi reset map)
void setDefaultValue(unsigned char c)
{
default_value_ = c;
}
/// Trả về giá trị mặc định
unsigned char getDefaultValue()
{
return default_value_;
}
/**
* Đặt cost cho một vùng đa giác lồi (convex polygon)
* Dùng để đánh dấu vùng chướng ngại vật hoặc vùng forbidden.
* @brief Sets the cost of a convex polygon to a desired value
* @param polygon The polygon to perform the operation on
* @param cost_value The value to set costs to
* @return True if the polygon was filled... false if it could not be filled
*/
bool setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value);
/**
* Lấy danh sách các ô nằm trên viền (outline) của một polygon.
* @brief Get the map cells that make up the outline of a polygon
* @param polygon The polygon in map coordinates to rasterize
* @param polygon_cells Will be set to the cells contained in the outline of the polygon
*/
void polygonOutlineCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells);
/**
* Lấy danh sách các ô nằm bên trong polygon (điền vùng lồi).
* @brief Get the map cells that fill a convex polygon
* @param polygon The polygon in map coordinates to rasterize
* @param polygon_cells Will be set to the cells that fill the polygon
*/
void convexFillCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells);
/**
* Cập nhật lại gốc bản đồ (origin) sang vị trí mới,
* giữ lại dữ liệu cũ nếu có thể (thường dùng khi robot di chuyển xa).
* @brief Move the origin of the costmap to a new location.... keeping data when it can
* @param new_origin_x The x coordinate of the new origin
* @param new_origin_y The y coordinate of the new origin
*/
virtual void updateOrigin(double new_origin_x, double new_origin_y);
/**
* Lưu bản đồ ra file ảnh PGM (phục vụ debug hoặc visualization)
* @brief Save the costmap out to a pgm file
* @param file_name The name of the file to save
*/
bool saveMap(std::string file_name);
/**
* Thay đổi kích thước bản đồ (resize toàn bộ)
*/
void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x,
double origin_y);
/**
* Reset toàn bộ costmap trong vùng (x0, y0) → (xn, yn)
*/
void resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn);
/**
* Chuyển khoảng cách theo mét → số ô tương ứng
* @brief Given distance in the world... convert it to cells
* @param world_dist The world distance
* @return The equivalent cell distance
*/
unsigned int cellDistance(double world_dist);
/**
* Định nghĩa mutex để bảo vệ truy cập đồng thời.
*/
// Provide a typedef to ease future code maintenance
typedef boost::recursive_mutex mutex_t;
mutex_t* getMutex()
{
@@ -219,8 +299,17 @@ public:
protected:
/**
* Hàm template copy một vùng dữ liệu từ bản đồ nguồn → bản đồ đích
* Dữ liệu được copy bằng memcpy để đảm bảo nhanh.
* @brief Copy a region of a source map into a destination map
* @param source_map The source map
* @param sm_lower_left_x The lower left x point of the source map to start the copy
* @param sm_lower_left_y The lower left y point of the source map to start the copy
* @param sm_size_x The x size of the source map
* @param dest_map The destination map
* @param dm_lower_left_x The lower left x point of the destination map to start the copy
* @param dm_lower_left_y The lower left y point of the destination map to start the copy
* @param dm_size_x The x size of the destination map
* @param region_size_x The x size of the region to copy
* @param region_size_y The y size of the region to copy
*/
template<typename data_type>
void copyMapRegion(data_type* source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y,
@@ -228,60 +317,65 @@ protected:
unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x,
unsigned int region_size_y)
{
// Con trỏ bắt đầu tại vị trí góc dưới trái của vùng cần copy trong source
// we'll first need to compute the starting points for each map
data_type* sm_index = source_map + (sm_lower_left_y * sm_size_x + sm_lower_left_x);
// Con trỏ bắt đầu tại vị trí góc dưới trái trong dest
data_type* dm_index = dest_map + (dm_lower_left_y * dm_size_x + dm_lower_left_x);
// Lặp qua từng hàng trong vùng copy
// now, we'll copy the source map into the destination map
for (unsigned int i = 0; i < region_size_y; ++i)
{
// Copy nguyên hàng (memcpy nhanh hơn vòng for từng phần tử)
memcpy(dm_index, sm_index, region_size_x * sizeof(data_type));
sm_index += sm_size_x; // chuyển sang hàng tiếp theo của source
dm_index += dm_size_x; // chuyển sang hàng tiếp theo của dest
sm_index += sm_size_x;
dm_index += dm_size_x;
}
}
/**
* Xóa toàn bộ dữ liệu bản đồ (free bộ nhớ)
* @brief Deletes the costmap, static_map, and markers data structures
*/
virtual void deleteMaps();
/**
* Reset toàn bộ dữ liệu bản đồ về trạng thái UNKNOWN (NO_INFORMATION)
* @brief Resets the costmap and static_map to be unknown space
*/
virtual void resetMaps();
/**
* Cấp phát và khởi tạo bộ nhớ cho costmap
* @brief Initializes the costmap, static_map, and markers data structures
* @param size_x The x size to use for map initialization
* @param size_y The y size to use for map initialization
*/
virtual void initMaps(unsigned int size_x, unsigned int size_y);
/**
* Thuật toán raytracing Bresenham — đi từ điểm (x0,y0) → (x1,y1)
* Mỗi bước sẽ gọi “ActionType” (một functor) để đánh dấu hoặc xử lý từng ô.
* Thường dùng cho sensor update (quét laser).
* @brief Raytrace a line and apply some action at each step
* @param at The action to take... a functor
* @param x0 The starting x coordinate
* @param y0 The starting y coordinate
* @param x1 The ending x coordinate
* @param y1 The ending y coordinate
* @param max_length The maximum desired length of the segment... allows you to not go all the way to the endpoint
*/
template<class ActionType>
inline void raytraceLine(ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1,
unsigned int max_length = UINT_MAX)
{
int dx = x1 - x0; // Khoảng cách theo trục X
int dy = y1 - y0; // Khoảng cách theo trục Y
int dx = x1 - x0;
int dy = y1 - y0;
unsigned int abs_dx = abs(dx);
unsigned int abs_dy = abs(dy);
int offset_dx = sign(dx); // Hướng đi theo X
int offset_dy = sign(dy) * size_x_; // Hướng đi theo Y (theo stride của hàng)
int offset_dx = sign(dx);
int offset_dy = sign(dy) * size_x_;
unsigned int offset = y0 * size_x_ + x0; // Vị trí bắt đầu trong mảng costmap
unsigned int offset = y0 * size_x_ + x0;
double dist = hypot(dx, dy); // Chiều dài đoạn thẳng
// we need to chose how much to scale our dominant dimension, based on the maximum length of the line
double dist = hypot(dx, dy);
double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist);
// Xét hướng nào là trục chính (dominant)
// if x is dominant
if (abs_dx >= abs_dy)
{
int error_y = abs_dx / 2;
@@ -289,13 +383,14 @@ protected:
return;
}
// otherwise y is dominant
int error_x = abs_dy / 2;
bresenham2D(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy));
}
private:
/**
* Triển khai Bresenham 2D — đi qua từng ô trên đường thẳng và gọi “at(offset)” mỗi bước.
* @brief A 2D implementation of Bresenham's raytracing algorithm... applies an action at each step
*/
template<class ActionType>
inline void bresenham2D(ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a,
@@ -304,38 +399,33 @@ private:
unsigned int end = std::min(max_length, abs_da);
for (unsigned int i = 0; i < end; ++i)
{
at(offset); // Gọi hành động (thường là đánh dấu costmap)
offset += offset_a; // Tiến thêm 1 bước theo hướng chính
at(offset);
offset += offset_a;
error_b += abs_db;
if ((unsigned int)error_b >= abs_da)
{
offset += offset_b; // Bước sang hàng tiếp theo khi sai số tích lũy đủ lớn
offset += offset_b;
error_b -= abs_da;
}
}
at(offset); // Xử lý ô cuối cùng
at(offset);
}
/// Trả về dấu của số nguyên (x>0:1, x<0:-1)
inline int sign(int x)
{
return x > 0 ? 1.0 : -1.0;
}
mutex_t* access_; // Con trỏ đến mutex (đồng bộ truy cập costmap giữa các thread)
mutex_t* access_;
protected:
unsigned int size_x_; // Số lượng ô theo trục X
unsigned int size_y_; // Số lượng ô theo trục Y
double resolution_; // Độ phân giải: mét/ô
double origin_x_; // Tọa độ gốc bản đồ (theo thế giới) trục X
double origin_y_; // Tọa độ gốc bản đồ (theo thế giới) trục Y
unsigned char* costmap_; // Mảng 1 chiều chứa giá trị cost của toàn bộ bản đồ
unsigned char default_value_; // Giá trị mặc định cho các ô (thường = 0 hoặc 255 nếu unknown)
unsigned int size_x_;
unsigned int size_y_;
double resolution_;
double origin_x_;
double origin_y_;
unsigned char* costmap_;
unsigned char default_value_;
/**
* Functor nội bộ: dùng trong raytraceLine để đánh dấu một ô.
*/
class MarkCell
{
public:
@@ -345,16 +435,13 @@ protected:
}
inline void operator()(unsigned int offset)
{
costmap_[offset] = value_; // Gán cost của ô bằng giá trị value_
costmap_[offset] = value_;
}
private:
unsigned char* costmap_;
unsigned char value_;
};
/**
* Functor nội bộ: dùng để thu thập các ô nằm trên viền polygon.
*/
class PolygonOutlineCells
{
public:
@@ -363,11 +450,12 @@ protected:
{
}
// just push the relevant cells back onto the list
inline void operator()(unsigned int offset)
{
MapLocation loc;
costmap_.indexToCells(offset, loc.x, loc.y); // Chuyển index → (x, y)
cells_.push_back(loc); // Thêm ô này vào danh sách viền polygon
costmap_.indexToCells(offset, loc.x, loc.y);
cells_.push_back(loc);
}
private:
@@ -376,7 +464,6 @@ protected:
std::vector<MapLocation>& cells_;
};
};
} // namespace costmap_2d
#endif // COSTMAP_2D_COSTMAP_2D_H_
#endif // COSTMAP_2D_COSTMAP_2D_H

2
include/costmap_2d/costmap_2d_robot.h Normal file → Executable file
View File

@@ -253,7 +253,7 @@ private:
void readFootprintFromConfig(const std::vector<geometry_msgs::Point> &new_footprint,
const std::vector<geometry_msgs::Point> &old_footprint,
const double &robot_radius);
std::vector<std::function<PluginLayerPtr()>> creators_;
void checkMovement();
void mapUpdateLoop(double frequency);
bool map_update_thread_shutdown_;

2
include/costmap_2d/costmap_layer.h Normal file → Executable file
View File

@@ -37,9 +37,9 @@
*********************************************************************/
#ifndef COSTMAP_2D_COSTMAP_LAYER_H_
#define COSTMAP_2D_COSTMAP_LAYER_H_
#include <costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h>
#include <typeinfo>
namespace costmap_2d
{

0
include/costmap_2d/costmap_math.h Normal file → Executable file
View File

52
include/costmap_2d/footprint.h Normal file → Executable file
View File

@@ -37,17 +37,14 @@
*********************************************************************/
#ifndef COSTMAP_2D_FOOTPRINT_H
#define COSTMAP_2D_FOOTPRINT_H
#include <geometry_msgs/Point32.h>
#include <geometry_msgs/Polygon.h>
#include <geometry_msgs/PolygonStamped.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Point32.h>
#include <xmlrpcpp/XmlRpcValue.h>
#include<string.h>
#include<vector>
using namespace std;
namespace costmap_2d
{
@@ -62,22 +59,22 @@ void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footpr
double& min_dist, double& max_dist);
/**
* @brief Convert geometry_msgs::Point32 to geometry_msgs::Point
* @brief Convert Point32 to Point
*/
geometry_msgs::Point toPoint(geometry_msgs::Point32 pt);
/**
* @brief Convert geometry_msgs::Point to geometry_msgs::Point32
* @brief Convert Point to Point32
*/
geometry_msgs::Point32 toPoint32(geometry_msgs::Point pt);
/**
* @brief Convert vector of Points to geometry_msgs::Polygon msg
* @brief Convert vector of Points to Polygon msg
*/
geometry_msgs::Polygon toPolygon(std::vector<geometry_msgs::Point> pts);
/**
* @brief Convert geometry_msgs::Polygon msg to vector of Points.
* @brief Convert Polygon msg to vector of Points.
*/
std::vector<geometry_msgs::Point> toPointVector(geometry_msgs::Polygon polygon);
@@ -93,7 +90,7 @@ void transformFootprint(double x, double y, double theta, const std::vector<geom
std::vector<geometry_msgs::Point>& oriented_footprint);
/**
* @brief Given a pose and base footprint, build the oriented footprint of the robot (geometry_msgs::PolygonStamped)
* @brief Given a pose and base footprint, build the oriented footprint of the robot (PolygonStamped)
* @param x The x position of the robot
* @param y The y position of the robot
* @param theta The orientation of the robot
@@ -121,26 +118,11 @@ std::vector<geometry_msgs::Point> makeFootprintFromRadius(double radius);
*/
bool makeFootprintFromString(const std::string& footprint_string, std::vector<geometry_msgs::Point>& footprint);
//////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////
//////////////////CẦN THƯ VIỆN XmlRpcValue////////////////////
//////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////
//#include <xmlrpcpp/XmlRpcValue.h>
// https://github.com/ros/ros_comm/tree/d54e9be3421af71b70fc6b60a3bf916e779b43dc/utilities/xmlrpcpp/src
//////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////
/**
* @brief Read the ros-params "footprint" and/or "robot_radius" from
* the given NodeHandle using searchParam() to go up the tree.
*/
std::vector<geometry_msgs::Point> makeFootprintFromParams(const std::string& file_name);
// /**
// * @brief Read the ros-params "footprint" and/or "robot_radius" from
// * the given NodeHandle using searchParam() to go up the tree.
// */
// std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh);
/**
* @brief Create the footprint from the given XmlRpcValue.
@@ -156,10 +138,10 @@ std::vector<geometry_msgs::Point> makeFootprintFromParams(const std::string& fil
std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc,
const std::string& full_param_name);
/** @brief Write the current unpadded_footprint_ to the "footprint"
* parameter of the given NodeHandle so that dynamic_reconfigure
* will see the new value. */
void writeFootprintToParam(std::string& nh, const std::vector<geometry_msgs::Point>& footprint);
// /** @brief Write the current unpadded_footprint_ to the "footprint"
// * parameter of the given NodeHandle so that dynamic_reconfigure
// * will see the new value. */
// void writeFootprintToParam(ros::NodeHandle& nh, const std::vector<geometry_msgs::Point>& footprint);
} // end namespace costmap_2d

52
include/costmap_2d/inflation_layer.h Normal file → Executable file
View File

@@ -1,13 +1,46 @@
/*********************************************************************
*
* 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_INFLATION_LAYER_H_
#define COSTMAP_2D_INFLATION_LAYER_H_
#include <costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h>
#include <boost/thread.hpp>
#include <stdexcept>
#include <iostream>
namespace costmap_2d
{
@@ -43,16 +76,9 @@ 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();
@@ -106,8 +132,8 @@ protected:
private:
void handleImpl(const void* data,
const std::type_info& info,
const std::string& source) override;
const std::type_info& info,
const std::string& source) override;
/**
* @brief Lookup pre-computed distances
* @param mx The x coordinate of the current cell

8
include/costmap_2d/layer.h Normal file → Executable file
View File

@@ -39,10 +39,10 @@
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/utils.h>
#include <string>
#include <tf3/buffer_core.h>
#include <costmap_2d/layered_costmap.h>
#include <boost/shared_ptr.hpp>
namespace costmap_2d
{
@@ -135,8 +135,8 @@ public:
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) {
@@ -163,6 +163,8 @@ private:
std::vector<geometry_msgs::Point> footprint_spec_;
};
using PluginLayerPtr = std::shared_ptr<Layer>;
} // namespace costmap_2d
#endif // COSTMAP_2D_LAYER_H_

164
include/costmap_2d/layered_costmap.h Normal file → Executable file
View File

@@ -1,67 +1,84 @@
/*********************************************************************
*
* 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_LAYERED_COSTMAP_H_
#define COSTMAP_2D_LAYERED_COSTMAP_H_
#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 <costmap_2d/cost_values.h>
#include <costmap_2d/layer.h>
#include <costmap_2d/costmap_2d.h>
#include <vector>
#include <string>
#include <boost/dll/import.hpp>
namespace costmap_2d
{
class Layer;
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.
* Mỗi layer có thể chứa thông tin khác nhau: chướng ngại vật, vùng lạm phát, vùng cấm, v.v.
* @brief Instantiates different layer plugins and aggregates them into one score
*/
class LayeredCostmap
{
public:
/**
* @brief Hàm khởi tạo LayeredCostmap.
* @param global_frame: Tên frame toàn cục (ví dụ: "map" hoặc "odom").
* @param rolling_window: Nếu true, bản đồ sẽ "trượt" theo robot (rolling window).
* @param track_unknown: Nếu true, sẽ lưu trữ vùng chưa biết (NO_INFORMATION).
* @brief Constructor for a costmap
*/
LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown);
/**
* @brief Hàm huỷ lớp LayeredCostmap.
* @brief Destructor
*/
~LayeredCostmap();
/**
* @brief Cập nhật bản đồ tổng hợp từ tất cả các layer.
* @param robot_x, robot_y, robot_yaw: vị trí và hướng hiện tại của robot.
* @note Hàm này gọi updateBounds() và updateCosts() trên từng layer.
* @brief Update the underlying costmap with new data.
* If you want to update the map outside of the update loop that runs, you can call this.
*/
void updateMap(double robot_x, double robot_y, double robot_yaw);
/// @brief Trả về frame toàn cục (ví dụ "map").
inline const std::string& getGlobalFrameID() const noexcept
{
return global_frame_;
}
/**
* @brief Thay đổi kích thước bản đồ (số ô, độ phân giải, gốc).
* @param size_x, size_y: kích thước theo ô (cells).
* @param resolution: độ phân giải (mỗi ô = bao nhiêu mét).
* @param origin_x, origin_y: toạ độ gốc của bản đồ.
* @param size_locked: nếu true, không cho phép thay đổi kích thước sau này.
*/
void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y,
bool size_locked = false);
/**
* @brief Lấy vùng bản đồ bị thay đổi gần nhất sau lần cập nhật gần nhất.
* @param [out] minx, miny, maxx, maxy: toạ độ của vùng được cập nhật.
*/
void getUpdatedBounds(double& minx, double& miny, double& maxx, double& maxy)
{
minx = minx_;
@@ -70,66 +87,38 @@ public:
maxy = maxy_;
}
/**
* @brief Kiểm tra xem tất cả các layer hiện tại có dữ liệu mới nhất (current) hay không.
* @return true nếu tất cả layer đều cập nhật.
*/
bool isCurrent();
/**
* @brief Trả về con trỏ đến bản đồ Costmap2D cơ sở (master map).
*/
Costmap2D* getCostmap()
{
return &costmap_;
}
/**
* @brief Kiểm tra xem bản đồ có phải kiểu "rolling window" không.
*/
bool isRolling()
{
return rolling_window_;
}
/**
* @brief Kiểm tra xem bản đồ có đang lưu trữ vùng "chưa biết" không (NO_INFORMATION).
*/
bool isTrackingUnknown()
{
return costmap_.getDefaultValue() == costmap_2d::NO_INFORMATION;
}
/**
* @brief Trả về danh sách các plugin (layer) đã được nạp.
* @return vector các shared_ptr<Layer>.
*/
std::vector<costmap_2d::PluginLayerPtr>* getPlugins()
std::vector<std::shared_ptr<Layer> >* getPlugins()
{
return &plugins_;
}
/**
* @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(costmap_2d::PluginLayerPtr plugin)
void addPlugin(std::shared_ptr<Layer> plugin)
{
plugins_.push_back(plugin);
}
/**
* @brief Kiểm tra xem bản đồ có bị khoá kích thước không.
*/
bool isSizeLocked()
{
return size_locked_;
}
/**
* @brief Lấy giới hạn vùng bản đồ được cập nhật trong lần gọi update gần nhất.
* @param [out] x0, xn, y0, yn: chỉ số ô (cells) bắt đầu và kết thúc.
*/
void getBounds(unsigned int* x0, unsigned int* xn, unsigned int* y0, unsigned int* yn)
{
*x0 = bx0_;
@@ -138,56 +127,49 @@ public:
*yn = byn_;
}
/**
* @brief Kiểm tra xem lớp LayeredCostmap đã được khởi tạo hoàn chỉnh chưa.
*/
bool isInitialized()
{
return initialized_;
}
/**
* @brief Cập nhật footprint (hình dạng chiếm chỗ của robot).
* Đồng thời tính lại bán kính bao quanh và nội tiếp.
* Gọi hàm onFootprintChanged() của tất cả layer.
* @param footprint_spec: vector các điểm (geometry_msgs::geometry_msgs::Point) mô tả đa giác footprint.
*/
/** @brief Updates the stored footprint, updates the circumscribed
* and inscribed radii, and calls onFootprintChanged() in all
* layers. */
void setFootprint(const std::vector<geometry_msgs::Point>& footprint_spec);
/**
* @brief Trả về footprint hiện tại của robot.
*/
/** @brief Returns the latest footprint stored with setFootprint(). */
const std::vector<geometry_msgs::Point>& getFootprint() { return footprint_; }
/**
* @brief Bán kính đường tròn bao ngoài (circumscribed radius):
* bán kính nhỏ nhất của đường tròn chứa toàn bộ robot.
*/
/** @brief The radius of a circle centered at the origin of the
* robot which just surrounds all points on the robot's
* footprint.
*
* This is updated by setFootprint(). */
double getCircumscribedRadius() { return circumscribed_radius_; }
/**
* @brief Bán kính đường tròn nội tiếp (inscribed radius):
* bán kính lớn nhất của đường tròn nằm hoàn toàn bên trong footprint robot.
*/
/** @brief The radius of a circle centered at the origin of the
* robot which is just within all points and edges of the robot's
* footprint.
*
* This is updated by setFootprint(). */
double getInscribedRadius() { return inscribed_radius_; }
private:
Costmap2D costmap_; ///< Bản đồ 2D chính (lưu ma trận chi phí tổng hợp)
std::string global_frame_; ///< Frame toàn cục (thường là "map" hoặc "odom")
Costmap2D costmap_;
std::string global_frame_;
bool rolling_window_; ///< Nếu true, bản đồ sẽ di chuyển theo robot (bản đồ trượt)
bool rolling_window_; /// < @brief Whether or not the costmap should roll with the robot
bool current_; ///< Cờ đánh dấu tất cả các layer có dữ liệu mới nhất hay không
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)
bool current_;
double minx_, miny_, maxx_, maxy_;
unsigned int bx0_, bxn_, by0_, byn_;
std::vector<costmap_2d::PluginLayerPtr> plugins_; ///< Danh sách các layer plugin (ví dụ: StaticLayer, ObstacleLayer, InflationLayer)
std::vector<std::shared_ptr<Layer> > plugins_;
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
double circumscribed_radius_, inscribed_radius_; ///< Hai bán kính của footprint robot (bao ngoài và nội tiếp)
std::vector<geometry_msgs::Point> footprint_; ///< Đa giác mô tả footprint robot
bool initialized_;
bool size_locked_;
double circumscribed_radius_, inscribed_radius_;
std::vector<geometry_msgs::Point> footprint_;
};
} // namespace costmap_2d

2
include/costmap_2d/observation.h Normal file → Executable file
View File

@@ -32,8 +32,8 @@
#ifndef COSTMAP_2D_OBSERVATION_H_
#define COSTMAP_2D_OBSERVATION_H_
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/Point.h>
#include <sensor_msgs/PointCloud2.h>
namespace costmap_2d
{

49
include/costmap_2d/observation_buffer.h Normal file → Executable file
View File

@@ -1,19 +1,50 @@
/*********************************************************************
*
* 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
*********************************************************************/
#ifndef COSTMAP_2D_OBSERVATION_BUFFER_H_
#define COSTMAP_2D_OBSERVATION_BUFFER_H_
#include <vector>
#include <list>
#include <string>
#include <chrono>
// #include <ros/time.h>
#include <robot/time.h>
#include <robot/duration.h>
#include <costmap_2d/observation.h>
#include <tf3/buffer_core.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/Point.h>
#include <sensor_msgs/PointCloud2.h>
// Thread support
#include <boost/thread.hpp>
@@ -27,7 +58,6 @@ namespace costmap_2d
class ObservationBuffer
{
public:
/**
* @brief Constructs an observation buffer
* @param topic_name The topic of the observations, used as an identifier for error and warning messages
@@ -37,7 +67,7 @@ public:
* @param max_obstacle_height The minimum height of a hitpoint to be considered legal
* @param obstacle_range The range to which the sensor should be trusted for inserting obstacles
* @param raytrace_range The range to which the sensor should be trusted for raytracing to clear out space
* @param tf3_buffer A reference to a tf3 Buffer
* @param tf2_buffer A reference to a tf2 Buffer
* @param global_frame The frame to transform PointClouds into
* @param sensor_frame The frame of the origin of the sensor, can be left blank to be read from the messages
* @param tf_tolerance The amount of time to wait for a transform to be available when setting a new global frame
@@ -108,9 +138,6 @@ private:
void purgeStaleObservations();
tf3::BufferCore& tf3_buffer_;
// const ros::Duration observation_keep_time_;
// const ros::Duration expected_update_rate_;
// ros::Time last_updated_;
const robot::Duration observation_keep_time_;
const robot::Duration expected_update_rate_;
robot::Time last_updated_;

42
include/costmap_2d/obstacle_layer.h Normal file → Executable file
View File

@@ -42,19 +42,15 @@
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/observation_buffer.h>
#include <costmap_2d/footprint.h>
#include <costmap_2d/utils.h>
#include <nav_msgs/OccupancyGrid.h>
#include <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 <laser_geometry/laser_geometry.hpp>
// #include <tf3_ros/message_filter.h>
// #include <message_filters/subscriber.h>
namespace costmap_2d
{
@@ -81,19 +77,41 @@ public:
void clearStaticObservations(bool marking, bool clearing);
protected:
void handleImpl(const void* data,
const std::type_info&,
const std::string& topic) override;
void laserScanCallback(const sensor_msgs::LaserScan& message,
const boost::shared_ptr<ObservationBuffer>& buffer);
void laserScanValidInfCallback(const sensor_msgs::LaserScan& message,
/**
* @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::LaserScan& 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::LaserScan& 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::PointCloud& message,
const boost::shared_ptr<ObservationBuffer>& buffer);
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::PointCloud2& message,
const boost::shared_ptr<ObservationBuffer>& buffer);
// void process(const map_msgs::OccupancyGridUpdate& update);
const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
/**
* @brief Get the observations used to mark space

54
include/costmap_2d/static_layer.h Normal file → Executable file
View File

@@ -1,12 +1,49 @@
/*********************************************************************
*
* 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_STATIC_LAYER_H_
#define COSTMAP_2D_STATIC_LAYER_H_
#include <costmap_2d/costmap_layer.h>
#include <costmap_2d/layered_costmap.h>
#include <nav_msgs/OccupancyGrid.h>
#include <map_msgs/OccupancyGridUpdate.h>
#include <string>
#include <geometry_msgs/TransformStamped.h>
namespace costmap_2d
{
@@ -30,10 +67,9 @@ protected:
void handleImpl(const void* data,
const std::type_info& type,
const std::string& topic) override;
void incomingMap(const nav_msgs::OccupancyGrid& new_map);
void incomingUpdate(const map_msgs::OccupancyGridUpdate& update);
virtual unsigned char interpretValue(unsigned char value);
virtual void incomingMap(const nav_msgs::OccupancyGrid& new_map);
virtual void incomingUpdate(const map_msgs::OccupancyGridUpdate& update);
unsigned char* threshold_;
std::string base_frame_id_;
std::string global_frame_; ///< @brief The global frame for the costmap
@@ -49,19 +85,11 @@ protected:
bool trinary_costmap_;
bool map_shutdown_ = false;
bool map_update_shutdown_ = false;
// ros::Subscriber map_sub_, map_update_sub_;
unsigned char lethal_threshold_, unknown_cost_value_;
private:
/**
* @brief Callback to update the costmap's map from the map_server
* @param new_map The map to put into the costmap. The origin of the new
* 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_;
bool getParams();
};
} // namespace costmap_2d

2
include/costmap_2d/utils.h
View File

@@ -15,7 +15,7 @@ namespace costmap_2d
return default_value;
}
std::string getSourceFile(const std::string& root, const std::string& config_file_name)
inline std::string getSourceFile(const std::string& root, const std::string& config_file_name)
{
std::string path_source = " ";
std::string sub_folder = "config";

18
include/costmap_2d/voxel_layer.h Normal file → Executable file
View File

@@ -42,28 +42,23 @@
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/observation_buffer.h>
#include <costmap_2d/voxel_grid.h>
#include <costmap_2d/obstacle_layer.h>
#include <costmap_2d/utils.h>
#include <nav_msgs/OccupancyGrid.h>
#include <sensor_msgs/LaserScan.h>
// #include <laser_geometry/laser_geometry.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 <message_filters/subscriber.h>
// #include <dynamic_reconfigure/server.h>
// #include <costmap_2d/VoxelPluginConfig.h>
#include <costmap_2d/obstacle_layer.h>
#include <voxel_grid/voxel_grid.h>
namespace costmap_2d
{
class VoxelLayer : public ObstacleLayer
{
public:
VoxelLayer() : voxel_grid_(0, 0, 0)
VoxelLayer() :
voxel_grid_(0, 0, 0)
{
costmap_ = NULL; // this is the unsigned char* member of parent class's parent class Costmap2D.
}
@@ -79,19 +74,21 @@ public:
{
return true;
}
virtual void matchSize();
virtual void reset();
protected:
virtual void resetMaps();
private:
bool getParams();
void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
double* max_x, double* max_y);
bool publish_voxel_;
voxel_grid::VoxelGrid voxel_grid_;
double z_resolution_, origin_z_;
@@ -138,7 +135,6 @@ private:
{
return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0));
}
bool getParams();
};
} // namespace costmap_2d