542 lines
23 KiB
C++
Executable File
542 lines
23 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
|
||
// // *********************************************************************/
|
||
// // #ifndef ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
|
||
// // #define ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
|
||
|
||
// // #include <vector>
|
||
// // #include <list>
|
||
// // #include <string>
|
||
// // #include <robot/robot.h>
|
||
// // #include <robot_costmap_2d/observation.h>
|
||
// // #include <tf3/buffer_core.h>
|
||
// // #include <robot_sensor_msgs/PointCloud2.h>
|
||
|
||
// // // Thread support
|
||
// // #include <boost/thread.hpp>
|
||
|
||
// // namespace robot_costmap_2d
|
||
// // {
|
||
// // /**
|
||
// // * @class ObservationBuffer
|
||
// // * @brief Takes in point clouds from sensors, transforms them to the desired frame, and stores them
|
||
// // */
|
||
// // class ObservationBuffer
|
||
// // {
|
||
// // public:
|
||
// // /**
|
||
// // * @brief Constructs an observation buffer
|
||
// // * @param topic_name The topic of the observations, used as an identifier for error and warning messages
|
||
// // * @param observation_keep_time Defines the persistence of observations in seconds, 0 means only keep the latest
|
||
// // * @param expected_update_rate How often this buffer is expected to be updated, 0 means there is no limit
|
||
// // * @param min_obstacle_height The minimum height of a hitpoint to be considered legal
|
||
// // * @param max_obstacle_height The minimum height of a hitpoint to be considered legal
|
||
// // * @param obstacle_range The range to which the sensor should be trusted for inserting obstacles
|
||
// // * @param raytrace_range The range to which the sensor should be trusted for raytracing to clear out space
|
||
// // * @param tf2_buffer A reference to a tf2 Buffer
|
||
// // * @param global_frame The frame to transform PointClouds into
|
||
// // * @param sensor_frame The frame of the origin of the sensor, can be left blank to be read from the messages
|
||
// // * @param tf_tolerance The amount of time to wait for a transform to be available when setting a new global frame
|
||
// // */
|
||
// // ObservationBuffer(std::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, std::string global_frame,
|
||
// // std::string sensor_frame, double tf_tolerance);
|
||
|
||
// // /**
|
||
// // * @brief Destructor... cleans up
|
||
// // */
|
||
// // ~ObservationBuffer();
|
||
|
||
// // /**
|
||
// // * @brief Sets the global frame of an observation buffer. This will
|
||
// // * transform all the currently cached observations to the new global
|
||
// // * frame
|
||
// // * @param new_global_frame The name of the new global frame.
|
||
// // * @return True if the operation succeeds, false otherwise
|
||
// // */
|
||
// // bool setGlobalFrame(const std::string new_global_frame);
|
||
|
||
// // /**
|
||
// // * @brief Transforms a PointCloud to the global frame and buffers it
|
||
// // * <b>Note: The burden is on the user to make sure the transform is available... ie they should use a MessageNotifier</b>
|
||
// // * @param cloud The cloud to be buffered
|
||
// // */
|
||
// // void bufferCloud(const robot_sensor_msgs::PointCloud2& cloud);
|
||
|
||
// // /**
|
||
// // * @brief Pushes copies of all current observations onto the end of the vector passed in
|
||
// // * @param observations The vector to be filled
|
||
// // */
|
||
// // void getObservations(std::vector<Observation>& observations);
|
||
|
||
// // /**
|
||
// // * @brief Check if the observation buffer is being update at its expected rate
|
||
// // * @return True if it is being updated at the expected rate, false otherwise
|
||
// // */
|
||
// // bool isCurrent() const;
|
||
|
||
// // /**
|
||
// // * @brief Lock the observation buffer
|
||
// // */
|
||
// // inline void lock()
|
||
// // {
|
||
// // lock_.lock();
|
||
// // }
|
||
|
||
// // /**
|
||
// // * @brief Lock the observation buffer
|
||
// // */
|
||
// // inline void unlock()
|
||
// // {
|
||
// // lock_.unlock();
|
||
// // }
|
||
|
||
// // /**
|
||
// // * @brief Reset last updated timestamp
|
||
// // */
|
||
// // void resetLastUpdated();
|
||
|
||
// // private:
|
||
// // /**
|
||
// // * @brief Removes any stale observations from the buffer list
|
||
// // */
|
||
// // void purgeStaleObservations();
|
||
|
||
// // // Helper: trích 4×4 transform matrix từ TransformStampedMsg
|
||
// // // Tránh gọi tf3::doTransform per-point (overhead virtual dispatch + exception check)
|
||
// // struct Transform4x4 {
|
||
// // double m[4][4];
|
||
// // };
|
||
|
||
// // static inline Transform4x4 extractMatrix(const tf3::TransformStampedMsg& tfm)
|
||
// // {
|
||
// // // Quaternion → rotation matrix + translation
|
||
// // const auto& t = tfm.transform.translation;
|
||
// // const auto& q = tfm.transform.rotation;
|
||
|
||
// // double qx = q.x, qy = q.y, qz = q.z, qw = q.w;
|
||
// // Transform4x4 M;
|
||
// // M.m[0][0] = 1 - 2*(qy*qy + qz*qz); M.m[0][1] = 2*(qx*qy - qz*qw); M.m[0][2] = 2*(qx*qz + qy*qw); M.m[0][3] = t.x;
|
||
// // M.m[1][0] = 2*(qx*qy + qz*qw); M.m[1][1] = 1 - 2*(qx*qx + qz*qz); M.m[1][2] = 2*(qy*qz - qx*qw); M.m[1][3] = t.y;
|
||
// // M.m[2][0] = 2*(qx*qz - qy*qw); M.m[2][1] = 2*(qy*qz + qx*qw); M.m[2][2] = 1 - 2*(qx*qx + qy*qy); M.m[2][3] = t.z;
|
||
// // M.m[3][0] = 0; M.m[3][1] = 0; M.m[3][2] = 0; M.m[3][3] = 1;
|
||
// // return M;
|
||
// // }
|
||
|
||
// // double voxel_size_;
|
||
// // tf3::BufferCore& tf3_buffer_;
|
||
// // const robot::Duration observation_keep_time_;
|
||
// // const robot::Duration expected_update_rate_;
|
||
// // robot::Time last_updated_;
|
||
// // std::string global_frame_;
|
||
// // std::string sensor_frame_;
|
||
// // std::list<Observation> observation_list_;
|
||
// // std::string topic_name_;
|
||
// // double min_obstacle_height_, max_obstacle_height_;
|
||
// // boost::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely
|
||
// // double obstacle_range_, raytrace_range_;
|
||
// // double tf_tolerance_;
|
||
// // };
|
||
// // } // namespace robot_costmap_2d
|
||
// // #endif // ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
|
||
|
||
// /*********************************************************************
|
||
// *
|
||
// * 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
|
||
// *********************************************************************/
|
||
// #ifndef ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
|
||
// #define ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
|
||
|
||
// #include <vector>
|
||
// #include <list>
|
||
// #include <string>
|
||
// #include <unordered_map>
|
||
// #include <cmath>
|
||
// #include <cstring>
|
||
|
||
// #include <robot/robot.h>
|
||
// #include <robot_costmap_2d/observation.h>
|
||
// #include <tf3/buffer_core.h>
|
||
// #include <robot_sensor_msgs/PointCloud2.h>
|
||
|
||
// // Thread support
|
||
// #include <boost/thread.hpp>
|
||
|
||
// namespace robot_costmap_2d
|
||
// {
|
||
// /**
|
||
// * @class ObservationBuffer
|
||
// * @brief Takes in point clouds from sensors, transforms them to the desired frame, and stores them.
|
||
// *
|
||
// * Optimizations vs original:
|
||
// * - bufferCloud: single-pass transform+filter+voxel-downsample.
|
||
// * Reduces 6.5 M points to at most (map_w × map_h) representative points,
|
||
// * which cuts CPU in updateBounds/raytraceFreespace by ~100–200×.
|
||
// * - extractMatrix: inline quaternion→rotation, avoids per-point virtual dispatch.
|
||
// * - voxel_size_ (default = costmap resolution, 0.05 m): configurable via
|
||
// * setVoxelSize() so ObstacleLayer can pass the real resolution.
|
||
// */
|
||
// class ObservationBuffer
|
||
// {
|
||
// public:
|
||
// /**
|
||
// * @brief Constructs an observation buffer
|
||
// * @param topic_name The topic of the observations, used as an identifier for error and warning messages
|
||
// * @param observation_keep_time Defines the persistence of observations in seconds, 0 means only keep the latest
|
||
// * @param expected_update_rate How often this buffer is expected to be updated, 0 means there is no limit
|
||
// * @param min_obstacle_height The minimum height of a hitpoint to be considered legal
|
||
// * @param max_obstacle_height The maximum height of a hitpoint to be considered legal
|
||
// * @param obstacle_range The range to which the sensor should be trusted for inserting obstacles
|
||
// * @param raytrace_range The range to which the sensor should be trusted for raytracing to clear out space
|
||
// * @param tf2_buffer A reference to a tf2 Buffer
|
||
// * @param global_frame The frame to transform PointClouds into
|
||
// * @param sensor_frame The frame of the origin of the sensor, can be left blank to be read from the messages
|
||
// * @param tf_tolerance The amount of time to wait for a transform to be available when setting a new global frame
|
||
// */
|
||
// ObservationBuffer(std::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, std::string global_frame,
|
||
// std::string sensor_frame, double tf_tolerance);
|
||
|
||
// /**
|
||
// * @brief Destructor... cleans up
|
||
// */
|
||
// ~ObservationBuffer();
|
||
|
||
// /**
|
||
// * @brief Sets the global frame of an observation buffer. This will
|
||
// * transform all the currently cached observations to the new global frame
|
||
// * @param new_global_frame The name of the new global frame.
|
||
// * @return True if the operation succeeds, false otherwise
|
||
// */
|
||
// bool setGlobalFrame(const std::string new_global_frame);
|
||
|
||
// /**
|
||
// * @brief Set the voxel size used for downsampling in bufferCloud().
|
||
// * Should match the costmap resolution (default 0.05 m).
|
||
// */
|
||
// inline void setVoxelSize(double voxel_size)
|
||
// {
|
||
// voxel_size_ = voxel_size;
|
||
// inv_voxel_size_ = (voxel_size > 1e-9) ? 1.0 / voxel_size : 20.0;
|
||
// }
|
||
|
||
// /**
|
||
// * @brief Transforms a PointCloud to the global frame, downsamples it via
|
||
// * voxel grid (one representative point per costmap cell), applies
|
||
// * height filtering, and buffers the result.
|
||
// */
|
||
// void bufferCloud(const robot_sensor_msgs::PointCloud2& cloud);
|
||
|
||
// /**
|
||
// * @brief Pushes copies of all current observations onto the end of the vector passed in
|
||
// * @param observations The vector to be filled
|
||
// */
|
||
// void getObservations(std::vector<Observation>& observations);
|
||
|
||
// /**
|
||
// * @brief Check if the observation buffer is being updated at its expected rate
|
||
// * @return True if it is being updated at the expected rate, false otherwise
|
||
// */
|
||
// bool isCurrent() const;
|
||
|
||
// /**
|
||
// * @brief Lock the observation buffer
|
||
// */
|
||
// inline void lock() { lock_.lock(); }
|
||
|
||
// /**
|
||
// * @brief Unlock the observation buffer
|
||
// */
|
||
// inline void unlock() { lock_.unlock(); }
|
||
|
||
// /**
|
||
// * @brief Reset last updated timestamp
|
||
// */
|
||
// void resetLastUpdated();
|
||
|
||
// private:
|
||
// /**
|
||
// * @brief Removes any stale observations from the buffer list
|
||
// */
|
||
// void purgeStaleObservations();
|
||
|
||
// // ── Transform helper ────────────────────────────────────────────────────
|
||
// // Encode a TF transform as a plain 4×4 double matrix so the hot loop in
|
||
// // bufferCloud can do a simple FMA multiply without any virtual dispatch,
|
||
// // exception handling, or iterator overhead.
|
||
// struct Transform4x4
|
||
// {
|
||
// double m[4][4];
|
||
// };
|
||
|
||
// static inline Transform4x4 extractMatrix(const tf3::TransformStampedMsg& tfm)
|
||
// {
|
||
// const auto& t = tfm.transform.translation;
|
||
// const auto& q = tfm.transform.rotation;
|
||
|
||
// const double qx = q.x, qy = q.y, qz = q.z, qw = q.w;
|
||
// Transform4x4 M;
|
||
// // Row 0
|
||
// M.m[0][0] = 1.0 - 2.0*(qy*qy + qz*qz);
|
||
// M.m[0][1] = 2.0*(qx*qy - qz*qw);
|
||
// M.m[0][2] = 2.0*(qx*qz + qy*qw);
|
||
// M.m[0][3] = t.x;
|
||
// // Row 1
|
||
// M.m[1][0] = 2.0*(qx*qy + qz*qw);
|
||
// M.m[1][1] = 1.0 - 2.0*(qx*qx + qz*qz);
|
||
// M.m[1][2] = 2.0*(qy*qz - qx*qw);
|
||
// M.m[1][3] = t.y;
|
||
// // Row 2
|
||
// M.m[2][0] = 2.0*(qx*qz - qy*qw);
|
||
// M.m[2][1] = 2.0*(qy*qz + qx*qw);
|
||
// M.m[2][2] = 1.0 - 2.0*(qx*qx + qy*qy);
|
||
// M.m[2][3] = t.z;
|
||
// // Row 3 (homogeneous)
|
||
// M.m[3][0] = 0.0; M.m[3][1] = 0.0; M.m[3][2] = 0.0; M.m[3][3] = 1.0;
|
||
// return M;
|
||
// }
|
||
|
||
// // ── Data members ────────────────────────────────────────────────────────
|
||
// tf3::BufferCore& tf3_buffer_;
|
||
// const robot::Duration observation_keep_time_;
|
||
// const robot::Duration expected_update_rate_;
|
||
// robot::Time last_updated_;
|
||
// std::string global_frame_;
|
||
// std::string sensor_frame_;
|
||
// std::list<Observation> observation_list_;
|
||
// std::string topic_name_;
|
||
// double min_obstacle_height_;
|
||
// double max_obstacle_height_;
|
||
// boost::recursive_mutex lock_;
|
||
// double obstacle_range_;
|
||
// double raytrace_range_;
|
||
// double tf_tolerance_;
|
||
|
||
// // Voxel-grid downsampling parameters (set via setVoxelSize)
|
||
// double voxel_size_ = 0.05; // metres – match costmap resolution
|
||
// double inv_voxel_size_ = 20.0; // 1/voxel_size_, cached
|
||
// };
|
||
|
||
// } // namespace robot_costmap_2d
|
||
// #endif // ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
|
||
/*********************************************************************
|
||
*
|
||
* 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
|
||
*********************************************************************/
|
||
#ifndef ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
|
||
#define ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
|
||
|
||
#include <vector>
|
||
#include <list>
|
||
#include <string>
|
||
#include <robot/robot.h>
|
||
#include <robot_costmap_2d/observation.h>
|
||
#include <tf3/buffer_core.h>
|
||
#include <robot_sensor_msgs/PointCloud2.h>
|
||
|
||
// Thread support
|
||
#include <boost/thread.hpp>
|
||
|
||
namespace robot_costmap_2d
|
||
{
|
||
/**
|
||
* @class ObservationBuffer
|
||
* @brief Takes in point clouds from sensors, transforms them to the desired frame, and stores them
|
||
*/
|
||
class ObservationBuffer
|
||
{
|
||
public:
|
||
/**
|
||
* @brief Constructs an observation buffer
|
||
* @param topic_name The topic of the observations, used as an identifier for error and warning messages
|
||
* @param observation_keep_time Defines the persistence of observations in seconds, 0 means only keep the latest
|
||
* @param expected_update_rate How often this buffer is expected to be updated, 0 means there is no limit
|
||
* @param min_obstacle_height The minimum height of a hitpoint to be considered legal
|
||
* @param max_obstacle_height The minimum height of a hitpoint to be considered legal
|
||
* @param obstacle_range The range to which the sensor should be trusted for inserting obstacles
|
||
* @param raytrace_range The range to which the sensor should be trusted for raytracing to clear out space
|
||
* @param tf2_buffer A reference to a tf2 Buffer
|
||
* @param global_frame The frame to transform PointClouds into
|
||
* @param sensor_frame The frame of the origin of the sensor, can be left blank to be read from the messages
|
||
* @param tf_tolerance The amount of time to wait for a transform to be available when setting a new global frame
|
||
*/
|
||
ObservationBuffer(std::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, std::string global_frame,
|
||
std::string sensor_frame, double tf_tolerance);
|
||
|
||
/**
|
||
* @brief Destructor... cleans up
|
||
*/
|
||
~ObservationBuffer();
|
||
|
||
/**
|
||
* @brief Sets the global frame of an observation buffer. This will
|
||
* transform all the currently cached observations to the new global
|
||
* frame
|
||
* @param new_global_frame The name of the new global frame.
|
||
* @return True if the operation succeeds, false otherwise
|
||
*/
|
||
bool setGlobalFrame(const std::string new_global_frame);
|
||
|
||
/**
|
||
* @brief Transforms a PointCloud to the global frame and buffers it
|
||
* <b>Note: The burden is on the user to make sure the transform is available... ie they should use a MessageNotifier</b>
|
||
* @param cloud The cloud to be buffered
|
||
*/
|
||
void bufferCloud(const robot_sensor_msgs::PointCloud2& cloud);
|
||
|
||
/**
|
||
* @brief Pushes copies of all current observations onto the end of the vector passed in
|
||
* @param observations The vector to be filled
|
||
*/
|
||
void getObservations(std::vector<Observation>& observations);
|
||
|
||
/**
|
||
* @brief Check if the observation buffer is being update at its expected rate
|
||
* @return True if it is being updated at the expected rate, false otherwise
|
||
*/
|
||
bool isCurrent() const;
|
||
|
||
/**
|
||
* @brief Lock the observation buffer
|
||
*/
|
||
inline void lock()
|
||
{
|
||
lock_.lock();
|
||
}
|
||
|
||
/**
|
||
* @brief Lock the observation buffer
|
||
*/
|
||
inline void unlock()
|
||
{
|
||
lock_.unlock();
|
||
}
|
||
|
||
/**
|
||
* @brief Reset last updated timestamp
|
||
*/
|
||
void resetLastUpdated();
|
||
|
||
private:
|
||
/**
|
||
* @brief Removes any stale observations from the buffer list
|
||
*/
|
||
void purgeStaleObservations();
|
||
|
||
tf3::BufferCore& tf3_buffer_;
|
||
const robot::Duration observation_keep_time_;
|
||
const robot::Duration expected_update_rate_;
|
||
robot::Time last_updated_;
|
||
std::string global_frame_;
|
||
std::string sensor_frame_;
|
||
std::list<Observation> observation_list_;
|
||
std::string topic_name_;
|
||
double min_obstacle_height_, max_obstacle_height_;
|
||
boost::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely
|
||
double obstacle_range_, raytrace_range_;
|
||
double tf_tolerance_;
|
||
};
|
||
} // namespace robot_costmap_2d
|
||
#endif // ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
|