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

first commit

Browse Source
This commit is contained in:
duongtd
2025-10-28 13:43:40 +07:00
parent 63dc18f3f0
commit 5d4d1943fe
56 changed files with 1660 additions and 144 deletions
Download Patch File Download Diff File Expand all files Collapse all files

104
src/observation_buffer.cpp
View File

@@ -1,44 +1,13 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#include <costmap_2d/observation_buffer.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
// #include <tf2_sensor_msgs/tf2_sensor_msgs.h>
// #include <sensor_msgs/point_cloud2_iterator.h>
#include<tf2/convert.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <cstdint>
using namespace std;
using namespace tf2;
@@ -47,10 +16,11 @@ namespace costmap_2d
{
ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate,
double min_obstacle_height, double max_obstacle_height, double obstacle_range,
double raytrace_range, tf2_ros::Buffer& tf2_buffer, string global_frame,
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_(ros::Time::now()), global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
last_updated_(std::chrono::system_clock::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)
{
@@ -62,13 +32,17 @@ ObservationBuffer::~ObservationBuffer()
bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
{
ros::Time transform_time = ros::Time::now();
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();
std::string tf_error;
geometry_msgs::TransformStamped transformStamped;
if (!tf2_buffer_.canTransform(new_global_frame, global_frame_, transform_time, ros::Duration(tf_tolerance_), &tf_error))
if (!tf2_buffer_.canTransform(new_global_frame, global_frame_, tf2::Time::now(), &tf_error))
{
ROS_ERROR("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.", new_global_frame.c_str(),
global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
return false;
}
@@ -86,15 +60,21 @@ 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_buffer_.transform(origin, origin, new_global_frame);
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_buffer_.transform(*(obs.cloud_), *(obs.cloud_), 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)
{
ROS_ERROR("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", global_frame_.c_str(),
new_global_frame.c_str(), ex.what());
return false;
}
@@ -124,7 +104,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_buffer_.transform(local_origin, global_origin, global_frame_);
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
@@ -134,7 +117,10 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
sensor_msgs::PointCloud2 global_frame_cloud;
// transform the point cloud
tf2_buffer_.transform(cloud, global_frame_cloud, global_frame_);
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
@@ -176,13 +162,13 @@ 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();
ROS_ERROR("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", sensor_frame_.c_str(),
cloud.header.frame_id.c_str(), ex.what());
return;
}
// if the update was successful, we want to update the last updated time
last_updated_ = ros::Time::now();
last_updated_ = std::chrono::system_clock::now();
// we'll also remove any stale observations from the list
purgeStaleObservations();
@@ -208,7 +194,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_ == ros::Duration(0.0))
if (observation_keep_time_ == std::chrono::duration<double>(0.0))
{
observation_list_.erase(++obs_it, observation_list_.end());
return;
@@ -219,7 +205,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 ((last_updated_ - obs.cloud_->header.stamp) > observation_keep_time_)
if ((std::chrono::duration<double>(last_updated_.time_since_epoch()).count() - obs.cloud_->header.stamp) > observation_keep_time_.count())
{
observation_list_.erase(obs_it, observation_list_.end());
return;
@@ -230,22 +216,24 @@ void ObservationBuffer::purgeStaleObservations()
bool ObservationBuffer::isCurrent() const
{
if (expected_update_rate_ == ros::Duration(0.0))
if (expected_update_rate_ == std::chrono::duration<double>(0.0))
return true;
bool current = (ros::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();
bool current = (std::chrono::system_clock::now() - last_updated_) <= expected_update_rate_;
if (!current)
{
ROS_WARN(
printf(
"The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.",
topic_name_.c_str(), (ros::Time::now() - last_updated_).toSec(), expected_update_rate_.toSec());
topic_name_.c_str(), std::chrono::duration<double>(std::chrono::system_clock::now() - last_updated_).count(),
expected_update_rate_.count());
}
return current;
return true;
}
void ObservationBuffer::resetLastUpdated()
{
last_updated_ = ros::Time::now();
last_updated_ = std::chrono::system_clock::now();
}
} // namespace costmap_2d