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

first commit

Browse Source
This commit is contained in:
duongtd
2025-10-28 14:44:05 +07:00
parent 5d4d1943fe
commit 5d8981eab6
30 changed files with 792 additions and 218 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
include/costmap_2d/costmap_2d.h
View File

@@ -4,7 +4,7 @@
#include <vector> // Dùng cho std::vector
#include <queue> // Dùng trong các thuật toán quét ô (flood fill, BFS)
#include <boost/thread.hpp> // Dùng mutex để đồng bộ truy cập giữa các thread (multi-thread safe)
#include <costmap_2d/msg.h>
#include <geometry_msgs/Point.h>
namespace costmap_2d // Mọi thứ nằm trong namespace costmap_2d
{
@@ -169,7 +169,7 @@ public:
* Đặt cost cho một vùng đa giác lồi (convex polygon)
* Dùng để đánh dấu vùng chướng ngại vật hoặc vùng forbidden.
*/
bool setConvexPolygonCost(const std::vector<Point>& polygon, unsigned char cost_value);
bool setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value);
/**
* Lấy danh sách các ô nằm trên viền (outline) của một polygon.

6
include/costmap_2d/costmap_math.h
View File

@@ -41,7 +41,7 @@
#include <math.h>
#include <algorithm>
#include <vector>
#include<costmap_2d/msg.h>
#include <geometry_msgs/Point.h>
/** @brief Return -1 if x < 0, +1 otherwise. */
inline double sign(double x)
@@ -62,8 +62,8 @@ inline double distance(double x0, double y0, double x1, double y1)
double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1);
bool intersects(std::vector<costmap_2d::Point>& polygon, float testx, float testy);
bool intersects(std::vector<geometry_msgs::Point>& polygon, float testx, float testy);
bool intersects(std::vector<costmap_2d::Point>& polygon1, std::vector<costmap_2d::Point>& polygon2);
bool intersects(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometry_msgs::Point>& polygon2);
#endif // COSTMAP_2D_COSTMAP_MATH_H_

98
include/costmap_2d/footprint.h
View File

@@ -37,8 +37,11 @@
*********************************************************************/
#ifndef COSTMAP_2D_FOOTPRINT_H
#define COSTMAP_2D_FOOTPRINT_H
#include <geometry_msgs/Point32.h>
#include <geometry_msgs/Polygon.h>
#include <geometry_msgs/PolygonStamped.h>
#include<costmap_2d/msg.h>
// #include<costmap_2d/msg.h>
#include<string.h>
#include<vector>
@@ -54,28 +57,28 @@ namespace costmap_2d
* @param min_dist Output parameter of the minimum distance
* @param max_dist Output parameter of the maximum distance
*/
void calculateMinAndMaxDistances(const std::vector<Point>& footprint,
void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footprint,
double& min_dist, double& max_dist);
/**
* @brief Convert Point32 to Point
* @brief Convert geometry_msgs::Point32 to geometry_msgs::Point
*/
Point toPoint(Point32 pt);
geometry_msgs::Point toPoint(geometry_msgs::Point32 pt);
/**
* @brief Convert Point to Point32
* @brief Convert geometry_msgs::Point to geometry_msgs::Point32
*/
Point32 toPoint32(Point pt);
geometry_msgs::Point32 toPoint32(geometry_msgs::Point pt);
/**
* @brief Convert vector of Points to Polygon msg
* @brief Convert vector of Points to geometry_msgs::Polygon msg
*/
Polygon toPolygon(std::vector<Point> pts);
geometry_msgs::Polygon toPolygon(std::vector<geometry_msgs::Point> pts);
/**
* @brief Convert Polygon msg to vector of Points.
* @brief Convert geometry_msgs::Polygon msg to vector of Points.
*/
std::vector<Point> toPointVector(Polygon polygon);
std::vector<geometry_msgs::Point> toPointVector(geometry_msgs::Polygon polygon);
/**
* @brief Given a pose and base footprint, build the oriented footprint of the robot (list of Points)
@@ -85,29 +88,29 @@ std::vector<Point> toPointVector(Polygon polygon);
* @param footprint_spec Basic shape of the footprint
* @param oriented_footprint Will be filled with the points in the oriented footprint of the robot
*/
void transformFootprint(double x, double y, double theta, const std::vector<Point>& footprint_spec,
std::vector<Point>& oriented_footprint);
void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec,
std::vector<geometry_msgs::Point>& oriented_footprint);
/**
* @brief Given a pose and base footprint, build the oriented footprint of the robot (PolygonStamped)
* @brief Given a pose and base footprint, build the oriented footprint of the robot (geometry_msgs::PolygonStamped)
* @param x The x position of the robot
* @param y The y position of the robot
* @param theta The orientation of the robot
* @param footprint_spec Basic shape of the footprint
* @param oriented_footprint Will be filled with the points in the oriented footprint of the robot
*/
void transformFootprint(double x, double y, double theta, const std::vector<Point>& footprint_spec,
PolygonStamped & oriented_footprint);
void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec,
geometry_msgs::PolygonStamped & oriented_footprint);
/**
* @brief Adds the specified amount of padding to the footprint (in place)
*/
void padFootprint(std::vector<Point>& footprint, double padding);
void padFootprint(std::vector<geometry_msgs::Point>& footprint, double padding);
/**
* @brief Create a circular footprint from a given radius
*/
std::vector<Point> makeFootprintFromRadius(double radius);
std::vector<geometry_msgs::Point> makeFootprintFromRadius(double radius);
/**
* @brief Make the footprint from the given string.
@@ -115,32 +118,47 @@ std::vector<Point> makeFootprintFromRadius(double radius);
* Format should be bracketed array of arrays of floats, like so: [[1.0, 2.2], [3.3, 4.2], ...]
*
*/
bool makeFootprintFromString(const std::string& footprint_string, std::vector<Point>& footprint);
bool makeFootprintFromString(const std::string& footprint_string, std::vector<geometry_msgs::Point>& footprint);
/**
* @brief Read the ros-params "footprint" and/or "robot_radius" from
* the given NodeHandle using searchParam() to go up the tree.
*/
std::vector<Point> makeFootprintFromParams(std::string& nh);
/**
* @brief Create the footprint from the given XmlRpcValue.
*
* @param footprint_xmlrpc should be an array of arrays, where the
* top-level array should have 3 or more elements, and the
* sub-arrays should all have exactly 2 elements (x and y
* coordinates).
*
* @param full_param_name this is the full name of the rosparam from
* which the footprint_xmlrpc value came. It is used only for
* reporting errors. */
std::vector<Point> makeFootprintFromXMLRPC(XmlRpcValue& footprint_xmlrpc,
const std::string& full_param_name);
/** @brief Write the current unpadded_footprint_ to the "footprint"
* parameter of the given NodeHandle so that dynamic_reconfigure
* will see the new value. */
void writeFootprintToParam(std::string& nh, const std::vector<Point>& footprint);
//////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////
//////////////////CẦN THƯ VIỆN XmlRpcValue////////////////////
//////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////
//#include <xmlrpcpp/XmlRpcValue.h>
// https://github.com/ros/ros_comm/tree/d54e9be3421af71b70fc6b60a3bf916e779b43dc/utilities/xmlrpcpp/src
//////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////
// /**
// * @brief Read the ros-params "footprint" and/or "robot_radius" from
// * the given NodeHandle using searchParam() to go up the tree.
// */
// std::vector<geometry_msgs::Point> makeFootprintFromParams(std::string& nh);
// /**
// * @brief Create the footprint from the given XmlRpcValue.
// *
// * @param footprint_xmlrpc should be an array of arrays, where the
// * top-level array should have 3 or more elements, and the
// * sub-arrays should all have exactly 2 elements (x and y
// * coordinates).
// *
// * @param full_param_name this is the full name of the rosparam from
// * which the footprint_xmlrpc value came. It is used only for
// * reporting errors. */
// std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpcValue& footprint_xmlrpc,
// const std::string& full_param_name);
// /** @brief Write the current unpadded_footprint_ to the "footprint"
// * parameter of the given NodeHandle so that dynamic_reconfigure
// * will see the new value. */
// void writeFootprintToParam(std::string& nh, const std::vector<geometry_msgs::Point>& footprint);
} // end namespace costmap_2d

105
include/costmap_2d/inflation_layer.h
View File

@@ -161,4 +161,107 @@
// } // namespace costmap_2d
#endif // COSTMAP_2D_INFLATION_LAYER_H_
// #endif // COSTMAP_2D_INFLATION_LAYER_H_
// /////////////////////////////////////////
// #ifndef INFLATION_LAYER_H_
// #define INFLATION_LAYER_H_
// #include "share.h"
// #include <cmath>
// #include <map>
// #include <vector>
// #include <boost/thread/recursive_mutex.hpp>
// /**
// * @brief Lưu thông tin 1 cell khi thực hiện inflation
// */
// class CellData
// {
// public:
// CellData(double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy)
// : index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy) {}
// unsigned int index_;
// unsigned int x_, y_;
// unsigned int src_x_, src_y_;
// };
// /**
// * @brief Lớp thực hiện "inflation" cho costmap (mở rộng vùng vật cản)
// */
// class InflationLayer : public Layer
// {
// public:
// InflationLayer();
// virtual ~InflationLayer();
// virtual void onInitialize();
// virtual void updateBounds(double robot_x, double robot_y, double robot_yaw,
// double* min_x, double* min_y, double* max_x, double* max_y);
// virtual void updateCosts(Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
// virtual bool isDiscretized() { return true; }
// virtual void matchSize();
// virtual void reset() { onInitialize(); }
// void setInflationParameters(double inflation_radius, double cost_scaling_factor);
// protected:
// virtual void onFootprintChanged();
// boost::recursive_mutex inflation_access_;
// double resolution_;
// double inflation_radius_;
// double inscribed_radius_;
// double weight_;
// bool inflate_unknown_;
// private:
// inline double distanceLookup(int mx, int my, int src_x, int src_y);
// inline unsigned char costLookup(int mx, int my, int src_x, int src_y);
// inline unsigned char computeCost(double distance) const;
// void computeCaches();
// void deleteKernels();
// void inflate_area(int min_i, int min_j, int max_i, int max_j, unsigned char* master_grid);
// inline void enqueue(unsigned int index, unsigned int mx, unsigned int my,
// unsigned int src_x, unsigned int src_y);
// unsigned int cell_inflation_radius_;
// unsigned int cached_cell_inflation_radius_;
// std::map<double, std::vector<CellData>> inflation_cells_;
// bool* seen_;
// int seen_size_;
// unsigned char** cached_costs_;
// double** cached_distances_;
// double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
// bool need_reinflation_;
// };
#endif // INFLATION_LAYER_H_

4
include/costmap_2d/layer.h
View File

@@ -122,7 +122,7 @@ public:
}
/** @brief Convenience function for layered_costmap_->getFootprint(). */
const std::vector<Point>& getFootprint() const;
const std::vector<geometry_msgs::Point>& getFootprint() const;
/** @brief LayeredCostmap calls this whenever the footprint there
* changes (via LayeredCostmap::setFootprint()). Override to be
@@ -143,7 +143,7 @@ protected:
std::shared_ptr<tf2::BufferCore> *tf_;
private:
std::vector<Point> footprint_spec_;
std::vector<geometry_msgs::Point> footprint_spec_;
};
} // namespace costmap_2d

8
include/costmap_2d/layered_costmap.h
View File

@@ -148,14 +148,14 @@ public:
* @brief Cập nhật footprint (hình dạng chiếm chỗ của robot).
* Đồng thời tính lại bán kính bao quanh và nội tiếp.
* Gọi hàm onFootprintChanged() của tất cả layer.
* @param footprint_spec: vector các điểm (geometry_msgs::Point) mô tả đa giác footprint.
* @param footprint_spec: vector các điểm (geometry_msgs::geometry_msgs::Point) mô tả đa giác footprint.
*/
void setFootprint(const std::vector<Point>& footprint_spec);
void setFootprint(const std::vector<geometry_msgs::Point>& footprint_spec);
/**
* @brief Trả về footprint hiện tại của robot.
*/
const std::vector<Point>& getFootprint() { return footprint_; }
const std::vector<geometry_msgs::Point>& getFootprint() { return footprint_; }
/**
* @brief Bán kính đường tròn bao ngoài (circumscribed radius):
@@ -185,7 +185,7 @@ private:
bool size_locked_; ///< Có cho phép thay đổi kích thước bản đồ sau này không
double circumscribed_radius_, inscribed_radius_; ///< Hai bán kính của footprint robot (bao ngoài và nội tiếp)
std::vector<Point> footprint_; ///< Đa giác mô tả footprint robot
std::vector<geometry_msgs::Point> footprint_; ///< Đa giác mô tả footprint robot
};
} // namespace costmap_2d

2
include/costmap_2d/observation.h
View File

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

2
include/costmap_2d/observation_buffer.h
View File

@@ -11,7 +11,7 @@
#include <costmap_2d/observation.h>
#include <tf2/buffer_core.h>
#include <msg/PointCloud2.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/Point.h>

37
include/costmap_2d/static_layer.h
View File

@@ -1,40 +1,3 @@
/*********************************************************************
*
* 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_