00001 #ifndef B21_DRIVER_H 00002 #define B21_DRIVER_H 00003 00004 #include <rflex/rflex_driver.h> 00005 #include <sensor_msgs/PointCloud.h> 00006 00032 class B21 : public RFLEX { 00033 public: 00034 B21(); 00035 virtual ~B21(); 00036 void setSonarPower(bool); 00037 float getDistance(); 00038 float getBearing(); 00039 float getTranslationalVelocity() const; 00040 float getRotationalVelocity() const; 00041 float getVoltage() const; 00042 bool isPluggedIn() const; 00043 int getNumBodySonars() const; 00044 int getNumBaseSonars() const; 00045 00049 void getBodySonarReadings(float* readings) const; 00053 void getBaseSonarReadings(float* readings) const; 00054 00057 void getBodySonarPoints(sensor_msgs::PointCloud* cloud) const; 00058 00061 void getBaseSonarPoints(sensor_msgs::PointCloud* cloud) const; 00062 00067 int getBodyBumps(sensor_msgs::PointCloud* cloud) const; 00068 00072 int getBaseBumps(sensor_msgs::PointCloud* cloud) const; 00073 00078 void setMovement(float tvel, float rvel, float acceleration); 00079 00083 void processDioEvent(unsigned char address, unsigned short data); 00084 00088 bool isOdomReady() const { 00089 return odomReady==3; 00090 } 00091 00092 private: 00095 void getSonarReadings(const int ringi, float* readings) const; 00098 void getSonarPoints(const int ringi, sensor_msgs::PointCloud* cloud) const; 00099 00104 int getBumps(const int index, sensor_msgs::PointCloud* cloud) const; 00105 00106 int first_distance; 00107 bool found_distance; 00108 int home_bearing; 00109 int** bumps; 00110 00111 // Not allowed to use these 00112 B21(const B21 &b21); 00113 B21 &operator=(const B21 &b21); 00114 }; 00115 00116 #endif 00117