22 #include "pcl_adapter.h" 24 #include <pcl/point_cloud.h> 25 #include <pcl/point_types.h> 26 #include <pcl/ros/conversions.h> 27 #include <logging/logger.h> 28 #include <aspect/pointcloud/pointcloud_manager.h> 34 class RosPointCloudAdapter::StorageAdapter
71 : __pcl_manager(pcl_manager)
79 std::map<std::string, StorageAdapter *>::iterator i;
80 for (i = __sas.begin(); i != __sas.end(); ++i) {
94 template <
typename Po
intT>
97 unsigned int &width,
unsigned int &height,
98 std::string &frame_id,
bool &is_dense,
103 frame_id = p->header.frame_id;
104 is_dense = p->is_dense;
106 std::vector<sensor_msgs::PointField> pfields;
107 pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>
108 (pcl::detail::FieldAdder<PointT>(pfields));
111 pfi.resize(pfields.size());
112 for (
unsigned int i = 0; i < pfields.size(); ++i) {
113 sensor_msgs::PointField &pf = pfields[i];
115 pf.datatype, pf.count);
129 unsigned int &width,
unsigned int &height,
130 std::string &frame_id,
bool &is_dense,
133 if (__sas.find(
id) == __sas.end()) {
140 fill_info(p, width, height, frame_id, is_dense, pfi);
145 fill_info(p, width, height, frame_id, is_dense, pfi);
148 throw Exception(
"PointCloud '%s' does not exist or unknown type",
id.c_str());
164 unsigned int &width,
unsigned int &height,
fawkes::Time &time,
165 void **data_ptr,
size_t &point_size,
size_t &num_points)
167 if (__sas.find(
id) == __sas.end()) {
187 if (__sas.find(
id) != __sas.end()) {
virtual unsigned int height() const =0
Get height of point cloud.
const StorageAdapter * get_storage_adapter(const char *id)
Get a storage adapter.
void get_data(const std::string &id, unsigned int &width, unsigned int &height, fawkes::Time &time, void **data_ptr, size_t &point_size, size_t &num_points)
Get current data of point cloud.
Information about the data fields.
virtual void get_time(fawkes::Time &time) const =0
Get last capture time.
void close(const std::string &id)
Close an adapter.
Fawkes library namespace.
A class for handling time.
virtual unsigned int width() const =0
Get width of point cloud.
void get_info(std::string &id, unsigned int &width, unsigned int &height, std::string &frame_id, bool &is_dense, V_PointFieldInfo &pfi)
Get info about point cloud.
~RosPointCloudAdapter()
Destructor.
virtual void * data_ptr() const =0
Get pointer on data.
const RefPtr< const pcl::PointCloud< PointT > > get_pointcloud(const char *id)
Get point cloud.
Base class for exceptions in Fawkes.
virtual size_t num_points() const =0
Get numer of points in point cloud.
std::vector< PointFieldInfo > V_PointFieldInfo
Vector of PointFieldInfo.
RosPointCloudAdapter(fawkes::PointCloudManager *pcl_manager, fawkes::Logger *logger)
Constructor.
virtual size_t point_size() const =0
Get size of a point.
RefPtr<> is a reference-counting shared smartpointer.
bool exists_pointcloud(const char *id)
Check if point cloud exists.