/********************************************************************* * * 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 #include #include #include 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::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 iter_z(global_frame_cloud, "z"); std::vector::const_iterator iter_global = global_frame_cloud.data.begin(), iter_global_end = global_frame_cloud.data.end(); std::vector::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& 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::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::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