00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00023 #ifndef __VELODYNE_DATA_SCAN_H
00024 #define __VELODYNE_DATA_SCAN_H
00025
00026 #include <velodyne/data_base.h>
00027
00028 namespace Velodyne
00029 {
00038 typedef struct laserscan
00039 {
00040 float range;
00041 float heading;
00042 float pitch;
00043 uint16_t revolution;
00044 uint8_t laser_number;
00045 uint8_t intensity;
00046 } laserscan_t;
00047
00054 typedef boost::function<void(const std::vector<laserscan_t> &scan,
00055 ros::Time time,
00056 const std::string &frame_id)> scanCallback;
00057
00059 class DataScans: public Data
00060 {
00061 public:
00062
00063 DataScans(std::string ofile="", std::string anglesFile="");
00064 virtual int print(void);
00065 virtual void processPacket(const velodyne_msgs::VelodynePacket *pkt,
00066 const std::string &frame_id);
00067
00076 ros::Subscriber subscribe(ros::NodeHandle node,
00077 const std::string &topic,
00078 uint32_t queue_size,
00079 const scanCallback callback,
00080 const ros::TransportHints &transport_hints
00081 = ros::TransportHints())
00082 {
00083 ROS_INFO("generic scan callback defined");
00084 cb_ = callback;
00085 return node.subscribe(topic, queue_size,
00086 &Data::processScan, (Data *) this,
00087 transport_hints);
00088 }
00089
00090 protected:
00091 void packet2scans(const raw_packet_t *raw, laserscan_t *scans);
00092
00093
00094 std::vector<laserscan_t> scans_;
00095
00096 private:
00097 scanCallback cb_;
00098 };
00099
00100 }
00101
00102 #endif // __VELODYNE_DATA_SCAN_H