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

HiepLM update

Browse Source
This commit is contained in:
HiepLM
2025-12-30 09:08:14 +07:00
parent 71adf1390f
commit 2c3d7d586d
23 changed files with 117 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files

62
src/footprint.cpp
View File

@@ -33,12 +33,12 @@
#include <boost/algorithm/string.hpp>
#include <costmap_2d/footprint.h>
#include <costmap_2d/array_parser.h>
#include <geometry_msgs/Point32.h>
#include <robot_geometry_msgs/Point32.h>
namespace costmap_2d
{
void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footprint, double& min_dist, double& max_dist)
void calculateMinAndMaxDistances(const std::vector<robot_geometry_msgs::Point>& footprint, double& min_dist, double& max_dist)
{
min_dist = std::numeric_limits<double>::max();
max_dist = 0.0;
@@ -66,36 +66,36 @@ void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footpr
max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
}
geometry_msgs::Point32 toPoint32(geometry_msgs::Point pt)
robot_geometry_msgs::Point32 toPoint32(robot_geometry_msgs::Point pt)
{
geometry_msgs::Point32 point32;
robot_geometry_msgs::Point32 point32;
point32.x = pt.x;
point32.y = pt.y;
point32.z = pt.z;
return point32;
}
geometry_msgs::Point toPoint(geometry_msgs::Point32 pt)
robot_geometry_msgs::Point toPoint(robot_geometry_msgs::Point32 pt)
{
geometry_msgs::Point point;
robot_geometry_msgs::Point point;
point.x = pt.x;
point.y = pt.y;
point.z = pt.z;
return point;
}
geometry_msgs::Polygon toPolygon(std::vector<geometry_msgs::Point> pts)
robot_geometry_msgs::Polygon toPolygon(std::vector<robot_geometry_msgs::Point> pts)
{
geometry_msgs::Polygon polygon;
robot_geometry_msgs::Polygon polygon;
for (int i = 0; i < pts.size(); i++){
polygon.points.push_back(toPoint32(pts[i]));
}
return polygon;
}
std::vector<geometry_msgs::Point> toPointVector(geometry_msgs::Polygon polygon)
std::vector<robot_geometry_msgs::Point> toPointVector(robot_geometry_msgs::Polygon polygon)
{
std::vector<geometry_msgs::Point> pts;
std::vector<robot_geometry_msgs::Point> pts;
for (int i = 0; i < polygon.points.size(); i++)
{
pts.push_back(toPoint(polygon.points[i]));
@@ -103,8 +103,8 @@ std::vector<geometry_msgs::Point> toPointVector(geometry_msgs::Polygon polygon)
return pts;
}
void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec,
std::vector<geometry_msgs::Point>& oriented_footprint)
void transformFootprint(double x, double y, double theta, const std::vector<robot_geometry_msgs::Point>& footprint_spec,
std::vector<robot_geometry_msgs::Point>& oriented_footprint)
{
// build the oriented footprint at a given location
oriented_footprint.clear();
@@ -112,15 +112,15 @@ void transformFootprint(double x, double y, double theta, const std::vector<geom
double sin_th = sin(theta);
for (unsigned int i = 0; i < footprint_spec.size(); ++i)
{
geometry_msgs::Point new_pt;
robot_geometry_msgs::Point new_pt;
new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
oriented_footprint.push_back(new_pt);
}
}
void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec,
geometry_msgs::PolygonStamped& oriented_footprint)
void transformFootprint(double x, double y, double theta, const std::vector<robot_geometry_msgs::Point>& footprint_spec,
robot_geometry_msgs::PolygonStamped& oriented_footprint)
{
// build the oriented footprint at a given location
oriented_footprint.polygon.points.clear();
@@ -128,32 +128,32 @@ void transformFootprint(double x, double y, double theta, const std::vector<geom
double sin_th = sin(theta);
for (unsigned int i = 0; i < footprint_spec.size(); ++i)
{
geometry_msgs::Point32 new_pt;
robot_geometry_msgs::Point32 new_pt;
new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
oriented_footprint.polygon.points.push_back(new_pt);
}
}
void padFootprint(std::vector<geometry_msgs::Point>& footprint, double padding)
void padFootprint(std::vector<robot_geometry_msgs::Point>& footprint, double padding)
{
// pad footprint in place
for (unsigned int i = 0; i < footprint.size(); i++)
{
geometry_msgs::Point& pt = footprint[ i ];
robot_geometry_msgs::Point& pt = footprint[ i ];
pt.x += sign0(pt.x) * padding;
pt.y += sign0(pt.y) * padding;
}
}
std::vector<geometry_msgs::Point> makeFootprintFromRadius(double radius)
std::vector<robot_geometry_msgs::Point> makeFootprintFromRadius(double radius)
{
std::vector<geometry_msgs::Point> points;
std::vector<robot_geometry_msgs::Point> points;
// Loop over 16 angles around a circle making a point each time
int N = 16;
geometry_msgs::Point pt;
robot_geometry_msgs::Point pt;
for (int i = 0; i < N; ++i)
{
double angle = i * 2 * M_PI / N;
@@ -167,7 +167,7 @@ std::vector<geometry_msgs::Point> makeFootprintFromRadius(double radius)
}
bool makeFootprintFromString(const std::string& footprint_string, std::vector<geometry_msgs::Point>& footprint)
bool makeFootprintFromString(const std::string& footprint_string, std::vector<robot_geometry_msgs::Point>& footprint)
{
std::string error;
std::vector<std::vector<float> > vvf = parseVVF(footprint_string, error);
@@ -190,7 +190,7 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
{
if (vvf[ i ].size() == 2)
{
geometry_msgs::Point point;
robot_geometry_msgs::Point point;
point.x = vvf[ i ][ 0 ];
point.y = vvf[ i ][ 1 ];
point.z = 0;
@@ -207,17 +207,17 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
return true;
}
std::vector<geometry_msgs::Point> makeFootprintFromParams(robot::NodeHandle& nh)
std::vector<robot_geometry_msgs::Point> makeFootprintFromParams(robot::NodeHandle& nh)
{
YAML::Node ft = nh.getParamValue("footprint");
std::vector<geometry_msgs::Point> footprint;
std::vector<robot_geometry_msgs::Point> footprint;
if (ft && ft.IsSequence())
{
for (size_t i = 0; i < ft.size(); ++i)
{
auto pt = ft[i];
geometry_msgs::Point p;
robot_geometry_msgs::Point p;
p.x = pt[0].as<double>();
p.y = pt[1].as<double>();
p.z = 0.0;
@@ -228,13 +228,13 @@ std::vector<geometry_msgs::Point> makeFootprintFromParams(robot::NodeHandle& nh)
return footprint;
}
// void writeFootprintToParam(ros::NodeHandle& nh, const std::vector<geometry_msgs::Point>& footprint)
// void writeFootprintToParam(ros::NodeHandle& nh, const std::vector<robot_geometry_msgs::Point>& footprint)
// {
// std::ostringstream oss;
// bool first = true;
// for (unsigned int i = 0; i < footprint.size(); i++)
// {
// geometry_msgs::Point p = footprint[ i ];
// robot_geometry_msgs::Point p = footprint[ i ];
// if (first)
// {
// oss << "[[" << p.x << "," << p.y << "]";
@@ -263,7 +263,7 @@ double getNumberFromXMLRPC(robot::XmlRpc::XmlRpcValue& value, const std::string&
return value.getType() == robot::XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value);
}
std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(robot::XmlRpc::XmlRpcValue& footprint_xmlrpc,
std::vector<robot_geometry_msgs::Point> makeFootprintFromXMLRPC(robot::XmlRpc::XmlRpcValue& footprint_xmlrpc,
const std::string& full_param_name)
{
// Make sure we have an array of at least 3 elements.
@@ -276,8 +276,8 @@ std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(robot::XmlRpc::XmlRpcV
"3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
}
std::vector<geometry_msgs::Point> footprint;
geometry_msgs::Point pt;
std::vector<robot_geometry_msgs::Point> footprint;
robot_geometry_msgs::Point pt;
for (int i = 0; i < footprint_xmlrpc.size(); ++i)
{