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

them file test static_layer

Browse Source
This commit is contained in:
duongtd
2025-11-07 17:44:42 +07:00
parent 79e706b798
commit 498b606e15
148 changed files with 6363 additions and 1599 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
src/footprint.cpp
View File

@@ -33,7 +33,6 @@
#include <boost/algorithm/string.hpp>
#include <costmap_2d/footprint.h>
#include <costmap_2d/array_parser.h>
// #include<costmap_2d/msg.h>
namespace costmap_2d
{

78
src/observation_buffer.cpp
View File

@@ -8,6 +8,7 @@
using namespace std;
using namespace tf2;
@@ -18,7 +19,7 @@ ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_
double raytrace_range, tf2::BufferCore& tf2_buffer, string global_frame,
string sensor_frame, double tf_tolerance) :
tf2_buffer_(tf2_buffer), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
last_updated_(std::chrono::system_clock::now()),
last_updated_(robot::Time::now()),
global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height),
obstacle_range_(obstacle_range), raytrace_range_(raytrace_range), tf_tolerance_(tf_tolerance)
@@ -33,18 +34,19 @@ 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();
// 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(),
global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
return false;
}
// 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(),
// global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
// return false;
// }
list<Observation>::iterator obs_it;
for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
@@ -59,17 +61,17 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
origin.point = obs.origin_;
// 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)));
obs.origin_ = origin.point;
// tf2::doTransform(origin, origin,
// tf2_buffer_.lookupTransform(new_global_frame,
// tf2::getFrameId(origin),
// tf2::getTimestamp(origin)));
// 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_))));
// // 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_))));
}
catch (TransformException& ex)
{
@@ -103,10 +105,10 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
local_origin.point.x = 0;
local_origin.point.y = 0;
local_origin.point.z = 0;
tf2::doTransform(local_origin, global_origin,
tf2_buffer_.lookupTransform(global_frame_,
tf2::getFrameId(local_origin),
tf2::getTimestamp(local_origin)));
// tf2::doTransform(local_origin, global_origin,
// tf2_buffer_.lookupTransform(global_frame_,
// tf2::getFrameId(local_origin),
// tf2::getTimestamp(local_origin)));
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
@@ -115,12 +117,12 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
sensor_msgs::PointCloud2 global_frame_cloud;
// transform the point cloud
tf2::doTransform(cloud, global_frame_cloud,
tf2_buffer_.lookupTransform(global_frame_,
tf2::getFrameId(cloud),
tf2::getTimestamp(cloud)));
global_frame_cloud.header.stamp = cloud.header.stamp;
// // transform the point cloud
// tf2::doTransform(cloud, global_frame_cloud,
// tf2_buffer_.lookupTransform(global_frame_,
// tf2::getFrameId(cloud),
// tf2::getTimestamp(cloud)));
// 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
sensor_msgs::PointCloud2& observation_cloud = *(observation_list_.front().cloud_);
@@ -167,7 +169,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
}
// if the update was successful, we want to update the last updated time
last_updated_ = std::chrono::system_clock::now();
last_updated_ = robot::Time::now();
// we'll also remove any stale observations from the list
purgeStaleObservations();
@@ -193,7 +195,7 @@ void ObservationBuffer::purgeStaleObservations()
{
list<Observation>::iterator obs_it = observation_list_.begin();
// if we're keeping observations for no time... then we'll only keep one observation
if (observation_keep_time_ == std::chrono::duration<double>(0.0))
if (observation_keep_time_ == robot::Duration(0.0))
{
observation_list_.erase(++obs_it, observation_list_.end());
return;
@@ -204,7 +206,7 @@ void ObservationBuffer::purgeStaleObservations()
{
Observation& obs = *obs_it;
// check if the observation is out of date... and if it is, remove it and those that follow from the list
if ((std::chrono::duration<double>(last_updated_.time_since_epoch()).count() - obs.cloud_->header.stamp) > observation_keep_time_.count())
if ((last_updated_ - obs.cloud_->header.stamp) > observation_keep_time_)
{
observation_list_.erase(obs_it, observation_list_.end());
return;
@@ -215,16 +217,16 @@ void ObservationBuffer::purgeStaleObservations()
bool ObservationBuffer::isCurrent() const
{
if (expected_update_rate_ == std::chrono::duration<double>(0.0))
if (expected_update_rate_ == robot::Duration(0.0))
return true;
bool current = (std::chrono::system_clock::now() - last_updated_) <= expected_update_rate_;
bool current = (robot::Time::now() - last_updated_) <= expected_update_rate_;
if (!current)
{
printf(
"The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.",
topic_name_.c_str(), std::chrono::duration<double>(std::chrono::system_clock::now() - last_updated_).count(),
expected_update_rate_.count());
topic_name_.c_str(), (robot::Time::now() - last_updated_).toSec(),
expected_update_rate_.toSec());
}
return current;
return true;
@@ -232,7 +234,7 @@ bool ObservationBuffer::isCurrent() const
void ObservationBuffer::resetLastUpdated()
{
last_updated_ = std::chrono::system_clock::now();
last_updated_ = robot::Time::now();
}
} // namespace costmap_2d