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

2
src/costmap_2d.cpp
View File

@@ -312,7 +312,7 @@ void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y)
delete[] local_map;
}
bool Costmap2D::setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value)
bool Costmap2D::setConvexPolygonCost(const std::vector<robot_geometry_msgs::Point>& polygon, unsigned char cost_value)
{
// we assume the polygon is given in the global_frame... we need to transform it to map coordinates
std::vector<MapLocation> map_polygon;

26
src/costmap_2d_robot.cpp
View File

@@ -227,7 +227,7 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
}
}
std::vector<geometry_msgs::Point> new_footprint;// = loadParam(layer, "footprint", 0.0);
std::vector<robot_geometry_msgs::Point> new_footprint;// = loadParam(layer, "footprint", 0.0);
new_footprint = loadFootprint(layer["footprint"], new_footprint);
transform_tolerance_ = loadParam(layer, "transform_tolerance", 0.0);
@@ -306,7 +306,7 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
}
}
void Costmap2DROBOT::setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint)
void Costmap2DROBOT::setUnpaddedRobotFootprintPolygon(const robot_geometry_msgs::Polygon& footprint)
{
setUnpaddedRobotFootprint(toPointVector(footprint));
}
@@ -324,8 +324,8 @@ Costmap2DROBOT::~Costmap2DROBOT()
}
void Costmap2DROBOT::readFootprintFromConfig(const std::vector<geometry_msgs::Point> &new_footprint,
const std::vector<geometry_msgs::Point> &old_footprint,
void Costmap2DROBOT::readFootprintFromConfig(const std::vector<robot_geometry_msgs::Point> &new_footprint,
const std::vector<robot_geometry_msgs::Point> &old_footprint,
const double &robot_radius)
{
// Only change the footprint if footprint or robot_radius has
@@ -354,7 +354,7 @@ void Costmap2DROBOT::readFootprintFromConfig(const std::vector<geometry_msgs::Po
}
}
void Costmap2DROBOT::setUnpaddedRobotFootprint(const std::vector<geometry_msgs::Point>& points)
void Costmap2DROBOT::setUnpaddedRobotFootprint(const std::vector<robot_geometry_msgs::Point>& points)
{
unpadded_footprint_ = points;
padded_footprint_ = points;
@@ -365,7 +365,7 @@ void Costmap2DROBOT::setUnpaddedRobotFootprint(const std::vector<geometry_msgs::
void Costmap2DROBOT::checkMovement()
{
geometry_msgs::PoseStamped new_pose;
robot_geometry_msgs::PoseStamped new_pose;
if (!getRobotPose(new_pose))
std::cout << "Cannot get robot pose\n";
}
@@ -390,14 +390,14 @@ void Costmap2DROBOT::updateMap()
if (!stop_updates_)
{
// get global pose
geometry_msgs::PoseStamped pose;
robot_geometry_msgs::PoseStamped pose;
if (getRobotPose (pose))
{
double x = pose.pose.position.x,
y = pose.pose.position.y,
yaw = data_convert::getYaw(pose.pose.orientation);
layered_costmap_->updateMap(x, y, yaw);
geometry_msgs::PolygonStamped footprint;
robot_geometry_msgs::PolygonStamped footprint;
footprint.header.frame_id = global_frame_;
footprint.header.stamp = robot::Time::now();
transformFootprint(x, y, yaw, padded_footprint_, footprint);
@@ -495,10 +495,10 @@ void Costmap2DROBOT::resetLayers()
}
}
bool Costmap2DROBOT::getRobotPose(geometry_msgs::PoseStamped& global_pose) const
bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose) const
{
geometry_msgs::PoseStamped robot_pose;
geometry_msgs::Pose pose_default;
robot_geometry_msgs::PoseStamped robot_pose;
robot_geometry_msgs::Pose pose_default;
global_pose.pose = pose_default;
robot_pose.pose = pose_default;
@@ -555,10 +555,10 @@ bool Costmap2DROBOT::getRobotPose(geometry_msgs::PoseStamped& global_pose) const
return true;
}
void Costmap2DROBOT::getOrientedFootprint(std::vector<geometry_msgs::Point>& oriented_footprint) const
void Costmap2DROBOT::getOrientedFootprint(std::vector<robot_geometry_msgs::Point>& oriented_footprint) const
{
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::getOrientedFootprint");
geometry_msgs::PoseStamped global_pose;
robot_geometry_msgs::PoseStamped global_pose;
if (!getRobotPose(global_pose))
return;

6
src/costmap_math.cpp
View File

@@ -61,7 +61,7 @@ double distanceToLine(double pX, double pY, double x0, double y0, double x1, dou
return distance(pX, pY, xx, yy);
}
bool intersects(std::vector<geometry_msgs::Point>& polygon, float testx, float testy)
bool intersects(std::vector<robot_geometry_msgs::Point>& polygon, float testx, float testy)
{
bool c = false;
int i, j, nvert = polygon.size();
@@ -75,7 +75,7 @@ bool intersects(std::vector<geometry_msgs::Point>& polygon, float testx, float t
return c;
}
bool intersects_helper(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometry_msgs::Point>& polygon2)
bool intersects_helper(std::vector<robot_geometry_msgs::Point>& polygon1, std::vector<robot_geometry_msgs::Point>& polygon2)
{
for (unsigned int i = 0; i < polygon1.size(); i++)
if (intersects(polygon2, polygon1[i].x, polygon1[i].y))
@@ -83,7 +83,7 @@ bool intersects_helper(std::vector<geometry_msgs::Point>& polygon1, std::vector<
return false;
}
bool intersects(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometry_msgs::Point>& polygon2)
bool intersects(std::vector<robot_geometry_msgs::Point>& polygon1, std::vector<robot_geometry_msgs::Point>& polygon2)
{
return intersects_helper(polygon1, polygon2) || intersects_helper(polygon2, polygon1);
}

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)
{

2
src/layer.cpp
View File

@@ -48,7 +48,7 @@ void Layer::initialize(LayeredCostmap* parent, std::string name, tf3::BufferCore
onInitialize();
}
const std::vector<geometry_msgs::Point>& Layer::getFootprint() const
const std::vector<robot_geometry_msgs::Point>& Layer::getFootprint() const
{
return layered_costmap_->getFootprint();
}

2
src/layered_costmap.cpp
View File

@@ -175,7 +175,7 @@ namespace costmap_2d
return current_;
}
void LayeredCostmap::setFootprint(const std::vector<geometry_msgs::Point> &footprint_spec)
void LayeredCostmap::setFootprint(const std::vector<robot_geometry_msgs::Point> &footprint_spec)
{
footprint_ = footprint_spec;
costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_);

8
src/observation_buffer.cpp
View File

@@ -65,7 +65,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
tf3::Time transform_time = tf3::Time::now();
std::string tf_error;
geometry_msgs::TransformStamped transformStamped;
robot_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(),
@@ -80,7 +80,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
{
Observation& obs = *obs_it;
geometry_msgs::PointStamped origin;
robot_geometry_msgs::PointStamped origin;
origin.header.frame_id = global_frame_;
origin.header.stamp = data_convert::convertTime(transform_time);
origin.point = obs.origin_;
@@ -119,7 +119,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
{
geometry_msgs::PointStamped global_origin;
robot_geometry_msgs::PointStamped global_origin;
// create a new observation on the list to be populated
observation_list_.push_front(Observation());
@@ -130,7 +130,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
try
{
// given these observations come from sensors... we'll need to store the origin pt of the sensor
geometry_msgs::PointStamped local_origin;
robot_geometry_msgs::PointStamped local_origin;
local_origin.header.stamp = cloud.header.stamp;
local_origin.header.frame_id = origin_frame;
local_origin.point.x = 0;