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

remove file data_convert

Browse Source
This commit is contained in:
duongtd
2025-11-26 16:27:41 +07:00
parent 2e0a4348dd
commit 5ad8e83761
7 changed files with 25 additions and 180 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
src/costmap_2d_robot.cpp
View File

@@ -84,7 +84,7 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
}
else
{
std::cout << "/cfg folder not found!" << std::endl;
std::cout<< config_file_name << " file not found!" << std::endl;
}
@@ -102,8 +102,8 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
{
if (last_error + robot::Duration(5.0) < robot::Time::now())
{
printf("Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n",
robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
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
@@ -293,7 +293,7 @@ void Costmap2DROBOT::updateMap()
{
double x = pose.pose.position.x,
y = pose.pose.position.y,
yaw = getYaw(pose.pose.orientation);
yaw = data_convert::getYaw(pose.pose.orientation);
layered_costmap_->updateMap(x, y, yaw);
geometry_msgs::PolygonStamped footprint;
footprint.header.frame_id = global_frame_;
@@ -408,9 +408,9 @@ bool Costmap2DROBOT::getRobotPose(geometry_msgs::PoseStamped& global_pose) const
try
{
// use current time if possible (makes sure it's not in the future)
if (tf_.canTransform(global_frame_, robot_base_frame_, convertTime(current_time)))
if (tf_.canTransform(global_frame_, robot_base_frame_, data_convert::convertTime(current_time)))
{
tf3::TransformStampedMsg transform = tf_.lookupTransform(global_frame_, robot_base_frame_, convertTime(current_time));
tf3::TransformStampedMsg transform = tf_.lookupTransform(global_frame_, robot_base_frame_, data_convert::convertTime(current_time));
tf3::doTransform(robot_pose, global_pose, transform);
}
// use the latest otherwise
@@ -420,7 +420,7 @@ bool Costmap2DROBOT::getRobotPose(geometry_msgs::PoseStamped& global_pose) const
tf3::TransformStampedMsg transform = tf_.lookupTransform(
global_frame_, // frame đích
robot_base_frame_, // frame nguồn
tf3::convertTime(robot_pose.header.stamp)
data_convert::convertTime(robot_pose.header.stamp)
);
tf3::doTransform(robot_pose, global_pose, transform);
}
@@ -460,7 +460,7 @@ void Costmap2DROBOT::getOrientedFootprint(std::vector<geometry_msgs::Point>& ori
if (!getRobotPose(global_pose))
return;
double yaw = getYaw(global_pose.pose.orientation);
double yaw = data_convert::getYaw(global_pose.pose.orientation);
transformFootprint(global_pose.pose.position.x, global_pose.pose.position.y, yaw,
padded_footprint_, oriented_footprint);
}