fix core dumped err when loadplugin
This commit is contained in:
parent
2439f2a71d
commit
11ff1baa79
|
|
@ -62,7 +62,7 @@ add_library(costmap_2d
|
|||
# --- Link các thư viện phụ thuộc ---
|
||||
target_link_libraries(costmap_2d
|
||||
${Boost_LIBRARIES} # Boost
|
||||
std_msgs # ROS msgs
|
||||
std_msgs
|
||||
sensor_msgs
|
||||
geometry_msgs
|
||||
nav_msgs
|
||||
|
|
@ -100,11 +100,6 @@ install(DIRECTORY include/${PROJECT_NAME}/
|
|||
DESTINATION include/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
# # --- Export CMake targets ---
|
||||
# install(EXPORT costmap_2dTargets
|
||||
# DESTINATION lib/cmake/costmap_2d
|
||||
# )
|
||||
|
||||
# --- Plugin libraries ---
|
||||
# Tạo các plugin shared library
|
||||
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(BUILD_COSTMAP_TESTS "Build costmap_2d test executables" OFF)
|
||||
option(BUILD_COSTMAP_TESTS "Build costmap_2d test executables" ON)
|
||||
|
||||
if(BUILD_COSTMAP_TESTS)
|
||||
# --- Test executables ---
|
||||
|
|
|
|||
0
include/costmap_2d/array_parser.h
Normal file → Executable file
0
include/costmap_2d/array_parser.h
Normal file → Executable file
0
include/costmap_2d/cost_values.h
Normal file → Executable file
0
include/costmap_2d/cost_values.h
Normal file → Executable file
311
include/costmap_2d/costmap_2d.h
Normal file → Executable file
311
include/costmap_2d/costmap_2d.h
Normal file → Executable file
|
|
@ -1,103 +1,172 @@
|
|||
#ifndef COSTMAP_2D_COSTMAP_2D_H_ // Bảo vệ tránh include file nhiều lần
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2008, 2013, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* Author: Eitan Marder-Eppstein
|
||||
* David V. Lu!!
|
||||
*********************************************************************/
|
||||
#ifndef COSTMAP_2D_COSTMAP_2D_H_
|
||||
#define COSTMAP_2D_COSTMAP_2D_H_
|
||||
|
||||
#include <vector> // Dùng cho std::vector
|
||||
#include <queue> // Dùng trong các thuật toán quét ô (flood fill, BFS)
|
||||
#include <boost/thread.hpp> // Dùng mutex để đồng bộ truy cập giữa các thread (multi-thread safe)
|
||||
#include <vector>
|
||||
#include <queue>
|
||||
#include <geometry_msgs/Point.h>
|
||||
#include <boost/thread.hpp>
|
||||
|
||||
namespace costmap_2d // Mọi thứ nằm trong namespace costmap_2d
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
||||
// Cấu trúc tiện dụng để lưu trữ cặp tọa độ (x, y) trên bản đồ (dạng ô)
|
||||
// convenient for storing x/y point pairs
|
||||
struct MapLocation
|
||||
{
|
||||
unsigned int x; // Chỉ số hàng (hoặc cột) theo trục X trong costmap
|
||||
unsigned int y; // Chỉ số hàng (hoặc cột) theo trục Y trong costmap
|
||||
unsigned int x;
|
||||
unsigned int y;
|
||||
};
|
||||
|
||||
/**
|
||||
* @class Costmap2D
|
||||
* @brief Lớp này đại diện cho bản đồ chi phí 2D (costmap)
|
||||
*
|
||||
* Nó ánh xạ giữa không gian thế giới (tọa độ thực, mét) và lưới ô (cells),
|
||||
* mỗi ô chứa "cost" – tức là chi phí để robot đi qua ô đó (ví dụ: 0 là free, 255 là obstacle).
|
||||
* @brief A 2D costmap provides a mapping between points in the world and their associated "costs".
|
||||
*/
|
||||
class Costmap2D
|
||||
{
|
||||
friend class CostmapTester; // Cho phép lớp test (gtest) truy cập vào các thành viên private
|
||||
friend class CostmapTester; // Need this for gtest to work correctly
|
||||
public:
|
||||
/**
|
||||
* Constructor khởi tạo bản đồ costmap
|
||||
* @param cells_size_x: số ô theo chiều X
|
||||
* @param cells_size_y: số ô theo chiều Y
|
||||
* @param resolution: kích thước 1 ô (mét / ô)
|
||||
* @param origin_x: gốc bản đồ (tọa độ thế giới) theo trục X
|
||||
* @param origin_y: gốc bản đồ (tọa độ thế giới) theo trục Y
|
||||
* @param default_value: giá trị mặc định của mỗi ô (thường là 0 = free)
|
||||
* @brief Constructor for a costmap
|
||||
* @param cells_size_x The x size of the map in cells
|
||||
* @param cells_size_y The y size of the map in cells
|
||||
* @param resolution The resolution of the map in meters/cell
|
||||
* @param origin_x The x origin of the map
|
||||
* @param origin_y The y origin of the map
|
||||
* @param default_value Default Value
|
||||
*/
|
||||
Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
|
||||
double origin_x, double origin_y, unsigned char default_value = 0);
|
||||
|
||||
/**
|
||||
* Constructor sao chép (copy constructor)
|
||||
* Dùng để tạo bản sao nhanh của một bản đồ costmap khác.
|
||||
* @brief Copy constructor for a costmap, creates a copy efficiently
|
||||
* @param map The costmap to copy
|
||||
*/
|
||||
Costmap2D(const Costmap2D& map);
|
||||
|
||||
/**
|
||||
* Toán tử gán (=) được overload để sao chép dữ liệu từ bản đồ khác.
|
||||
* @brief Overloaded assignment operator
|
||||
* @param map The costmap to copy
|
||||
* @return A reference to the map after the copy has finished
|
||||
*/
|
||||
Costmap2D& operator=(const Costmap2D& map);
|
||||
|
||||
/**
|
||||
* Tạo bản sao từ một vùng (window) của bản đồ khác.
|
||||
* Dùng khi ta chỉ muốn sao chép một phần nhỏ của bản đồ lớn.
|
||||
* @brief Turn this costmap into a copy of a window of a costmap passed in
|
||||
* @param map The costmap to copy
|
||||
* @param win_origin_x The x origin (lower left corner) for the window to copy, in meters
|
||||
* @param win_origin_y The y origin (lower left corner) for the window to copy, in meters
|
||||
* @param win_size_x The x size of the window, in meters
|
||||
* @param win_size_y The y size of the window, in meters
|
||||
*/
|
||||
bool copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y, double win_size_x,
|
||||
double win_size_y);
|
||||
|
||||
/// Constructor mặc định (không khởi tạo kích thước)
|
||||
/**
|
||||
* @brief Default constructor
|
||||
*/
|
||||
Costmap2D();
|
||||
|
||||
/// Destructor
|
||||
/**
|
||||
* @brief Destructor
|
||||
*/
|
||||
virtual ~Costmap2D();
|
||||
|
||||
/**
|
||||
* Trả về giá trị chi phí (cost) của một ô (mx, my)
|
||||
* @brief Get the cost of a cell in the costmap
|
||||
* @param mx The x coordinate of the cell
|
||||
* @param my The y coordinate of the cell
|
||||
* @return The cost of the cell
|
||||
*/
|
||||
unsigned char getCost(unsigned int mx, unsigned int my) const;
|
||||
|
||||
/**
|
||||
* Đặt giá trị cost cho một ô (mx, my)
|
||||
* @brief Set the cost of a cell in the costmap
|
||||
* @param mx The x coordinate of the cell
|
||||
* @param my The y coordinate of the cell
|
||||
* @param cost The cost to set the cell to
|
||||
*/
|
||||
void setCost(unsigned int mx, unsigned int my, unsigned char cost);
|
||||
|
||||
/**
|
||||
* Chuyển đổi tọa độ map (ô) → tọa độ thế giới (mét)
|
||||
* Dùng để vẽ, debug hoặc xác định vị trí thực tế của một ô.
|
||||
* @brief Convert from map coordinates to world coordinates
|
||||
* @param mx The x map coordinate
|
||||
* @param my The y map coordinate
|
||||
* @param wx Will be set to the associated world x coordinate
|
||||
* @param wy Will be set to the associated world y coordinate
|
||||
*/
|
||||
void mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const;
|
||||
|
||||
/**
|
||||
* Chuyển đổi tọa độ thế giới → tọa độ map (ô)
|
||||
* Nếu tọa độ nằm ngoài bản đồ → trả về false
|
||||
* @brief Convert from world coordinates to map coordinates
|
||||
* @param wx The x world coordinate
|
||||
* @param wy The y world coordinate
|
||||
* @param mx Will be set to the associated map x coordinate
|
||||
* @param my Will be set to the associated map y coordinate
|
||||
* @return True if the conversion was successful (legal bounds) false otherwise
|
||||
*/
|
||||
bool worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const;
|
||||
|
||||
/**
|
||||
* Chuyển đổi mà không kiểm tra biên hợp lệ (có thể nằm ngoài bản đồ)
|
||||
* @brief Convert from world coordinates to map coordinates without checking for legal bounds
|
||||
* @param wx The x world coordinate
|
||||
* @param wy The y world coordinate
|
||||
* @param mx Will be set to the associated map x coordinate
|
||||
* @param my Will be set to the associated map y coordinate
|
||||
* @note The returned map coordinates <b>are not guaranteed to lie within the map.</b>
|
||||
*/
|
||||
void worldToMapNoBounds(double wx, double wy, int& mx, int& my) const;
|
||||
|
||||
/**
|
||||
* Chuyển đổi nhưng ép kết quả nằm trong biên của bản đồ (clamped)
|
||||
* @brief Convert from world coordinates to map coordinates, constraining results to legal bounds.
|
||||
* @param wx The x world coordinate
|
||||
* @param wy The y world coordinate
|
||||
* @param mx Will be set to the associated map x coordinate
|
||||
* @param my Will be set to the associated map y coordinate
|
||||
* @note The returned map coordinates are guaranteed to lie within the map.
|
||||
*/
|
||||
void worldToMapEnforceBounds(double wx, double wy, int& mx, int& my) const;
|
||||
|
||||
/**
|
||||
* Tính chỉ số 1D của ô từ cặp tọa độ (mx, my)
|
||||
* Công thức: index = my * size_x_ + mx
|
||||
* @brief Given two map coordinates... compute the associated index
|
||||
* @param mx The x coordinate
|
||||
* @param my The y coordinate
|
||||
* @return The associated index
|
||||
*/
|
||||
inline unsigned int getIndex(unsigned int mx, unsigned int my) const
|
||||
{
|
||||
|
|
@ -105,7 +174,10 @@ public:
|
|||
}
|
||||
|
||||
/**
|
||||
* Chuyển ngược lại: từ chỉ số 1D → cặp tọa độ (mx, my)
|
||||
* @brief Given an index... compute the associated map coordinates
|
||||
* @param index The index
|
||||
* @param mx Will be set to the x coordinate
|
||||
* @param my Will be set to the y coordinate
|
||||
*/
|
||||
inline void indexToCells(unsigned int index, unsigned int& mx, unsigned int& my) const
|
||||
{
|
||||
|
|
@ -114,103 +186,111 @@ public:
|
|||
}
|
||||
|
||||
/**
|
||||
* Trả về con trỏ đến mảng dữ liệu costmap (unsigned char array)
|
||||
* @brief Will return a pointer to the underlying unsigned char array used as the costmap
|
||||
* @return A pointer to the underlying unsigned char array storing cost values
|
||||
*/
|
||||
unsigned char* getCharMap() const;
|
||||
|
||||
/**
|
||||
* Trả về số ô theo trục X
|
||||
* @brief Accessor for the x size of the costmap in cells
|
||||
* @return The x size of the costmap
|
||||
*/
|
||||
unsigned int getSizeInCellsX() const;
|
||||
|
||||
/**
|
||||
* Trả về số ô theo trục Y
|
||||
* @brief Accessor for the y size of the costmap in cells
|
||||
* @return The y size of the costmap
|
||||
*/
|
||||
unsigned int getSizeInCellsY() const;
|
||||
|
||||
/**
|
||||
* Trả về kích thước bản đồ theo mét (theo X)
|
||||
* @brief Accessor for the x size of the costmap in meters
|
||||
* @return The x size of the costmap (returns the centerpoint of the last legal cell in the map)
|
||||
*/
|
||||
double getSizeInMetersX() const;
|
||||
|
||||
/**
|
||||
* Trả về kích thước bản đồ theo mét (theo Y)
|
||||
* @brief Accessor for the y size of the costmap in meters
|
||||
* @return The y size of the costmap (returns the centerpoint of the last legal cell in the map)
|
||||
*/
|
||||
double getSizeInMetersY() const;
|
||||
|
||||
/**
|
||||
* Trả về tọa độ gốc bản đồ theo trục X (tọa độ thế giới)
|
||||
* @brief Accessor for the x origin of the costmap
|
||||
* @return The x origin of the costmap
|
||||
*/
|
||||
double getOriginX() const;
|
||||
|
||||
/**
|
||||
* Trả về tọa độ gốc bản đồ theo trục Y (tọa độ thế giới)
|
||||
* @brief Accessor for the y origin of the costmap
|
||||
* @return The y origin of the costmap
|
||||
*/
|
||||
double getOriginY() const;
|
||||
|
||||
/**
|
||||
* Trả về độ phân giải (mét / ô)
|
||||
* @brief Accessor for the resolution of the costmap
|
||||
* @return The resolution of the costmap
|
||||
*/
|
||||
double getResolution() const;
|
||||
|
||||
/// Đặt giá trị mặc định (cost khi reset map)
|
||||
void setDefaultValue(unsigned char c)
|
||||
{
|
||||
default_value_ = c;
|
||||
}
|
||||
|
||||
/// Trả về giá trị mặc định
|
||||
unsigned char getDefaultValue()
|
||||
{
|
||||
return default_value_;
|
||||
}
|
||||
|
||||
/**
|
||||
* Đặt cost cho một vùng đa giác lồi (convex polygon)
|
||||
* Dùng để đánh dấu vùng chướng ngại vật hoặc vùng forbidden.
|
||||
* @brief Sets the cost of a convex polygon to a desired value
|
||||
* @param polygon The polygon to perform the operation on
|
||||
* @param cost_value The value to set costs to
|
||||
* @return True if the polygon was filled... false if it could not be filled
|
||||
*/
|
||||
bool setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value);
|
||||
|
||||
/**
|
||||
* Lấy danh sách các ô nằm trên viền (outline) của một polygon.
|
||||
* @brief Get the map cells that make up the outline of a polygon
|
||||
* @param polygon The polygon in map coordinates to rasterize
|
||||
* @param polygon_cells Will be set to the cells contained in the outline of the polygon
|
||||
*/
|
||||
void polygonOutlineCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells);
|
||||
|
||||
/**
|
||||
* Lấy danh sách các ô nằm bên trong polygon (điền vùng lồi).
|
||||
* @brief Get the map cells that fill a convex polygon
|
||||
* @param polygon The polygon in map coordinates to rasterize
|
||||
* @param polygon_cells Will be set to the cells that fill the polygon
|
||||
*/
|
||||
void convexFillCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells);
|
||||
|
||||
/**
|
||||
* Cập nhật lại gốc bản đồ (origin) sang vị trí mới,
|
||||
* giữ lại dữ liệu cũ nếu có thể (thường dùng khi robot di chuyển xa).
|
||||
* @brief Move the origin of the costmap to a new location.... keeping data when it can
|
||||
* @param new_origin_x The x coordinate of the new origin
|
||||
* @param new_origin_y The y coordinate of the new origin
|
||||
*/
|
||||
virtual void updateOrigin(double new_origin_x, double new_origin_y);
|
||||
|
||||
/**
|
||||
* Lưu bản đồ ra file ảnh PGM (phục vụ debug hoặc visualization)
|
||||
* @brief Save the costmap out to a pgm file
|
||||
* @param file_name The name of the file to save
|
||||
*/
|
||||
bool saveMap(std::string file_name);
|
||||
|
||||
/**
|
||||
* Thay đổi kích thước bản đồ (resize toàn bộ)
|
||||
*/
|
||||
void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x,
|
||||
double origin_y);
|
||||
|
||||
/**
|
||||
* Reset toàn bộ costmap trong vùng (x0, y0) → (xn, yn)
|
||||
*/
|
||||
void resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn);
|
||||
|
||||
/**
|
||||
* Chuyển khoảng cách theo mét → số ô tương ứng
|
||||
* @brief Given distance in the world... convert it to cells
|
||||
* @param world_dist The world distance
|
||||
* @return The equivalent cell distance
|
||||
*/
|
||||
unsigned int cellDistance(double world_dist);
|
||||
|
||||
/**
|
||||
* Định nghĩa mutex để bảo vệ truy cập đồng thời.
|
||||
*/
|
||||
// Provide a typedef to ease future code maintenance
|
||||
typedef boost::recursive_mutex mutex_t;
|
||||
mutex_t* getMutex()
|
||||
{
|
||||
|
|
@ -219,8 +299,17 @@ public:
|
|||
|
||||
protected:
|
||||
/**
|
||||
* Hàm template copy một vùng dữ liệu từ bản đồ nguồn → bản đồ đích
|
||||
* Dữ liệu được copy bằng memcpy để đảm bảo nhanh.
|
||||
* @brief Copy a region of a source map into a destination map
|
||||
* @param source_map The source map
|
||||
* @param sm_lower_left_x The lower left x point of the source map to start the copy
|
||||
* @param sm_lower_left_y The lower left y point of the source map to start the copy
|
||||
* @param sm_size_x The x size of the source map
|
||||
* @param dest_map The destination map
|
||||
* @param dm_lower_left_x The lower left x point of the destination map to start the copy
|
||||
* @param dm_lower_left_y The lower left y point of the destination map to start the copy
|
||||
* @param dm_size_x The x size of the destination map
|
||||
* @param region_size_x The x size of the region to copy
|
||||
* @param region_size_y The y size of the region to copy
|
||||
*/
|
||||
template<typename data_type>
|
||||
void copyMapRegion(data_type* source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y,
|
||||
|
|
@ -228,60 +317,65 @@ protected:
|
|||
unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x,
|
||||
unsigned int region_size_y)
|
||||
{
|
||||
// Con trỏ bắt đầu tại vị trí góc dưới trái của vùng cần copy trong source
|
||||
// we'll first need to compute the starting points for each map
|
||||
data_type* sm_index = source_map + (sm_lower_left_y * sm_size_x + sm_lower_left_x);
|
||||
// Con trỏ bắt đầu tại vị trí góc dưới trái trong dest
|
||||
data_type* dm_index = dest_map + (dm_lower_left_y * dm_size_x + dm_lower_left_x);
|
||||
|
||||
// Lặp qua từng hàng trong vùng copy
|
||||
// now, we'll copy the source map into the destination map
|
||||
for (unsigned int i = 0; i < region_size_y; ++i)
|
||||
{
|
||||
// Copy nguyên hàng (memcpy nhanh hơn vòng for từng phần tử)
|
||||
memcpy(dm_index, sm_index, region_size_x * sizeof(data_type));
|
||||
sm_index += sm_size_x; // chuyển sang hàng tiếp theo của source
|
||||
dm_index += dm_size_x; // chuyển sang hàng tiếp theo của dest
|
||||
sm_index += sm_size_x;
|
||||
dm_index += dm_size_x;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Xóa toàn bộ dữ liệu bản đồ (free bộ nhớ)
|
||||
* @brief Deletes the costmap, static_map, and markers data structures
|
||||
*/
|
||||
virtual void deleteMaps();
|
||||
|
||||
/**
|
||||
* Reset toàn bộ dữ liệu bản đồ về trạng thái UNKNOWN (NO_INFORMATION)
|
||||
* @brief Resets the costmap and static_map to be unknown space
|
||||
*/
|
||||
virtual void resetMaps();
|
||||
|
||||
/**
|
||||
* Cấp phát và khởi tạo bộ nhớ cho costmap
|
||||
* @brief Initializes the costmap, static_map, and markers data structures
|
||||
* @param size_x The x size to use for map initialization
|
||||
* @param size_y The y size to use for map initialization
|
||||
*/
|
||||
virtual void initMaps(unsigned int size_x, unsigned int size_y);
|
||||
|
||||
/**
|
||||
* Thuật toán raytracing Bresenham — đi từ điểm (x0,y0) → (x1,y1)
|
||||
* Mỗi bước sẽ gọi “ActionType” (một functor) để đánh dấu hoặc xử lý từng ô.
|
||||
* Thường dùng cho sensor update (quét laser).
|
||||
* @brief Raytrace a line and apply some action at each step
|
||||
* @param at The action to take... a functor
|
||||
* @param x0 The starting x coordinate
|
||||
* @param y0 The starting y coordinate
|
||||
* @param x1 The ending x coordinate
|
||||
* @param y1 The ending y coordinate
|
||||
* @param max_length The maximum desired length of the segment... allows you to not go all the way to the endpoint
|
||||
*/
|
||||
template<class ActionType>
|
||||
inline void raytraceLine(ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1,
|
||||
unsigned int max_length = UINT_MAX)
|
||||
{
|
||||
int dx = x1 - x0; // Khoảng cách theo trục X
|
||||
int dy = y1 - y0; // Khoảng cách theo trục Y
|
||||
int dx = x1 - x0;
|
||||
int dy = y1 - y0;
|
||||
|
||||
unsigned int abs_dx = abs(dx);
|
||||
unsigned int abs_dy = abs(dy);
|
||||
|
||||
int offset_dx = sign(dx); // Hướng đi theo X
|
||||
int offset_dy = sign(dy) * size_x_; // Hướng đi theo Y (theo stride của hàng)
|
||||
int offset_dx = sign(dx);
|
||||
int offset_dy = sign(dy) * size_x_;
|
||||
|
||||
unsigned int offset = y0 * size_x_ + x0; // Vị trí bắt đầu trong mảng costmap
|
||||
unsigned int offset = y0 * size_x_ + x0;
|
||||
|
||||
double dist = hypot(dx, dy); // Chiều dài đoạn thẳng
|
||||
// we need to chose how much to scale our dominant dimension, based on the maximum length of the line
|
||||
double dist = hypot(dx, dy);
|
||||
double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist);
|
||||
|
||||
// Xét hướng nào là trục chính (dominant)
|
||||
// if x is dominant
|
||||
if (abs_dx >= abs_dy)
|
||||
{
|
||||
int error_y = abs_dx / 2;
|
||||
|
|
@ -289,13 +383,14 @@ protected:
|
|||
return;
|
||||
}
|
||||
|
||||
// otherwise y is dominant
|
||||
int error_x = abs_dy / 2;
|
||||
bresenham2D(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy));
|
||||
}
|
||||
|
||||
private:
|
||||
/**
|
||||
* Triển khai Bresenham 2D — đi qua từng ô trên đường thẳng và gọi “at(offset)” mỗi bước.
|
||||
* @brief A 2D implementation of Bresenham's raytracing algorithm... applies an action at each step
|
||||
*/
|
||||
template<class ActionType>
|
||||
inline void bresenham2D(ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a,
|
||||
|
|
@ -304,38 +399,33 @@ private:
|
|||
unsigned int end = std::min(max_length, abs_da);
|
||||
for (unsigned int i = 0; i < end; ++i)
|
||||
{
|
||||
at(offset); // Gọi hành động (thường là đánh dấu costmap)
|
||||
offset += offset_a; // Tiến thêm 1 bước theo hướng chính
|
||||
at(offset);
|
||||
offset += offset_a;
|
||||
error_b += abs_db;
|
||||
if ((unsigned int)error_b >= abs_da)
|
||||
{
|
||||
offset += offset_b; // Bước sang hàng tiếp theo khi sai số tích lũy đủ lớn
|
||||
offset += offset_b;
|
||||
error_b -= abs_da;
|
||||
}
|
||||
}
|
||||
at(offset); // Xử lý ô cuối cùng
|
||||
at(offset);
|
||||
}
|
||||
|
||||
/// Trả về dấu của số nguyên (x>0:1, x<0:-1)
|
||||
inline int sign(int x)
|
||||
{
|
||||
return x > 0 ? 1.0 : -1.0;
|
||||
}
|
||||
|
||||
mutex_t* access_; // Con trỏ đến mutex (đồng bộ truy cập costmap giữa các thread)
|
||||
|
||||
mutex_t* access_;
|
||||
protected:
|
||||
unsigned int size_x_; // Số lượng ô theo trục X
|
||||
unsigned int size_y_; // Số lượng ô theo trục Y
|
||||
double resolution_; // Độ phân giải: mét/ô
|
||||
double origin_x_; // Tọa độ gốc bản đồ (theo thế giới) trục X
|
||||
double origin_y_; // Tọa độ gốc bản đồ (theo thế giới) trục Y
|
||||
unsigned char* costmap_; // Mảng 1 chiều chứa giá trị cost của toàn bộ bản đồ
|
||||
unsigned char default_value_; // Giá trị mặc định cho các ô (thường = 0 hoặc 255 nếu unknown)
|
||||
unsigned int size_x_;
|
||||
unsigned int size_y_;
|
||||
double resolution_;
|
||||
double origin_x_;
|
||||
double origin_y_;
|
||||
unsigned char* costmap_;
|
||||
unsigned char default_value_;
|
||||
|
||||
/**
|
||||
* Functor nội bộ: dùng trong raytraceLine để đánh dấu một ô.
|
||||
*/
|
||||
class MarkCell
|
||||
{
|
||||
public:
|
||||
|
|
@ -345,16 +435,13 @@ protected:
|
|||
}
|
||||
inline void operator()(unsigned int offset)
|
||||
{
|
||||
costmap_[offset] = value_; // Gán cost của ô bằng giá trị value_
|
||||
costmap_[offset] = value_;
|
||||
}
|
||||
private:
|
||||
unsigned char* costmap_;
|
||||
unsigned char value_;
|
||||
};
|
||||
|
||||
/**
|
||||
* Functor nội bộ: dùng để thu thập các ô nằm trên viền polygon.
|
||||
*/
|
||||
class PolygonOutlineCells
|
||||
{
|
||||
public:
|
||||
|
|
@ -363,11 +450,12 @@ protected:
|
|||
{
|
||||
}
|
||||
|
||||
// just push the relevant cells back onto the list
|
||||
inline void operator()(unsigned int offset)
|
||||
{
|
||||
MapLocation loc;
|
||||
costmap_.indexToCells(offset, loc.x, loc.y); // Chuyển index → (x, y)
|
||||
cells_.push_back(loc); // Thêm ô này vào danh sách viền polygon
|
||||
costmap_.indexToCells(offset, loc.x, loc.y);
|
||||
cells_.push_back(loc);
|
||||
}
|
||||
|
||||
private:
|
||||
|
|
@ -376,7 +464,6 @@ protected:
|
|||
std::vector<MapLocation>& cells_;
|
||||
};
|
||||
};
|
||||
|
||||
} // namespace costmap_2d
|
||||
|
||||
#endif // COSTMAP_2D_COSTMAP_2D_H_
|
||||
#endif // COSTMAP_2D_COSTMAP_2D_H
|
||||
|
|
|
|||
2
include/costmap_2d/costmap_2d_robot.h
Normal file → Executable file
2
include/costmap_2d/costmap_2d_robot.h
Normal file → Executable file
|
|
@ -253,7 +253,7 @@ private:
|
|||
void readFootprintFromConfig(const std::vector<geometry_msgs::Point> &new_footprint,
|
||||
const std::vector<geometry_msgs::Point> &old_footprint,
|
||||
const double &robot_radius);
|
||||
|
||||
std::vector<std::function<PluginLayerPtr()>> creators_;
|
||||
void checkMovement();
|
||||
void mapUpdateLoop(double frequency);
|
||||
bool map_update_thread_shutdown_;
|
||||
|
|
|
|||
2
include/costmap_2d/costmap_layer.h
Normal file → Executable file
2
include/costmap_2d/costmap_layer.h
Normal file → Executable file
|
|
@ -37,9 +37,9 @@
|
|||
*********************************************************************/
|
||||
#ifndef COSTMAP_2D_COSTMAP_LAYER_H_
|
||||
#define COSTMAP_2D_COSTMAP_LAYER_H_
|
||||
|
||||
#include <costmap_2d/layer.h>
|
||||
#include <costmap_2d/layered_costmap.h>
|
||||
#include <typeinfo>
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
|
|
|||
0
include/costmap_2d/costmap_math.h
Normal file → Executable file
0
include/costmap_2d/costmap_math.h
Normal file → Executable file
52
include/costmap_2d/footprint.h
Normal file → Executable file
52
include/costmap_2d/footprint.h
Normal file → Executable file
|
|
@ -37,17 +37,14 @@
|
|||
*********************************************************************/
|
||||
#ifndef COSTMAP_2D_FOOTPRINT_H
|
||||
#define COSTMAP_2D_FOOTPRINT_H
|
||||
#include <geometry_msgs/Point32.h>
|
||||
|
||||
#include <geometry_msgs/Polygon.h>
|
||||
#include <geometry_msgs/PolygonStamped.h>
|
||||
#include <geometry_msgs/Point.h>
|
||||
#include <geometry_msgs/Point32.h>
|
||||
|
||||
#include <xmlrpcpp/XmlRpcValue.h>
|
||||
|
||||
#include<string.h>
|
||||
#include<vector>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
||||
|
|
@ -62,22 +59,22 @@ void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footpr
|
|||
double& min_dist, double& max_dist);
|
||||
|
||||
/**
|
||||
* @brief Convert geometry_msgs::Point32 to geometry_msgs::Point
|
||||
* @brief Convert Point32 to Point
|
||||
*/
|
||||
geometry_msgs::Point toPoint(geometry_msgs::Point32 pt);
|
||||
|
||||
/**
|
||||
* @brief Convert geometry_msgs::Point to geometry_msgs::Point32
|
||||
* @brief Convert Point to Point32
|
||||
*/
|
||||
geometry_msgs::Point32 toPoint32(geometry_msgs::Point pt);
|
||||
|
||||
/**
|
||||
* @brief Convert vector of Points to geometry_msgs::Polygon msg
|
||||
* @brief Convert vector of Points to Polygon msg
|
||||
*/
|
||||
geometry_msgs::Polygon toPolygon(std::vector<geometry_msgs::Point> pts);
|
||||
|
||||
/**
|
||||
* @brief Convert geometry_msgs::Polygon msg to vector of Points.
|
||||
* @brief Convert Polygon msg to vector of Points.
|
||||
*/
|
||||
std::vector<geometry_msgs::Point> toPointVector(geometry_msgs::Polygon polygon);
|
||||
|
||||
|
|
@ -93,7 +90,7 @@ void transformFootprint(double x, double y, double theta, const std::vector<geom
|
|||
std::vector<geometry_msgs::Point>& oriented_footprint);
|
||||
|
||||
/**
|
||||
* @brief Given a pose and base footprint, build the oriented footprint of the robot (geometry_msgs::PolygonStamped)
|
||||
* @brief Given a pose and base footprint, build the oriented footprint of the robot (PolygonStamped)
|
||||
* @param x The x position of the robot
|
||||
* @param y The y position of the robot
|
||||
* @param theta The orientation of the robot
|
||||
|
|
@ -121,26 +118,11 @@ std::vector<geometry_msgs::Point> makeFootprintFromRadius(double radius);
|
|||
*/
|
||||
bool makeFootprintFromString(const std::string& footprint_string, std::vector<geometry_msgs::Point>& footprint);
|
||||
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////
|
||||
//////////////////CẦN THƯ VIỆN XmlRpcValue////////////////////
|
||||
//////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////
|
||||
//#include <xmlrpcpp/XmlRpcValue.h>
|
||||
// https://github.com/ros/ros_comm/tree/d54e9be3421af71b70fc6b60a3bf916e779b43dc/utilities/xmlrpcpp/src
|
||||
//////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////
|
||||
|
||||
/**
|
||||
* @brief Read the ros-params "footprint" and/or "robot_radius" from
|
||||
* the given NodeHandle using searchParam() to go up the tree.
|
||||
*/
|
||||
std::vector<geometry_msgs::Point> makeFootprintFromParams(const std::string& file_name);
|
||||
// /**
|
||||
// * @brief Read the ros-params "footprint" and/or "robot_radius" from
|
||||
// * the given NodeHandle using searchParam() to go up the tree.
|
||||
// */
|
||||
// std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh);
|
||||
|
||||
/**
|
||||
* @brief Create the footprint from the given XmlRpcValue.
|
||||
|
|
@ -156,10 +138,10 @@ std::vector<geometry_msgs::Point> makeFootprintFromParams(const std::string& fil
|
|||
std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc,
|
||||
const std::string& full_param_name);
|
||||
|
||||
/** @brief Write the current unpadded_footprint_ to the "footprint"
|
||||
* parameter of the given NodeHandle so that dynamic_reconfigure
|
||||
* will see the new value. */
|
||||
void writeFootprintToParam(std::string& nh, const std::vector<geometry_msgs::Point>& footprint);
|
||||
// /** @brief Write the current unpadded_footprint_ to the "footprint"
|
||||
// * parameter of the given NodeHandle so that dynamic_reconfigure
|
||||
// * will see the new value. */
|
||||
// void writeFootprintToParam(ros::NodeHandle& nh, const std::vector<geometry_msgs::Point>& footprint);
|
||||
|
||||
} // end namespace costmap_2d
|
||||
|
||||
|
|
|
|||
48
include/costmap_2d/inflation_layer.h
Normal file → Executable file
48
include/costmap_2d/inflation_layer.h
Normal file → Executable file
|
|
@ -1,13 +1,46 @@
|
|||
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2008, 2013, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* Author: Eitan Marder-Eppstein
|
||||
* David V. Lu!!
|
||||
*********************************************************************/
|
||||
#ifndef COSTMAP_2D_INFLATION_LAYER_H_
|
||||
#define COSTMAP_2D_INFLATION_LAYER_H_
|
||||
|
||||
#include <costmap_2d/layer.h>
|
||||
#include <costmap_2d/layered_costmap.h>
|
||||
#include <boost/thread.hpp>
|
||||
#include <stdexcept>
|
||||
#include <iostream>
|
||||
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
|
@ -43,16 +76,9 @@ public:
|
|||
|
||||
virtual ~InflationLayer()
|
||||
{
|
||||
// ✅ FIX: Delete mutex được tạo bằng new (phải delete trước để tránh lock trong destructor)
|
||||
if (inflation_access_)
|
||||
delete inflation_access_;
|
||||
inflation_access_ = nullptr;
|
||||
|
||||
// Delete kernels và seen array
|
||||
deleteKernels();
|
||||
if (seen_)
|
||||
delete[] seen_;
|
||||
seen_ = nullptr;
|
||||
}
|
||||
|
||||
virtual void onInitialize();
|
||||
|
|
|
|||
8
include/costmap_2d/layer.h
Normal file → Executable file
8
include/costmap_2d/layer.h
Normal file → Executable file
|
|
@ -39,10 +39,10 @@
|
|||
|
||||
#include <costmap_2d/costmap_2d.h>
|
||||
#include <costmap_2d/layered_costmap.h>
|
||||
#include <costmap_2d/utils.h>
|
||||
#include <string>
|
||||
#include <tf3/buffer_core.h>
|
||||
#include <costmap_2d/layered_costmap.h>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
|
@ -135,8 +135,8 @@ public:
|
|||
void dataCallBack(const T& value, const std::string& topic) {
|
||||
handle(value, topic);
|
||||
}
|
||||
|
||||
protected:
|
||||
|
||||
// Hàm template public, dùng để gửi dữ liệu
|
||||
template<typename T>
|
||||
void handle(const T& data, const std::string& topic) {
|
||||
|
|
@ -163,6 +163,8 @@ private:
|
|||
std::vector<geometry_msgs::Point> footprint_spec_;
|
||||
};
|
||||
|
||||
using PluginLayerPtr = std::shared_ptr<Layer>;
|
||||
|
||||
} // namespace costmap_2d
|
||||
|
||||
#endif // COSTMAP_2D_LAYER_H_
|
||||
|
|
|
|||
164
include/costmap_2d/layered_costmap.h
Normal file → Executable file
164
include/costmap_2d/layered_costmap.h
Normal file → Executable file
|
|
@ -1,67 +1,84 @@
|
|||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2008, 2013, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* Author: Eitan Marder-Eppstein
|
||||
* David V. Lu!!
|
||||
*********************************************************************/
|
||||
#ifndef COSTMAP_2D_LAYERED_COSTMAP_H_
|
||||
#define COSTMAP_2D_LAYERED_COSTMAP_H_
|
||||
|
||||
#include <costmap_2d/layer.h> // Lớp cơ sở cho các lớp (layer) con: static layer, obstacle layer, inflation layer...
|
||||
#include <costmap_2d/cost_values.h> // Chứa các hằng số giá trị chi phí (ví dụ: FREE_SPACE, LETHAL_OBSTACLE, NO_INFORMATION)
|
||||
#include <costmap_2d/costmap_2d.h> // Định nghĩa lớp Costmap2D (bản đồ 2D cơ bản)
|
||||
#include <costmap_2d/cost_values.h>
|
||||
#include <costmap_2d/layer.h>
|
||||
#include <costmap_2d/costmap_2d.h>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <boost/dll/import.hpp>
|
||||
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
class Layer;
|
||||
|
||||
class Layer; // Khai báo trước để dùng trong vector plugin
|
||||
using PluginLayerPtr = std::shared_ptr<Layer>;
|
||||
/**
|
||||
* @class LayeredCostmap
|
||||
* @brief Lớp này quản lý nhiều "layer" (tầng bản đồ) khác nhau và kết hợp chúng thành một bản đồ chi phí tổng hợp.
|
||||
* Mỗi layer có thể chứa thông tin khác nhau: chướng ngại vật, vùng lạm phát, vùng cấm, v.v.
|
||||
* @brief Instantiates different layer plugins and aggregates them into one score
|
||||
*/
|
||||
class LayeredCostmap
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Hàm khởi tạo LayeredCostmap.
|
||||
* @param global_frame: Tên frame toàn cục (ví dụ: "map" hoặc "odom").
|
||||
* @param rolling_window: Nếu true, bản đồ sẽ "trượt" theo robot (rolling window).
|
||||
* @param track_unknown: Nếu true, sẽ lưu trữ vùng chưa biết (NO_INFORMATION).
|
||||
* @brief Constructor for a costmap
|
||||
*/
|
||||
LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown);
|
||||
|
||||
/**
|
||||
* @brief Hàm huỷ lớp LayeredCostmap.
|
||||
* @brief Destructor
|
||||
*/
|
||||
~LayeredCostmap();
|
||||
|
||||
/**
|
||||
* @brief Cập nhật bản đồ tổng hợp từ tất cả các layer.
|
||||
* @param robot_x, robot_y, robot_yaw: vị trí và hướng hiện tại của robot.
|
||||
* @note Hàm này gọi updateBounds() và updateCosts() trên từng layer.
|
||||
* @brief Update the underlying costmap with new data.
|
||||
* If you want to update the map outside of the update loop that runs, you can call this.
|
||||
*/
|
||||
void updateMap(double robot_x, double robot_y, double robot_yaw);
|
||||
|
||||
/// @brief Trả về frame toàn cục (ví dụ "map").
|
||||
inline const std::string& getGlobalFrameID() const noexcept
|
||||
{
|
||||
return global_frame_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Thay đổi kích thước bản đồ (số ô, độ phân giải, gốc).
|
||||
* @param size_x, size_y: kích thước theo ô (cells).
|
||||
* @param resolution: độ phân giải (mỗi ô = bao nhiêu mét).
|
||||
* @param origin_x, origin_y: toạ độ gốc của bản đồ.
|
||||
* @param size_locked: nếu true, không cho phép thay đổi kích thước sau này.
|
||||
*/
|
||||
void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y,
|
||||
bool size_locked = false);
|
||||
|
||||
/**
|
||||
* @brief Lấy vùng bản đồ bị thay đổi gần nhất sau lần cập nhật gần nhất.
|
||||
* @param [out] minx, miny, maxx, maxy: toạ độ của vùng được cập nhật.
|
||||
*/
|
||||
void getUpdatedBounds(double& minx, double& miny, double& maxx, double& maxy)
|
||||
{
|
||||
minx = minx_;
|
||||
|
|
@ -70,66 +87,38 @@ public:
|
|||
maxy = maxy_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Kiểm tra xem tất cả các layer hiện tại có dữ liệu mới nhất (current) hay không.
|
||||
* @return true nếu tất cả layer đều cập nhật.
|
||||
*/
|
||||
bool isCurrent();
|
||||
|
||||
/**
|
||||
* @brief Trả về con trỏ đến bản đồ Costmap2D cơ sở (master map).
|
||||
*/
|
||||
Costmap2D* getCostmap()
|
||||
{
|
||||
return &costmap_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Kiểm tra xem bản đồ có phải kiểu "rolling window" không.
|
||||
*/
|
||||
bool isRolling()
|
||||
{
|
||||
return rolling_window_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Kiểm tra xem bản đồ có đang lưu trữ vùng "chưa biết" không (NO_INFORMATION).
|
||||
*/
|
||||
bool isTrackingUnknown()
|
||||
{
|
||||
return costmap_.getDefaultValue() == costmap_2d::NO_INFORMATION;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Trả về danh sách các plugin (layer) đã được nạp.
|
||||
* @return vector các shared_ptr<Layer>.
|
||||
*/
|
||||
std::vector<costmap_2d::PluginLayerPtr>* getPlugins()
|
||||
std::vector<std::shared_ptr<Layer> >* getPlugins()
|
||||
{
|
||||
return &plugins_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Thêm một plugin (layer) mới vào bản đồ.
|
||||
* @param plugin: con trỏ thông minh đến lớp layer.
|
||||
*/
|
||||
void addPlugin(costmap_2d::PluginLayerPtr plugin)
|
||||
void addPlugin(std::shared_ptr<Layer> plugin)
|
||||
{
|
||||
plugins_.push_back(plugin);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Kiểm tra xem bản đồ có bị khoá kích thước không.
|
||||
*/
|
||||
bool isSizeLocked()
|
||||
{
|
||||
return size_locked_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Lấy giới hạn vùng bản đồ được cập nhật trong lần gọi update gần nhất.
|
||||
* @param [out] x0, xn, y0, yn: chỉ số ô (cells) bắt đầu và kết thúc.
|
||||
*/
|
||||
void getBounds(unsigned int* x0, unsigned int* xn, unsigned int* y0, unsigned int* yn)
|
||||
{
|
||||
*x0 = bx0_;
|
||||
|
|
@ -138,56 +127,49 @@ public:
|
|||
*yn = byn_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Kiểm tra xem lớp LayeredCostmap đã được khởi tạo hoàn chỉnh chưa.
|
||||
*/
|
||||
bool isInitialized()
|
||||
{
|
||||
return initialized_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cập nhật footprint (hình dạng chiếm chỗ của robot).
|
||||
* Đồng thời tính lại bán kính bao quanh và nội tiếp.
|
||||
* Gọi hàm onFootprintChanged() của tất cả layer.
|
||||
* @param footprint_spec: vector các điểm (geometry_msgs::geometry_msgs::Point) mô tả đa giác footprint.
|
||||
*/
|
||||
/** @brief Updates the stored footprint, updates the circumscribed
|
||||
* and inscribed radii, and calls onFootprintChanged() in all
|
||||
* layers. */
|
||||
void setFootprint(const std::vector<geometry_msgs::Point>& footprint_spec);
|
||||
|
||||
/**
|
||||
* @brief Trả về footprint hiện tại của robot.
|
||||
*/
|
||||
/** @brief Returns the latest footprint stored with setFootprint(). */
|
||||
const std::vector<geometry_msgs::Point>& getFootprint() { return footprint_; }
|
||||
|
||||
/**
|
||||
* @brief Bán kính đường tròn bao ngoài (circumscribed radius):
|
||||
* bán kính nhỏ nhất của đường tròn chứa toàn bộ robot.
|
||||
*/
|
||||
/** @brief The radius of a circle centered at the origin of the
|
||||
* robot which just surrounds all points on the robot's
|
||||
* footprint.
|
||||
*
|
||||
* This is updated by setFootprint(). */
|
||||
double getCircumscribedRadius() { return circumscribed_radius_; }
|
||||
|
||||
/**
|
||||
* @brief Bán kính đường tròn nội tiếp (inscribed radius):
|
||||
* bán kính lớn nhất của đường tròn nằm hoàn toàn bên trong footprint robot.
|
||||
*/
|
||||
/** @brief The radius of a circle centered at the origin of the
|
||||
* robot which is just within all points and edges of the robot's
|
||||
* footprint.
|
||||
*
|
||||
* This is updated by setFootprint(). */
|
||||
double getInscribedRadius() { return inscribed_radius_; }
|
||||
|
||||
private:
|
||||
Costmap2D costmap_; ///< Bản đồ 2D chính (lưu ma trận chi phí tổng hợp)
|
||||
std::string global_frame_; ///< Frame toàn cục (thường là "map" hoặc "odom")
|
||||
Costmap2D costmap_;
|
||||
std::string global_frame_;
|
||||
|
||||
bool rolling_window_; ///< Nếu true, bản đồ sẽ di chuyển theo robot (bản đồ trượt)
|
||||
bool rolling_window_; /// < @brief Whether or not the costmap should roll with the robot
|
||||
|
||||
bool current_; ///< Cờ đánh dấu tất cả các layer có dữ liệu mới nhất hay không
|
||||
double minx_, miny_, maxx_, maxy_; ///< Giới hạn vùng bản đồ được cập nhật (tọa độ thế giới)
|
||||
unsigned int bx0_, bxn_, by0_, byn_; ///< Giới hạn vùng bản đồ được cập nhật (tọa độ lưới)
|
||||
bool current_;
|
||||
double minx_, miny_, maxx_, maxy_;
|
||||
unsigned int bx0_, bxn_, by0_, byn_;
|
||||
|
||||
std::vector<costmap_2d::PluginLayerPtr> plugins_; ///< Danh sách các layer plugin (ví dụ: StaticLayer, ObstacleLayer, InflationLayer)
|
||||
std::vector<std::shared_ptr<Layer> > plugins_;
|
||||
|
||||
bool initialized_; ///< Đã được khởi tạo hoàn toàn hay chưa
|
||||
bool size_locked_; ///< Có cho phép thay đổi kích thước bản đồ sau này không
|
||||
|
||||
double circumscribed_radius_, inscribed_radius_; ///< Hai bán kính của footprint robot (bao ngoài và nội tiếp)
|
||||
std::vector<geometry_msgs::Point> footprint_; ///< Đa giác mô tả footprint robot
|
||||
bool initialized_;
|
||||
bool size_locked_;
|
||||
double circumscribed_radius_, inscribed_radius_;
|
||||
std::vector<geometry_msgs::Point> footprint_;
|
||||
};
|
||||
|
||||
} // namespace costmap_2d
|
||||
|
|
|
|||
2
include/costmap_2d/observation.h
Normal file → Executable file
2
include/costmap_2d/observation.h
Normal file → Executable file
|
|
@ -32,8 +32,8 @@
|
|||
#ifndef COSTMAP_2D_OBSERVATION_H_
|
||||
#define COSTMAP_2D_OBSERVATION_H_
|
||||
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <geometry_msgs/Point.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
|
|
|||
49
include/costmap_2d/observation_buffer.h
Normal file → Executable file
49
include/costmap_2d/observation_buffer.h
Normal file → Executable file
|
|
@ -1,19 +1,50 @@
|
|||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2008, 2013, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* Author: Eitan Marder-Eppstein
|
||||
*********************************************************************/
|
||||
#ifndef COSTMAP_2D_OBSERVATION_BUFFER_H_
|
||||
#define COSTMAP_2D_OBSERVATION_BUFFER_H_
|
||||
|
||||
#include <vector>
|
||||
#include <list>
|
||||
#include <string>
|
||||
#include <chrono>
|
||||
// #include <ros/time.h>
|
||||
#include <robot/time.h>
|
||||
#include <robot/duration.h>
|
||||
#include <costmap_2d/observation.h>
|
||||
#include <tf3/buffer_core.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <geometry_msgs/PointStamped.h>
|
||||
#include <geometry_msgs/Point.h>
|
||||
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
|
||||
// Thread support
|
||||
#include <boost/thread.hpp>
|
||||
|
|
@ -27,7 +58,6 @@ namespace costmap_2d
|
|||
class ObservationBuffer
|
||||
{
|
||||
public:
|
||||
|
||||
/**
|
||||
* @brief Constructs an observation buffer
|
||||
* @param topic_name The topic of the observations, used as an identifier for error and warning messages
|
||||
|
|
@ -37,7 +67,7 @@ public:
|
|||
* @param max_obstacle_height The minimum height of a hitpoint to be considered legal
|
||||
* @param obstacle_range The range to which the sensor should be trusted for inserting obstacles
|
||||
* @param raytrace_range The range to which the sensor should be trusted for raytracing to clear out space
|
||||
* @param tf3_buffer A reference to a tf3 Buffer
|
||||
* @param tf2_buffer A reference to a tf2 Buffer
|
||||
* @param global_frame The frame to transform PointClouds into
|
||||
* @param sensor_frame The frame of the origin of the sensor, can be left blank to be read from the messages
|
||||
* @param tf_tolerance The amount of time to wait for a transform to be available when setting a new global frame
|
||||
|
|
@ -108,9 +138,6 @@ private:
|
|||
void purgeStaleObservations();
|
||||
|
||||
tf3::BufferCore& tf3_buffer_;
|
||||
// const ros::Duration observation_keep_time_;
|
||||
// const ros::Duration expected_update_rate_;
|
||||
// ros::Time last_updated_;
|
||||
const robot::Duration observation_keep_time_;
|
||||
const robot::Duration expected_update_rate_;
|
||||
robot::Time last_updated_;
|
||||
|
|
|
|||
36
include/costmap_2d/obstacle_layer.h
Normal file → Executable file
36
include/costmap_2d/obstacle_layer.h
Normal file → Executable file
|
|
@ -42,19 +42,15 @@
|
|||
#include <costmap_2d/layered_costmap.h>
|
||||
#include <costmap_2d/observation_buffer.h>
|
||||
#include <costmap_2d/footprint.h>
|
||||
#include <costmap_2d/utils.h>
|
||||
|
||||
#include <nav_msgs/OccupancyGrid.h>
|
||||
|
||||
#include <sensor_msgs/LaserScan.h>
|
||||
#include <laser_geometry/laser_geometry.hpp>
|
||||
#include <sensor_msgs/PointCloud.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/point_cloud_conversion.h>
|
||||
|
||||
#include <laser_geometry/laser_geometry.hpp>
|
||||
// #include <tf3_ros/message_filter.h>
|
||||
// #include <message_filters/subscriber.h>
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
||||
|
|
@ -81,19 +77,41 @@ public:
|
|||
void clearStaticObservations(bool marking, bool clearing);
|
||||
|
||||
protected:
|
||||
|
||||
void handleImpl(const void* data,
|
||||
const std::type_info&,
|
||||
const std::string& topic) override;
|
||||
|
||||
/**
|
||||
* @brief A callback to handle buffering LaserScan messages
|
||||
* @param message The message returned from a message notifier
|
||||
* @param buffer A pointer to the observation buffer to update
|
||||
*/
|
||||
void laserScanCallback(const sensor_msgs::LaserScan& message,
|
||||
const boost::shared_ptr<ObservationBuffer>& buffer);
|
||||
|
||||
/**
|
||||
* @brief A callback to handle buffering LaserScan messages which need filtering to turn Inf values into range_max.
|
||||
* @param message The message returned from a message notifier
|
||||
* @param buffer A pointer to the observation buffer to update
|
||||
*/
|
||||
void laserScanValidInfCallback(const sensor_msgs::LaserScan& message,
|
||||
const boost::shared_ptr<ObservationBuffer>& buffer);
|
||||
|
||||
/**
|
||||
* @brief A callback to handle buffering PointCloud messages
|
||||
* @param message The message returned from a message notifier
|
||||
* @param buffer A pointer to the observation buffer to update
|
||||
*/
|
||||
void pointCloudCallback(const sensor_msgs::PointCloud& message,
|
||||
const boost::shared_ptr<ObservationBuffer>& buffer);
|
||||
const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
|
||||
|
||||
/**
|
||||
* @brief A callback to handle buffering PointCloud2 messages
|
||||
* @param message The message returned from a message notifier
|
||||
* @param buffer A pointer to the observation buffer to update
|
||||
*/
|
||||
void pointCloud2Callback(const sensor_msgs::PointCloud2& message,
|
||||
const boost::shared_ptr<ObservationBuffer>& buffer);
|
||||
// void process(const map_msgs::OccupancyGridUpdate& update);
|
||||
const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
|
||||
|
||||
/**
|
||||
* @brief Get the observations used to mark space
|
||||
|
|
|
|||
54
include/costmap_2d/static_layer.h
Normal file → Executable file
54
include/costmap_2d/static_layer.h
Normal file → Executable file
|
|
@ -1,12 +1,49 @@
|
|||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2008, 2013, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* Author: Eitan Marder-Eppstein
|
||||
* David V. Lu!!
|
||||
*********************************************************************/
|
||||
#ifndef COSTMAP_2D_STATIC_LAYER_H_
|
||||
#define COSTMAP_2D_STATIC_LAYER_H_
|
||||
|
||||
#include <costmap_2d/costmap_layer.h>
|
||||
#include <costmap_2d/layered_costmap.h>
|
||||
|
||||
#include <nav_msgs/OccupancyGrid.h>
|
||||
#include <map_msgs/OccupancyGridUpdate.h>
|
||||
#include <string>
|
||||
#include <geometry_msgs/TransformStamped.h>
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
|
@ -30,10 +67,9 @@ protected:
|
|||
void handleImpl(const void* data,
|
||||
const std::type_info& type,
|
||||
const std::string& topic) override;
|
||||
void incomingMap(const nav_msgs::OccupancyGrid& new_map);
|
||||
void incomingUpdate(const map_msgs::OccupancyGridUpdate& update);
|
||||
|
||||
virtual unsigned char interpretValue(unsigned char value);
|
||||
virtual void incomingMap(const nav_msgs::OccupancyGrid& new_map);
|
||||
virtual void incomingUpdate(const map_msgs::OccupancyGridUpdate& update);
|
||||
unsigned char* threshold_;
|
||||
std::string base_frame_id_;
|
||||
std::string global_frame_; ///< @brief The global frame for the costmap
|
||||
|
|
@ -49,19 +85,11 @@ protected:
|
|||
bool trinary_costmap_;
|
||||
bool map_shutdown_ = false;
|
||||
bool map_update_shutdown_ = false;
|
||||
// ros::Subscriber map_sub_, map_update_sub_;
|
||||
unsigned char lethal_threshold_, unknown_cost_value_;
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Callback to update the costmap's map from the map_server
|
||||
* @param new_map The map to put into the costmap. The origin of the new
|
||||
* map along with its size will determine what parts of the costmap's
|
||||
* static map are overwritten.
|
||||
*/
|
||||
// void reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level);
|
||||
// dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> *dsrv_;
|
||||
bool getParams();
|
||||
|
||||
};
|
||||
|
||||
} // namespace costmap_2d
|
||||
|
|
|
|||
|
|
@ -15,7 +15,7 @@ namespace costmap_2d
|
|||
return default_value;
|
||||
}
|
||||
|
||||
std::string getSourceFile(const std::string& root, const std::string& config_file_name)
|
||||
inline std::string getSourceFile(const std::string& root, const std::string& config_file_name)
|
||||
{
|
||||
std::string path_source = " ";
|
||||
std::string sub_folder = "config";
|
||||
|
|
|
|||
18
include/costmap_2d/voxel_layer.h
Normal file → Executable file
18
include/costmap_2d/voxel_layer.h
Normal file → Executable file
|
|
@ -42,28 +42,23 @@
|
|||
#include <costmap_2d/layered_costmap.h>
|
||||
#include <costmap_2d/observation_buffer.h>
|
||||
#include <costmap_2d/voxel_grid.h>
|
||||
#include <costmap_2d/obstacle_layer.h>
|
||||
#include <costmap_2d/utils.h>
|
||||
|
||||
#include <nav_msgs/OccupancyGrid.h>
|
||||
#include <sensor_msgs/LaserScan.h>
|
||||
// #include <laser_geometry/laser_geometry.h>
|
||||
#include <laser_geometry/laser_geometry.hpp>
|
||||
#include <sensor_msgs/PointCloud.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/point_cloud_conversion.h>
|
||||
// #include <message_filters/subscriber.h>
|
||||
// #include <dynamic_reconfigure/server.h>
|
||||
// #include <costmap_2d/VoxelPluginConfig.h>
|
||||
#include <costmap_2d/obstacle_layer.h>
|
||||
#include <voxel_grid/voxel_grid.h>
|
||||
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
||||
class VoxelLayer : public ObstacleLayer
|
||||
{
|
||||
public:
|
||||
VoxelLayer() : voxel_grid_(0, 0, 0)
|
||||
VoxelLayer() :
|
||||
voxel_grid_(0, 0, 0)
|
||||
{
|
||||
costmap_ = NULL; // this is the unsigned char* member of parent class's parent class Costmap2D.
|
||||
}
|
||||
|
|
@ -79,19 +74,21 @@ public:
|
|||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
virtual void matchSize();
|
||||
virtual void reset();
|
||||
|
||||
|
||||
protected:
|
||||
|
||||
virtual void resetMaps();
|
||||
|
||||
private:
|
||||
bool getParams();
|
||||
void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
|
||||
virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
|
||||
double* max_x, double* max_y);
|
||||
|
||||
|
||||
bool publish_voxel_;
|
||||
voxel_grid::VoxelGrid voxel_grid_;
|
||||
double z_resolution_, origin_z_;
|
||||
|
|
@ -138,7 +135,6 @@ private:
|
|||
{
|
||||
return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0));
|
||||
}
|
||||
bool getParams();
|
||||
};
|
||||
|
||||
} // namespace costmap_2d
|
||||
|
|
|
|||
|
|
@ -36,12 +36,12 @@ void CriticalLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, i
|
|||
}
|
||||
|
||||
// Export factory function
|
||||
static PluginLayerPtr create_critical_plugin() {
|
||||
static std::shared_ptr<Layer> create_critical_plugin() {
|
||||
return std::make_shared<CriticalLayer>();
|
||||
}
|
||||
|
||||
// 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)
|
||||
|
||||
|
||||
}
|
||||
|
|
@ -430,10 +430,10 @@ namespace costmap_2d
|
|||
}
|
||||
|
||||
// Export factory function
|
||||
static PluginLayerPtr create_directional_plugin() {
|
||||
static std::shared_ptr<Layer> create_directional_plugin() {
|
||||
return std::make_shared<DirectionalLayer>();
|
||||
}
|
||||
|
||||
// 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
70
plugins/inflation_layer.cpp
Normal file → Executable 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 <costmap_2d/inflation_layer.h>
|
||||
#include <costmap_2d/costmap_math.h>
|
||||
#include <costmap_2d/footprint.h>
|
||||
#include <costmap_2d/utils.h>
|
||||
#include <boost/thread.hpp>
|
||||
#include <boost/dll/alias.hpp>
|
||||
|
||||
|
||||
using costmap_2d::LETHAL_OBSTACLE;
|
||||
using costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
|
||||
using costmap_2d::NO_INFORMATION;
|
||||
|
|
@ -21,7 +58,6 @@ InflationLayer::InflationLayer()
|
|||
, inflate_unknown_(false)
|
||||
, cell_inflation_radius_(0)
|
||||
, cached_cell_inflation_radius_(0)
|
||||
// , dsrv_(NULL)
|
||||
, seen_(NULL)
|
||||
, cached_costs_(NULL)
|
||||
, cached_distances_(NULL)
|
||||
|
|
@ -44,16 +80,27 @@ void InflationLayer::onInitialize()
|
|||
seen_size_ = 0;
|
||||
need_reinflation_ = false;
|
||||
|
||||
getParams();
|
||||
}
|
||||
|
||||
InflationLayer::matchSize();
|
||||
matchSize();
|
||||
}
|
||||
|
||||
bool InflationLayer::getParams()
|
||||
{
|
||||
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"];
|
||||
double cost_scaling_factor = loadParam(layer, "cost_scaling_factor", 15.0);
|
||||
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;
|
||||
|
||||
// make sure the inflation list is empty at the beginning of the cycle (should always be true)
|
||||
if (!inflation_cells_.empty()) {
|
||||
throw std::runtime_error("The inflation list must be empty at the beginning of inflation");
|
||||
}
|
||||
printf("The inflation list must be empty at the beginning of inflation\n");
|
||||
|
||||
unsigned char* master_array = master_grid.getCharMap();
|
||||
unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
|
||||
|
||||
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_ = new bool[seen_size_];
|
||||
}
|
||||
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_;
|
||||
seen_size_ = size_x * size_y;
|
||||
seen_ = new bool[seen_size_];
|
||||
|
|
@ -357,12 +402,13 @@ void InflationLayer::handleImpl(const void* data,
|
|||
printf("This function is not available!\n");
|
||||
}
|
||||
|
||||
|
||||
// Export factory function
|
||||
static PluginLayerPtr create_inflation_plugin() {
|
||||
static std::shared_ptr<Layer> create_inflation_plugin() {
|
||||
return std::make_shared<InflationLayer>();
|
||||
}
|
||||
|
||||
// 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
|
||||
|
|
|
|||
69
plugins/obstacle_layer.cpp
Normal file → Executable file
69
plugins/obstacle_layer.cpp
Normal file → Executable 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/costmap_math.h>
|
||||
#include <costmap_2d/utils.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>
|
||||
|
||||
|
||||
using costmap_2d::NO_INFORMATION;
|
||||
using costmap_2d::LETHAL_OBSTACLE;
|
||||
using costmap_2d::FREE_SPACE;
|
||||
|
|
@ -27,14 +65,26 @@ void ObstacleLayer::onInitialize()
|
|||
getParams();
|
||||
}
|
||||
|
||||
|
||||
ObstacleLayer::~ObstacleLayer()
|
||||
{}
|
||||
|
||||
bool ObstacleLayer::getParams()
|
||||
{
|
||||
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["obstacle_layer"];
|
||||
|
||||
bool track_unknown_space = loadParam(layer, "track_unknown_space", layered_costmap_->isTrackingUnknown());
|
||||
|
|
@ -121,7 +171,7 @@ void ObstacleLayer::handleImpl(const void* data,
|
|||
if(!stop_receiving_data_)
|
||||
{
|
||||
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") {
|
||||
laserScanCallback(*static_cast<const sensor_msgs::LaserScan*>(data), buffer);
|
||||
} 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
|
||||
sensor_msgs::PointCloud2 cloud;
|
||||
cloud.header = message.header;
|
||||
// printf("TEST PLUGIN OBSTACLE!!!\n");
|
||||
|
||||
// project the scan into a point cloud
|
||||
try
|
||||
|
|
@ -175,7 +224,6 @@ void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScan& message,
|
|||
void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScan& raw_message,
|
||||
const boost::shared_ptr<ObservationBuffer>& buffer)
|
||||
{
|
||||
// printf("TEST PLUGIN OBSTACLE 2!!!\n");
|
||||
// Filter positive infinities ("Inf"s) to max_range.
|
||||
float epsilon = 0.0001; // a tenth of a millimeter
|
||||
sensor_msgs::LaserScan message = raw_message;
|
||||
|
|
@ -218,7 +266,6 @@ void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScan& raw_
|
|||
void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloud& message,
|
||||
const boost::shared_ptr<ObservationBuffer>& buffer)
|
||||
{
|
||||
// printf("TEST PLUGIN OBSTACLE 3!!!\n");
|
||||
sensor_msgs::PointCloud2 cloud2;
|
||||
|
||||
if (!sensor_msgs::convertPointCloudToPointCloud2(message, cloud2))
|
||||
|
|
@ -516,11 +563,11 @@ void ObstacleLayer::reset()
|
|||
}
|
||||
|
||||
// Export factory function
|
||||
static PluginLayerPtr create_obstacle_plugin() {
|
||||
static std::shared_ptr<Layer> create_obstacle_plugin() {
|
||||
return std::make_shared<ObstacleLayer>();
|
||||
}
|
||||
|
||||
// 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
|
||||
|
|
|
|||
|
|
@ -25,11 +25,11 @@ unsigned char PreferredLayer::interpretValue(unsigned char value)
|
|||
}
|
||||
|
||||
// Export factory function
|
||||
static PluginLayerPtr create_preferred_plugin() {
|
||||
static std::shared_ptr<Layer> create_preferred_plugin() {
|
||||
return std::make_shared<PreferredLayer>();
|
||||
}
|
||||
|
||||
// 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
70
plugins/static_layer.cpp
Normal file → Executable 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/costmap_math.h>
|
||||
#include <data_convert/data_convert.h>
|
||||
#include <costmap_2d/utils.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 <boost/dll/alias.hpp>
|
||||
|
||||
|
||||
using costmap_2d::NO_INFORMATION;
|
||||
using costmap_2d::LETHAL_OBSTACLE;
|
||||
using costmap_2d::FREE_SPACE;
|
||||
|
|
@ -137,7 +171,6 @@ void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map)
|
|||
if(!map_shutdown_)
|
||||
{
|
||||
std::cout << "Received new map!" << std::endl;
|
||||
|
||||
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);
|
||||
|
|
@ -188,18 +221,17 @@ void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map)
|
|||
height_ = size_y_;
|
||||
map_received_ = true;
|
||||
has_updated_data_ = true;
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("Stop receive new map!");
|
||||
}
|
||||
|
||||
// shutdown the map subscrber if firt_map_only_ flag is on
|
||||
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_sub_.shutdown();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -302,12 +334,7 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
|
|||
tf3::TransformStampedMsg transformMsg;
|
||||
try
|
||||
{
|
||||
// transformMsg = tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0));
|
||||
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());
|
||||
transformMsg = tf_->lookupTransform(map_frame_, global_frame_, tf3::Time::now());
|
||||
}
|
||||
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
|
||||
tf3::Transform tf3_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 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
|
||||
static PluginLayerPtr create_static_plugin() {
|
||||
static std::shared_ptr<Layer> create_static_plugin() {
|
||||
return std::make_shared<StaticLayer>();
|
||||
}
|
||||
|
||||
// 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
|
||||
|
|
|
|||
|
|
@ -23,11 +23,11 @@ unsigned char UnPreferredLayer::interpretValue(unsigned char value)
|
|||
}
|
||||
|
||||
// Export factory function
|
||||
static PluginLayerPtr create_unpreferred_plugin() {
|
||||
static std::shared_ptr<Layer> create_unpreferred_plugin() {
|
||||
return std::make_shared<UnPreferredLayer>();
|
||||
}
|
||||
|
||||
// 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)
|
||||
|
||||
}
|
||||
49
plugins/voxel_layer.cpp
Normal file → Executable file
49
plugins/voxel_layer.cpp
Normal file → Executable file
|
|
@ -36,10 +36,8 @@
|
|||
* David V. Lu!!
|
||||
*********************************************************************/
|
||||
#include <costmap_2d/voxel_layer.h>
|
||||
#include <costmap_2d/utils.h>
|
||||
|
||||
#include <boost/dll/alias.hpp>
|
||||
#include <sensor_msgs/point_cloud2_iterator.h>
|
||||
#include <boost/dll/alias.hpp>
|
||||
|
||||
#define VOXEL_BITS 16
|
||||
|
||||
|
|
@ -59,10 +57,25 @@ void VoxelLayer::onInitialize()
|
|||
getParams();
|
||||
}
|
||||
|
||||
VoxelLayer::~VoxelLayer()
|
||||
{}
|
||||
|
||||
bool VoxelLayer::getParams()
|
||||
{
|
||||
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"];
|
||||
|
||||
// publish_voxel_ = loadParam(layer, "publish_voxel_map", false);
|
||||
|
|
@ -83,12 +96,8 @@ bool VoxelLayer::getParams()
|
|||
}
|
||||
|
||||
return true;
|
||||
|
||||
}
|
||||
|
||||
VoxelLayer::~VoxelLayer()
|
||||
{}
|
||||
|
||||
void VoxelLayer::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.header.frame_id = global_frame_;
|
||||
// grid_msg.header.stamp = robot::Time::now();
|
||||
|
||||
// ///////////////////////////////////////////
|
||||
// ////////////THAY THẾ PUBLISH NÀY///////////
|
||||
// ///////////////////////////////////////////
|
||||
// // voxel_pub_.publish(grid_msg);
|
||||
// ///////////////////////////////////////////
|
||||
// ///////////////////////////////////////////
|
||||
// ///////////////////////////////////////////
|
||||
// voxel_pub_.publish(grid_msg);
|
||||
// }
|
||||
|
||||
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))
|
||||
{
|
||||
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);
|
||||
return;
|
||||
}
|
||||
|
||||
///////////////////////////////////////////
|
||||
////////////THAY THẾ PUBLISH NÀY///////////
|
||||
///////////////////////////////////////////
|
||||
// bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0);
|
||||
// if (publish_clearing_points)
|
||||
// {
|
||||
clearing_endpoints_.points.clear();
|
||||
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
|
||||
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_);
|
||||
|
||||
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_,
|
||||
unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
|
||||
cell_raytrace_range);
|
||||
|
|
@ -461,11 +458,11 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
|
|||
}
|
||||
|
||||
// Export factory function
|
||||
static PluginLayerPtr create_voxel_plugin() {
|
||||
static std::shared_ptr<Layer> create_voxel_plugin() {
|
||||
return std::make_shared<VoxelLayer>();
|
||||
}
|
||||
|
||||
// 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
|
||||
|
|
|
|||
0
src/array_parser.cpp
Normal file → Executable file
0
src/array_parser.cpp
Normal file → Executable file
375
src/costmap_2d.cpp
Normal file → Executable file
375
src/costmap_2d.cpp
Normal file → Executable 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> // Header định nghĩa lớp Costmap2D
|
||||
#include <cstdio> // Dùng cho thao tác file (fopen, fprintf, ...)
|
||||
#include <costmap_2d/costmap_2d.h>
|
||||
#include <cstdio>
|
||||
|
||||
using namespace std;
|
||||
|
||||
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,
|
||||
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),
|
||||
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_);
|
||||
|
||||
// Gán toàn bộ bản đồ bằng giá trị mặc định (ví dụ: NO_INFORMATION)
|
||||
resetMaps();
|
||||
}
|
||||
|
||||
/*****************************************************
|
||||
* Xóa bộ nhớ costmap_
|
||||
*****************************************************/
|
||||
void Costmap2D::deleteMaps()
|
||||
{
|
||||
boost::unique_lock<mutex_t> lock(*access_); // Giữ mutex để đảm bảo thread-safe
|
||||
delete[] costmap_; // Giải phóng vùng nhớ
|
||||
// clean up data
|
||||
boost::unique_lock<mutex_t> lock(*access_);
|
||||
delete[] costmap_;
|
||||
costmap_ = NULL;
|
||||
}
|
||||
|
||||
/*****************************************************
|
||||
* Cấp phát vùng nhớ cho costmap_
|
||||
*****************************************************/
|
||||
void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y)
|
||||
{
|
||||
boost::unique_lock<mutex_t> lock(*access_);
|
||||
if (costmap_ != nullptr) {
|
||||
delete[] costmap_;
|
||||
}
|
||||
costmap_ = new unsigned char[size_x * size_y]; // Mỗi ô lưu 1 byte (0–255)
|
||||
costmap_ = new unsigned char[size_x * size_y];
|
||||
}
|
||||
|
||||
/*****************************************************
|
||||
* 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,
|
||||
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_y_ = origin_y;
|
||||
|
||||
initMaps(size_x, size_y); // Cấp phát vùng nhớ mới
|
||||
resetMaps(); // Reset toàn bộ dữ liệu
|
||||
initMaps(size_x, size_y);
|
||||
|
||||
// reset our maps to have no information
|
||||
resetMaps();
|
||||
}
|
||||
|
||||
/*****************************************************
|
||||
* Reset toàn bộ bản đồ về giá trị mặc định
|
||||
*****************************************************/
|
||||
void Costmap2D::resetMaps()
|
||||
{
|
||||
boost::unique_lock<mutex_t> lock(*access_);
|
||||
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)
|
||||
{
|
||||
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));
|
||||
}
|
||||
|
||||
/*****************************************************
|
||||
* Tạo bản đồ mới chỉ bao gồm 1 cửa sổ (window) con của map hiện tại
|
||||
*****************************************************/
|
||||
bool Costmap2D::copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y,
|
||||
double win_size_x, 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)
|
||||
{
|
||||
// ROS_ERROR("Cannot convert this costmap into a window of itself");
|
||||
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;
|
||||
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))
|
||||
{
|
||||
// ROS_ERROR("Cannot window a map that the window bounds don't fit inside of");
|
||||
return false;
|
||||
}
|
||||
|
||||
// Tính kích thước vùng con
|
||||
size_x_ = upper_right_x - lower_left_x;
|
||||
size_y_ = upper_right_y - lower_left_y;
|
||||
resolution_ = map.resolution_;
|
||||
origin_x_ = win_origin_x;
|
||||
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
|
||||
copyMapRegion(map.costmap_, lower_left_x, lower_left_y, map.size_x_,
|
||||
costmap_, 0, 0, size_x_, size_x_, size_y_);
|
||||
// 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_, costmap_, 0, 0, size_x_, size_x_, size_y_);
|
||||
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)
|
||||
{
|
||||
// check for self assignement
|
||||
if (this == &map)
|
||||
return *this;
|
||||
|
||||
deleteMaps(); // Xóa map cũ
|
||||
// clean up old data
|
||||
deleteMaps();
|
||||
|
||||
size_x_ = map.size_x_;
|
||||
size_y_ = map.size_y_;
|
||||
|
|
@ -138,85 +149,62 @@ Costmap2D& Costmap2D::operator=(const Costmap2D& map)
|
|||
origin_x_ = map.origin_x_;
|
||||
origin_y_ = map.origin_y_;
|
||||
|
||||
initMaps(size_x_, size_y_); // Tạo map mới
|
||||
memcpy(costmap_, map.costmap_, size_x_ * size_y_ * sizeof(unsigned char)); // Sao chép dữ liệu
|
||||
// initialize our various maps
|
||||
initMaps(size_x_, size_y_);
|
||||
|
||||
// copy the cost map
|
||||
memcpy(costmap_, map.costmap_, size_x_ * size_y_ * sizeof(unsigned char));
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
/*****************************************************
|
||||
* Constructor sao chép
|
||||
*****************************************************/
|
||||
Costmap2D::Costmap2D(const Costmap2D& map) : costmap_(NULL)
|
||||
Costmap2D::Costmap2D(const Costmap2D& map) :
|
||||
costmap_(NULL)
|
||||
{
|
||||
access_ = new mutex_t();
|
||||
*this = map; // Gọi lại toán tử gán
|
||||
*this = map;
|
||||
}
|
||||
|
||||
/*****************************************************
|
||||
* Constructor mặc định (chưa có bản đồ)
|
||||
*****************************************************/
|
||||
// just initialize everything to NULL by default
|
||||
Costmap2D::Costmap2D() :
|
||||
size_x_(0), size_y_(0), resolution_(0.0),
|
||||
origin_x_(0.0), origin_y_(0.0), costmap_(NULL)
|
||||
size_x_(0), size_y_(0), resolution_(0.0), origin_x_(0.0), origin_y_(0.0), costmap_(NULL)
|
||||
{
|
||||
access_ = new mutex_t();
|
||||
}
|
||||
|
||||
/*****************************************************
|
||||
* Destructor: giải phóng bộ nhớ
|
||||
*****************************************************/
|
||||
Costmap2D::~Costmap2D()
|
||||
{
|
||||
deleteMaps();
|
||||
delete access_;
|
||||
}
|
||||
|
||||
/*****************************************************
|
||||
* Chuyển khoảng cách thực (mét) sang đơn vị ô
|
||||
*****************************************************/
|
||||
unsigned int Costmap2D::cellDistance(double world_dist)
|
||||
{
|
||||
double cells_dist = max(0.0, ceil(world_dist / resolution_));
|
||||
return (unsigned int)cells_dist;
|
||||
}
|
||||
|
||||
/*****************************************************
|
||||
* Trả về con trỏ dữ liệu costmap
|
||||
*****************************************************/
|
||||
unsigned char* Costmap2D::getCharMap() const
|
||||
{
|
||||
return costmap_;
|
||||
}
|
||||
|
||||
/*****************************************************
|
||||
* Lấy giá trị cost tại ô (mx, my)
|
||||
*****************************************************/
|
||||
unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const
|
||||
{
|
||||
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)
|
||||
{
|
||||
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
|
||||
{
|
||||
wx = origin_x_ + (mx + 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
|
||||
{
|
||||
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_);
|
||||
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
|
||||
{
|
||||
mx = (int)((wx - origin_x_) / 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
|
||||
{
|
||||
// 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_)
|
||||
{
|
||||
mx = 0;
|
||||
}
|
||||
else if (wx >= resolution_ * size_x_ + origin_x_)
|
||||
{
|
||||
mx = size_x_ - 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
mx = (int)((wx - origin_x_) / resolution_);
|
||||
}
|
||||
|
||||
if (wy < origin_y_)
|
||||
{
|
||||
my = 0;
|
||||
}
|
||||
else if (wy >= resolution_ * size_y_ + origin_y_)
|
||||
{
|
||||
my = size_y_ - 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
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)
|
||||
{
|
||||
int cell_ox = int((new_origin_x - origin_x_) / resolution_);
|
||||
int cell_oy = int((new_origin_y - origin_y_) / resolution_);
|
||||
// project the new origin into the grid
|
||||
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)
|
||||
return;
|
||||
|
||||
// Cập nhật lại gốc bản đồ mới (theo bội số của resolution)
|
||||
double new_grid_ox = origin_x_ + cell_ox * resolution_;
|
||||
double new_grid_oy = origin_y_ + cell_oy * resolution_;
|
||||
// compute the associated world coordinates for the origin cell
|
||||
// because we want to keep things grid-aligned
|
||||
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_y = size_y_;
|
||||
|
||||
// Tính vùng chồng lấn giữa bản đồ cũ và bản đồ mới
|
||||
int lower_left_x = min(max(cell_ox, 0), size_x);
|
||||
int lower_left_y = min(max(cell_oy, 0), size_y);
|
||||
int upper_right_x = min(max(cell_ox + size_x, 0), size_x);
|
||||
int upper_right_y = min(max(cell_oy + size_y, 0), size_y);
|
||||
// we need to compute the overlap of the new and existing windows
|
||||
int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
|
||||
lower_left_x = min(max(cell_ox, 0), size_x);
|
||||
lower_left_y = min(max(cell_oy, 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_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];
|
||||
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_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_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)
|
||||
{
|
||||
// we assume the polygon is given in the global_frame... we need to transform it to map coordinates
|
||||
std::vector<MapLocation> map_polygon;
|
||||
for (unsigned int i = 0; i < polygon.size(); ++i)
|
||||
{
|
||||
MapLocation loc;
|
||||
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);
|
||||
}
|
||||
|
||||
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)
|
||||
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;
|
||||
}
|
||||
|
||||
/*****************************************************
|
||||
* 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)
|
||||
void Costmap2D::polygonOutlineCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells)
|
||||
{
|
||||
PolygonOutlineCells cell_gatherer(*this, costmap_, polygon_cells);
|
||||
|
||||
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);
|
||||
|
||||
}
|
||||
if (!polygon.empty())
|
||||
{
|
||||
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
|
||||
raytraceLine(cell_gatherer, polygon[last_index].x, polygon[last_index].y,
|
||||
polygon[0].x, polygon[0].y);
|
||||
// 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, polygon[0].x, polygon[0].y);
|
||||
}
|
||||
}
|
||||
|
||||
/*****************************************************
|
||||
* 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)
|
||||
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;
|
||||
|
||||
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;
|
||||
unsigned int i = 0;
|
||||
while (i < polygon_cells.size() - 1)
|
||||
|
|
@ -371,20 +375,25 @@ void Costmap2D::convexFillCells(const std::vector<MapLocation>& polygon,
|
|||
swap = polygon_cells[i];
|
||||
polygon_cells[i] = polygon_cells[i + 1];
|
||||
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;
|
||||
MapLocation min_pt, max_pt;
|
||||
MapLocation min_pt;
|
||||
MapLocation max_pt;
|
||||
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)
|
||||
{
|
||||
if (i >= polygon_cells.size() - 1) break;
|
||||
if (i >= polygon_cells.size() - 1)
|
||||
break;
|
||||
|
||||
if (polygon_cells[i].y < polygon_cells[i + 1].y)
|
||||
{
|
||||
|
|
@ -407,37 +416,69 @@ void Costmap2D::convexFillCells(const std::vector<MapLocation>& polygon,
|
|||
++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)
|
||||
polygon_cells.push_back({x, y});
|
||||
{
|
||||
pt.x = x;
|
||||
pt.y = y;
|
||||
polygon_cells.push_back(pt);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*****************************************************
|
||||
* Các hàm getter cơ bản
|
||||
*****************************************************/
|
||||
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_; }
|
||||
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_; }
|
||||
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_;
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
FILE *fp = fopen(file_name.c_str(), "w");
|
||||
|
||||
if (!fp)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
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 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");
|
||||
}
|
||||
fclose(fp);
|
||||
|
|
|
|||
|
|
@ -48,6 +48,8 @@
|
|||
#include <tf3/utils.h>
|
||||
#include <exception>
|
||||
|
||||
#include <boost/dll/import.hpp>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace costmap_2d
|
||||
|
|
@ -97,19 +99,19 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
|
|||
robot::Time last_error = robot::Time::now();
|
||||
std::string tf_error;
|
||||
|
||||
// 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))
|
||||
{
|
||||
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",
|
||||
robot::Time::now().toSec(), robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
|
||||
last_error = robot::Time::now();
|
||||
}
|
||||
// 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.
|
||||
tf_error.clear();
|
||||
}
|
||||
// // 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))
|
||||
// {
|
||||
// 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",
|
||||
// robot::Time::now().toSec(), robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
|
||||
// last_error = robot::Time::now();
|
||||
// }
|
||||
// // 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.
|
||||
// tf_error.clear();
|
||||
// }
|
||||
|
||||
// check if we want a rolling window version of the 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; };
|
||||
std::vector<PluginInfo> plugins_to_load = {
|
||||
{"./costmap_2d/libstatic_layer.so", "static_layer"},
|
||||
{"./costmap_2d/libinflation_layer.so", "inflation_layer"},
|
||||
{"./costmap_2d/libobstacle_layer.so", "obstacle_layer"},
|
||||
{"./costmap_2d/libpreferred_layer.so", "preferred_layer"}
|
||||
{"./costmap_2d/libstatic_layer.so", "create_static_layer"},
|
||||
{"./costmap_2d/libinflation_layer.so", "create_inflation_layer"},
|
||||
{"./costmap_2d/libobstacle_layer.so", "create_obstacle_layer"},
|
||||
{"./costmap_2d/libpreferred_layer.so", "create_preferred_layer"}
|
||||
};
|
||||
|
||||
// if (private_nh.hasParam("plugins"))
|
||||
|
|
@ -140,10 +142,11 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
|
|||
try
|
||||
{
|
||||
// copyParentParameters(pname, type, private_nh);
|
||||
auto creator = boost::dll::import_alias<PluginLayerPtr()>(
|
||||
info.path, "create_plugin", boost::dll::load_mode::append_decorations
|
||||
creators_.push_back(
|
||||
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;
|
||||
plugin->initialize(layered_costmap_, info.name, &tf_);
|
||||
layered_costmap_->addPlugin(plugin);
|
||||
|
|
|
|||
0
src/costmap_layer.cpp
Normal file → Executable file
0
src/costmap_layer.cpp
Normal file → Executable file
0
src/costmap_math.cpp
Normal file → Executable file
0
src/costmap_math.cpp
Normal file → Executable file
137
src/footprint.cpp
Normal file → Executable file
137
src/footprint.cpp
Normal file → Executable file
|
|
@ -27,12 +27,13 @@
|
|||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <costmap_2d/costmap_math.h>
|
||||
#include<costmap_2d/costmap_math.h>
|
||||
#include <boost/tokenizer.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/algorithm/string.hpp>
|
||||
#include <costmap_2d/footprint.h>
|
||||
#include <costmap_2d/array_parser.h>
|
||||
#include<geometry_msgs/Point32.h>
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
|
@ -173,15 +174,15 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
|
|||
|
||||
if (error != "")
|
||||
{
|
||||
std::printf("Error parsing footprint parameter: '%s'", error.c_str());
|
||||
std::printf(" Footprint string was '%s'.", footprint_string.c_str());
|
||||
printf("Error parsing footprint parameter: '%s'\n", error.c_str());
|
||||
printf(" Footprint string was '%s'.\n", footprint_string.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
// convert vvf into points.
|
||||
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;
|
||||
}
|
||||
footprint.reserve(vvf.size());
|
||||
|
|
@ -197,7 +198,7 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
|
|||
}
|
||||
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()));
|
||||
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::string full_param_name;
|
||||
std::string full_radius_param_name;
|
||||
std::vector<geometry_msgs::Point> points;
|
||||
// std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh)
|
||||
// {
|
||||
// std::string full_param_name;
|
||||
// std::string full_radius_param_name;
|
||||
// std::vector<geometry_msgs::Point> points;
|
||||
|
||||
// if (nh.searchParam("footprint", full_param_name))
|
||||
// {
|
||||
// XmlRpc::XmlRpcValue footprint_xmlrpc;
|
||||
// nh.getParam(full_param_name, footprint_xmlrpc);
|
||||
// if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString &&
|
||||
// footprint_xmlrpc != "" && footprint_xmlrpc != "[]")
|
||||
// {
|
||||
// if (makeFootprintFromString(std::string(footprint_xmlrpc), points))
|
||||
// {
|
||||
// writeFootprintToParam(nh, points);
|
||||
// return points;
|
||||
// }
|
||||
// }
|
||||
// else if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
|
||||
// {
|
||||
// points = makeFootprintFromXMLRPC(footprint_xmlrpc, full_param_name);
|
||||
// writeFootprintToParam(nh, points);
|
||||
// return points;
|
||||
// }
|
||||
// }
|
||||
// if (nh.searchParam("footprint", full_param_name))
|
||||
// {
|
||||
// XmlRpc::XmlRpcValue footprint_xmlrpc;
|
||||
// nh.getParam(full_param_name, footprint_xmlrpc);
|
||||
// if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString &&
|
||||
// footprint_xmlrpc != "" && footprint_xmlrpc != "[]")
|
||||
// {
|
||||
// if (makeFootprintFromString(std::string(footprint_xmlrpc), points))
|
||||
// {
|
||||
// writeFootprintToParam(nh, points);
|
||||
// return points;
|
||||
// }
|
||||
// }
|
||||
// else if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
|
||||
// {
|
||||
// points = makeFootprintFromXMLRPC(footprint_xmlrpc, full_param_name);
|
||||
// writeFootprintToParam(nh, points);
|
||||
// return points;
|
||||
// }
|
||||
// }
|
||||
|
||||
// if (nh.searchParam("robot_radius", full_radius_param_name))
|
||||
// {
|
||||
// double robot_radius;
|
||||
// nh.param(full_radius_param_name, robot_radius, 1.234);
|
||||
// points = makeFootprintFromRadius(robot_radius);
|
||||
// nh.setParam("robot_radius", robot_radius);
|
||||
// }
|
||||
// // Else neither param was found anywhere this knows about, so
|
||||
// // defaults will come from dynamic_reconfigure stuff, set in
|
||||
// // cfg/Costmap2D.cfg and read in this file in reconfigureCB().
|
||||
// return points;
|
||||
}
|
||||
// if (nh.searchParam("robot_radius", full_radius_param_name))
|
||||
// {
|
||||
// double robot_radius;
|
||||
// nh.param(full_radius_param_name, robot_radius, 1.234);
|
||||
// points = makeFootprintFromRadius(robot_radius);
|
||||
// nh.setParam("robot_radius", robot_radius);
|
||||
// }
|
||||
// // Else neither param was found anywhere this knows about, so
|
||||
// // defaults will come from dynamic_reconfigure stuff, set in
|
||||
// // cfg/Costmap2D.cfg and read in this file in reconfigureCB().
|
||||
// return points;
|
||||
// }
|
||||
|
||||
void writeFootprintToParam(std::string& nh, const std::vector<geometry_msgs::Point>& footprint)
|
||||
{
|
||||
// std::ostringstream oss;
|
||||
// bool first = true;
|
||||
// for (unsigned int i = 0; i < footprint.size(); i++)
|
||||
// {
|
||||
// geometry_msgs::Point p = footprint[ i ];
|
||||
// if (first)
|
||||
// {
|
||||
// oss << "[[" << p.x << "," << p.y << "]";
|
||||
// first = false;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// oss << ",[" << p.x << "," << p.y << "]";
|
||||
// }
|
||||
// }
|
||||
// oss << "]";
|
||||
// nh.setParam("footprint", oss.str().c_str());
|
||||
}
|
||||
// void writeFootprintToParam(ros::NodeHandle& nh, const std::vector<geometry_msgs::Point>& footprint)
|
||||
// {
|
||||
// std::ostringstream oss;
|
||||
// bool first = true;
|
||||
// for (unsigned int i = 0; i < footprint.size(); i++)
|
||||
// {
|
||||
// geometry_msgs::Point p = footprint[ i ];
|
||||
// if (first)
|
||||
// {
|
||||
// oss << "[[" << p.x << "," << p.y << "]";
|
||||
// first = false;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// oss << ",[" << p.x << "," << p.y << "]";
|
||||
// }
|
||||
// }
|
||||
// oss << "]";
|
||||
// nh.setParam("footprint", oss.str().c_str());
|
||||
// }
|
||||
|
||||
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)
|
||||
{
|
||||
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());
|
||||
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);
|
||||
}
|
||||
|
|
@ -290,10 +291,10 @@ std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& f
|
|||
if (footprint_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray ||
|
||||
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());
|
||||
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;
|
||||
|
|
@ -306,11 +307,11 @@ std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& f
|
|||
if (point.getType() != XmlRpc::XmlRpcValue::TypeArray ||
|
||||
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",
|
||||
full_param_name.c_str());
|
||||
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);
|
||||
|
|
|
|||
31
src/layer.cpp
Normal file → Executable file
31
src/layer.cpp
Normal file → Executable 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"
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
||||
|
||||
|
||||
Layer::Layer()
|
||||
: layered_costmap_(NULL)
|
||||
, current_(false)
|
||||
|
|
|
|||
54
src/layered_costmap.cpp
Normal file → Executable file
54
src/layered_costmap.cpp
Normal file → Executable 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/footprint.h>
|
||||
#include <cstdio>
|
||||
#include <string>
|
||||
#include <algorithm>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
using std::vector;
|
||||
|
||||
|
|
@ -49,7 +85,7 @@ namespace costmap_2d
|
|||
boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
|
||||
size_locked_ = size_locked;
|
||||
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)->matchSize();
|
||||
|
|
@ -76,7 +112,7 @@ namespace costmap_2d
|
|||
minx_ = miny_ = 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)
|
||||
{
|
||||
if (!(*plugin)->isEnabled())
|
||||
|
|
@ -88,8 +124,8 @@ namespace costmap_2d
|
|||
(*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)
|
||||
{
|
||||
std::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",
|
||||
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\n",
|
||||
prev_minx, prev_miny, prev_maxx, prev_maxy,
|
||||
minx_, miny_, maxx_, maxy_,
|
||||
(*plugin)->getName().c_str());
|
||||
|
|
@ -105,14 +141,14 @@ namespace costmap_2d
|
|||
y0 = std::max(0, y0);
|
||||
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)
|
||||
return;
|
||||
|
||||
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)
|
||||
{
|
||||
if ((*plugin)->isEnabled())
|
||||
|
|
@ -130,7 +166,7 @@ namespace costmap_2d
|
|||
bool LayeredCostmap::isCurrent()
|
||||
{
|
||||
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)
|
||||
{
|
||||
if ((*plugin)->isEnabled())
|
||||
|
|
@ -144,7 +180,7 @@ namespace costmap_2d
|
|||
footprint_ = footprint_spec;
|
||||
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)->onFootprintChanged();
|
||||
|
|
|
|||
106
src/observation_buffer.cpp
Normal file → Executable file
106
src/observation_buffer.cpp
Normal file → Executable 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 <data_convert/data_convert.h>
|
||||
|
||||
#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
|
||||
#include <tf3_sensor_msgs/tf3_sensor_msgs.h>
|
||||
#include<tf3/convert.h>
|
||||
#include <sensor_msgs/point_cloud2_iterator.h>
|
||||
#include <cstdint>
|
||||
|
||||
|
||||
using namespace std;
|
||||
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,
|
||||
string sensor_frame, double tf_tolerance) :
|
||||
tf3_buffer_(tf3_buffer), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
|
||||
last_updated_(robot::Time::now()),
|
||||
global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
|
||||
last_updated_(robot::Time::now()), global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
|
||||
min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height),
|
||||
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)
|
||||
{
|
||||
robot::Time transform_time = robot::Time::now();
|
||||
|
||||
tf3::Time transform_time = tf3::Time::now();
|
||||
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(),
|
||||
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;
|
||||
origin.header.frame_id = global_frame_;
|
||||
origin.header.stamp = transform_time;
|
||||
origin.header.stamp = data_convert::convertTime(transform_time);
|
||||
origin.point = obs.origin_;
|
||||
|
||||
// we need to transform the origin of the observation to the new global frame
|
||||
tf3::doTransform(origin, origin,
|
||||
tf3_buffer_.lookupTransform(new_global_frame,
|
||||
origin.header.frame_id,
|
||||
data_convert::convertTime(origin.header.stamp)));
|
||||
// tf3_buffer_.transform(origin, origin, new_global_frame);
|
||||
tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform(
|
||||
new_global_frame, // frame đích
|
||||
origin.header.frame_id, // frame nguồn
|
||||
transform_time
|
||||
);
|
||||
tf3::doTransform(origin, origin, tfm_1);
|
||||
obs.origin_ = origin.point;
|
||||
|
||||
// we also need to transform the cloud of the observation to the new global frame
|
||||
tf3::doTransform(*(obs.cloud_), *(obs.cloud_),
|
||||
tf3_buffer_.lookupTransform(new_global_frame,
|
||||
obs.cloud_->header.frame_id,
|
||||
data_convert::convertTime(obs.cloud_->header.stamp)));
|
||||
// tf3_buffer_.transform(*(obs.cloud_), *(obs.cloud_), new_global_frame);
|
||||
tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform(
|
||||
new_global_frame, // frame đích
|
||||
obs.cloud_->header.frame_id, // frame nguồn
|
||||
transform_time
|
||||
);
|
||||
tf3::doTransform(*(obs.cloud_), *(obs.cloud_), tfm_2);
|
||||
}
|
||||
catch (TransformException& ex)
|
||||
{
|
||||
|
|
@ -98,11 +136,19 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
|
|||
local_origin.point.x = 0;
|
||||
local_origin.point.y = 0;
|
||||
local_origin.point.z = 0;
|
||||
tf3::doTransform(local_origin, global_origin,
|
||||
tf3_buffer_.lookupTransform(global_frame_,
|
||||
local_origin.header.frame_id,
|
||||
data_convert::convertTime(local_origin.header.stamp)));
|
||||
// tf3_buffer_.transform(local_origin, global_origin, global_frame_);
|
||||
tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform(
|
||||
global_frame_, // frame đích
|
||||
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_);
|
||||
/////////////////////////////////////////////////
|
||||
/////////////////////////////////////////////////
|
||||
|
||||
// make sure to pass on the raytrace/obstacle range of the observation buffer to the observations
|
||||
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;
|
||||
|
||||
// transform the point cloud
|
||||
tf3::doTransform(cloud, global_frame_cloud,
|
||||
tf3_buffer_.lookupTransform(global_frame_,
|
||||
(cloud.header.frame_id),
|
||||
data_convert::convertTime(cloud.header.stamp)));
|
||||
// tf3_buffer_.transform(cloud, global_frame_cloud, global_frame_);
|
||||
tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform(
|
||||
global_frame_, // frame đích
|
||||
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;
|
||||
|
||||
// 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))
|
||||
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)
|
||||
{
|
||||
printf(
|
||||
"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(),
|
||||
expected_update_rate_.toSec());
|
||||
topic_name_.c_str(), (robot::Time::now() - last_updated_).toSec(), expected_update_rate_.toSec());
|
||||
}
|
||||
return current;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user