diff --git a/CMakeLists.txt b/CMakeLists.txt index 897bbbb..616f63f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 --- diff --git a/include/costmap_2d/array_parser.h b/include/costmap_2d/array_parser.h old mode 100644 new mode 100755 diff --git a/include/costmap_2d/cost_values.h b/include/costmap_2d/cost_values.h old mode 100644 new mode 100755 diff --git a/include/costmap_2d/costmap_2d.h b/include/costmap_2d/costmap_2d.h old mode 100644 new mode 100755 index 83ec6ac..38658e8 --- a/include/costmap_2d/costmap_2d.h +++ b/include/costmap_2d/costmap_2d.h @@ -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 // Dùng cho std::vector -#include // Dùng trong các thuật toán quét ô (flood fill, BFS) -#include // Dùng mutex để đồng bộ truy cập giữa các thread (multi-thread safe) +#include +#include #include +#include -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 are not guaranteed to lie within the map. */ 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& 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& polygon, std::vector& 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& polygon, std::vector& 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 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 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 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& cells_; }; }; - } // namespace costmap_2d -#endif // COSTMAP_2D_COSTMAP_2D_H_ +#endif // COSTMAP_2D_COSTMAP_2D_H diff --git a/include/costmap_2d/costmap_2d_robot.h b/include/costmap_2d/costmap_2d_robot.h old mode 100644 new mode 100755 index 704f4da..6b2477e --- a/include/costmap_2d/costmap_2d_robot.h +++ b/include/costmap_2d/costmap_2d_robot.h @@ -253,7 +253,7 @@ private: void readFootprintFromConfig(const std::vector &new_footprint, const std::vector &old_footprint, const double &robot_radius); - + std::vector> creators_; void checkMovement(); void mapUpdateLoop(double frequency); bool map_update_thread_shutdown_; diff --git a/include/costmap_2d/costmap_layer.h b/include/costmap_2d/costmap_layer.h old mode 100644 new mode 100755 index 5e6f4d6..f3c19e2 --- a/include/costmap_2d/costmap_layer.h +++ b/include/costmap_2d/costmap_layer.h @@ -37,9 +37,9 @@ *********************************************************************/ #ifndef COSTMAP_2D_COSTMAP_LAYER_H_ #define COSTMAP_2D_COSTMAP_LAYER_H_ + #include #include -#include namespace costmap_2d { diff --git a/include/costmap_2d/costmap_math.h b/include/costmap_2d/costmap_math.h old mode 100644 new mode 100755 diff --git a/include/costmap_2d/footprint.h b/include/costmap_2d/footprint.h old mode 100644 new mode 100755 index 9547cec..297fef1 --- a/include/costmap_2d/footprint.h +++ b/include/costmap_2d/footprint.h @@ -37,17 +37,14 @@ *********************************************************************/ #ifndef COSTMAP_2D_FOOTPRINT_H #define COSTMAP_2D_FOOTPRINT_H -#include + #include #include +#include +#include #include -#include -#include - -using namespace std; - namespace costmap_2d { @@ -62,22 +59,22 @@ void calculateMinAndMaxDistances(const std::vector& 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 pts); /** - * @brief Convert geometry_msgs::Polygon msg to vector of Points. + * @brief Convert Polygon msg to vector of Points. */ std::vector toPointVector(geometry_msgs::Polygon polygon); @@ -93,7 +90,7 @@ void transformFootprint(double x, double y, double theta, const std::vector& 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 makeFootprintFromRadius(double radius); */ bool makeFootprintFromString(const std::string& footprint_string, std::vector& footprint); - - -////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////// -//////////////////CẦN THƯ VIỆN XmlRpcValue//////////////////// -////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////// -//#include -// 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 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 makeFootprintFromParams(ros::NodeHandle& nh); /** * @brief Create the footprint from the given XmlRpcValue. @@ -156,10 +138,10 @@ std::vector makeFootprintFromParams(const std::string& fil std::vector 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& 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& footprint); } // end namespace costmap_2d diff --git a/include/costmap_2d/inflation_layer.h b/include/costmap_2d/inflation_layer.h old mode 100644 new mode 100755 index 2b49135..44cdabb --- a/include/costmap_2d/inflation_layer.h +++ b/include/costmap_2d/inflation_layer.h @@ -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 #include #include -#include -#include - namespace costmap_2d { @@ -43,16 +76,9 @@ public: virtual ~InflationLayer() { - // ✅ FIX: Delete mutex được tạo bằng new (phải delete trước để tránh lock trong destructor) - if (inflation_access_) - delete inflation_access_; - inflation_access_ = nullptr; - - // Delete kernels và seen array deleteKernels(); if (seen_) delete[] seen_; - seen_ = nullptr; } virtual void onInitialize(); @@ -106,8 +132,8 @@ protected: private: void handleImpl(const void* data, - const std::type_info& info, - const std::string& source) override; + const std::type_info& info, + const std::string& source) override; /** * @brief Lookup pre-computed distances * @param mx The x coordinate of the current cell diff --git a/include/costmap_2d/layer.h b/include/costmap_2d/layer.h old mode 100644 new mode 100755 index d023fde..0c4313f --- a/include/costmap_2d/layer.h +++ b/include/costmap_2d/layer.h @@ -39,10 +39,10 @@ #include #include +#include #include #include -#include -#include + 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 void handle(const T& data, const std::string& topic) { @@ -163,6 +163,8 @@ private: std::vector footprint_spec_; }; +using PluginLayerPtr = std::shared_ptr; + } // namespace costmap_2d #endif // COSTMAP_2D_LAYER_H_ diff --git a/include/costmap_2d/layered_costmap.h b/include/costmap_2d/layered_costmap.h old mode 100644 new mode 100755 index f04b50d..59c49b9 --- a/include/costmap_2d/layered_costmap.h +++ b/include/costmap_2d/layered_costmap.h @@ -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 // Lớp cơ sở cho các lớp (layer) con: static layer, obstacle layer, inflation layer... -#include // Chứa các hằng số giá trị chi phí (ví dụ: FREE_SPACE, LETHAL_OBSTACLE, NO_INFORMATION) -#include // Định nghĩa lớp Costmap2D (bản đồ 2D cơ bản) +#include +#include +#include #include #include -#include - namespace costmap_2d { +class Layer; -class Layer; // Khai báo trước để dùng trong vector plugin -using PluginLayerPtr = std::shared_ptr; /** * @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. - */ - std::vector* getPlugins() + std::vector >* 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 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& footprint_spec); - /** - * @brief Trả về footprint hiện tại của robot. - */ + /** @brief Returns the latest footprint stored with setFootprint(). */ const std::vector& 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 plugins_; ///< Danh sách các layer plugin (ví dụ: StaticLayer, ObstacleLayer, InflationLayer) + std::vector > 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 footprint_; ///< Đa giác mô tả footprint robot + bool initialized_; + bool size_locked_; + double circumscribed_radius_, inscribed_radius_; + std::vector footprint_; }; } // namespace costmap_2d diff --git a/include/costmap_2d/observation.h b/include/costmap_2d/observation.h old mode 100644 new mode 100755 index e6aee65..695892b --- a/include/costmap_2d/observation.h +++ b/include/costmap_2d/observation.h @@ -32,8 +32,8 @@ #ifndef COSTMAP_2D_OBSERVATION_H_ #define COSTMAP_2D_OBSERVATION_H_ -#include #include +#include namespace costmap_2d { diff --git a/include/costmap_2d/observation_buffer.h b/include/costmap_2d/observation_buffer.h old mode 100644 new mode 100755 index 7238409..5c6441d --- a/include/costmap_2d/observation_buffer.h +++ b/include/costmap_2d/observation_buffer.h @@ -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 #include #include -#include -// #include #include -#include #include #include -#include -#include -#include +#include // Thread support #include @@ -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_; diff --git a/include/costmap_2d/obstacle_layer.h b/include/costmap_2d/obstacle_layer.h old mode 100644 new mode 100755 index 666621a..4665f4f --- a/include/costmap_2d/obstacle_layer.h +++ b/include/costmap_2d/obstacle_layer.h @@ -42,19 +42,15 @@ #include #include #include -#include #include #include +#include #include #include #include -#include -// #include -// #include - namespace costmap_2d { @@ -81,19 +77,41 @@ public: void clearStaticObservations(bool marking, bool clearing); protected: - void handleImpl(const void* data, const std::type_info&, const std::string& topic) override; - void laserScanCallback(const sensor_msgs::LaserScan& message, - const boost::shared_ptr& buffer); - void laserScanValidInfCallback(const sensor_msgs::LaserScan& message, + + /** + * @brief A callback to handle buffering LaserScan messages + * @param message The message returned from a message notifier + * @param buffer A pointer to the observation buffer to update + */ + void laserScanCallback(const sensor_msgs::LaserScan& message, + const boost::shared_ptr& 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& 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& buffer); + const boost::shared_ptr& 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& buffer); - // void process(const map_msgs::OccupancyGridUpdate& update); + const boost::shared_ptr& buffer); /** * @brief Get the observations used to mark space diff --git a/include/costmap_2d/static_layer.h b/include/costmap_2d/static_layer.h old mode 100644 new mode 100755 index c70be56..4174dbc --- a/include/costmap_2d/static_layer.h +++ b/include/costmap_2d/static_layer.h @@ -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 #include + #include #include #include -#include 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 *dsrv_; bool getParams(); + }; } // namespace costmap_2d diff --git a/include/costmap_2d/utils.h b/include/costmap_2d/utils.h index 51fa0bd..ee5a852 100644 --- a/include/costmap_2d/utils.h +++ b/include/costmap_2d/utils.h @@ -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"; diff --git a/include/costmap_2d/voxel_layer.h b/include/costmap_2d/voxel_layer.h old mode 100644 new mode 100755 index 21b6a83..7e5de7b --- a/include/costmap_2d/voxel_layer.h +++ b/include/costmap_2d/voxel_layer.h @@ -42,28 +42,23 @@ #include #include #include -#include -#include - #include #include -// #include +#include #include #include #include -// #include -// #include -// #include +#include #include - 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 diff --git a/plugins/critical_layer.cpp b/plugins/critical_layer.cpp index 005d646..aacfddc 100644 --- a/plugins/critical_layer.cpp +++ b/plugins/critical_layer.cpp @@ -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 create_critical_plugin() { return std::make_shared(); } // 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) } \ No newline at end of file diff --git a/plugins/directional_layer.cpp b/plugins/directional_layer.cpp index 1c5a534..d22dc65 100644 --- a/plugins/directional_layer.cpp +++ b/plugins/directional_layer.cpp @@ -430,10 +430,10 @@ namespace costmap_2d } // Export factory function - static PluginLayerPtr create_directional_plugin() { + static std::shared_ptr create_directional_plugin() { return std::make_shared(); } // 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) } \ No newline at end of file diff --git a/plugins/inflation_layer.cpp b/plugins/inflation_layer.cpp old mode 100644 new mode 100755 index c83320d..f9b514a --- a/plugins/inflation_layer.cpp +++ b/plugins/inflation_layer.cpp @@ -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 #include #include #include -#include #include #include + 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" < create_inflation_plugin() { return std::make_shared(); } // 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 diff --git a/plugins/obstacle_layer.cpp b/plugins/obstacle_layer.cpp old mode 100644 new mode 100755 index b88704f..8e1a1d9 --- a/plugins/obstacle_layer.cpp +++ b/plugins/obstacle_layer.cpp @@ -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 #include -#include + #include -#include -#include + +#include #include + using costmap_2d::NO_INFORMATION; using costmap_2d::LETHAL_OBSTACLE; using costmap_2d::FREE_SPACE; @@ -27,91 +65,103 @@ void ObstacleLayer::onInitialize() getParams(); } - ObstacleLayer::~ObstacleLayer() {} bool ObstacleLayer::getParams() { - try { - YAML::Node config = YAML::LoadFile("/home/duongtd/robotics_core/costmap_2d/config/config.yaml"); - YAML::Node layer = config["obstacle_layer"]; - - bool track_unknown_space = loadParam(layer, "track_unknown_space", layered_costmap_->isTrackingUnknown()); - if (track_unknown_space) - default_value_ = NO_INFORMATION; - else - default_value_ = FREE_SPACE; - - double transform_tolerance = loadParam(layer,"transform_tolerance", 0.2); - // get the topics that we'll subscribe to from the parameter server - std::string topics_string = loadParam(layer,"observation_sources", std::string("")); - printf(" Subscribed to Topics: %s\n", topics_string.c_str()); - - // get the parameters for the specific topic - double observation_keep_time = 0, expected_update_rate = 0, min_obstacle_height = 0, max_obstacle_height = 2; - std::string topic = "map", sensor_frame = "laser_frame", data_type = "PointCloud"; - bool inf_is_valid = false, clearing=false, marking=true; - topic = loadParam(layer,"topic", topic); - sensor_frame = loadParam(layer,"sensor_frame", std::string("")); - observation_keep_time = loadParam(layer,"observation_persistence", 0.0); - expected_update_rate = loadParam(layer,"expected_update_rate", 0.0); - data_type = loadParam(layer,"data_type", std::string("PointCloud")); - min_obstacle_height = loadParam(layer,"min_obstacle_height", 0.0); - max_obstacle_height = loadParam(layer,"max_obstacle_height", 2.0); - inf_is_valid = loadParam(layer,"inf_is_valid", false); - clearing = loadParam(layer,"clearing", false); - marking = loadParam(layer,"marking", true); - if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan")) + try { + 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 != " ") { - printf("Only topics that use point clouds or laser scans are currently supported\n"); - throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported"); + std::cout << "Path source: " << path_source << std::endl; + } + else + { + std::cout << "/cfg folder not found!" << std::endl; } - std::string raytrace_range_param_name, obstacle_range_param_name; + + YAML::Node config = YAML::LoadFile(path_source); + YAML::Node layer = config["obstacle_layer"]; - double obstacle_range = 2.5; - obstacle_range = loadParam(layer,"obstacle_range", obstacle_range); - double raytrace_range = 3.0; - raytrace_range = loadParam(layer,"raytrace_range", raytrace_range); + bool track_unknown_space = loadParam(layer, "track_unknown_space", layered_costmap_->isTrackingUnknown()); + if (track_unknown_space) + default_value_ = NO_INFORMATION; + else + default_value_ = FREE_SPACE; + double transform_tolerance = loadParam(layer,"transform_tolerance", 0.2); + // get the topics that we'll subscribe to from the parameter server + std::string topics_string = loadParam(layer,"observation_sources", std::string("")); + printf(" Subscribed to Topics: %s\n", topics_string.c_str()); - bool footprint_clearing_enabled = loadParam(layer, "footprint_clearing_enabled", true); - int combination_method = loadParam(layer, "combination_method", 1); - - // enabled_ = enabled; - footprint_clearing_enabled_ = footprint_clearing_enabled; - max_obstacle_height_ = max_obstacle_height; - combination_method_ = combination_method; - - printf("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(), - sensor_frame.c_str()); - - // create an observation buffer - observation_buffers_.push_back( - boost::shared_ptr < ObservationBuffer - > (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height, - max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_, - sensor_frame, transform_tolerance))); - if (marking) - marking_buffers_.push_back(observation_buffers_.back()); - - // check if we'll also add this buffer to our clearing observation buffers - if (clearing) - clearing_buffers_.push_back(observation_buffers_.back()); - - printf( - "Created an observation buffer for topic %s, global frame: %s, " - "expected update rate: %.2f, observation persistence: %.2f\n", - topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time); - - } - catch (const YAML::BadFile& e) { - std::cerr << "Cannot open YAML file: " << e.what() << std::endl; - return false; + // get the parameters for the specific topic + double observation_keep_time = 0, expected_update_rate = 0, min_obstacle_height = 0, max_obstacle_height = 2; + std::string topic = "map", sensor_frame = "laser_frame", data_type = "PointCloud"; + bool inf_is_valid = false, clearing=false, marking=true; + topic = loadParam(layer,"topic", topic); + sensor_frame = loadParam(layer,"sensor_frame", std::string("")); + observation_keep_time = loadParam(layer,"observation_persistence", 0.0); + expected_update_rate = loadParam(layer,"expected_update_rate", 0.0); + data_type = loadParam(layer,"data_type", std::string("PointCloud")); + min_obstacle_height = loadParam(layer,"min_obstacle_height", 0.0); + max_obstacle_height = loadParam(layer,"max_obstacle_height", 2.0); + inf_is_valid = loadParam(layer,"inf_is_valid", false); + clearing = loadParam(layer,"clearing", false); + marking = loadParam(layer,"marking", true); + if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan")) + { + printf("Only topics that use point clouds or laser scans are currently supported\n"); + throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported"); } - return true; + std::string raytrace_range_param_name, obstacle_range_param_name; + + double obstacle_range = 2.5; + obstacle_range = loadParam(layer,"obstacle_range", obstacle_range); + double raytrace_range = 3.0; + raytrace_range = loadParam(layer,"raytrace_range", raytrace_range); + + + bool footprint_clearing_enabled = loadParam(layer, "footprint_clearing_enabled", true); + int combination_method = loadParam(layer, "combination_method", 1); + + // enabled_ = enabled; + footprint_clearing_enabled_ = footprint_clearing_enabled; + max_obstacle_height_ = max_obstacle_height; + combination_method_ = combination_method; + + printf("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(), + sensor_frame.c_str()); + + // create an observation buffer + observation_buffers_.push_back( + boost::shared_ptr < ObservationBuffer + > (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height, + max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_, + sensor_frame, transform_tolerance))); + if (marking) + marking_buffers_.push_back(observation_buffers_.back()); + + // check if we'll also add this buffer to our clearing observation buffers + if (clearing) + clearing_buffers_.push_back(observation_buffers_.back()); + + printf( + "Created an observation buffer for topic %s, global frame: %s, " + "expected update rate: %.2f, observation persistence: %.2f\n", + topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time); + + } + catch (const YAML::BadFile& e) { + std::cerr << "Cannot open YAML file: " << e.what() << std::endl; + return false; + } + + return true; } void ObstacleLayer::handleImpl(const void* data, @@ -121,7 +171,7 @@ void ObstacleLayer::handleImpl(const void* data, if(!stop_receiving_data_) { if(observation_buffers_.empty()) return; - boost::shared_ptr buffer = observation_buffers_.back(); + boost::shared_ptr buffer = observation_buffers_.back(); if (type == typeid(sensor_msgs::LaserScan) && topic == "laser") { laserScanCallback(*static_cast(data), buffer); } else if (type == typeid(sensor_msgs::LaserScan) && topic == "laser_valid") { @@ -141,13 +191,12 @@ void ObstacleLayer::handleImpl(const void* data, } } -void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScan& message, +void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScan& message, const boost::shared_ptr& buffer) { // 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 @@ -172,10 +221,9 @@ void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScan& message, buffer->unlock(); } -void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScan& raw_message, +void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScan& raw_message, const boost::shared_ptr& 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; @@ -216,9 +264,8 @@ void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScan& raw_ } void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloud& message, - const boost::shared_ptr& buffer) + const boost::shared_ptr& 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 create_obstacle_plugin() { return std::make_shared(); } // 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 diff --git a/plugins/preferred_layer.cpp b/plugins/preferred_layer.cpp index 19bfb79..3592c09 100644 --- a/plugins/preferred_layer.cpp +++ b/plugins/preferred_layer.cpp @@ -25,11 +25,11 @@ unsigned char PreferredLayer::interpretValue(unsigned char value) } // Export factory function -static PluginLayerPtr create_preferred_plugin() { +static std::shared_ptr create_preferred_plugin() { return std::make_shared(); } // 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) } \ No newline at end of file diff --git a/plugins/static_layer.cpp b/plugins/static_layer.cpp old mode 100644 new mode 100755 index 7826c62..47d95dd --- a/plugins/static_layer.cpp +++ b/plugins/static_layer.cpp @@ -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 #include -#include -#include + #include -#include -#include -#include -#include -#include #include +#include + + 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 create_static_plugin() { return std::make_shared(); } // 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 diff --git a/plugins/unpreferred_layer.cpp b/plugins/unpreferred_layer.cpp index 1cb861a..327f359 100644 --- a/plugins/unpreferred_layer.cpp +++ b/plugins/unpreferred_layer.cpp @@ -23,11 +23,11 @@ unsigned char UnPreferredLayer::interpretValue(unsigned char value) } // Export factory function -static PluginLayerPtr create_unpreferred_plugin() { +static std::shared_ptr create_unpreferred_plugin() { return std::make_shared(); } // 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) } \ No newline at end of file diff --git a/plugins/voxel_layer.cpp b/plugins/voxel_layer.cpp old mode 100644 new mode 100755 index 932ab7f..01f11eb --- a/plugins/voxel_layer.cpp +++ b/plugins/voxel_layer.cpp @@ -36,10 +36,8 @@ * David V. Lu!! *********************************************************************/ #include -#include - -#include #include +#include #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); @@ -399,7 +396,7 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub clearing_endpoints_.header.stamp = clearing_observation.cloud_->header.stamp; clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq; - // clearing_endpoints_pub_.publish(clearing_endpoints_); + // clearing_endpoints_pub_.publish(clearing_endpoints_); // } } @@ -461,11 +458,11 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y) } // Export factory function -static PluginLayerPtr create_voxel_plugin() { +static std::shared_ptr create_voxel_plugin() { return std::make_shared(); } // 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 diff --git a/src/array_parser.cpp b/src/array_parser.cpp old mode 100644 new mode 100755 diff --git a/src/costmap_2d.cpp b/src/costmap_2d.cpp old mode 100644 new mode 100755 index e94a043..3d50327 --- a/src/costmap_2d.cpp +++ b/src/costmap_2d.cpp @@ -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 // Header định nghĩa lớp Costmap2D -#include // Dùng cho thao tác file (fopen, fprintf, ...) +#include +#include 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 lock(*access_); // Giữ mutex để đảm bảo thread-safe - delete[] costmap_; // Giải phóng vùng nhớ + // clean up data + boost::unique_lock 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 lock(*access_); - if (costmap_ != nullptr) { - delete[] costmap_; - } - costmap_ = new unsigned char[size_x * size_y]; // Mỗi ô lưu 1 byte (0–255) + delete[] costmap_; + 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 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 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& 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 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 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& polygon, - std::vector& polygon_cells) +void Costmap2D::polygonOutlineCells(const std::vector& polygon, std::vector& 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& polygon, - std::vector& polygon_cells) +void Costmap2D::convexFillCells(const std::vector& polygon, std::vector& 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& 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& 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); diff --git a/src/costmap_2d_robot.cpp b/src/costmap_2d_robot.cpp index 45d81c4..9ca51ea 100644 --- a/src/costmap_2d_robot.cpp +++ b/src/costmap_2d_robot.cpp @@ -48,6 +48,8 @@ #include #include +#include + 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 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( - info.path, "create_plugin", boost::dll::load_mode::append_decorations + creators_.push_back( + boost::dll::import_alias( + 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); diff --git a/src/costmap_layer.cpp b/src/costmap_layer.cpp old mode 100644 new mode 100755 diff --git a/src/costmap_math.cpp b/src/costmap_math.cpp old mode 100644 new mode 100755 diff --git a/src/footprint.cpp b/src/footprint.cpp old mode 100644 new mode 100755 index de2c0be..9029803 --- a/src/footprint.cpp +++ b/src/footprint.cpp @@ -27,12 +27,13 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include +#include #include #include #include #include #include +#include namespace costmap_2d { @@ -173,15 +174,15 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector makeFootprintFromParams(const std::string& file_name) -{ - std::string full_param_name; - std::string full_radius_param_name; - std::vector points; +// std::vector makeFootprintFromParams(ros::NodeHandle& nh) +// { +// std::string full_param_name; +// std::string full_radius_param_name; +// std::vector 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& 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& 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 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 footprint; @@ -306,11 +307,11 @@ std::vector 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); diff --git a/src/layer.cpp b/src/layer.cpp old mode 100644 new mode 100755 index 93a7cc7..a320e6d --- a/src/layer.cpp +++ b/src/layer.cpp @@ -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) diff --git a/src/layered_costmap.cpp b/src/layered_costmap.cpp old mode 100644 new mode 100755 index a94d727..6e26057 --- a/src/layered_costmap.cpp +++ b/src/layered_costmap.cpp @@ -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 #include #include #include #include #include -#include using std::vector; @@ -49,7 +85,7 @@ namespace costmap_2d boost::unique_lock lock(*(costmap_.getMutex())); size_locked_ = size_locked; costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y); - for (vector::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + for (vector>::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::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + for (vector>::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::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + for (vector>::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::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + for (vector>::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::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { (*plugin)->onFootprintChanged(); diff --git a/src/observation_buffer.cpp b/src/observation_buffer.cpp old mode 100644 new mode 100755 index c338826..7452fff --- a/src/observation_buffer.cpp +++ b/src/observation_buffer.cpp @@ -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 -#include + #include #include -#include #include -#include - 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; }