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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
CMakeLists.txt
View File

@ -62,7 +62,7 @@ add_library(costmap_2d
# --- Link các thư viện phụ thuộc --- # --- Link các thư viện phụ thuộc ---
target_link_libraries(costmap_2d target_link_libraries(costmap_2d
${Boost_LIBRARIES} # Boost ${Boost_LIBRARIES} # Boost
std_msgs # ROS msgs std_msgs
sensor_msgs sensor_msgs
geometry_msgs geometry_msgs
nav_msgs nav_msgs
@ -100,11 +100,6 @@ install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION include/${PROJECT_NAME} DESTINATION include/${PROJECT_NAME}
) )
# # --- Export CMake targets ---
# install(EXPORT costmap_2dTargets
# DESTINATION lib/cmake/costmap_2d
# )
# --- Plugin libraries --- # --- Plugin libraries ---
# Tạo các plugin shared library # Tạo các plugin shared library
add_library(static_layer SHARED plugins/static_layer.cpp) add_library(static_layer SHARED plugins/static_layer.cpp)
@ -133,7 +128,7 @@ target_link_libraries(unpreferred_layer costmap_2d static_layer ${Boost_LIBRARIE
# --- Option để bật/tắt test --- # --- Option để bật/tắt test ---
option(BUILD_COSTMAP_TESTS "Build costmap_2d test executables" OFF) option(BUILD_COSTMAP_TESTS "Build costmap_2d test executables" ON)
if(BUILD_COSTMAP_TESTS) if(BUILD_COSTMAP_TESTS)
# --- Test executables --- # --- Test executables ---

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_ #define COSTMAP_2D_COSTMAP_2D_H_
#include <vector> // Dùng cho std::vector #include <vector>
#include <queue> // Dùng trong các thuật toán quét ô (flood fill, BFS) #include <queue>
#include <boost/thread.hpp> // Dùng mutex để đồng bộ truy cập giữa các thread (multi-thread safe)
#include <geometry_msgs/Point.h> #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 struct MapLocation
{ {
unsigned int x; // Chỉ số hàng (hoặc cột) theo trục X trong costmap unsigned int x;
unsigned int y; // Chỉ số hàng (hoặc cột) theo trục Y trong costmap unsigned int y;
}; };
/** /**
* @class Costmap2D * @class Costmap2D
* @brief Lớp này đại diện cho bản đồ chi phí 2D (costmap) * @brief A 2D costmap provides a mapping between points in the world and their associated "costs".
*
* 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 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: public:
/** /**
* Constructor khởi tạo bản đồ costmap * @brief Constructor for a costmap
* @param cells_size_x: số ô theo chiều X * @param cells_size_x The x size of the map in cells
* @param cells_size_y: số ô theo chiều Y * @param cells_size_y The y size of the map in cells
* @param resolution: kích thước 1 ô (mét / ô) * @param resolution The resolution of the map in meters/cell
* @param origin_x: gốc bản đồ (tọa độ thế giới) theo trục X * @param origin_x The x origin of the map
* @param origin_y: gốc bản đồ (tọa độ thế giới) theo trục Y * @param origin_y The y origin of the map
* @param default_value: giá trị mặc định của mỗi ô (thường là 0 = free) * @param default_value Default Value
*/ */
Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution, Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
double origin_x, double origin_y, unsigned char default_value = 0); double origin_x, double origin_y, unsigned char default_value = 0);
/** /**
* Constructor sao chép (copy constructor) * @brief Copy constructor for a costmap, creates a copy efficiently
* Dùng để tạo bản sao nhanh của một bản đồ costmap khác. * @param map The costmap to copy
*/ */
Costmap2D(const Costmap2D& map); 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); Costmap2D& operator=(const Costmap2D& map);
/** /**
* Tạo bản sao từ một vùng (window) của bản đồ khác. * @brief Turn this costmap into a copy of a window of a costmap passed in
* Dùng khi ta chỉ muốn sao chép một phần nhỏ của bản đồ lớn. * @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, bool copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y, double win_size_x,
double win_size_y); double win_size_y);
/// Constructor mặc định (không khởi tạo kích thước) /**
* @brief Default constructor
*/
Costmap2D(); Costmap2D();
/// Destructor /**
* @brief Destructor
*/
virtual ~Costmap2D(); 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; 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); void setCost(unsigned int mx, unsigned int my, unsigned char cost);
/** /**
* Chuyển đổi tọa độ map (ô) → tọa độ thế giới (mét) * @brief Convert from map coordinates to world coordinates
* Dùng để vẽ, debug hoặc xác định vị trí thực tế của một ô. * @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; void mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const;
/** /**
* Chuyển đổi tọa độ thế giới → tọa độ map (ô) * @brief Convert from world coordinates to map coordinates
* Nếu tọa độ nằm ngoài bản đồ → trả về false * @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; 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; 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; 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) * @brief Given two map coordinates... compute the associated index
* Công thức: index = my * size_x_ + mx * @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 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 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; 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; 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; 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; 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; 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; 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; 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; double getResolution() const;
/// Đặt giá trị mặc định (cost khi reset map)
void setDefaultValue(unsigned char c) void setDefaultValue(unsigned char c)
{ {
default_value_ = c; default_value_ = c;
} }
/// Trả về giá trị mặc định
unsigned char getDefaultValue() unsigned char getDefaultValue()
{ {
return default_value_; return default_value_;
} }
/** /**
* Đặt cost cho một vùng đa giác lồi (convex polygon) * @brief Sets the cost of a convex polygon to a desired value
* Dùng để đánh dấu vùng chướng ngại vật hoặc vùng forbidden. * @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); 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); 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); 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, * @brief Move the origin of the costmap to a new location.... keeping data when it can
* giữ lại dữ liệu cũ nếu có thể (thường dùng khi robot di chuyển xa). * @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); 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); 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, void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x,
double origin_y); 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); 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); 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; typedef boost::recursive_mutex mutex_t;
mutex_t* getMutex() mutex_t* getMutex()
{ {
@ -219,8 +299,17 @@ public:
protected: protected:
/** /**
* Hàm template copy một vùng dữ liệu từ bản đồ nguồn → bản đồ đích * @brief Copy a region of a source map into a destination map
* Dữ liệu được copy bằng memcpy để đảm bảo nhanh. * @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> template<typename data_type>
void copyMapRegion(data_type* source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y, 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 dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x,
unsigned int region_size_y) 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); 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); 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) 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)); 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 sm_index += sm_size_x;
dm_index += dm_size_x; // chuyển sang hàng tiếp theo của dest 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(); 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(); 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); 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) * @brief Raytrace a line and apply some action at each step
* Mỗi bước sẽ gọi “ActionType” (một functor) để đánh dấu hoặc xử lý từng ô. * @param at The action to take... a functor
* Thường dùng cho sensor update (quét laser). * @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> template<class ActionType>
inline void raytraceLine(ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, inline void raytraceLine(ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1,
unsigned int max_length = UINT_MAX) unsigned int max_length = UINT_MAX)
{ {
int dx = x1 - x0; // Khoảng cách theo trục X int dx = x1 - x0;
int dy = y1 - y0; // Khoảng cách theo trục Y int dy = y1 - y0;
unsigned int abs_dx = abs(dx); unsigned int abs_dx = abs(dx);
unsigned int abs_dy = abs(dy); unsigned int abs_dy = abs(dy);
int offset_dx = sign(dx); // Hướng đi theo X int offset_dx = sign(dx);
int offset_dy = sign(dy) * size_x_; // Hướng đi theo Y (theo stride của hàng) 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); 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) if (abs_dx >= abs_dy)
{ {
int error_y = abs_dx / 2; int error_y = abs_dx / 2;
@ -289,13 +383,14 @@ protected:
return; return;
} }
// otherwise y is dominant
int error_x = abs_dy / 2; int error_x = abs_dy / 2;
bresenham2D(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy)); bresenham2D(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy));
} }
private: 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> template<class ActionType>
inline void bresenham2D(ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, 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); unsigned int end = std::min(max_length, abs_da);
for (unsigned int i = 0; i < end; ++i) for (unsigned int i = 0; i < end; ++i)
{ {
at(offset); // Gọi hành động (thường là đánh dấu costmap) at(offset);
offset += offset_a; // Tiến thêm 1 bước theo hướng chính offset += offset_a;
error_b += abs_db; error_b += abs_db;
if ((unsigned int)error_b >= abs_da) 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; 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) inline int sign(int x)
{ {
return x > 0 ? 1.0 : -1.0; 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: protected:
unsigned int size_x_; // Số lượng ô theo trục X unsigned int size_x_;
unsigned int size_y_; // Số lượng ô theo trục Y unsigned int size_y_;
double resolution_; // Độ phân giải: mét/ô double resolution_;
double origin_x_; // Tọa độ gốc bản đồ (theo thế giới) trục X double origin_x_;
double origin_y_; // Tọa độ gốc bản đồ (theo thế giới) trục Y double origin_y_;
unsigned char* costmap_; // Mảng 1 chiều chứa giá trị cost của toàn bộ bản đồ unsigned char* costmap_;
unsigned char default_value_; // Giá trị mặc định cho các ô (thường = 0 hoặc 255 nếu unknown) unsigned char default_value_;
/**
* Functor nội bộ: dùng trong raytraceLine để đánh dấu một ô.
*/
class MarkCell class MarkCell
{ {
public: public:
@ -345,16 +435,13 @@ protected:
} }
inline void operator()(unsigned int offset) inline void operator()(unsigned int offset)
{ {
costmap_[offset] = value_; // Gán cost của ô bằng giá trị value_ costmap_[offset] = value_;
} }
private: private:
unsigned char* costmap_; unsigned char* costmap_;
unsigned char value_; unsigned char value_;
}; };
/**
* Functor nội bộ: dùng để thu thập các ô nằm trên viền polygon.
*/
class PolygonOutlineCells class PolygonOutlineCells
{ {
public: public:
@ -363,11 +450,12 @@ protected:
{ {
} }
// just push the relevant cells back onto the list
inline void operator()(unsigned int offset) inline void operator()(unsigned int offset)
{ {
MapLocation loc; MapLocation loc;
costmap_.indexToCells(offset, loc.x, loc.y); // Chuyển index → (x, y) costmap_.indexToCells(offset, loc.x, loc.y);
cells_.push_back(loc); // Thêm ô này vào danh sách viền polygon cells_.push_back(loc);
} }
private: private:
@ -376,7 +464,6 @@ protected:
std::vector<MapLocation>& cells_; std::vector<MapLocation>& cells_;
}; };
}; };
} // namespace costmap_2d } // 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, void readFootprintFromConfig(const std::vector<geometry_msgs::Point> &new_footprint,
const std::vector<geometry_msgs::Point> &old_footprint, const std::vector<geometry_msgs::Point> &old_footprint,
const double &robot_radius); const double &robot_radius);
std::vector<std::function<PluginLayerPtr()>> creators_;
void checkMovement(); void checkMovement();
void mapUpdateLoop(double frequency); void mapUpdateLoop(double frequency);
bool map_update_thread_shutdown_; 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_ #ifndef COSTMAP_2D_COSTMAP_LAYER_H_
#define COSTMAP_2D_COSTMAP_LAYER_H_ #define COSTMAP_2D_COSTMAP_LAYER_H_
#include <costmap_2d/layer.h> #include <costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h> #include <costmap_2d/layered_costmap.h>
#include <typeinfo>
namespace costmap_2d 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 #ifndef COSTMAP_2D_FOOTPRINT_H
#define COSTMAP_2D_FOOTPRINT_H #define COSTMAP_2D_FOOTPRINT_H
#include <geometry_msgs/Point32.h>
#include <geometry_msgs/Polygon.h> #include <geometry_msgs/Polygon.h>
#include <geometry_msgs/PolygonStamped.h> #include <geometry_msgs/PolygonStamped.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Point32.h>
#include <xmlrpcpp/XmlRpcValue.h> #include <xmlrpcpp/XmlRpcValue.h>
#include<string.h>
#include<vector>
using namespace std;
namespace costmap_2d namespace costmap_2d
{ {
@ -62,22 +59,22 @@ void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footpr
double& min_dist, double& max_dist); 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); 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); 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); 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); 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); 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 x The x position of the robot
* @param y The y position of the robot * @param y The y position of the robot
* @param theta The orientation 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); bool makeFootprintFromString(const std::string& footprint_string, std::vector<geometry_msgs::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<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh);
//////////////////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 Create the footprint from the given XmlRpcValue. * @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, std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc,
const std::string& full_param_name); const std::string& full_param_name);
/** @brief Write the current unpadded_footprint_ to the "footprint" // /** @brief Write the current unpadded_footprint_ to the "footprint"
* parameter of the given NodeHandle so that dynamic_reconfigure // * parameter of the given NodeHandle so that dynamic_reconfigure
* will see the new value. */ // * will see the new value. */
void writeFootprintToParam(std::string& nh, const std::vector<geometry_msgs::Point>& footprint); // void writeFootprintToParam(ros::NodeHandle& nh, const std::vector<geometry_msgs::Point>& footprint);
} // end namespace costmap_2d } // 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_ #ifndef COSTMAP_2D_INFLATION_LAYER_H_
#define COSTMAP_2D_INFLATION_LAYER_H_ #define COSTMAP_2D_INFLATION_LAYER_H_
#include <costmap_2d/layer.h> #include <costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h> #include <costmap_2d/layered_costmap.h>
#include <boost/thread.hpp> #include <boost/thread.hpp>
#include <stdexcept>
#include <iostream>
namespace costmap_2d namespace costmap_2d
{ {
@ -43,16 +76,9 @@ public:
virtual ~InflationLayer() 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(); deleteKernels();
if (seen_) if (seen_)
delete[] seen_; delete[] seen_;
seen_ = nullptr;
} }
virtual void onInitialize(); virtual void onInitialize();
@ -106,8 +132,8 @@ protected:
private: private:
void handleImpl(const void* data, void handleImpl(const void* data,
const std::type_info& info, const std::type_info& info,
const std::string& source) override; const std::string& source) override;
/** /**
* @brief Lookup pre-computed distances * @brief Lookup pre-computed distances
* @param mx The x coordinate of the current cell * @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/costmap_2d.h>
#include <costmap_2d/layered_costmap.h> #include <costmap_2d/layered_costmap.h>
#include <costmap_2d/utils.h>
#include <string> #include <string>
#include <tf3/buffer_core.h> #include <tf3/buffer_core.h>
#include <costmap_2d/layered_costmap.h>
#include <boost/shared_ptr.hpp>
namespace costmap_2d namespace costmap_2d
{ {
@ -135,8 +135,8 @@ public:
void dataCallBack(const T& value, const std::string& topic) { void dataCallBack(const T& value, const std::string& topic) {
handle(value, topic); handle(value, topic);
} }
protected: protected:
// Hàm template public, dùng để gửi dữ liệu // Hàm template public, dùng để gửi dữ liệu
template<typename T> template<typename T>
void handle(const T& data, const std::string& topic) { void handle(const T& data, const std::string& topic) {
@ -163,6 +163,8 @@ private:
std::vector<geometry_msgs::Point> footprint_spec_; std::vector<geometry_msgs::Point> footprint_spec_;
}; };
using PluginLayerPtr = std::shared_ptr<Layer>;
} // namespace costmap_2d } // namespace costmap_2d
#endif // COSTMAP_2D_LAYER_H_ #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_ #ifndef COSTMAP_2D_LAYERED_COSTMAP_H_
#define 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>
#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>
#include <costmap_2d/costmap_2d.h> // Định nghĩa lớp Costmap2D (bản đồ 2D cơ bản) #include <costmap_2d/costmap_2d.h>
#include <vector> #include <vector>
#include <string> #include <string>
#include <boost/dll/import.hpp>
namespace costmap_2d 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 * @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. * @brief Instantiates different layer plugins and aggregates them into one score
* 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 class LayeredCostmap
{ {
public: public:
/** /**
* @brief Hàm khởi tạo LayeredCostmap. * @brief Constructor for a costmap
* @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); LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown);
/** /**
* @brief Hàm huỷ lớp LayeredCostmap. * @brief Destructor
*/ */
~LayeredCostmap(); ~LayeredCostmap();
/** /**
* @brief Cập nhật bản đồ tổng hợp từ tất cả các layer. * @brief Update the underlying costmap with new data.
* @param robot_x, robot_y, robot_yaw: vị trí và hướng hiện tại của robot. * If you want to update the map outside of the update loop that runs, you can call this.
* @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); 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 inline const std::string& getGlobalFrameID() const noexcept
{ {
return global_frame_; 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, void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y,
bool size_locked = false); 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) void getUpdatedBounds(double& minx, double& miny, double& maxx, double& maxy)
{ {
minx = minx_; minx = minx_;
@ -70,66 +87,38 @@ public:
maxy = maxy_; 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(); bool isCurrent();
/**
* @brief Trả về con trỏ đến bản đồ Costmap2D cơ sở (master map).
*/
Costmap2D* getCostmap() Costmap2D* getCostmap()
{ {
return &costmap_; return &costmap_;
} }
/**
* @brief Kiểm tra xem bản đồ có phải kiểu "rolling window" không.
*/
bool isRolling() bool isRolling()
{ {
return rolling_window_; 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() bool isTrackingUnknown()
{ {
return costmap_.getDefaultValue() == costmap_2d::NO_INFORMATION; return costmap_.getDefaultValue() == costmap_2d::NO_INFORMATION;
} }
/** std::vector<std::shared_ptr<Layer> >* getPlugins()
* @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()
{ {
return &plugins_; return &plugins_;
} }
/** void addPlugin(std::shared_ptr<Layer> plugin)
* @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)
{ {
plugins_.push_back(plugin); plugins_.push_back(plugin);
} }
/**
* @brief Kiểm tra xem bản đồ có bị khoá kích thước không.
*/
bool isSizeLocked() bool isSizeLocked()
{ {
return size_locked_; 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) void getBounds(unsigned int* x0, unsigned int* xn, unsigned int* y0, unsigned int* yn)
{ {
*x0 = bx0_; *x0 = bx0_;
@ -138,56 +127,49 @@ public:
*yn = byn_; *yn = byn_;
} }
/**
* @brief Kiểm tra xem lớp LayeredCostmap đã được khởi tạo hoàn chỉnh chưa.
*/
bool isInitialized() bool isInitialized()
{ {
return initialized_; return initialized_;
} }
/** /** @brief Updates the stored footprint, updates the circumscribed
* @brief Cập nhật footprint (hình dạng chiếm chỗ của robot). * and inscribed radii, and calls onFootprintChanged() in all
* Đồng thời tính lại bán kính bao quanh và nội tiếp. * layers. */
* 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.
*/
void setFootprint(const std::vector<geometry_msgs::Point>& footprint_spec); void setFootprint(const std::vector<geometry_msgs::Point>& footprint_spec);
/** /** @brief Returns the latest footprint stored with setFootprint(). */
* @brief Trả về footprint hiện tại của robot.
*/
const std::vector<geometry_msgs::Point>& getFootprint() { return footprint_; } const std::vector<geometry_msgs::Point>& getFootprint() { return footprint_; }
/** /** @brief The radius of a circle centered at the origin of the
* @brief Bán kính đường tròn bao ngoài (circumscribed radius): * robot which just surrounds all points on the robot's
* bán kính nhỏ nhất của đường tròn chứa toàn bộ robot. * footprint.
*/ *
* This is updated by setFootprint(). */
double getCircumscribedRadius() { return circumscribed_radius_; } double getCircumscribedRadius() { return circumscribed_radius_; }
/** /** @brief The radius of a circle centered at the origin of the
* @brief Bán kính đường tròn nội tiếp (inscribed radius): * robot which is just within all points and edges of the robot's
* bán kính lớn nhất của đường tròn nằm hoàn toàn bên trong footprint robot. * footprint.
*/ *
* This is updated by setFootprint(). */
double getInscribedRadius() { return inscribed_radius_; } double getInscribedRadius() { return inscribed_radius_; }
private: private:
Costmap2D costmap_; ///< Bản đồ 2D chính (lưu ma trận chi phí tổng hợp) Costmap2D costmap_;
std::string global_frame_; ///< Frame toàn cục (thường là "map" hoặc "odom") 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 bool current_;
double minx_, miny_, maxx_, maxy_; ///< Giới hạn vùng bản đồ được cập nhật (tọa độ thế giới) double minx_, miny_, maxx_, maxy_;
unsigned int bx0_, bxn_, by0_, byn_; ///< Giới hạn vùng bản đồ được cập nhật (tọa độ lưới) 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 initialized_;
bool size_locked_; ///< Có cho phép thay đổi kích thước bản đồ sau này không bool size_locked_;
double circumscribed_radius_, inscribed_radius_;
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_;
std::vector<geometry_msgs::Point> footprint_; ///< Đa giác mô tả footprint robot
}; };
} // namespace costmap_2d } // namespace costmap_2d

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

@ -32,8 +32,8 @@
#ifndef COSTMAP_2D_OBSERVATION_H_ #ifndef COSTMAP_2D_OBSERVATION_H_
#define COSTMAP_2D_OBSERVATION_H_ #define COSTMAP_2D_OBSERVATION_H_
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/Point.h> #include <geometry_msgs/Point.h>
#include <sensor_msgs/PointCloud2.h>
namespace costmap_2d 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_ #ifndef COSTMAP_2D_OBSERVATION_BUFFER_H_
#define COSTMAP_2D_OBSERVATION_BUFFER_H_ #define COSTMAP_2D_OBSERVATION_BUFFER_H_
#include <vector> #include <vector>
#include <list> #include <list>
#include <string> #include <string>
#include <chrono>
// #include <ros/time.h>
#include <robot/time.h> #include <robot/time.h>
#include <robot/duration.h>
#include <costmap_2d/observation.h> #include <costmap_2d/observation.h>
#include <tf3/buffer_core.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 // Thread support
#include <boost/thread.hpp> #include <boost/thread.hpp>
@ -27,7 +58,6 @@ namespace costmap_2d
class ObservationBuffer class ObservationBuffer
{ {
public: public:
/** /**
* @brief Constructs an observation buffer * @brief Constructs an observation buffer
* @param topic_name The topic of the observations, used as an identifier for error and warning messages * @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 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 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 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 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 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 * @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(); void purgeStaleObservations();
tf3::BufferCore& tf3_buffer_; 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 observation_keep_time_;
const robot::Duration expected_update_rate_; const robot::Duration expected_update_rate_;
robot::Time last_updated_; robot::Time last_updated_;

38
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/layered_costmap.h>
#include <costmap_2d/observation_buffer.h> #include <costmap_2d/observation_buffer.h>
#include <costmap_2d/footprint.h> #include <costmap_2d/footprint.h>
#include <costmap_2d/utils.h>
#include <nav_msgs/OccupancyGrid.h> #include <nav_msgs/OccupancyGrid.h>
#include <sensor_msgs/LaserScan.h> #include <sensor_msgs/LaserScan.h>
#include <laser_geometry/laser_geometry.hpp>
#include <sensor_msgs/PointCloud.h> #include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h> #include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.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 namespace costmap_2d
{ {
@ -81,19 +77,41 @@ public:
void clearStaticObservations(bool marking, bool clearing); void clearStaticObservations(bool marking, bool clearing);
protected: protected:
void handleImpl(const void* data, void handleImpl(const void* data,
const std::type_info&, const std::type_info&,
const std::string& topic) override; const std::string& topic) override;
/**
* @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, void laserScanCallback(const sensor_msgs::LaserScan& message,
const boost::shared_ptr<ObservationBuffer>& buffer); 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, void laserScanValidInfCallback(const sensor_msgs::LaserScan& message,
const boost::shared_ptr<ObservationBuffer>& buffer); 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, 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, void pointCloud2Callback(const sensor_msgs::PointCloud2& message,
const boost::shared_ptr<ObservationBuffer>& buffer); const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
// void process(const map_msgs::OccupancyGridUpdate& update);
/** /**
* @brief Get the observations used to mark space * @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_ #ifndef COSTMAP_2D_STATIC_LAYER_H_
#define COSTMAP_2D_STATIC_LAYER_H_ #define COSTMAP_2D_STATIC_LAYER_H_
#include <costmap_2d/costmap_layer.h> #include <costmap_2d/costmap_layer.h>
#include <costmap_2d/layered_costmap.h> #include <costmap_2d/layered_costmap.h>
#include <nav_msgs/OccupancyGrid.h> #include <nav_msgs/OccupancyGrid.h>
#include <map_msgs/OccupancyGridUpdate.h> #include <map_msgs/OccupancyGridUpdate.h>
#include <string> #include <string>
#include <geometry_msgs/TransformStamped.h>
namespace costmap_2d namespace costmap_2d
{ {
@ -30,10 +67,9 @@ protected:
void handleImpl(const void* data, void handleImpl(const void* data,
const std::type_info& type, const std::type_info& type,
const std::string& topic) override; 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 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_; unsigned char* threshold_;
std::string base_frame_id_; std::string base_frame_id_;
std::string global_frame_; ///< @brief The global frame for the costmap std::string global_frame_; ///< @brief The global frame for the costmap
@ -49,19 +85,11 @@ protected:
bool trinary_costmap_; bool trinary_costmap_;
bool map_shutdown_ = false; bool map_shutdown_ = false;
bool map_update_shutdown_ = false; bool map_update_shutdown_ = false;
// ros::Subscriber map_sub_, map_update_sub_;
unsigned char lethal_threshold_, unknown_cost_value_; unsigned char lethal_threshold_, unknown_cost_value_;
private: 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(); bool getParams();
}; };
} // namespace costmap_2d } // namespace costmap_2d

2
include/costmap_2d/utils.h
View File

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

4
plugins/critical_layer.cpp
View File

@ -36,12 +36,12 @@ void CriticalLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, i
} }
// Export factory function // Export factory function
static PluginLayerPtr create_critical_plugin() { static std::shared_ptr<Layer> create_critical_plugin() {
return std::make_shared<CriticalLayer>(); return std::make_shared<CriticalLayer>();
} }
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias) // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_critical_plugin, create_plugin) BOOST_DLL_ALIAS(create_critical_plugin, create_critical_layer)
} }

4
plugins/directional_layer.cpp
View File

@ -430,10 +430,10 @@ namespace costmap_2d
} }
// Export factory function // Export factory function
static PluginLayerPtr create_directional_plugin() { static std::shared_ptr<Layer> create_directional_plugin() {
return std::make_shared<DirectionalLayer>(); return std::make_shared<DirectionalLayer>();
} }
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias) // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_directional_plugin, create_plugin) BOOST_DLL_ALIAS(create_directional_plugin, create_directional_layer)
} }

70
plugins/inflation_layer.cpp Normal file → Executable file
View File

@ -1,11 +1,48 @@
/*********************************************************************
*
* 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!!
*********************************************************************/
#include <algorithm> #include <algorithm>
#include <costmap_2d/inflation_layer.h> #include <costmap_2d/inflation_layer.h>
#include <costmap_2d/costmap_math.h> #include <costmap_2d/costmap_math.h>
#include <costmap_2d/footprint.h> #include <costmap_2d/footprint.h>
#include <costmap_2d/utils.h>
#include <boost/thread.hpp> #include <boost/thread.hpp>
#include <boost/dll/alias.hpp> #include <boost/dll/alias.hpp>
using costmap_2d::LETHAL_OBSTACLE; using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::INSCRIBED_INFLATED_OBSTACLE; using costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
using costmap_2d::NO_INFORMATION; using costmap_2d::NO_INFORMATION;
@ -21,7 +58,6 @@ InflationLayer::InflationLayer()
, inflate_unknown_(false) , inflate_unknown_(false)
, cell_inflation_radius_(0) , cell_inflation_radius_(0)
, cached_cell_inflation_radius_(0) , cached_cell_inflation_radius_(0)
// , dsrv_(NULL)
, seen_(NULL) , seen_(NULL)
, cached_costs_(NULL) , cached_costs_(NULL)
, cached_distances_(NULL) , cached_distances_(NULL)
@ -44,16 +80,27 @@ void InflationLayer::onInitialize()
seen_size_ = 0; seen_size_ = 0;
need_reinflation_ = false; need_reinflation_ = false;
getParams();
} }
InflationLayer::matchSize(); matchSize();
} }
bool InflationLayer::getParams() bool InflationLayer::getParams()
{ {
try { try {
YAML::Node config = YAML::LoadFile("/home/duongtd/robotics_core/costmap_2d/config/config.yaml"); std::string config_file_name = "config.yaml";
std::string folder = COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name);
if(path_source != " ")
{
std::cout << "Path source: " << path_source << std::endl;
}
else
{
std::cout << "/cfg folder not found!" << std::endl;
}
YAML::Node config = YAML::LoadFile(path_source);
YAML::Node layer = config["inflation_layer"]; YAML::Node layer = config["inflation_layer"];
double cost_scaling_factor = loadParam(layer, "cost_scaling_factor", 15.0); double cost_scaling_factor = loadParam(layer, "cost_scaling_factor", 15.0);
double inflation_radius = loadParam(layer, "inflation_radius", 0.55); double inflation_radius = loadParam(layer, "inflation_radius", 0.55);
@ -146,21 +193,19 @@ void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i,
return; return;
// make sure the inflation list is empty at the beginning of the cycle (should always be true) // make sure the inflation list is empty at the beginning of the cycle (should always be true)
if (!inflation_cells_.empty()) { printf("The inflation list must be empty at the beginning of inflation\n");
throw std::runtime_error("The inflation list must be empty at the beginning of inflation");
}
unsigned char* master_array = master_grid.getCharMap(); unsigned char* master_array = master_grid.getCharMap();
unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY(); unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
if (seen_ == NULL) { if (seen_ == NULL) {
std::cerr <<"InflationLayer::updateCosts(): seen_ array is NULL" <<std::endl; printf("InflationLayer::updateCosts(): seen_ array is NULL\n");
seen_size_ = size_x * size_y; seen_size_ = size_x * size_y;
seen_ = new bool[seen_size_]; seen_ = new bool[seen_size_];
} }
else if (seen_size_ != size_x * size_y) else if (seen_size_ != size_x * size_y)
{ {
std::cerr <<"InflationLayer::updateCosts(): seen_ array size is wrong" <<std::endl; printf("InflationLayer::updateCosts(): seen_ array size is wrong\n");
delete[] seen_; delete[] seen_;
seen_size_ = size_x * size_y; seen_size_ = size_x * size_y;
seen_ = new bool[seen_size_]; seen_ = new bool[seen_size_];
@ -357,12 +402,13 @@ void InflationLayer::handleImpl(const void* data,
printf("This function is not available!\n"); printf("This function is not available!\n");
} }
// Export factory function // Export factory function
static PluginLayerPtr create_inflation_plugin() { static std::shared_ptr<Layer> create_inflation_plugin() {
return std::make_shared<InflationLayer>(); return std::make_shared<InflationLayer>();
} }
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias) // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_inflation_plugin, create_plugin) BOOST_DLL_ALIAS(create_inflation_plugin, create_inflation_layer)
} // namespace costmap_2d } // namespace costmap_2d

209
plugins/obstacle_layer.cpp Normal file → Executable file
View File

@ -1,11 +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!!
*********************************************************************/
#include <costmap_2d/obstacle_layer.h> #include <costmap_2d/obstacle_layer.h>
#include <costmap_2d/costmap_math.h> #include <costmap_2d/costmap_math.h>
#include <costmap_2d/utils.h>
#include <sensor_msgs/point_cloud2_iterator.h> #include <sensor_msgs/point_cloud2_iterator.h>
#include <tf3/convert.h>
#include <tf3/utils.h> #include <tf3/exceptions.h>
#include <boost/dll/alias.hpp> #include <boost/dll/alias.hpp>
using costmap_2d::NO_INFORMATION; using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE; using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::FREE_SPACE; using costmap_2d::FREE_SPACE;
@ -27,91 +65,103 @@ void ObstacleLayer::onInitialize()
getParams(); getParams();
} }
ObstacleLayer::~ObstacleLayer() ObstacleLayer::~ObstacleLayer()
{} {}
bool ObstacleLayer::getParams() bool ObstacleLayer::getParams()
{ {
try { try {
YAML::Node config = YAML::LoadFile("/home/duongtd/robotics_core/costmap_2d/config/config.yaml"); std::string config_file_name = "config.yaml";
YAML::Node layer = config["obstacle_layer"]; std::string folder = COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name);
bool track_unknown_space = loadParam(layer, "track_unknown_space", layered_costmap_->isTrackingUnknown()); if(path_source != " ")
if (track_unknown_space)
default_value_ = NO_INFORMATION;
else
default_value_ = FREE_SPACE;
double transform_tolerance = loadParam(layer,"transform_tolerance", 0.2);
// get the topics that we'll subscribe to from the parameter server
std::string topics_string = loadParam(layer,"observation_sources", std::string(""));
printf(" Subscribed to Topics: %s\n", topics_string.c_str());
// get the parameters for the specific topic
double observation_keep_time = 0, expected_update_rate = 0, min_obstacle_height = 0, max_obstacle_height = 2;
std::string topic = "map", sensor_frame = "laser_frame", data_type = "PointCloud";
bool inf_is_valid = false, clearing=false, marking=true;
topic = loadParam(layer,"topic", topic);
sensor_frame = loadParam(layer,"sensor_frame", std::string(""));
observation_keep_time = loadParam(layer,"observation_persistence", 0.0);
expected_update_rate = loadParam(layer,"expected_update_rate", 0.0);
data_type = loadParam(layer,"data_type", std::string("PointCloud"));
min_obstacle_height = loadParam(layer,"min_obstacle_height", 0.0);
max_obstacle_height = loadParam(layer,"max_obstacle_height", 2.0);
inf_is_valid = loadParam(layer,"inf_is_valid", false);
clearing = loadParam(layer,"clearing", false);
marking = loadParam(layer,"marking", true);
if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan"))
{ {
printf("Only topics that use point clouds or laser scans are currently supported\n"); std::cout << "Path source: " << path_source << std::endl;
throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported"); }
else
{
std::cout << "/cfg folder not found!" << std::endl;
} }
std::string raytrace_range_param_name, obstacle_range_param_name;
double obstacle_range = 2.5; YAML::Node config = YAML::LoadFile(path_source);
obstacle_range = loadParam(layer,"obstacle_range", obstacle_range); YAML::Node layer = config["obstacle_layer"];
double raytrace_range = 3.0;
raytrace_range = loadParam(layer,"raytrace_range", raytrace_range);
bool track_unknown_space = loadParam(layer, "track_unknown_space", layered_costmap_->isTrackingUnknown());
if (track_unknown_space)
default_value_ = NO_INFORMATION;
else
default_value_ = FREE_SPACE;
bool footprint_clearing_enabled = loadParam(layer, "footprint_clearing_enabled", true); double transform_tolerance = loadParam(layer,"transform_tolerance", 0.2);
int combination_method = loadParam(layer, "combination_method", 1); // get the topics that we'll subscribe to from the parameter server
std::string topics_string = loadParam(layer,"observation_sources", std::string(""));
printf(" Subscribed to Topics: %s\n", topics_string.c_str());
// enabled_ = enabled; // get the parameters for the specific topic
footprint_clearing_enabled_ = footprint_clearing_enabled; double observation_keep_time = 0, expected_update_rate = 0, min_obstacle_height = 0, max_obstacle_height = 2;
max_obstacle_height_ = max_obstacle_height; std::string topic = "map", sensor_frame = "laser_frame", data_type = "PointCloud";
combination_method_ = combination_method; bool inf_is_valid = false, clearing=false, marking=true;
topic = loadParam(layer,"topic", topic);
printf("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(), sensor_frame = loadParam(layer,"sensor_frame", std::string(""));
sensor_frame.c_str()); observation_keep_time = loadParam(layer,"observation_persistence", 0.0);
expected_update_rate = loadParam(layer,"expected_update_rate", 0.0);
// create an observation buffer data_type = loadParam(layer,"data_type", std::string("PointCloud"));
observation_buffers_.push_back( min_obstacle_height = loadParam(layer,"min_obstacle_height", 0.0);
boost::shared_ptr < ObservationBuffer max_obstacle_height = loadParam(layer,"max_obstacle_height", 2.0);
> (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height, inf_is_valid = loadParam(layer,"inf_is_valid", false);
max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_, clearing = loadParam(layer,"clearing", false);
sensor_frame, transform_tolerance))); marking = loadParam(layer,"marking", true);
if (marking) if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan"))
marking_buffers_.push_back(observation_buffers_.back()); {
printf("Only topics that use point clouds or laser scans are currently supported\n");
// check if we'll also add this buffer to our clearing observation buffers throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported");
if (clearing)
clearing_buffers_.push_back(observation_buffers_.back());
printf(
"Created an observation buffer for topic %s, global frame: %s, "
"expected update rate: %.2f, observation persistence: %.2f\n",
topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);
}
catch (const YAML::BadFile& e) {
std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
return false;
} }
return true; std::string raytrace_range_param_name, obstacle_range_param_name;
double obstacle_range = 2.5;
obstacle_range = loadParam(layer,"obstacle_range", obstacle_range);
double raytrace_range = 3.0;
raytrace_range = loadParam(layer,"raytrace_range", raytrace_range);
bool footprint_clearing_enabled = loadParam(layer, "footprint_clearing_enabled", true);
int combination_method = loadParam(layer, "combination_method", 1);
// enabled_ = enabled;
footprint_clearing_enabled_ = footprint_clearing_enabled;
max_obstacle_height_ = max_obstacle_height;
combination_method_ = combination_method;
printf("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(),
sensor_frame.c_str());
// create an observation buffer
observation_buffers_.push_back(
boost::shared_ptr < ObservationBuffer
> (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,
max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_,
sensor_frame, transform_tolerance)));
if (marking)
marking_buffers_.push_back(observation_buffers_.back());
// check if we'll also add this buffer to our clearing observation buffers
if (clearing)
clearing_buffers_.push_back(observation_buffers_.back());
printf(
"Created an observation buffer for topic %s, global frame: %s, "
"expected update rate: %.2f, observation persistence: %.2f\n",
topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);
}
catch (const YAML::BadFile& e) {
std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
return false;
}
return true;
} }
void ObstacleLayer::handleImpl(const void* data, void ObstacleLayer::handleImpl(const void* data,
@ -121,7 +171,7 @@ void ObstacleLayer::handleImpl(const void* data,
if(!stop_receiving_data_) if(!stop_receiving_data_)
{ {
if(observation_buffers_.empty()) return; if(observation_buffers_.empty()) return;
boost::shared_ptr<costmap_2d::ObservationBuffer> buffer = observation_buffers_.back(); boost::shared_ptr<ObservationBuffer> buffer = observation_buffers_.back();
if (type == typeid(sensor_msgs::LaserScan) && topic == "laser") { if (type == typeid(sensor_msgs::LaserScan) && topic == "laser") {
laserScanCallback(*static_cast<const sensor_msgs::LaserScan*>(data), buffer); laserScanCallback(*static_cast<const sensor_msgs::LaserScan*>(data), buffer);
} else if (type == typeid(sensor_msgs::LaserScan) && topic == "laser_valid") { } else if (type == typeid(sensor_msgs::LaserScan) && topic == "laser_valid") {
@ -147,7 +197,6 @@ void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScan& message,
// project the laser into a point cloud // project the laser into a point cloud
sensor_msgs::PointCloud2 cloud; sensor_msgs::PointCloud2 cloud;
cloud.header = message.header; cloud.header = message.header;
// printf("TEST PLUGIN OBSTACLE!!!\n");
// project the scan into a point cloud // project the scan into a point cloud
try try
@ -175,7 +224,6 @@ void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScan& message,
void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScan& raw_message, void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScan& raw_message,
const boost::shared_ptr<ObservationBuffer>& buffer) const boost::shared_ptr<ObservationBuffer>& buffer)
{ {
// printf("TEST PLUGIN OBSTACLE 2!!!\n");
// Filter positive infinities ("Inf"s) to max_range. // Filter positive infinities ("Inf"s) to max_range.
float epsilon = 0.0001; // a tenth of a millimeter float epsilon = 0.0001; // a tenth of a millimeter
sensor_msgs::LaserScan message = raw_message; sensor_msgs::LaserScan message = raw_message;
@ -216,9 +264,8 @@ void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScan& raw_
} }
void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloud& message, void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloud& message,
const boost::shared_ptr<ObservationBuffer>& buffer) const boost::shared_ptr<ObservationBuffer>& buffer)
{ {
// printf("TEST PLUGIN OBSTACLE 3!!!\n");
sensor_msgs::PointCloud2 cloud2; sensor_msgs::PointCloud2 cloud2;
if (!sensor_msgs::convertPointCloudToPointCloud2(message, cloud2)) if (!sensor_msgs::convertPointCloudToPointCloud2(message, cloud2))
@ -516,11 +563,11 @@ void ObstacleLayer::reset()
} }
// Export factory function // Export factory function
static PluginLayerPtr create_obstacle_plugin() { static std::shared_ptr<Layer> create_obstacle_plugin() {
return std::make_shared<ObstacleLayer>(); return std::make_shared<ObstacleLayer>();
} }
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias) // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_obstacle_plugin, create_plugin) BOOST_DLL_ALIAS(create_obstacle_plugin, create_obstacle_layer)
} // namespace costmap_2d } // namespace costmap_2d

4
plugins/preferred_layer.cpp
View File

@ -25,11 +25,11 @@ unsigned char PreferredLayer::interpretValue(unsigned char value)
} }
// Export factory function // Export factory function
static PluginLayerPtr create_preferred_plugin() { static std::shared_ptr<Layer> create_preferred_plugin() {
return std::make_shared<PreferredLayer>(); return std::make_shared<PreferredLayer>();
} }
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias) // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_preferred_plugin, create_plugin) BOOST_DLL_ALIAS(create_preferred_plugin, create_preferred_layer)
} }

70
plugins/static_layer.cpp Normal file → Executable file
View File

@ -1,16 +1,50 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* Copyright (c) 2015, Fetch Robotics, 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!!
*********************************************************************/
#include <costmap_2d/static_layer.h> #include <costmap_2d/static_layer.h>
#include <costmap_2d/costmap_math.h> #include <costmap_2d/costmap_math.h>
#include <data_convert/data_convert.h>
#include <costmap_2d/utils.h>
#include <tf3/convert.h> #include <tf3/convert.h>
#include <tf3/utils.h>
#include <boost/dll/alias.hpp>
#include <filesystem>
#include <string>
#include <iostream>
#include <tf3_geometry_msgs/tf3_geometry_msgs.h> #include <tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <boost/dll/alias.hpp>
using costmap_2d::NO_INFORMATION; using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE; using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::FREE_SPACE; using costmap_2d::FREE_SPACE;
@ -137,7 +171,6 @@ void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map)
if(!map_shutdown_) if(!map_shutdown_)
{ {
std::cout << "Received new map!" << std::endl; std::cout << "Received new map!" << std::endl;
unsigned int size_x = new_map.info.width, size_y = new_map.info.height; unsigned int size_x = new_map.info.width, size_y = new_map.info.height;
printf("Received a %d X %d map at %f m/pix\n", size_x, size_y, new_map.info.resolution); printf("Received a %d X %d map at %f m/pix\n", size_x, size_y, new_map.info.resolution);
@ -188,18 +221,17 @@ void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map)
height_ = size_y_; height_ = size_y_;
map_received_ = true; map_received_ = true;
has_updated_data_ = true; has_updated_data_ = true;
} }
else else
{ {
printf("Stop receive new map!"); printf("Stop receive new map!");
} }
// shutdown the map subscrber if firt_map_only_ flag is on // shutdown the map subscrber if firt_map_only_ flag is on
if (first_map_only_) if (first_map_only_)
{ {
printf("Shutting down the map subscriber. first_map_only flag is on"); printf("Shutting down the map subscriber. first_map_only flag is on\n");
map_shutdown_ = true; map_shutdown_ = true;
// map_sub_.shutdown();
} }
} }
@ -302,12 +334,7 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
tf3::TransformStampedMsg transformMsg; tf3::TransformStampedMsg transformMsg;
try try
{ {
// transformMsg = tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0)); transformMsg = tf_->lookupTransform(map_frame_, global_frame_, tf3::Time::now());
bool status =tf_->canTransform(map_frame_, global_frame_, tf3::Time());
if(!status) throw tf3::TransformException("[static_layer] Cannot transform");
transformMsg = tf_->lookupTransform(map_frame_,
global_frame_,
tf3::Time());
} }
catch (tf3::TransformException ex) catch (tf3::TransformException ex)
{ {
@ -317,7 +344,8 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
// Copy map data given proper transformations // Copy map data given proper transformations
tf3::Transform tf3_transform; tf3::Transform tf3_transform;
tf3_transform = data_convert::convertTotf3Transform(transformMsg.transform); tf3_transform = data_convert::convertTotf3Transform(transformMsg.transform);
// tf3::convert(transformMsg.transform, tf3_transform); // tf2::Transform tf2_transform;
// tf2::convert(transform.transform, tf2_transform);
for (unsigned int i = min_i; i < max_i; ++i) for (unsigned int i = min_i; i < max_i; ++i)
{ {
for (unsigned int j = min_j; j < max_j; ++j) for (unsigned int j = min_j; j < max_j; ++j)
@ -341,11 +369,11 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
} }
// Export factory function // Export factory function
static PluginLayerPtr create_static_plugin() { static std::shared_ptr<Layer> create_static_plugin() {
return std::make_shared<StaticLayer>(); return std::make_shared<StaticLayer>();
} }
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias) // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_static_plugin, create_plugin) BOOST_DLL_ALIAS(create_static_plugin, create_static_layer)
} // namespace costmap_2d } // namespace costmap_2d

4
plugins/unpreferred_layer.cpp
View File

@ -23,11 +23,11 @@ unsigned char UnPreferredLayer::interpretValue(unsigned char value)
} }
// Export factory function // Export factory function
static PluginLayerPtr create_unpreferred_plugin() { static std::shared_ptr<Layer> create_unpreferred_plugin() {
return std::make_shared<UnPreferredLayer>(); return std::make_shared<UnPreferredLayer>();
} }
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias) // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_unpreferred_plugin, create_plugin) BOOST_DLL_ALIAS(create_unpreferred_plugin, create_unpreferred_layer)
} }

51
plugins/voxel_layer.cpp Normal file → Executable file
View File

@ -36,10 +36,8 @@
* David V. Lu!! * David V. Lu!!
*********************************************************************/ *********************************************************************/
#include <costmap_2d/voxel_layer.h> #include <costmap_2d/voxel_layer.h>
#include <costmap_2d/utils.h>
#include <boost/dll/alias.hpp>
#include <sensor_msgs/point_cloud2_iterator.h> #include <sensor_msgs/point_cloud2_iterator.h>
#include <boost/dll/alias.hpp>
#define VOXEL_BITS 16 #define VOXEL_BITS 16
@ -59,10 +57,25 @@ void VoxelLayer::onInitialize()
getParams(); getParams();
} }
VoxelLayer::~VoxelLayer()
{}
bool VoxelLayer::getParams() bool VoxelLayer::getParams()
{ {
try { try {
YAML::Node config = YAML::LoadFile("/home/duongtd/robotics_core/costmap_2d/config/config.yaml"); std::string config_file_name = "config.yaml";
std::string folder = COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name);
if(path_source != " ")
{
std::cout << "Path source: " << path_source << std::endl;
}
else
{
std::cout << "/cfg folder not found!" << std::endl;
}
YAML::Node config = YAML::LoadFile(path_source);
YAML::Node layer = config["voxel_layer"]; YAML::Node layer = config["voxel_layer"];
// publish_voxel_ = loadParam(layer, "publish_voxel_map", false); // publish_voxel_ = loadParam(layer, "publish_voxel_map", false);
@ -83,12 +96,8 @@ bool VoxelLayer::getParams()
} }
return true; return true;
} }
VoxelLayer::~VoxelLayer()
{}
void VoxelLayer::matchSize() void VoxelLayer::matchSize()
{ {
ObstacleLayer::matchSize(); ObstacleLayer::matchSize();
@ -211,14 +220,7 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
// grid_msg.resolutions.z = z_resolution_; // grid_msg.resolutions.z = z_resolution_;
// grid_msg.header.frame_id = global_frame_; // grid_msg.header.frame_id = global_frame_;
// grid_msg.header.stamp = robot::Time::now(); // grid_msg.header.stamp = robot::Time::now();
// voxel_pub_.publish(grid_msg);
// ///////////////////////////////////////////
// ////////////THAY THẾ PUBLISH NÀY///////////
// ///////////////////////////////////////////
// // voxel_pub_.publish(grid_msg);
// ///////////////////////////////////////////
// ///////////////////////////////////////////
// ///////////////////////////////////////////
// } // }
updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
@ -289,23 +291,18 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z)) if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
{ {
printf("The origin for the sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.\n", printf(
"The origin for the sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.\n",
ox, oy, oz); ox, oy, oz);
return; return;
} }
///////////////////////////////////////////
////////////THAY THẾ PUBLISH NÀY///////////
///////////////////////////////////////////
// bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0); // bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0);
// if (publish_clearing_points) // if (publish_clearing_points)
// { // {
clearing_endpoints_.points.clear(); clearing_endpoints_.points.clear();
clearing_endpoints_.points.reserve(clearing_observation_cloud_size); clearing_endpoints_.points.reserve(clearing_observation_cloud_size);
// } // }
///////////////////////////////////////////
///////////////////////////////////////////
///////////////////////////////////////////
// we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
double map_end_x = origin_x_ + getSizeInMetersX(); double map_end_x = origin_x_ + getSizeInMetersX();
@ -375,7 +372,7 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
{ {
unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_); unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z); // voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_, voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,
unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION, unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
cell_raytrace_range); cell_raytrace_range);
@ -399,7 +396,7 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
clearing_endpoints_.header.stamp = clearing_observation.cloud_->header.stamp; clearing_endpoints_.header.stamp = clearing_observation.cloud_->header.stamp;
clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq; clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq;
// clearing_endpoints_pub_.publish(clearing_endpoints_); // clearing_endpoints_pub_.publish(clearing_endpoints_);
// } // }
} }
@ -461,11 +458,11 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
} }
// Export factory function // Export factory function
static PluginLayerPtr create_voxel_plugin() { static std::shared_ptr<Layer> create_voxel_plugin() {
return std::make_shared<VoxelLayer>(); return std::make_shared<VoxelLayer>();
} }
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias) // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_voxel_plugin, create_plugin) BOOST_DLL_ALIAS(create_voxel_plugin, create_voxel_layer)
} // namespace costmap_2d } // namespace costmap_2d

0
src/array_parser.cpp Normal file → Executable file
View File

377
src/costmap_2d.cpp Normal file → Executable file
View File

@ -1,61 +1,74 @@
/********************************************************************* /*********************************************************************
* License & Authors *
* - Cung cấp lớp Costmap2D để lưu trữ bản đồ chi phí 2D * 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!!
*********************************************************************/ *********************************************************************/
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/costmap_2d.h> // Header định nghĩa lớp Costmap2D #include <cstdio>
#include <cstdio> // Dùng cho thao tác file (fopen, fprintf, ...)
using namespace std; using namespace std;
namespace costmap_2d namespace costmap_2d
{ {
/*****************************************************
* Hàm khởi tạo chính của Costmap2D
* - Tạo bản đồ kích thước size_x × size_y
* - Gán độ phân giải, gốc tọa độ, và giá trị mặc định
*****************************************************/
Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution, Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
double origin_x, double origin_y, unsigned char default_value) : double origin_x, double origin_y, unsigned char default_value) :
size_x_(cells_size_x), size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x), size_x_(cells_size_x), size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x),
origin_y_(origin_y), costmap_(NULL), default_value_(default_value) origin_y_(origin_y), costmap_(NULL), default_value_(default_value)
{ {
access_ = new mutex_t(); // Khóa bảo vệ truy cập đa luồng access_ = new mutex_t();
// Cấp phát vùng nhớ cho bản đồ // create the costmap
initMaps(size_x_, size_y_); initMaps(size_x_, size_y_);
// Gán toàn bộ bản đồ bằng giá trị mặc định (ví dụ: NO_INFORMATION)
resetMaps(); resetMaps();
} }
/*****************************************************
* Xóa bộ nhớ costmap_
*****************************************************/
void Costmap2D::deleteMaps() void Costmap2D::deleteMaps()
{ {
boost::unique_lock<mutex_t> lock(*access_); // Giữ mutex để đảm bảo thread-safe // clean up data
delete[] costmap_; // Giải phóng vùng nhớ boost::unique_lock<mutex_t> lock(*access_);
delete[] costmap_;
costmap_ = NULL; costmap_ = NULL;
} }
/*****************************************************
* Cấp phát vùng nhớ cho costmap_
*****************************************************/
void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y) void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y)
{ {
boost::unique_lock<mutex_t> lock(*access_); boost::unique_lock<mutex_t> lock(*access_);
if (costmap_ != nullptr) { delete[] costmap_;
delete[] costmap_; costmap_ = new unsigned char[size_x * size_y];
}
costmap_ = new unsigned char[size_x * size_y]; // Mỗi ô lưu 1 byte (0–255)
} }
/*****************************************************
* Thay đổi kích thước bản đồ
* - Thường dùng khi tạo lại bản đồ mới hoặc khi map động
*****************************************************/
void Costmap2D::resizeMap(unsigned int size_x, unsigned int size_y, double resolution, void Costmap2D::resizeMap(unsigned int size_x, unsigned int size_y, double resolution,
double origin_x, double origin_y) double origin_x, double origin_y)
{ {
@ -65,22 +78,18 @@ void Costmap2D::resizeMap(unsigned int size_x, unsigned int size_y, double resol
origin_x_ = origin_x; origin_x_ = origin_x;
origin_y_ = origin_y; origin_y_ = origin_y;
initMaps(size_x, size_y); // Cấp phát vùng nhớ mới initMaps(size_x, size_y);
resetMaps(); // Reset toàn bộ dữ liệu
// reset our maps to have no information
resetMaps();
} }
/*****************************************************
* Reset toàn bộ bản đồ về giá trị mặc định
*****************************************************/
void Costmap2D::resetMaps() void Costmap2D::resetMaps()
{ {
boost::unique_lock<mutex_t> lock(*access_); boost::unique_lock<mutex_t> lock(*access_);
memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char)); memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char));
} }
/*****************************************************
* Reset một vùng con trong costmap
*****************************************************/
void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn) void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
{ {
boost::unique_lock<mutex_t> lock(*(access_)); boost::unique_lock<mutex_t> lock(*(access_));
@ -89,48 +98,50 @@ void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsi
memset(costmap_ + y, default_value_, len * sizeof(unsigned char)); memset(costmap_ + y, default_value_, len * sizeof(unsigned char));
} }
/***************************************************** bool Costmap2D::copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y, double win_size_x,
* Tạo bản đồ mới chỉ bao gồm 1 cửa sổ (window) con của map hiện tại double win_size_y)
*****************************************************/
bool Costmap2D::copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y,
double win_size_x, double win_size_y)
{ {
// Không thể tạo window của chính mình // check for self windowing
if (this == &map) if (this == &map)
{
// ROS_ERROR("Cannot convert this costmap into a window of itself");
return false; return false;
}
deleteMaps(); // Xóa dữ liệu cũ // clean up old data
deleteMaps();
// Chuyển đổi tọa độ thế giới sang tọa độ lưới // compute the bounds of our new map
unsigned int lower_left_x, lower_left_y, upper_right_x, upper_right_y; unsigned int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
if (!map.worldToMap(win_origin_x, win_origin_y, lower_left_x, lower_left_y) if (!map.worldToMap(win_origin_x, win_origin_y, lower_left_x, lower_left_y)
|| !map.worldToMap(win_origin_x + win_size_x, win_origin_y + win_size_y, upper_right_x, upper_right_y)) || !map.worldToMap(win_origin_x + win_size_x, win_origin_y + win_size_y, upper_right_x, upper_right_y))
{
// ROS_ERROR("Cannot window a map that the window bounds don't fit inside of");
return false; return false;
}
// Tính kích thước vùng con
size_x_ = upper_right_x - lower_left_x; size_x_ = upper_right_x - lower_left_x;
size_y_ = upper_right_y - lower_left_y; size_y_ = upper_right_y - lower_left_y;
resolution_ = map.resolution_; resolution_ = map.resolution_;
origin_x_ = win_origin_x; origin_x_ = win_origin_x;
origin_y_ = win_origin_y; origin_y_ = win_origin_y;
initMaps(size_x_, size_y_); // Cấp phát bộ nhớ mới // initialize our various maps and reset markers for inflation
initMaps(size_x_, size_y_);
// Sao chép dữ liệu vùng con từ bản đồ gốc // copy the window of the static map and the costmap that we're taking
copyMapRegion(map.costmap_, lower_left_x, lower_left_y, map.size_x_, copyMapRegion(map.costmap_, lower_left_x, lower_left_y, map.size_x_, costmap_, 0, 0, size_x_, size_x_, size_y_);
costmap_, 0, 0, size_x_, size_x_, size_y_);
return true; return true;
} }
/*****************************************************
* Toán tử gán (=): sao chép toàn bộ dữ liệu từ map khác
*****************************************************/
Costmap2D& Costmap2D::operator=(const Costmap2D& map) Costmap2D& Costmap2D::operator=(const Costmap2D& map)
{ {
// check for self assignement
if (this == &map) if (this == &map)
return *this; return *this;
deleteMaps(); // Xóa map cũ // clean up old data
deleteMaps();
size_x_ = map.size_x_; size_x_ = map.size_x_;
size_y_ = map.size_y_; size_y_ = map.size_y_;
@ -138,85 +149,62 @@ Costmap2D& Costmap2D::operator=(const Costmap2D& map)
origin_x_ = map.origin_x_; origin_x_ = map.origin_x_;
origin_y_ = map.origin_y_; origin_y_ = map.origin_y_;
initMaps(size_x_, size_y_); // Tạo map mới // initialize our various maps
memcpy(costmap_, map.costmap_, size_x_ * size_y_ * sizeof(unsigned char)); // Sao chép dữ liệu initMaps(size_x_, size_y_);
// copy the cost map
memcpy(costmap_, map.costmap_, size_x_ * size_y_ * sizeof(unsigned char));
return *this; return *this;
} }
/***************************************************** Costmap2D::Costmap2D(const Costmap2D& map) :
* Constructor sao chép costmap_(NULL)
*****************************************************/
Costmap2D::Costmap2D(const Costmap2D& map) : costmap_(NULL)
{ {
access_ = new mutex_t(); access_ = new mutex_t();
*this = map; // Gọi lại toán tử gán *this = map;
} }
/***************************************************** // just initialize everything to NULL by default
* Constructor mặc định (chưa có bản đồ)
*****************************************************/
Costmap2D::Costmap2D() : Costmap2D::Costmap2D() :
size_x_(0), size_y_(0), resolution_(0.0), size_x_(0), size_y_(0), resolution_(0.0), origin_x_(0.0), origin_y_(0.0), costmap_(NULL)
origin_x_(0.0), origin_y_(0.0), costmap_(NULL)
{ {
access_ = new mutex_t(); access_ = new mutex_t();
} }
/*****************************************************
* Destructor: giải phóng bộ nhớ
*****************************************************/
Costmap2D::~Costmap2D() Costmap2D::~Costmap2D()
{ {
deleteMaps(); deleteMaps();
delete access_; delete access_;
} }
/*****************************************************
* Chuyển khoảng cách thực (mét) sang đơn vị ô
*****************************************************/
unsigned int Costmap2D::cellDistance(double world_dist) unsigned int Costmap2D::cellDistance(double world_dist)
{ {
double cells_dist = max(0.0, ceil(world_dist / resolution_)); double cells_dist = max(0.0, ceil(world_dist / resolution_));
return (unsigned int)cells_dist; return (unsigned int)cells_dist;
} }
/*****************************************************
* Trả về con trỏ dữ liệu costmap
*****************************************************/
unsigned char* Costmap2D::getCharMap() const unsigned char* Costmap2D::getCharMap() const
{ {
return costmap_; return costmap_;
} }
/*****************************************************
* Lấy giá trị cost tại ô (mx, my)
*****************************************************/
unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const
{ {
return costmap_[getIndex(mx, my)]; return costmap_[getIndex(mx, my)];
} }
/*****************************************************
* Gán giá trị cost tại ô (mx, my)
*****************************************************/
void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost) void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost)
{ {
costmap_[getIndex(mx, my)] = cost; costmap_[getIndex(mx, my)] = cost;
} }
/*****************************************************
* Chuyển từ tọa độ lưới (ô) sang tọa độ thế giới
*****************************************************/
void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const
{ {
wx = origin_x_ + (mx + 0.5) * resolution_; wx = origin_x_ + (mx + 0.5) * resolution_;
wy = origin_y_ + (my + 0.5) * resolution_; wy = origin_y_ + (my + 0.5) * resolution_;
} }
/*****************************************************
* Chuyển từ tọa độ thế giới sang tọa độ lưới
*****************************************************/
bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const
{ {
if (wx < origin_x_ || wy < origin_y_) if (wx < origin_x_ || wy < origin_y_)
@ -225,143 +213,159 @@ bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int&
mx = (int)((wx - origin_x_) / resolution_); mx = (int)((wx - origin_x_) / resolution_);
my = (int)((wy - origin_y_) / resolution_); my = (int)((wy - origin_y_) / resolution_);
return (mx < size_x_ && my < size_y_); if (mx < size_x_ && my < size_y_)
return true;
return false;
} }
/*****************************************************
* Phiên bản không kiểm tra biên (có thể ra ngoài map)
*****************************************************/
void Costmap2D::worldToMapNoBounds(double wx, double wy, int& mx, int& my) const void Costmap2D::worldToMapNoBounds(double wx, double wy, int& mx, int& my) const
{ {
mx = (int)((wx - origin_x_) / resolution_); mx = (int)((wx - origin_x_) / resolution_);
my = (int)((wy - origin_y_) / resolution_); my = (int)((wy - origin_y_) / resolution_);
} }
/*****************************************************
* Phiên bản ép buộc tọa độ nằm trong biên bản đồ
*****************************************************/
void Costmap2D::worldToMapEnforceBounds(double wx, double wy, int& mx, int& my) const void Costmap2D::worldToMapEnforceBounds(double wx, double wy, int& mx, int& my) const
{ {
// Here we avoid doing any math to wx,wy before comparing them to
// the bounds, so their values can go out to the max and min values
// of double floating point.
if (wx < origin_x_) if (wx < origin_x_)
{
mx = 0; mx = 0;
}
else if (wx >= resolution_ * size_x_ + origin_x_) else if (wx >= resolution_ * size_x_ + origin_x_)
{
mx = size_x_ - 1; mx = size_x_ - 1;
}
else else
{
mx = (int)((wx - origin_x_) / resolution_); mx = (int)((wx - origin_x_) / resolution_);
}
if (wy < origin_y_) if (wy < origin_y_)
{
my = 0; my = 0;
}
else if (wy >= resolution_ * size_y_ + origin_y_) else if (wy >= resolution_ * size_y_ + origin_y_)
{
my = size_y_ - 1; my = size_y_ - 1;
}
else else
{
my = (int)((wy - origin_y_) / resolution_); my = (int)((wy - origin_y_) / resolution_);
}
} }
/*****************************************************
* Dịch chuyển gốc của bản đồ khi robot di chuyển
* => Dữ liệu được dịch và giữ lại phần chồng lấn
*****************************************************/
void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y) void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y)
{ {
int cell_ox = int((new_origin_x - origin_x_) / resolution_); // project the new origin into the grid
int cell_oy = int((new_origin_y - origin_y_) / resolution_); int cell_ox, cell_oy;
cell_ox = int((new_origin_x - origin_x_) / resolution_);
cell_oy = int((new_origin_y - origin_y_) / resolution_);
// Nothing to update
if (cell_ox == 0 && cell_oy == 0) if (cell_ox == 0 && cell_oy == 0)
return; return;
// Cập nhật lại gốc bản đồ mới (theo bội số của resolution) // compute the associated world coordinates for the origin cell
double new_grid_ox = origin_x_ + cell_ox * resolution_; // because we want to keep things grid-aligned
double new_grid_oy = origin_y_ + cell_oy * resolution_; double new_grid_ox, new_grid_oy;
new_grid_ox = origin_x_ + cell_ox * resolution_;
new_grid_oy = origin_y_ + cell_oy * resolution_;
// To save casting from unsigned int to int a bunch of times
int size_x = size_x_; int size_x = size_x_;
int size_y = size_y_; int size_y = size_y_;
// Tính vùng chồng lấn giữa bản đồ cũ và bản đồ mới // we need to compute the overlap of the new and existing windows
int lower_left_x = min(max(cell_ox, 0), size_x); int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
int lower_left_y = min(max(cell_oy, 0), size_y); lower_left_x = min(max(cell_ox, 0), size_x);
int upper_right_x = min(max(cell_ox + size_x, 0), size_x); lower_left_y = min(max(cell_oy, 0), size_y);
int upper_right_y = min(max(cell_oy + size_y, 0), size_y); upper_right_x = min(max(cell_ox + size_x, 0), size_x);
upper_right_y = min(max(cell_oy + size_y, 0), size_y);
unsigned int cell_size_x = upper_right_x - lower_left_x; unsigned int cell_size_x = upper_right_x - lower_left_x;
unsigned int cell_size_y = upper_right_y - lower_left_y; unsigned int cell_size_y = upper_right_y - lower_left_y;
// Tạo bản đồ tạm để lưu phần chồng lấn // we need a map to store the obstacles in the window temporarily
unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y]; unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_,
local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
resetMaps(); // Toàn bộ bản đồ được đặt lại giá trị mặc định // copy the local window in the costmap to the local map
copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
// now we'll set the costmap to be completely unknown if we track unknown space
resetMaps();
// update the origin with the appropriate world coordinates
origin_x_ = new_grid_ox; origin_x_ = new_grid_ox;
origin_y_ = new_grid_oy; origin_y_ = new_grid_oy;
// Copy vùng chồng lấn vào vị trí mới // compute the starting cell location for copying data back in
int start_x = lower_left_x - cell_ox; int start_x = lower_left_x - cell_ox;
int start_y = lower_left_y - cell_oy; int start_y = lower_left_y - cell_oy;
copyMapRegion(local_map, 0, 0, cell_size_x,
costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
delete[] local_map; // Giải phóng vùng nhớ tạm // now we want to copy the overlapping information back into the map, but in its new location
copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
// make sure to clean up
delete[] local_map;
} }
/*****************************************************
* Đặt giá trị cost cho vùng đa giác lồi
* (ví dụ: để đánh dấu vùng cấm hoặc vùng obstacle)
*****************************************************/
bool Costmap2D::setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value) bool Costmap2D::setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value)
{ {
// we assume the polygon is given in the global_frame... we need to transform it to map coordinates
std::vector<MapLocation> map_polygon; std::vector<MapLocation> map_polygon;
for (unsigned int i = 0; i < polygon.size(); ++i) for (unsigned int i = 0; i < polygon.size(); ++i)
{ {
MapLocation loc; MapLocation loc;
if (!worldToMap(polygon[i].x, polygon[i].y, loc.x, loc.y)) if (!worldToMap(polygon[i].x, polygon[i].y, loc.x, loc.y))
return false; // Nếu một điểm nằm ngoài map {
// ("Polygon lies outside map bounds, so we can't fill it");
return false;
}
map_polygon.push_back(loc); map_polygon.push_back(loc);
} }
std::vector<MapLocation> polygon_cells; std::vector<MapLocation> polygon_cells;
convexFillCells(map_polygon, polygon_cells); // Lấy toàn bộ ô bên trong đa giác
// get the cells that fill the polygon
convexFillCells(map_polygon, polygon_cells);
// set the cost of those cells
for (unsigned int i = 0; i < polygon_cells.size(); ++i) for (unsigned int i = 0; i < polygon_cells.size(); ++i)
costmap_[getIndex(polygon_cells[i].x, polygon_cells[i].y)] = cost_value; {
unsigned int index = getIndex(polygon_cells[i].x, polygon_cells[i].y);
costmap_[index] = cost_value;
}
return true; return true;
} }
/***************************************************** void Costmap2D::polygonOutlineCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells)
* polygonOutlineCells():
* - Tìm các ô nằm trên biên của đa giác
*****************************************************/
void Costmap2D::polygonOutlineCells(const std::vector<MapLocation>& polygon,
std::vector<MapLocation>& polygon_cells)
{ {
PolygonOutlineCells cell_gatherer(*this, costmap_, polygon_cells); PolygonOutlineCells cell_gatherer(*this, costmap_, polygon_cells);
for (unsigned int i = 0; i < polygon.size() - 1; ++i) for (unsigned int i = 0; i < polygon.size() - 1; ++i)
{
raytraceLine(cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y); raytraceLine(cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y);
}
if (!polygon.empty()) if (!polygon.empty())
{ {
unsigned int last_index = polygon.size() - 1; unsigned int last_index = polygon.size() - 1;
// Nối điểm cuối với điểm đầu để khép kín đa giác // we also need to close the polygon by going from the last point to the first
raytraceLine(cell_gatherer, polygon[last_index].x, polygon[last_index].y, raytraceLine(cell_gatherer, polygon[last_index].x, polygon[last_index].y, polygon[0].x, polygon[0].y);
polygon[0].x, polygon[0].y);
} }
} }
/***************************************************** void Costmap2D::convexFillCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells)
* convexFillCells():
* - Điền đầy các ô bên trong một đa giác lồi
*****************************************************/
void Costmap2D::convexFillCells(const std::vector<MapLocation>& polygon,
std::vector<MapLocation>& polygon_cells)
{ {
if (polygon.size() < 3) // Ít hơn 3 điểm thì không tạo thành đa giác // we need a minimum polygon of a triangle
if (polygon.size() < 3)
return; return;
polygonOutlineCells(polygon, polygon_cells); // Lấy biên ngoài // first get the cells that make up the outline of the polygon
polygonOutlineCells(polygon, polygon_cells);
// Sắp xếp theo trục X để dễ quét cột // quick bubble sort to sort points by x
MapLocation swap; MapLocation swap;
unsigned int i = 0; unsigned int i = 0;
while (i < polygon_cells.size() - 1) while (i < polygon_cells.size() - 1)
@ -371,20 +375,25 @@ void Costmap2D::convexFillCells(const std::vector<MapLocation>& polygon,
swap = polygon_cells[i]; swap = polygon_cells[i];
polygon_cells[i] = polygon_cells[i + 1]; polygon_cells[i] = polygon_cells[i + 1];
polygon_cells[i + 1] = swap; polygon_cells[i + 1] = swap;
if (i > 0) --i;
if (i > 0)
--i;
} }
else ++i; else
++i;
} }
// Quét từng cột X để điền đầy Y
i = 0; i = 0;
MapLocation min_pt, max_pt; MapLocation min_pt;
MapLocation max_pt;
unsigned int min_x = polygon_cells[0].x; unsigned int min_x = polygon_cells[0].x;
unsigned int max_x = polygon_cells.back().x; unsigned int max_x = polygon_cells[polygon_cells.size() - 1].x;
// walk through each column and mark cells inside the polygon
for (unsigned int x = min_x; x <= max_x; ++x) for (unsigned int x = min_x; x <= max_x; ++x)
{ {
if (i >= polygon_cells.size() - 1) break; if (i >= polygon_cells.size() - 1)
break;
if (polygon_cells[i].y < polygon_cells[i + 1].y) if (polygon_cells[i].y < polygon_cells[i + 1].y)
{ {
@ -407,37 +416,69 @@ void Costmap2D::convexFillCells(const std::vector<MapLocation>& polygon,
++i; ++i;
} }
// Điền đầy ô từ min_y đến max_y MapLocation pt;
// loop though cells in the column
for (unsigned int y = min_pt.y; y <= max_pt.y; ++y) for (unsigned int y = min_pt.y; y <= max_pt.y; ++y)
polygon_cells.push_back({x, y}); {
pt.x = x;
pt.y = y;
polygon_cells.push_back(pt);
}
} }
} }
/***************************************************** unsigned int Costmap2D::getSizeInCellsX() const
* Các hàm getter cơ bản {
*****************************************************/ return size_x_;
unsigned int Costmap2D::getSizeInCellsX() const { return size_x_; } }
unsigned int Costmap2D::getSizeInCellsY() const { return size_y_; }
double Costmap2D::getSizeInMetersX() const { return (size_x_ - 1 + 0.5) * resolution_; } unsigned int Costmap2D::getSizeInCellsY() const
double Costmap2D::getSizeInMetersY() const { return (size_y_ - 1 + 0.5) * resolution_; } {
double Costmap2D::getOriginX() const { return origin_x_; } return size_y_;
double Costmap2D::getOriginY() const { return origin_y_; } }
double Costmap2D::getResolution() const { return resolution_; }
double Costmap2D::getSizeInMetersX() const
{
return (size_x_ - 1 + 0.5) * resolution_;
}
double Costmap2D::getSizeInMetersY() const
{
return (size_y_ - 1 + 0.5) * resolution_;
}
double Costmap2D::getOriginX() const
{
return origin_x_;
}
double Costmap2D::getOriginY() const
{
return origin_y_;
}
double Costmap2D::getResolution() const
{
return resolution_;
}
/*****************************************************
* Lưu costmap ra file PGM (grayscale) để debug
*****************************************************/
bool Costmap2D::saveMap(std::string file_name) bool Costmap2D::saveMap(std::string file_name)
{ {
FILE *fp = fopen(file_name.c_str(), "w"); FILE *fp = fopen(file_name.c_str(), "w");
if (!fp) if (!fp)
{
return false; return false;
}
fprintf(fp, "P2\n%u\n%u\n%u\n", size_x_, size_y_, 0xff); fprintf(fp, "P2\n%u\n%u\n%u\n", size_x_, size_y_, 0xff);
for (unsigned int iy = 0; iy < size_y_; iy++) for (unsigned int iy = 0; iy < size_y_; iy++)
{ {
for (unsigned int ix = 0; ix < size_x_; ix++) for (unsigned int ix = 0; ix < size_x_; ix++)
fprintf(fp, "%d ", getCost(ix, iy)); {
unsigned char cost = getCost(ix, iy);
fprintf(fp, "%d ", cost);
}
fprintf(fp, "\n"); fprintf(fp, "\n");
} }
fclose(fp); fclose(fp);

43
src/costmap_2d_robot.cpp
View File

@ -48,6 +48,8 @@
#include <tf3/utils.h> #include <tf3/utils.h>
#include <exception> #include <exception>
#include <boost/dll/import.hpp>
using namespace std; using namespace std;
namespace costmap_2d namespace costmap_2d
@ -97,19 +99,19 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
robot::Time last_error = robot::Time::now(); robot::Time last_error = robot::Time::now();
std::string tf_error; std::string tf_error;
// we need to make sure that the transform between the robot base frame and the global frame is available // // we need to make sure that the transform between the robot base frame and the global frame is available
while (!tf_.canTransform(global_frame_, robot_base_frame_, tf3::Time::now(), &tf_error)) // while (!tf_.canTransform(global_frame_, robot_base_frame_, tf3::Time::now(), &tf_error))
{ // {
if (last_error + robot::Duration(5.0) < robot::Time::now()) // if (last_error + robot::Duration(5.0) < robot::Time::now())
{ // {
printf("%f: Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n", // printf("%f: Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n",
robot::Time::now().toSec(), robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str()); // robot::Time::now().toSec(), robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
last_error = robot::Time::now(); // last_error = robot::Time::now();
} // }
// The error string will accumulate and errors will typically be the same, so the last // // The error string will accumulate and errors will typically be the same, so the last
// will do for the warning above. Reset the string here to avoid accumulation. // // will do for the warning above. Reset the string here to avoid accumulation.
tf_error.clear(); // tf_error.clear();
} // }
// check if we want a rolling window version of the costmap // check if we want a rolling window version of the costmap
bool rolling_window, track_unknown_space, always_send_full_costmap; bool rolling_window, track_unknown_space, always_send_full_costmap;
@ -121,10 +123,10 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
struct PluginInfo { std::string path; std::string name; }; struct PluginInfo { std::string path; std::string name; };
std::vector<PluginInfo> plugins_to_load = { std::vector<PluginInfo> plugins_to_load = {
{"./costmap_2d/libstatic_layer.so", "static_layer"}, {"./costmap_2d/libstatic_layer.so", "create_static_layer"},
{"./costmap_2d/libinflation_layer.so", "inflation_layer"}, {"./costmap_2d/libinflation_layer.so", "create_inflation_layer"},
{"./costmap_2d/libobstacle_layer.so", "obstacle_layer"}, {"./costmap_2d/libobstacle_layer.so", "create_obstacle_layer"},
{"./costmap_2d/libpreferred_layer.so", "preferred_layer"} {"./costmap_2d/libpreferred_layer.so", "create_preferred_layer"}
}; };
// if (private_nh.hasParam("plugins")) // if (private_nh.hasParam("plugins"))
@ -140,10 +142,11 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
try try
{ {
// copyParentParameters(pname, type, private_nh); // copyParentParameters(pname, type, private_nh);
auto creator = boost::dll::import_alias<PluginLayerPtr()>( creators_.push_back(
info.path, "create_plugin", boost::dll::load_mode::append_decorations boost::dll::import_alias<PluginLayerPtr()>(
info.path, info.name, boost::dll::load_mode::append_decorations)
); );
PluginLayerPtr plugin = creator(); PluginLayerPtr plugin = creators_.back()();
std::cout << "Plugin created: " << info.name << std::endl; std::cout << "Plugin created: " << info.name << std::endl;
plugin->initialize(layered_costmap_, info.name, &tf_); plugin->initialize(layered_costmap_, info.name, &tf_);
layered_costmap_->addPlugin(plugin); layered_costmap_->addPlugin(plugin);

0
src/costmap_layer.cpp Normal file → Executable file
View File

0
src/costmap_math.cpp Normal file → Executable file
View File

137
src/footprint.cpp Normal file → Executable file
View File

@ -27,12 +27,13 @@
* POSSIBILITY OF SUCH DAMAGE. * POSSIBILITY OF SUCH DAMAGE.
*/ */
#include <costmap_2d/costmap_math.h> #include<costmap_2d/costmap_math.h>
#include <boost/tokenizer.hpp> #include <boost/tokenizer.hpp>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/algorithm/string.hpp> #include <boost/algorithm/string.hpp>
#include <costmap_2d/footprint.h> #include <costmap_2d/footprint.h>
#include <costmap_2d/array_parser.h> #include <costmap_2d/array_parser.h>
#include<geometry_msgs/Point32.h>
namespace costmap_2d namespace costmap_2d
{ {
@ -173,15 +174,15 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
if (error != "") if (error != "")
{ {
std::printf("Error parsing footprint parameter: '%s'", error.c_str()); printf("Error parsing footprint parameter: '%s'\n", error.c_str());
std::printf(" Footprint string was '%s'.", footprint_string.c_str()); printf(" Footprint string was '%s'.\n", footprint_string.c_str());
return false; return false;
} }
// convert vvf into points. // convert vvf into points.
if (vvf.size() < 3) if (vvf.size() < 3)
{ {
std::printf("You must specify at least three points for the robot footprint, reverting to previous footprint."); printf("You must specify at least three points for the robot footprint, reverting to previous footprint.\n");
return false; return false;
} }
footprint.reserve(vvf.size()); footprint.reserve(vvf.size());
@ -197,7 +198,7 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
} }
else else
{ {
std::printf("Points in the footprint specification must be pairs of numbers. Found a point with %d numbers.", printf("Points in the footprint specification must be pairs of numbers. Found a point with %d numbers.\n",
int(vvf[ i ].size())); int(vvf[ i ].size()));
return false; return false;
} }
@ -208,66 +209,66 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
std::vector<geometry_msgs::Point> makeFootprintFromParams(const std::string& file_name) // std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh)
{ // {
std::string full_param_name; // std::string full_param_name;
std::string full_radius_param_name; // std::string full_radius_param_name;
std::vector<geometry_msgs::Point> points; // std::vector<geometry_msgs::Point> points;
// if (nh.searchParam("footprint", full_param_name)) // if (nh.searchParam("footprint", full_param_name))
// { // {
// XmlRpc::XmlRpcValue footprint_xmlrpc; // XmlRpc::XmlRpcValue footprint_xmlrpc;
// nh.getParam(full_param_name, footprint_xmlrpc); // nh.getParam(full_param_name, footprint_xmlrpc);
// if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString && // if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString &&
// footprint_xmlrpc != "" && footprint_xmlrpc != "[]") // footprint_xmlrpc != "" && footprint_xmlrpc != "[]")
// { // {
// if (makeFootprintFromString(std::string(footprint_xmlrpc), points)) // if (makeFootprintFromString(std::string(footprint_xmlrpc), points))
// { // {
// writeFootprintToParam(nh, points); // writeFootprintToParam(nh, points);
// return points; // return points;
// } // }
// } // }
// else if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray) // else if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
// { // {
// points = makeFootprintFromXMLRPC(footprint_xmlrpc, full_param_name); // points = makeFootprintFromXMLRPC(footprint_xmlrpc, full_param_name);
// writeFootprintToParam(nh, points); // writeFootprintToParam(nh, points);
// return points; // return points;
// } // }
// } // }
// if (nh.searchParam("robot_radius", full_radius_param_name)) // if (nh.searchParam("robot_radius", full_radius_param_name))
// { // {
// double robot_radius; // double robot_radius;
// nh.param(full_radius_param_name, robot_radius, 1.234); // nh.param(full_radius_param_name, robot_radius, 1.234);
// points = makeFootprintFromRadius(robot_radius); // points = makeFootprintFromRadius(robot_radius);
// nh.setParam("robot_radius", robot_radius); // nh.setParam("robot_radius", robot_radius);
// } // }
// // Else neither param was found anywhere this knows about, so // // Else neither param was found anywhere this knows about, so
// // defaults will come from dynamic_reconfigure stuff, set in // // defaults will come from dynamic_reconfigure stuff, set in
// // cfg/Costmap2D.cfg and read in this file in reconfigureCB(). // // cfg/Costmap2D.cfg and read in this file in reconfigureCB().
// return points; // return points;
} // }
void writeFootprintToParam(std::string& nh, const std::vector<geometry_msgs::Point>& footprint) // void writeFootprintToParam(ros::NodeHandle& nh, const std::vector<geometry_msgs::Point>& footprint)
{ // {
// std::ostringstream oss; // std::ostringstream oss;
// bool first = true; // bool first = true;
// for (unsigned int i = 0; i < footprint.size(); i++) // for (unsigned int i = 0; i < footprint.size(); i++)
// { // {
// geometry_msgs::Point p = footprint[ i ]; // geometry_msgs::Point p = footprint[ i ];
// if (first) // if (first)
// { // {
// oss << "[[" << p.x << "," << p.y << "]"; // oss << "[[" << p.x << "," << p.y << "]";
// first = false; // first = false;
// } // }
// else // else
// { // {
// oss << ",[" << p.x << "," << p.y << "]"; // oss << ",[" << p.x << "," << p.y << "]";
// } // }
// } // }
// oss << "]"; // oss << "]";
// nh.setParam("footprint", oss.str().c_str()); // nh.setParam("footprint", oss.str().c_str());
} // }
double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name) double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name)
{ {
@ -276,9 +277,9 @@ double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_p
value.getType() != XmlRpc::XmlRpcValue::TypeDouble) value.getType() != XmlRpc::XmlRpcValue::TypeDouble)
{ {
std::string& value_string = value; std::string& value_string = value;
std::printf("Values in the footprint specification (param %s) must be numbers. Found value %s.\n", printf("Values in the footprint specification (param %s) must be numbers. Found value %s.\n",
full_param_name.c_str(), value_string.c_str()); full_param_name.c_str(), value_string.c_str());
throw std::runtime_error("Values in the footprint specification must be numbers\n"); throw std::runtime_error("Values in the footprint specification must be numbers");
} }
return value.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value); return value.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value);
} }
@ -290,10 +291,10 @@ std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& f
if (footprint_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray || if (footprint_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray ||
footprint_xmlrpc.size() < 3) footprint_xmlrpc.size() < 3)
{ {
std::printf("The footprint must be specified as list of lists on the parameter server, %s was specified as %s\n", printf("The footprint must be specified as list of lists on the parameter server, %s was specified as %s\n",
full_param_name.c_str(), std::string(footprint_xmlrpc).c_str()); full_param_name.c_str(), std::string(footprint_xmlrpc).c_str());
throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least " throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least "
"3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]\n"); "3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
} }
std::vector<geometry_msgs::Point> footprint; std::vector<geometry_msgs::Point> footprint;
@ -306,11 +307,11 @@ std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& f
if (point.getType() != XmlRpc::XmlRpcValue::TypeArray || if (point.getType() != XmlRpc::XmlRpcValue::TypeArray ||
point.size() != 2) point.size() != 2)
{ {
std::printf("The footprint (parameter %s) must be specified as list of lists on the parameter server eg: " printf("The footprint (parameter %s) must be specified as list of lists on the parameter server eg: "
"[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form.\n", "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form.\n",
full_param_name.c_str()); full_param_name.c_str());
throw std::runtime_error("The footprint must be specified as list of lists on the parameter server eg: " throw std::runtime_error("The footprint must be specified as list of lists on the parameter server eg: "
"[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form\n"); "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form");
} }
pt.x = getNumberFromXMLRPC(point[ 0 ], full_param_name); pt.x = getNumberFromXMLRPC(point[ 0 ], full_param_name);

31
src/layer.cpp Normal file → Executable file
View File

@ -1,10 +1,37 @@
/*
* Copyright (c) 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 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.
*/
#include "costmap_2d/layer.h" #include "costmap_2d/layer.h"
namespace costmap_2d namespace costmap_2d
{ {
Layer::Layer() Layer::Layer()
: layered_costmap_(NULL) : layered_costmap_(NULL)
, current_(false) , current_(false)

54
src/layered_costmap.cpp Normal file → Executable file
View File

@ -1,10 +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!!
*********************************************************************/
#include <costmap_2d/layered_costmap.h> #include <costmap_2d/layered_costmap.h>
#include <costmap_2d/footprint.h> #include <costmap_2d/footprint.h>
#include <cstdio> #include <cstdio>
#include <string> #include <string>
#include <algorithm> #include <algorithm>
#include <vector> #include <vector>
#include <memory>
using std::vector; using std::vector;
@ -49,7 +85,7 @@ namespace costmap_2d
boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex())); boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
size_locked_ = size_locked; size_locked_ = size_locked;
costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y); costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y);
for (vector<costmap_2d::PluginLayerPtr>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin) ++plugin)
{ {
(*plugin)->matchSize(); (*plugin)->matchSize();
@ -76,7 +112,7 @@ namespace costmap_2d
minx_ = miny_ = 1e30; minx_ = miny_ = 1e30;
maxx_ = maxy_ = -1e30; maxx_ = maxy_ = -1e30;
for (vector<costmap_2d::PluginLayerPtr>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin) ++plugin)
{ {
if (!(*plugin)->isEnabled()) if (!(*plugin)->isEnabled())
@ -88,8 +124,8 @@ namespace costmap_2d
(*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_); (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy) if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy)
{ {
std::printf("Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but " printf("Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
"is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s", "is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s\n",
prev_minx, prev_miny, prev_maxx, prev_maxy, prev_minx, prev_miny, prev_maxx, prev_maxy,
minx_, miny_, maxx_, maxy_, minx_, miny_, maxx_, maxy_,
(*plugin)->getName().c_str()); (*plugin)->getName().c_str());
@ -105,14 +141,14 @@ namespace costmap_2d
y0 = std::max(0, y0); y0 = std::max(0, y0);
yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1); yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1);
std::printf("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn); printf("Updating area x: [%d, %d] y: [%d, %d]\n", x0, xn, y0, yn);
if (xn < x0 || yn < y0) if (xn < x0 || yn < y0)
return; return;
costmap_.resetMap(x0, y0, xn, yn); costmap_.resetMap(x0, y0, xn, yn);
for (vector<costmap_2d::PluginLayerPtr>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin) ++plugin)
{ {
if ((*plugin)->isEnabled()) if ((*plugin)->isEnabled())
@ -130,7 +166,7 @@ namespace costmap_2d
bool LayeredCostmap::isCurrent() bool LayeredCostmap::isCurrent()
{ {
current_ = true; current_ = true;
for (vector<costmap_2d::PluginLayerPtr>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin) ++plugin)
{ {
if ((*plugin)->isEnabled()) if ((*plugin)->isEnabled())
@ -144,7 +180,7 @@ namespace costmap_2d
footprint_ = footprint_spec; footprint_ = footprint_spec;
costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_); costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_);
for (vector<costmap_2d::PluginLayerPtr>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin) ++plugin)
{ {
(*plugin)->onFootprintChanged(); (*plugin)->onFootprintChanged();

106
src/observation_buffer.cpp Normal file → Executable file
View File

@ -1,11 +1,44 @@
/*********************************************************************
*
* 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
*********************************************************************/
#include <costmap_2d/observation_buffer.h> #include <costmap_2d/observation_buffer.h>
#include <data_convert/data_convert.h>
#include <tf3_geometry_msgs/tf3_geometry_msgs.h> #include <tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <tf3_sensor_msgs/tf3_sensor_msgs.h> #include <tf3_sensor_msgs/tf3_sensor_msgs.h>
#include<tf3/convert.h>
#include <sensor_msgs/point_cloud2_iterator.h> #include <sensor_msgs/point_cloud2_iterator.h>
#include <cstdint>
using namespace std; using namespace std;
using namespace tf3; using namespace tf3;
@ -17,8 +50,7 @@ ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_
double raytrace_range, tf3::BufferCore& tf3_buffer, string global_frame, double raytrace_range, tf3::BufferCore& tf3_buffer, string global_frame,
string sensor_frame, double tf_tolerance) : string sensor_frame, double tf_tolerance) :
tf3_buffer_(tf3_buffer), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate), tf3_buffer_(tf3_buffer), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
last_updated_(robot::Time::now()), last_updated_(robot::Time::now()), global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height), min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height),
obstacle_range_(obstacle_range), raytrace_range_(raytrace_range), tf_tolerance_(tf_tolerance) obstacle_range_(obstacle_range), raytrace_range_(raytrace_range), tf_tolerance_(tf_tolerance)
{ {
@ -30,11 +62,11 @@ ObservationBuffer::~ObservationBuffer()
bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame) bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
{ {
robot::Time transform_time = robot::Time::now(); tf3::Time transform_time = tf3::Time::now();
std::string tf_error; std::string tf_error;
if (!tf3_buffer_.canTransform(new_global_frame, global_frame_, tf3::Time::now(), &tf_error)) geometry_msgs::TransformStamped transformStamped;
if (!tf3_buffer_.canTransform(new_global_frame, global_frame_, transform_time, &tf_error))
{ {
printf("Transform between %s and %s with tolerance %.2f failed: %s.\n", new_global_frame.c_str(), printf("Transform between %s and %s with tolerance %.2f failed: %s.\n", new_global_frame.c_str(),
global_frame_.c_str(), tf_tolerance_, tf_error.c_str()); global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
@ -50,21 +82,27 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
geometry_msgs::PointStamped origin; geometry_msgs::PointStamped origin;
origin.header.frame_id = global_frame_; origin.header.frame_id = global_frame_;
origin.header.stamp = transform_time; origin.header.stamp = data_convert::convertTime(transform_time);
origin.point = obs.origin_; origin.point = obs.origin_;
// we need to transform the origin of the observation to the new global frame // we need to transform the origin of the observation to the new global frame
tf3::doTransform(origin, origin, // tf3_buffer_.transform(origin, origin, new_global_frame);
tf3_buffer_.lookupTransform(new_global_frame, tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform(
origin.header.frame_id, new_global_frame, // frame đích
data_convert::convertTime(origin.header.stamp))); origin.header.frame_id, // frame nguồn
transform_time
);
tf3::doTransform(origin, origin, tfm_1);
obs.origin_ = origin.point; obs.origin_ = origin.point;
// we also need to transform the cloud of the observation to the new global frame // we also need to transform the cloud of the observation to the new global frame
tf3::doTransform(*(obs.cloud_), *(obs.cloud_), // tf3_buffer_.transform(*(obs.cloud_), *(obs.cloud_), new_global_frame);
tf3_buffer_.lookupTransform(new_global_frame, tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform(
obs.cloud_->header.frame_id, new_global_frame, // frame đích
data_convert::convertTime(obs.cloud_->header.stamp))); obs.cloud_->header.frame_id, // frame nguồn
transform_time
);
tf3::doTransform(*(obs.cloud_), *(obs.cloud_), tfm_2);
} }
catch (TransformException& ex) catch (TransformException& ex)
{ {
@ -98,11 +136,19 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
local_origin.point.x = 0; local_origin.point.x = 0;
local_origin.point.y = 0; local_origin.point.y = 0;
local_origin.point.z = 0; local_origin.point.z = 0;
tf3::doTransform(local_origin, global_origin, // tf3_buffer_.transform(local_origin, global_origin, global_frame_);
tf3_buffer_.lookupTransform(global_frame_, tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform(
local_origin.header.frame_id, global_frame_, // frame đích
data_convert::convertTime(local_origin.header.stamp))); local_origin.header.frame_id, // frame nguồn
data_convert::convertTime(local_origin.header.stamp)
);
tf3::doTransform(local_origin, global_origin, tfm_1);
/////////////////////////////////////////////////
///////////chú ý hàm này/////////////////////////
tf3::convert(global_origin.point, observation_list_.front().origin_); tf3::convert(global_origin.point, observation_list_.front().origin_);
/////////////////////////////////////////////////
/////////////////////////////////////////////////
// make sure to pass on the raytrace/obstacle range of the observation buffer to the observations // make sure to pass on the raytrace/obstacle range of the observation buffer to the observations
observation_list_.front().raytrace_range_ = raytrace_range_; observation_list_.front().raytrace_range_ = raytrace_range_;
@ -111,10 +157,13 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
sensor_msgs::PointCloud2 global_frame_cloud; sensor_msgs::PointCloud2 global_frame_cloud;
// transform the point cloud // transform the point cloud
tf3::doTransform(cloud, global_frame_cloud, // tf3_buffer_.transform(cloud, global_frame_cloud, global_frame_);
tf3_buffer_.lookupTransform(global_frame_, tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform(
(cloud.header.frame_id), global_frame_, // frame đích
data_convert::convertTime(cloud.header.stamp))); cloud.header.frame_id, // frame nguồn
data_convert::convertTime(cloud.header.stamp)
);
tf3::doTransform(cloud, global_frame_cloud, tfm_2);
global_frame_cloud.header.stamp = cloud.header.stamp; global_frame_cloud.header.stamp = cloud.header.stamp;
// now we need to remove observations from the cloud that are below or above our height thresholds // now we need to remove observations from the cloud that are below or above our height thresholds
@ -213,13 +262,12 @@ bool ObservationBuffer::isCurrent() const
if (expected_update_rate_ == robot::Duration(0.0)) if (expected_update_rate_ == robot::Duration(0.0))
return true; return true;
bool current = (robot::Time::now() - last_updated_) <= expected_update_rate_; bool current = (robot::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();
if (!current) if (!current)
{ {
printf( printf(
"The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.\n", "The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.\n",
topic_name_.c_str(), (robot::Time::now() - last_updated_).toSec(), topic_name_.c_str(), (robot::Time::now() - last_updated_).toSec(), expected_update_rate_.toSec());
expected_update_rate_.toSec());
} }
return current; return current;
} }