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

fix core dumped err when loadplugin

Browse Source
This commit is contained in:
duongtd
2025-12-01 17:34:23 +07:00
parent 2439f2a71d
commit 11ff1baa79
34 changed files with 1177 additions and 760 deletions
Download Patch File Download Diff File Expand all files Collapse all files

106
src/observation_buffer.cpp Normal file → Executable file
View File

@@ -1,11 +1,44 @@
/*********************************************************************
*
* 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 <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>
using namespace std;
using namespace tf3;
@@ -17,8 +50,7 @@ ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_
double raytrace_range, tf3::BufferCore& tf3_buffer, string global_frame,
string sensor_frame, double tf_tolerance) :
tf3_buffer_(tf3_buffer), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
last_updated_(robot::Time::now()),
global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
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)
{
@@ -30,11 +62,11 @@ ObservationBuffer::~ObservationBuffer()
bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
{
robot::Time transform_time = robot::Time::now();
tf3::Time transform_time = tf3::Time::now();
std::string tf_error;
if (!tf3_buffer_.canTransform(new_global_frame, global_frame_, tf3::Time::now(), &tf_error))
geometry_msgs::TransformStamped transformStamped;
if (!tf3_buffer_.canTransform(new_global_frame, global_frame_, transform_time, &tf_error))
{
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());
@@ -50,21 +82,27 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
geometry_msgs::PointStamped origin;
origin.header.frame_id = global_frame_;
origin.header.stamp = transform_time;
origin.header.stamp = data_convert::convertTime(transform_time);
origin.point = obs.origin_;
// we need to transform the origin of the observation to the new global frame
tf3::doTransform(origin, origin,
tf3_buffer_.lookupTransform(new_global_frame,
origin.header.frame_id,
data_convert::convertTime(origin.header.stamp)));
// tf3_buffer_.transform(origin, origin, new_global_frame);
tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform(
new_global_frame, // frame đích
origin.header.frame_id, // frame nguồn
transform_time
);
tf3::doTransform(origin, origin, tfm_1);
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,
data_convert::convertTime(obs.cloud_->header.stamp)));
// tf3_buffer_.transform(*(obs.cloud_), *(obs.cloud_), new_global_frame);
tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform(
new_global_frame, // frame đích
obs.cloud_->header.frame_id, // frame nguồn
transform_time
);
tf3::doTransform(*(obs.cloud_), *(obs.cloud_), tfm_2);
}
catch (TransformException& ex)
{
@@ -98,11 +136,19 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
local_origin.point.x = 0;
local_origin.point.y = 0;
local_origin.point.z = 0;
tf3::doTransform(local_origin, global_origin,
tf3_buffer_.lookupTransform(global_frame_,
local_origin.header.frame_id,
data_convert::convertTime(local_origin.header.stamp)));
// tf3_buffer_.transform(local_origin, global_origin, global_frame_);
tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform(
global_frame_, // frame đích
local_origin.header.frame_id, // frame nguồn
data_convert::convertTime(local_origin.header.stamp)
);
tf3::doTransform(local_origin, global_origin, tfm_1);
/////////////////////////////////////////////////
///////////chú ý hàm này/////////////////////////
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
observation_list_.front().raytrace_range_ = raytrace_range_;
@@ -111,10 +157,13 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
sensor_msgs::PointCloud2 global_frame_cloud;
// transform the point cloud
tf3::doTransform(cloud, global_frame_cloud,
tf3_buffer_.lookupTransform(global_frame_,
(cloud.header.frame_id),
data_convert::convertTime(cloud.header.stamp)));
// tf3_buffer_.transform(cloud, global_frame_cloud, global_frame_);
tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform(
global_frame_, // frame đích
cloud.header.frame_id, // frame nguồn
data_convert::convertTime(cloud.header.stamp)
);
tf3::doTransform(cloud, global_frame_cloud, tfm_2);
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
@@ -213,13 +262,12 @@ bool ObservationBuffer::isCurrent() const
if (expected_update_rate_ == robot::Duration(0.0))
return true;
bool current = (robot::Time::now() - last_updated_) <= expected_update_rate_;
bool current = (robot::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();
if (!current)
{
printf(
"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());
topic_name_.c_str(), (robot::Time::now() - last_updated_).toSec(), expected_update_rate_.toSec());
}
return current;
}