128 lines
4.3 KiB
C++
128 lines
4.3 KiB
C++
#ifndef COSTMAP_2D_OBSERVATION_BUFFER_H_
|
|
#define COSTMAP_2D_OBSERVATION_BUFFER_H_
|
|
|
|
#include <vector>
|
|
#include <list>
|
|
#include <string>
|
|
#include <chrono>
|
|
// #include <ros/time.h>
|
|
#include <robot/time.h>
|
|
#include <robot/duration.h>
|
|
#include <costmap_2d/observation.h>
|
|
#include <tf3/buffer_core.h>
|
|
#include <sensor_msgs/PointCloud2.h>
|
|
#include <geometry_msgs/PointStamped.h>
|
|
#include <geometry_msgs/Point.h>
|
|
|
|
|
|
// Thread support
|
|
#include <boost/thread.hpp>
|
|
|
|
namespace 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 tf3 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& tf2_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 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& tf2_buffer_;
|
|
// const ros::Duration observation_keep_time_;
|
|
// const ros::Duration expected_update_rate_;
|
|
// ros::Time last_updated_;
|
|
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 costmap_2d
|
|
#endif // COSTMAP_2D_OBSERVATION_BUFFER_H_
|