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

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;