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);
}

14
src/observation_buffer.cpp
View File

@@ -1,7 +1,7 @@
#include <costmap_2d/observation_buffer.h>
#include <costmap_2d/data_convert.h>
// #include <tf3_geometry_msgs/tf3_geometry_msgs.h>
// #include <tf3_sensor_msgs/tf3_sensor_msgs.h>
#include <data_convert/data_convert.h>
#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <tf3_sensor_msgs/tf3_sensor_msgs.h>
#include<tf3/convert.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <cstdint>
@@ -59,14 +59,14 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
tf3::doTransform(origin, origin,
tf3_buffer_.lookupTransform(new_global_frame,
origin.header.frame_id,
convertTime(origin.header.stamp)));
data_convert::convertTime(origin.header.stamp)));
obs.origin_ = origin.point;
// we also need to transform the cloud of the observation to the new global frame
tf3::doTransform(*(obs.cloud_), *(obs.cloud_),
tf3_buffer_.lookupTransform(new_global_frame,
obs.cloud_->header.frame_id,
convertTime(obs.cloud_->header.stamp)));
data_convert::convertTime(obs.cloud_->header.stamp)));
}
catch (TransformException& ex)
{
@@ -103,7 +103,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
tf3::doTransform(local_origin, global_origin,
tf3_buffer_.lookupTransform(global_frame_,
local_origin.header.frame_id,
convertTime(local_origin.header.stamp)));
data_convert::convertTime(local_origin.header.stamp)));
tf3::convert(global_origin.point, observation_list_.front().origin_);
// make sure to pass on the raytrace/obstacle range of the observation buffer to the observations
@@ -116,7 +116,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
tf3::doTransform(cloud, global_frame_cloud,
tf3_buffer_.lookupTransform(global_frame_,
(cloud.header.frame_id),
convertTime(cloud.header.stamp)));
data_convert::convertTime(cloud.header.stamp)));
global_frame_cloud.header.stamp = cloud.header.stamp;
// now we need to remove observations from the cloud that are below or above our height thresholds