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

1726_24102025

Browse Source
This commit is contained in:
duongtd
2025-10-24 17:26:44 +07:00
parent 6d86ad65a2
commit 63dc18f3f0
77 changed files with 8301 additions and 248 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
include/costmap_2d/array_parser.h Normal file
View File

@@ -0,0 +1,51 @@
/*
* Copyright (c) 2012, 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 the 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: Dave Hershberger
*/
#ifndef COSTMAP_2D_ARRAY_PARSER_H
#define COSTMAP_2D_ARRAY_PARSER_H
#include <vector>
#include <string>
namespace costmap_2d
{
/** @brief Parse a vector of vectors of floats from a string.
* @param error_return If no error, error_return is set to "". If
* error, error_return will describe the error.
* Syntax is [[1.0, 2.0], [3.3, 4.4, 5.5], ...]
*
* On error, error_return is set and the return value could be
* anything, like part of a successful parse. */
std::vector<std::vector<float> > parseVVF(const std::string& input, std::string& error_return);
} // end namespace costmap_2d
#endif // COSTMAP_2D_ARRAY_PARSER_H

289
include/costmap_2d/costmap_2d.h
View File

@@ -1,137 +1,103 @@
#ifndef COSTMAP_2D_COSTMAP_2D_H_
#ifndef COSTMAP_2D_COSTMAP_2D_H_ // Bảo vệ tránh include file nhiều lần
#define COSTMAP_2D_COSTMAP_2D_H_
#include <vector>
#include <queue>
#include <boost/thread.hpp>
#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 <costmap_2d/msg.h>
namespace costmap_2d
namespace costmap_2d // Mọi thứ nằm trong namespace costmap_2d
{
// convenient for storing x/y point pairs
struct MapLocation
{
unsigned int x;
unsigned int y;
};
struct Point
{
double x;
double y;
};
/**
// Cấu trúc tiện dụng để lưu trữ cặp tọa độ (x, y) trên bản đồ (dạng ô)
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
};
/**
* @class Costmap2D
* @brief A 2D costmap provides a mapping between points in the world and their associated "costs".
* @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).
*/
class Costmap2D
{
// friend class CostmapTester; // Cho phép lớp test (gtest) truy cập vào các thành viên private
public:
/**
* @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
* 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)
*/
Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
double origin_x, double origin_y, unsigned char default_value = 0);
/**
* @brief Copy constructor for a costmap, creates a copy efficiently
* @param map The costmap to copy
* Constructor sao chép (copy constructor)
* Dùng để tạo bản sao nhanh của một bản đồ costmap khác.
*/
Costmap2D(const Costmap2D& map);
/**
* @brief Overloaded assignment operator
* @param map The costmap to copy
* @return A reference to the map after the copy has finished
* Toán tử gán (=) được overload để sao chép dữ liệu từ bản đồ khác.
*/
Costmap2D& operator=(const Costmap2D& map);
/**
* @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
* 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.
*/
bool copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y, double win_size_x,
double win_size_y);
/**
* @brief Default constructor
*/
/// Constructor mặc định (không khởi tạo kích thước)
Costmap2D();
/**
* @brief Destructor
*/
/// Destructor
virtual ~Costmap2D();
/**
* @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
* Trả về giá trị chi phí (cost) của một ô (mx, my)
*/
unsigned char getCost(unsigned int mx, unsigned int my) const;
/**
* @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
* Đặt giá trị cost cho một ô (mx, my)
*/
void setCost(unsigned int mx, unsigned int my, unsigned char cost);
/**
* @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
* 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 ô.
*/
void mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const;
/**
* @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
* Chuyển đổi tọa độ thế giới → tọa độ map (ô)
* Nếu tọa độ nằm ngoài bản đồ → trả về false
*/
bool worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const;
/**
* @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>
* Chuyển đổi mà không kiểm tra biên hợp lệ (có thể nằm ngoài bản đồ)
*/
void worldToMapNoBounds(double wx, double wy, int& mx, int& my) const;
/**
* @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.
* Chuyển đổi nhưng ép kết quả nằm trong biên của bản đồ (clamped)
*/
void worldToMapEnforceBounds(double wx, double wy, int& mx, int& my) const;
/**
* @brief Given two map coordinates... compute the associated index
* @param mx The x coordinate
* @param my The y coordinate
* @return The associated index
* Tính chỉ số 1D của ô từ cặp tọa độ (mx, my)
* Công thức: index = my * size_x_ + mx
*/
inline unsigned int getIndex(unsigned int mx, unsigned int my) const
{
@@ -139,10 +105,7 @@ public:
}
/**
* @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
* Chuyển ngược lại: từ chỉ số 1D → cặp tọa độ (mx, my)
*/
inline void indexToCells(unsigned int index, unsigned int& mx, unsigned int& my) const
{
@@ -151,111 +114,103 @@ public:
}
/**
* @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
* Trả về con trỏ đến mảng dữ liệu costmap (unsigned char array)
*/
unsigned char* getCharMap() const;
/**
* @brief Accessor for the x size of the costmap in cells
* @return The x size of the costmap
* Trả về số ô theo trục X
*/
unsigned int getSizeInCellsX() const;
/**
* @brief Accessor for the y size of the costmap in cells
* @return The y size of the costmap
* Trả về số ô theo trục Y
*/
unsigned int getSizeInCellsY() const;
/**
* @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)
* Trả về kích thước bản đồ theo mét (theo X)
*/
double getSizeInMetersX() const;
/**
* @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)
* Trả về kích thước bản đồ theo mét (theo Y)
*/
double getSizeInMetersY() const;
/**
* @brief Accessor for the x origin of the costmap
* @return The x origin of the costmap
* Trả về tọa độ gốc bản đồ theo trục X (tọa độ thế giới)
*/
double getOriginX() const;
/**
* @brief Accessor for the y origin of the costmap
* @return The y origin of the costmap
* Trả về tọa độ gốc bản đồ theo trục Y (tọa độ thế giới)
*/
double getOriginY() const;
/**
* @brief Accessor for the resolution of the costmap
* @return The resolution of the costmap
* Trả về độ phân giải (mét / ô)
*/
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_;
}
/**
* @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
* Đặ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.
*/
bool setConvexPolygonCost(const std::vector<Point>& polygon, unsigned char cost_value);
/**
* @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
* Lấy danh sách các ô nằm trên viền (outline) của một polygon.
*/
void polygonOutlineCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells);
/**
* @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
* Lấy danh sách các ô nằm bên trong polygon (điền vùng lồi).
*/
void convexFillCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells);
/**
* @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
* 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).
*/
virtual void updateOrigin(double new_origin_x, double new_origin_y);
/**
* @brief Save the costmap out to a pgm file
* @param file_name The name of the file to save
* Lưu bản đồ ra file ảnh PGM (phục vụ debug hoặc visualization)
*/
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);
/**
* @brief Given distance in the world... convert it to cells
* @param world_dist The world distance
* @return The equivalent cell distance
* Chuyển khoảng cách theo mét → số ô tương ứng
*/
unsigned int cellDistance(double world_dist);
// Provide a typedef to ease future code maintenance
/**
* Định nghĩa mutex để bảo vệ truy cập đồng thời.
*/
typedef boost::recursive_mutex mutex_t;
mutex_t* getMutex()
{
@@ -264,17 +219,8 @@ public:
protected:
/**
* @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
* 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.
*/
template<typename data_type>
void copyMapRegion(data_type* source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y,
@@ -282,65 +228,60 @@ protected:
unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x,
unsigned int region_size_y)
{
// we'll first need to compute the starting points for each map
// Con trỏ bắt đầu tại vị trí góc dưới trái của vùng cần copy trong source
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);
// now, we'll copy the source map into the destination map
// Lặp qua từng hàng trong vùng copy
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;
dm_index += dm_size_x;
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
}
}
/**
* @brief Deletes the costmap, static_map, and markers data structures
* Xóa toàn bộ dữ liệu bản đồ (free bộ nhớ)
*/
virtual void deleteMaps();
/**
* @brief Resets the costmap and static_map to be unknown space
* Reset toàn bộ dữ liệu bản đồ về trạng thái UNKNOWN (NO_INFORMATION)
*/
virtual void resetMaps();
/**
* @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
* Cấp phát và khởi tạo bộ nhớ cho costmap
*/
virtual void initMaps(unsigned int size_x, unsigned int size_y);
/**
* @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
* 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).
*/
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;
int dy = y1 - y0;
int dx = x1 - x0; // Khoảng cách theo trục X
int dy = y1 - y0; // Khoảng cách theo trục Y
unsigned int abs_dx = abs(dx);
unsigned int abs_dy = abs(dy);
int offset_dx = sign(dx);
int offset_dy = sign(dy) * size_x_;
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)
unsigned int offset = y0 * size_x_ + x0;
unsigned int offset = y0 * size_x_ + x0; // Vị trí bắt đầu trong mảng costmap
// 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 dist = hypot(dx, dy); // Chiều dài đoạn thẳng
double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist);
// if x is dominant
// Xét hướng nào là trục chính (dominant)
if (abs_dx >= abs_dy)
{
int error_y = abs_dx / 2;
@@ -348,14 +289,13 @@ 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:
/**
* @brief A 2D implementation of Bresenham's raytracing algorithm... applies an action at each step
* Triển khai Bresenham 2D — đi qua từng ô trên đường thẳng và gọi “at(offset)” mỗi bước.
*/
template<class ActionType>
inline void bresenham2D(ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a,
@@ -364,33 +304,38 @@ private:
unsigned int end = std::min(max_length, abs_da);
for (unsigned int i = 0; i < end; ++i)
{
at(offset);
offset += offset_a;
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
error_b += abs_db;
if ((unsigned int)error_b >= abs_da)
{
offset += offset_b;
offset += offset_b; // Bước sang hàng tiếp theo khi sai số tích lũy đủ lớn
error_b -= abs_da;
}
}
at(offset);
at(offset); // Xử lý ô cuối cùng
}
/// 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_;
protected:
unsigned int size_x_;
unsigned int size_y_;
double resolution_;
double origin_x_;
double origin_y_;
unsigned char* costmap_;
unsigned char default_value_;
mutex_t* access_; // Con trỏ đến mutex (đồng bộ truy cập costmap giữa các thread)
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)
/**
* Functor nội bộ: dùng trong raytraceLine để đánh dấu một ô.
*/
class MarkCell
{
public:
@@ -400,13 +345,16 @@ protected:
}
inline void operator()(unsigned int offset)
{
costmap_[offset] = value_;
costmap_[offset] = value_; // Gán cost của ô bằng giá trị 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:
@@ -415,12 +363,11 @@ 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);
cells_.push_back(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
}
private:
@@ -432,4 +379,4 @@ protected:
} // namespace costmap_2d
#endif // COSTMAP_2D_COSTMAP_2D_H
#endif // COSTMAP_2D_COSTMAP_2D_H_

151
include/costmap_2d/costmap_layer.h Normal file
View File

@@ -0,0 +1,151 @@
/*********************************************************************
*
* 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_LAYER_H_
#define COSTMAP_2D_COSTMAP_LAYER_H_
#include <ros/ros.h>
#include <costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h>
namespace costmap_2d
{
class CostmapLayer : public Layer, public Costmap2D
{
public:
CostmapLayer() : has_extra_bounds_(false),
extra_min_x_(1e6), extra_max_x_(-1e6),
extra_min_y_(1e6), extra_max_y_(-1e6) {}
bool isDiscretized()
{
return true;
}
virtual void matchSize();
virtual void clearArea(int start_x, int start_y, int end_x, int end_y, bool invert_area=false);
/**
* If an external source changes values in the costmap,
* it should call this method with the area that it changed
* to ensure that the costmap includes this region as well.
* @param mx0 Minimum x value of the bounding box
* @param my0 Minimum y value of the bounding box
* @param mx1 Maximum x value of the bounding box
* @param my1 Maximum y value of the bounding box
*/
void addExtraBounds(double mx0, double my0, double mx1, double my1);
protected:
/*
* Updates the master_grid within the specified
* bounding box using this layer's values.
*
* TrueOverwrite means every value from this layer
* is written into the master grid.
*/
void updateWithTrueOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
/*
* Updates the master_grid within the specified
* bounding box using this layer's values.
*
* Overwrite means every valid value from this layer
* is written into the master grid (does not copy NO_INFORMATION)
*/
void updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
/*
* Updates the master_grid within the specified
* bounding box using this layer's values.
*
* Sets the new value to the maximum of the master_grid's value
* and this layer's value. If the master value is NO_INFORMATION,
* it is overwritten. If the layer's value is NO_INFORMATION,
* the master value does not change.
*/
void updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
/*
* Updates the master_grid within the specified
* bounding box using this layer's values.
*
* Sets the new value to the sum of the master grid's value
* and this layer's value. If the master value is NO_INFORMATION,
* it is overwritten with the layer's value. If the layer's value
* is NO_INFORMATION, then the master value does not change.
*
* If the sum value is larger than INSCRIBED_INFLATED_OBSTACLE,
* the master value is set to (INSCRIBED_INFLATED_OBSTACLE - 1).
*/
void updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
/**
* Updates the bounding box specified in the parameters to include
* the location (x,y)
*
* @param x x-coordinate to include
* @param y y-coordinate to include
* @param min_x bounding box
* @param min_y bounding box
* @param max_x bounding box
* @param max_y bounding box
*/
void touch(double x, double y, double* min_x, double* min_y, double* max_x, double* max_y);
/*
* Updates the bounding box specified in the parameters
* to include the bounding box from the addExtraBounds
* call. If addExtraBounds was not called, the method will do nothing.
*
* Should be called at the beginning of the updateBounds method
*
* @param min_x bounding box (input and output)
* @param min_y bounding box (input and output)
* @param max_x bounding box (input and output)
* @param max_y bounding box (input and output)
*/
void useExtraBounds(double* min_x, double* min_y, double* max_x, double* max_y);
bool has_extra_bounds_;
private:
double extra_min_x_, extra_max_x_, extra_min_y_, extra_max_y_;
};
} // namespace costmap_2d
#endif // COSTMAP_2D_COSTMAP_LAYER_H_

69
include/costmap_2d/costmap_math.h Normal file
View File

@@ -0,0 +1,69 @@
/*********************************************************************
*
* 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_MATH_H_
#define COSTMAP_2D_COSTMAP_MATH_H_
#include <math.h>
#include <algorithm>
#include <vector>
#include<costmap_2d/msg.h>
/** @brief Return -1 if x < 0, +1 otherwise. */
inline double sign(double x)
{
return x < 0.0 ? -1.0 : 1.0;
}
/** @brief Same as sign(x) but returns 0 if x is 0. */
inline double sign0(double x)
{
return x < 0.0 ? -1.0 : (x > 0.0 ? 1.0 : 0.0);
}
inline double distance(double x0, double y0, double x1, double y1)
{
return hypot(x1 - x0, y1 - y0);
}
double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1);
bool intersects(std::vector<costmap_2d::Point>& polygon, float testx, float testy);
bool intersects(std::vector<costmap_2d::Point>& polygon1, std::vector<costmap_2d::Point>& polygon2);
#endif // COSTMAP_2D_COSTMAP_MATH_H_

19
include/costmap_2d/critical_layer.h Normal file
View File

@@ -0,0 +1,19 @@
#ifndef COSTMAP_2D_CRITICAL_LAYER_H_
#define COSTMAP_2D_CRITICAL_LAYER_H_
#include <costmap_2d/static_layer.h>
namespace costmap_2d
{
class CriticalLayer : public StaticLayer
{
public:
CriticalLayer();
virtual ~CriticalLayer();
private:
unsigned char interpretValue(unsigned char value);
void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
};
}
#endif // COSTMAP_2D_CRITICAL_LAYER_H_

147
include/costmap_2d/footprint.h Normal file
View File

@@ -0,0 +1,147 @@
/*********************************************************************
*
* 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_FOOTPRINT_H
#define COSTMAP_2D_FOOTPRINT_H
#include<costmap_2d/msg.h>
#include<string.h>
#include<vector>
using namespace std;
namespace costmap_2d
{
/**
* @brief Calculate the extreme distances for the footprint
*
* @param footprint The footprint to examine
* @param min_dist Output parameter of the minimum distance
* @param max_dist Output parameter of the maximum distance
*/
void calculateMinAndMaxDistances(const std::vector<Point>& footprint,
double& min_dist, double& max_dist);
/**
* @brief Convert Point32 to Point
*/
Point toPoint(Point32 pt);
/**
* @brief Convert Point to Point32
*/
Point32 toPoint32(Point pt);
/**
* @brief Convert vector of Points to Polygon msg
*/
Polygon toPolygon(std::vector<Point> pts);
/**
* @brief Convert Polygon msg to vector of Points.
*/
std::vector<Point> toPointVector(Polygon polygon);
/**
* @brief Given a pose and base footprint, build the oriented footprint of the robot (list of Points)
* @param x The x position of the robot
* @param y The y position of the robot
* @param theta The orientation of the robot
* @param footprint_spec Basic shape of the footprint
* @param oriented_footprint Will be filled with the points in the oriented footprint of the robot
*/
void transformFootprint(double x, double y, double theta, const std::vector<Point>& footprint_spec,
std::vector<Point>& oriented_footprint);
/**
* @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
* @param footprint_spec Basic shape of the footprint
* @param oriented_footprint Will be filled with the points in the oriented footprint of the robot
*/
void transformFootprint(double x, double y, double theta, const std::vector<Point>& footprint_spec,
PolygonStamped & oriented_footprint);
/**
* @brief Adds the specified amount of padding to the footprint (in place)
*/
void padFootprint(std::vector<Point>& footprint, double padding);
/**
* @brief Create a circular footprint from a given radius
*/
std::vector<Point> makeFootprintFromRadius(double radius);
/**
* @brief Make the footprint from the given string.
*
* Format should be bracketed array of arrays of floats, like so: [[1.0, 2.2], [3.3, 4.2], ...]
*
*/
bool makeFootprintFromString(const std::string& footprint_string, std::vector<Point>& footprint);
/**
* @brief Read the ros-params "footprint" and/or "robot_radius" from
* the given NodeHandle using searchParam() to go up the tree.
*/
std::vector<Point> makeFootprintFromParams(std::string& nh);
/**
* @brief Create the footprint from the given XmlRpcValue.
*
* @param footprint_xmlrpc should be an array of arrays, where the
* top-level array should have 3 or more elements, and the
* sub-arrays should all have exactly 2 elements (x and y
* coordinates).
*
* @param full_param_name this is the full name of the rosparam from
* which the footprint_xmlrpc value came. It is used only for
* reporting errors. */
std::vector<Point> makeFootprintFromXMLRPC(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<Point>& footprint);
} // end namespace costmap_2d
#endif // COSTMAP_2D_FOOTPRINT_H

164
include/costmap_2d/inflation_layer.h Normal file
View File

@@ -0,0 +1,164 @@
#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>
// 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();
// virtual ~InflationLayer()
// {
// deleteKernels();
// if (dsrv_)
// delete dsrv_;
// if (seen_)
// delete[] seen_;
// }
// 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 void reset() { onInitialize(); }
// /** @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;
// }
// /**
// * @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);
// protected:
// virtual void onFootprintChanged();
// boost::recursive_mutex* inflation_access_;
// double resolution_;
// double inflation_radius_;
// double inscribed_radius_;
// double weight_;
// bool inflate_unknown_;
// 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];
// }
// /**
// * @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];
// }
// void computeCaches();
// void deleteKernels();
// void inflate_area(int min_i, int min_j, int max_i, int max_j, unsigned char* master_grid);
// unsigned int cellDistance(double world_dist)
// {
// return layered_costmap_->getCostmap()->cellDistance(world_dist);
// }
// 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_;
// 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.
// };
// } // namespace costmap_2d
#endif // COSTMAP_2D_INFLATION_LAYER_H_

151
include/costmap_2d/layer.h Normal file
View File

@@ -0,0 +1,151 @@
/*********************************************************************
*
* 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: David V. Lu!!
*********************************************************************/
#ifndef COSTMAP_2D_LAYER_H_
#define COSTMAP_2D_LAYER_H_
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/layered_costmap.h>
#include <string>
#include "tf2/buffer_core.h"
namespace costmap_2d
{
class LayeredCostmap;
class Layer
{
public:
Layer();
void initialize(LayeredCostmap* parent, std::string name, std::shared_ptr<tf2::BufferCore> *tf);
/**
* @brief This is called by the LayeredCostmap to poll this plugin as to how
* much of the costmap it needs to update. Each layer can increase
* the size of this bounds.
*
* For more details, see "Layered Costmaps for Context-Sensitive Navigation",
* by Lu et. Al, IROS 2014.
*/
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y) {}
/**
* @brief Actually update the underlying costmap, only within the bounds
* calculated during UpdateBounds().
*/
virtual void updateCosts(Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) {}
/** @brief Stop publishers. */
virtual void deactivate() {}
/** @brief Restart publishers if they've been stopped. */
virtual void activate() {}
virtual void reset() {}
virtual ~Layer() {}
/**
* @brief Check to make sure all the data in the layer is up to date.
* If the layer is not up to date, then it may be unsafe to
* plan using the data from this layer, and the planner may
* need to know.
*
* A layer's current state should be managed by the protected
* variable current_.
* @return Whether the data in the layer is up to date.
*/
bool isCurrent() const
{
return current_;
}
/**
* @brief getter if the current layer is enabled.
*
* The user may enable/disable a layer though dynamic reconfigure.
* Disabled layers won't receive calls to
* - Layer::updateCosts
* - Layer::updateBounds
* - Layer::isCurrent
* from the LayeredCostmap.
*
* Calls to Layer::activate, Layer::deactivate and Layer::reset won't be
* blocked.
*/
inline bool isEnabled() const noexcept
{
return enabled_;
}
/** @brief Implement this to make this layer match the size of the parent costmap. */
virtual void matchSize() {}
inline const std::string& getName() const noexcept
{
return name_;
}
/** @brief Convenience function for layered_costmap_->getFootprint(). */
const std::vector<Point>& getFootprint() const;
/** @brief LayeredCostmap calls this whenever the footprint there
* changes (via LayeredCostmap::setFootprint()). Override to be
* notified of changes to the robot's footprint. */
virtual void onFootprintChanged() {}
protected:
/** @brief This is called at the end of initialize(). Override to
* implement subclass-specific initialization.
*
* tf_, name_, and layered_costmap_ will all be set already when this is called. */
virtual void onInitialize() {}
LayeredCostmap* layered_costmap_;
bool current_;
bool enabled_;
std::string name_;
std::shared_ptr<tf2::BufferCore> *tf_;
private:
std::vector<Point> footprint_spec_;
};
} // namespace costmap_2d
#endif // COSTMAP_2D_LAYER_H_

193
include/costmap_2d/layered_costmap.h
View File

@@ -0,0 +1,193 @@
#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/costmap_2d.h> // Định nghĩa lớp Costmap2D (bản đồ 2D cơ bản)
#include <vector>
#include <string>
namespace costmap_2d
{
class Layer; // Khai báo trước để dùng trong vector plugin
/**
* @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.
*/
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).
*/
LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown);
/**
* @brief Hàm huỷ lớp LayeredCostmap.
*/
~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.
*/
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_;
miny = miny_;
maxx = maxx_;
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<boost::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(boost::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_;
*xn = bxn_;
*y0 = by0_;
*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::Point) mô tả đa giác footprint.
*/
void setFootprint(const std::vector<Point>& footprint_spec);
/**
* @brief Trả về footprint hiện tại của robot.
*/
const std::vector<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.
*/
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.
*/
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")
bool rolling_window_; ///< Nếu true, bản đồ sẽ di chuyển theo robot (bản đồ trượt)
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)
std::vector<boost::shared_ptr<Layer> > 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
double circumscribed_radius_, inscribed_radius_; ///< Hai bán kính của footprint robot (bao ngoài và nội tiếp)
std::vector<Point> footprint_; ///< Đa giác mô tả footprint robot
};
} // namespace costmap_2d
#endif // COSTMAP_2D_LAYERED_COSTMAP_H_

66
include/costmap_2d/msg.h Normal file
View File

@@ -0,0 +1,66 @@
#ifndef COSTMAP_2D_COST_VALUES_H_MSG
#define COSTMAP_2D_COST_VALUES_H_MSG
/** Provides a mapping for often used cost values */
#include<vector>
namespace costmap_2d
{
struct Point
{
double x;
double y;
double z;
};
struct Point32
{
float x;
float y;
float z;
};
struct Polygon
{
std::vector<Point32> points;
};
struct PolygonStamped
{
// std_msgs/Header header; // Ignored for simplicity
Polygon polygon;
};
struct XmlRpcValue
{
std::vector<Point32> points;
};
struct PointField
{
uint8_t INT8=1;
uint8_t UINT8=2;
uint8_t INT16=3;
uint8_t UINT16=4;
uint8_t INT32=5;
uint8_t UINT32=6;
uint8_t FLOAT32=7;
uint8_t FLOAT64=8;
std::string name;
uint32_t offset;
uint8_t datatype;
uint32_t count;
};
struct PointCloud2
{
uint32_t height;
uint32_t width;
std::vector<PointField> fields;
bool is_bigendian;
uint32_t point_step;
uint32_t row_step;
std::vector<uint8_t> data;
bool is_dense;
};
}
#endif // COSTMAP_2D_COST_VALUES_H_MSG

154
include/costmap_2d/observation_buffer.h Normal file
View File

@@ -0,0 +1,154 @@
/*********************************************************************
*
* 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 <ros/time.h>
#include <costmap_2d/observation.h>
#include <tf2_ros/buffer.h>
#include <sensor_msgs/PointCloud2.h>
// Thread support
#include <boost/thread.hpp>
namespace costmap_2d
{
/**
* @class ObservationBuffer
* @brief Takes in point clouds from sensors, transforms them to the desired frame, and stores them
*/
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
* @param observation_keep_time Defines the persistence of observations in seconds, 0 means only keep the latest
* @param expected_update_rate How often this buffer is expected to be updated, 0 means there is no limit
* @param min_obstacle_height The minimum height of a hitpoint to be considered legal
* @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 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
*/
ObservationBuffer(std::string topic_name, double observation_keep_time, double expected_update_rate,
double min_obstacle_height, double max_obstacle_height, double obstacle_range,
double raytrace_range, tf2_ros::Buffer& tf2_buffer, std::string global_frame,
std::string sensor_frame, double tf_tolerance);
/**
* @brief Destructor... cleans up
*/
~ObservationBuffer();
/**
* @brief Sets the global frame of an observation buffer. This will
* transform all the currently cached observations to the new global
* frame
* @param new_global_frame The name of the new global frame.
* @return True if the operation succeeds, false otherwise
*/
bool setGlobalFrame(const std::string new_global_frame);
/**
* @brief Transforms a PointCloud to the global frame and buffers it
* <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);
/**
* @brief Pushes copies of all current observations onto the end of the vector passed in
* @param observations The vector to be filled
*/
void getObservations(std::vector<Observation>& observations);
/**
* @brief Check if the observation buffer is being update at its expected rate
* @return True if it is being updated at the expected rate, false otherwise
*/
bool isCurrent() const;
/**
* @brief Lock the observation buffer
*/
inline void lock()
{
lock_.lock();
}
/**
* @brief Lock the observation buffer
*/
inline void unlock()
{
lock_.unlock();
}
/**
* @brief Reset last updated timestamp
*/
void resetLastUpdated();
private:
/**
* @brief Removes any stale observations from the buffer list
*/
void purgeStaleObservations();
tf2_ros::Buffer& tf2_buffer_;
const ros::Duration observation_keep_time_;
const ros::Duration expected_update_rate_;
ros::Time last_updated_;
std::string global_frame_;
std::string sensor_frame_;
std::list<Observation> observation_list_;
std::string topic_name_;
double min_obstacle_height_, max_obstacle_height_;
boost::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely
double obstacle_range_, raytrace_range_;
double tf_tolerance_;
};
} // namespace costmap_2d
#endif // COSTMAP_2D_OBSERVATION_BUFFER_H_

101
include/costmap_2d/static_layer.h Normal file
View File

@@ -0,0 +1,101 @@
/*********************************************************************
*
* 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 <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 <string>
namespace costmap_2d
{
class StaticLayer : public CostmapLayer
{
public:
StaticLayer();
virtual ~StaticLayer();
virtual void onInitialize();
virtual void activate();
virtual void deactivate();
virtual void reset();
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 matchSize();
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
std::string map_frame_; /// @brief frame that map is located in
std::string map_topic_; // Hiep thêm vào mục đich phân biết zones
bool subscribe_to_updates_;
bool map_received_;
bool has_updated_data_;
unsigned int x_, y_, width_, height_;
bool track_unknown_space_;
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_;
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_;
};
} // namespace costmap_2d
#endif // COSTMAP_2D_STATIC_LAYER_H_