00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00023 #ifndef __VELODYNE_DATA_XYZ_H
00024 #define __VELODYNE_DATA_XYZ_H
00025
00026 #include <velodyne/data_scans.h>
00027
00028 namespace Velodyne
00029 {
00034 typedef struct laserscan_xyz
00035 {
00036 float x, y, z;
00037 float heading;
00038 uint16_t revolution;
00039 uint8_t laser_number;
00040 uint8_t intensity;
00041 } laserscan_xyz_t;
00042
00049 typedef boost::function<void(const std::vector<laserscan_xyz_t> &scan,
00050 ros::Time stamp,
00051 const std::string &frame_id)> xyzCallback;
00052
00054 class DataXYZ: public DataScans
00055 {
00056 public:
00057
00058 DataXYZ(std::string ofile="", std::string anglesFile="");
00059 virtual int print(void);
00060 virtual void processPacket(const velodyne_msgs::VelodynePacket *pkt,
00061 const std::string &frame_id);
00062
00071 ros::Subscriber subscribe(ros::NodeHandle node,
00072 const std::string &topic,
00073 uint32_t queue_size,
00074 const xyzCallback callback,
00075 const ros::TransportHints &transport_hints
00076 = ros::TransportHints())
00077 {
00078 ROS_INFO("XYZ callback subscribed");
00079 cb_ = callback;
00080 return node.subscribe(topic, queue_size,
00081 &Data::processScan, (Data *) this,
00082 transport_hints);
00083 }
00084
00085 protected:
00086 void scan2xyz(const laserscan_t *scan, laserscan_xyz_t *point);
00087
00088
00089 std::vector<laserscan_xyz_t> xyzScans_;
00090
00091 private:
00092 xyzCallback cb_;
00093 };
00094
00095
00096 }
00097
00098 #endif // __VELODYNE_DATA_XYZ_H