Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <ros/ros.h>
00038 #include <cereal_port/CerealPort.h>
00039
00040 #include <list>
00041 #include <vector>
00042 #include <string>
00043
00044 #include <lse_sensor_msgs/Thermistor.h>
00045 #include <lse_sensor_msgs/Anemometer.h>
00046 #include <lse_sensor_msgs/Nostril.h>
00047 #include <lse_sensor_msgs/TPA.h>
00048 #include <lse_sensor_msgs/Range.h>
00049
00050 #define VERSION 10
00051
00052
00053 #define ARDUSIM_RANGE 0
00054 #define ARDUSIM_NOSE 1
00055 #define ARDUSIM_TPA 2
00056 #define ARDUSIM_ANEMOMETER 3
00057 #define ARDUSIM_NUNCHUCK 4
00058 #define ARDUSIM_SHTXX 5
00059
00060
00061 #define ARDUSIM_QUERY 'Q'
00062 #define ARDUSIM_STREAM 'S'
00063 #define ARDUSIM_INFO 'I'
00064 #define ARDUSIM_AUTO 'A'
00065 #define ARDUSIM_VERSION 'V'
00066
00067
00068 #define STRT '@'
00069 #define END 'e'
00070 #define SPRT ','
00071
00072 namespace ardusim
00073 {
00074
00080 class Ardusim
00081 {
00082 public:
00083
00085
00090 Ardusim(std::string port_name);
00092 ~Ardusim();
00093
00095
00102 int checkVersion(int timeout);
00103
00105
00115 bool sensorDiscovery(int timeout){ return autoRequests(timeout, ARDUSIM_AUTO); }
00117
00127 bool setAutoRequests(int timeout){ return autoRequests(timeout, ARDUSIM_INFO); }
00128
00130
00137 void setRequests(int * requests, int num_of_requests);
00139
00145 void getRequests(std::list<int> * requests);
00147
00159 bool getSensorData(int timeout);
00160
00162
00171 bool loadAnemometerCalibFile(std::string * file_path);
00172
00173
00174 bool getRange(std::vector<lse_sensor_msgs::Range> * range);
00175 bool getNose(std::vector<lse_sensor_msgs::Nostril> * nose);
00176 bool getTPA(std::vector<lse_sensor_msgs::TPA> * tpa);
00177 bool getAnemometer(std::vector<lse_sensor_msgs::Anemometer> * anemometer);
00178 bool getThermistor(std::vector<lse_sensor_msgs::Thermistor> * thermistor);
00179
00180 private:
00181
00182
00183 cereal::CerealPort * serial_port_;
00184
00186 std::list<int> requests_;
00188 ros::Time now_;
00189
00191
00200 bool sendCommand(int timeout, char mode, std::string * reply);
00201
00203
00214 bool autoRequests(int timeout, char mode);
00215
00217
00226 void addValue(std::string * data, int value);
00228
00237 int getValue(std::string * data);
00238
00239
00241
00248 bool parseData(std::string * data);
00249
00251 void parseRange(std::string * data);
00253 void parseNose(std::string * data);
00255 void parseTPA(std::string * data);
00257 void parseAnemometer(std::string * data);
00258
00260 std::vector<lse_sensor_msgs::Range> range_msgs_;
00262 std::vector<lse_sensor_msgs::Nostril> nose_msgs_;
00264 std::vector<lse_sensor_msgs::TPA> tpa_msgs_;
00266 std::vector<lse_sensor_msgs::Anemometer> anemometer_msgs_;
00268 std::vector<lse_sensor_msgs::Thermistor> thermistor_msgs_;
00269
00270 struct AnemometerCalibData
00271 {
00272 float angle;
00273 float velocity;
00274 int ch0, ch1, ch2, ch3;
00275 };
00276
00277 std::vector<AnemometerCalibData> calib_data_;
00278
00279 struct TopDistances
00280 {
00281 float velocity;
00282
00283 int index[2];
00284 float distance[2];
00285 float weight[2];
00286 };
00287 };
00288
00289 }
00290
00291
00292