00001 #ifndef ATRVJR_DRIVER_H 00002 #define ATRVJR_DRIVER_H 00003 00004 #include <rflex/rflex_driver.h> 00005 #include <sensor_msgs/PointCloud.h> 00006 00033 class ATRVJR : public RFLEX { 00034 public: 00035 ATRVJR(); 00036 virtual ~ATRVJR(); 00037 void setSonarPower(bool); 00038 float getDistance(); 00039 float getBearing(); 00040 float getTranslationalVelocity() const; 00041 float getRotationalVelocity() const; 00042 float getVoltage() const; 00043 bool isPluggedIn() const; 00044 int getNumBodySonars() const; 00045 int getNumBaseSonars() const; 00046 00050 void getBodySonarReadings(float* readings) const; 00054 void getBaseSonarReadings(float* readings) const; 00055 00058 void getBodySonarPoints(sensor_msgs::PointCloud* cloud) const; 00059 00062 void getBaseSonarPoints(sensor_msgs::PointCloud* cloud) const; 00063 00068 int getBodyBumps(sensor_msgs::PointCloud* cloud) const; 00069 00073 int getBaseBumps(sensor_msgs::PointCloud* cloud) const; 00074 00079 void setMovement(float tvel, float rvel, float acceleration); 00080 00084 void processDioEvent(unsigned char address, unsigned short data); 00085 00089 bool isOdomReady() const { 00090 return odomReady==3; 00091 } 00092 00093 private: 00096 void getSonarReadings(const int ringi, float* readings) const; 00099 void getSonarPoints(const int ringi, sensor_msgs::PointCloud* cloud) const; 00100 00105 int getBumps(const int index, sensor_msgs::PointCloud* cloud) const; 00106 00107 int first_distance; 00108 bool found_distance; 00109 int home_bearing; 00110 int** bumps; 00111 00112 // Not allowed to use these 00113 ATRVJR(const ATRVJR &atrvjr); 00114 ATRVJR &operator=(const ATRVJR &atrvjr); 00115 }; 00116 00117 #endif 00118