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

update file obstacle_layer and file test plugin

Browse Source
This commit is contained in:
duongtd
2025-11-12 17:38:38 +07:00
parent 19683269c3
commit c94de60a7b
12 changed files with 393 additions and 517 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
src/observation_buffer.cpp
View File

@@ -1,5 +1,5 @@
#include <costmap_2d/observation_buffer.h>
#include <costmap_2d/data_convert.h>
// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
// #include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include<tf2/convert.h>
@@ -32,18 +32,13 @@ ObservationBuffer::~ObservationBuffer()
bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
{
geometry_msgs::Point A;
// ros::Time transform_time = ros::Time::now();
// double transform_time =
// std::chrono::duration<double>(
// std::chrono::system_clock::now().time_since_epoch()).count();
robot::Time transform_time = robot::Time::now();
std::string tf_error;
if (!tf2_buffer_.canTransform(new_global_frame, global_frame_, tf2::Time::now(), &tf_error))
{
printf("Transform between %s and %s with tolerance %.2f failed: %s.", new_global_frame.c_str(),
printf("Transform between %s and %s with tolerance %.2f failed: %s.\n", new_global_frame.c_str(),
global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
return false;
}
@@ -63,19 +58,19 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
// we need to transform the origin of the observation to the new global frame
tf2::doTransform(origin, origin,
tf2_buffer_.lookupTransform(new_global_frame,
tf2::getFrameId(origin),
tf2::getTimestamp(origin)));
origin.header.frame_id,
convertTime(origin.header.stamp)));
obs.origin_ = origin.point;
// we also need to transform the cloud of the observation to the new global frame
tf2::doTransform(*(obs.cloud_), *(obs.cloud_),
tf2_buffer_.lookupTransform(new_global_frame,
tf2::getFrameId(*(obs.cloud_)),
tf2::getTimestamp(*(obs.cloud_))));
obs.cloud_->header.frame_id,
convertTime(obs.cloud_->header.stamp)));
}
catch (TransformException& ex)
{
printf("TF Error attempting to transform an observation from %s to %s: %s", global_frame_.c_str(),
printf("TF Error attempting to transform an observation from %s to %s: %s\n", global_frame_.c_str(),
new_global_frame.c_str(), ex.what());
return false;
}
@@ -107,8 +102,8 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
local_origin.point.z = 0;
tf2::doTransform(local_origin, global_origin,
tf2_buffer_.lookupTransform(global_frame_,
tf2::getFrameId(local_origin),
tf2::getTimestamp(local_origin)));
local_origin.header.frame_id,
convertTime(local_origin.header.stamp)));
tf2::convert(global_origin.point, observation_list_.front().origin_);
// make sure to pass on the raytrace/obstacle range of the observation buffer to the observations
@@ -120,8 +115,8 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
// transform the point cloud
tf2::doTransform(cloud, global_frame_cloud,
tf2_buffer_.lookupTransform(global_frame_,
tf2::getFrameId(cloud),
tf2::getTimestamp(cloud)));
(cloud.header.frame_id),
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
@@ -163,7 +158,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
{
// if an exception occurs, we need to remove the empty observation from the list
observation_list_.pop_front();
printf("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", sensor_frame_.c_str(),
printf("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s\n", sensor_frame_.c_str(),
cloud.header.frame_id.c_str(), ex.what());
return;
}
@@ -224,7 +219,7 @@ bool ObservationBuffer::isCurrent() const
if (!current)
{
printf(
"The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.",
"The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.\n",
topic_name_.c_str(), (robot::Time::now() - last_updated_).toSec(),
expected_update_rate_.toSec());
}