281 lines
12 KiB
C++
Executable File
281 lines
12 KiB
C++
Executable File
/*********************************************************************
|
|
*
|
|
* 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 <robot_costmap_2d/observation_buffer.h>
|
|
|
|
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
|
|
#include <robot_tf3_sensor_msgs/tf3_sensor_msgs.h>
|
|
#include <robot_sensor_msgs/point_cloud2_iterator.h>
|
|
|
|
using namespace std;
|
|
using namespace tf3;
|
|
|
|
namespace robot_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, 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),
|
|
min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height),
|
|
obstacle_range_(obstacle_range), raytrace_range_(raytrace_range), tf_tolerance_(tf_tolerance)
|
|
{
|
|
}
|
|
|
|
ObservationBuffer::~ObservationBuffer()
|
|
{
|
|
}
|
|
|
|
bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
|
|
{
|
|
tf3::Time transform_time = tf3::Time::now();
|
|
std::string tf_error;
|
|
|
|
robot_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());
|
|
return false;
|
|
}
|
|
|
|
list<Observation>::iterator obs_it;
|
|
for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
|
|
{
|
|
try
|
|
{
|
|
Observation& obs = *obs_it;
|
|
|
|
robot_geometry_msgs::PointStamped origin;
|
|
origin.header.frame_id = global_frame_;
|
|
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_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_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)
|
|
{
|
|
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;
|
|
}
|
|
}
|
|
|
|
// now we need to update our global_frame member
|
|
global_frame_ = new_global_frame;
|
|
return true;
|
|
}
|
|
|
|
void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
|
|
{
|
|
robot_geometry_msgs::PointStamped global_origin;
|
|
|
|
// create a new observation on the list to be populated
|
|
observation_list_.push_front(Observation());
|
|
|
|
// check whether the origin frame has been set explicitly or whether we should get it from the cloud
|
|
string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_;
|
|
|
|
try
|
|
{
|
|
// given these observations come from sensors... we'll need to store the origin pt of the sensor
|
|
robot_geometry_msgs::PointStamped local_origin;
|
|
local_origin.header.stamp = cloud.header.stamp;
|
|
local_origin.header.frame_id = origin_frame;
|
|
local_origin.point.x = 0;
|
|
local_origin.point.y = 0;
|
|
local_origin.point.z = 0;
|
|
// 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_;
|
|
observation_list_.front().obstacle_range_ = obstacle_range_;
|
|
|
|
robot_sensor_msgs::PointCloud2 global_frame_cloud;
|
|
|
|
// transform the point cloud
|
|
// 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
|
|
robot_sensor_msgs::PointCloud2& observation_cloud = *(observation_list_.front().cloud_);
|
|
observation_cloud.height = global_frame_cloud.height;
|
|
observation_cloud.width = global_frame_cloud.width;
|
|
observation_cloud.fields = global_frame_cloud.fields;
|
|
observation_cloud.is_bigendian = global_frame_cloud.is_bigendian;
|
|
observation_cloud.point_step = global_frame_cloud.point_step;
|
|
observation_cloud.row_step = global_frame_cloud.row_step;
|
|
observation_cloud.is_dense = global_frame_cloud.is_dense;
|
|
|
|
unsigned int cloud_size = global_frame_cloud.height*global_frame_cloud.width;
|
|
robot_sensor_msgs::PointCloud2Modifier modifier(observation_cloud);
|
|
modifier.resize(cloud_size);
|
|
unsigned int point_count = 0;
|
|
|
|
// copy over the points that are within our height bounds
|
|
robot_sensor_msgs::PointCloud2Iterator<float> iter_z(global_frame_cloud, "z");
|
|
std::vector<unsigned char>::const_iterator iter_global = global_frame_cloud.data.begin(), iter_global_end = global_frame_cloud.data.end();
|
|
std::vector<unsigned char>::iterator iter_obs = observation_cloud.data.begin();
|
|
for (; iter_global != iter_global_end; ++iter_z, iter_global += global_frame_cloud.point_step)
|
|
{
|
|
if ((*iter_z) <= max_obstacle_height_
|
|
&& (*iter_z) >= min_obstacle_height_)
|
|
{
|
|
std::copy(iter_global, iter_global + global_frame_cloud.point_step, iter_obs);
|
|
iter_obs += global_frame_cloud.point_step;
|
|
++point_count;
|
|
}
|
|
}
|
|
|
|
// resize the cloud for the number of legal points
|
|
modifier.resize(point_count);
|
|
observation_cloud.header.stamp = cloud.header.stamp;
|
|
observation_cloud.header.frame_id = global_frame_cloud.header.frame_id;
|
|
}
|
|
catch (TransformException& ex)
|
|
{
|
|
// 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\n", 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_ = robot::Time::now();
|
|
|
|
// we'll also remove any stale observations from the list
|
|
purgeStaleObservations();
|
|
}
|
|
|
|
// returns a copy of the observations
|
|
void ObservationBuffer::getObservations(vector<Observation>& observations)
|
|
{
|
|
// first... let's make sure that we don't have any stale observations
|
|
purgeStaleObservations();
|
|
|
|
// now we'll just copy the observations for the caller
|
|
list<Observation>::iterator obs_it;
|
|
for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
|
|
{
|
|
observations.push_back(*obs_it);
|
|
}
|
|
}
|
|
|
|
void ObservationBuffer::purgeStaleObservations()
|
|
{
|
|
if (!observation_list_.empty())
|
|
{
|
|
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_ == robot::Duration(0.0))
|
|
{
|
|
observation_list_.erase(++obs_it, observation_list_.end());
|
|
return;
|
|
}
|
|
|
|
// otherwise... we'll have to loop through the observations to see which ones are stale
|
|
for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
|
|
{
|
|
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_)
|
|
{
|
|
observation_list_.erase(obs_it, observation_list_.end());
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
bool ObservationBuffer::isCurrent() const
|
|
{
|
|
if (expected_update_rate_ == robot::Duration(0.0))
|
|
return true;
|
|
|
|
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());
|
|
}
|
|
return current;
|
|
}
|
|
|
|
void ObservationBuffer::resetLastUpdated()
|
|
{
|
|
last_updated_ = robot::Time::now();
|
|
}
|
|
} // namespace robot_costmap_2d
|
|
|