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 <stdlib.h>
00038 #include <stdio.h>
00039 #include <math.h>
00040 #include <string.h>
00041
00042 #include "ardusim/Ardusim.h"
00043
00044 using namespace ardusim;
00045
00046 void mapFrame(std::string * name, const char * base_name, int id)
00047 {
00048 char temp_name[128];
00049 sprintf(temp_name, "%s_%d", base_name, id);
00050
00051 name->clear();
00052 name->assign(temp_name);
00053 }
00054
00055 int main(int argc, char** argv)
00056 {
00057 ros::init(argc, argv, "general_node");
00058
00059 ros::NodeHandle n;
00060 ros::NodeHandle pn("~");
00061
00062 std::vector<lse_sensor_msgs::Range> range_msgs;
00063 std::vector<lse_sensor_msgs::Nostril> nose_msgs;
00064 std::vector<lse_sensor_msgs::TPA> tpa_msgs;
00065 std::vector<lse_sensor_msgs::Anemometer> anemometer_msgs;
00066
00067 ros::Publisher range_pub;
00068 ros::Publisher nose_pub;
00069 ros::Publisher tpa_pub;
00070 ros::Publisher anemometer_pub;
00071
00072
00073 std::string port;
00074 pn.param<std::string>("port", port, "/dev/ttyUSB0");
00075 std::string sonar_frame_id;
00076 pn.param<std::string>("sonar_frame_id", sonar_frame_id, "/base_sonar");
00077 std::string nose_frame_id;
00078 pn.param<std::string>("nose_frame_id", nose_frame_id, "/base_nose");
00079 std::string tpa_frame_id;
00080 pn.param<std::string>("tpa_frame_id", tpa_frame_id, "/base_tpa");
00081 std::string anemometer_frame_id;
00082 pn.param<std::string>("anemometer_frame_id", anemometer_frame_id, "/base_anemometer");
00083 std::string file_path;
00084 pn.param<std::string>("calibration_file_path", file_path, "anemometer.csv");
00085 bool scan_sensors;
00086 pn.param("scan_sensors", scan_sensors, true);
00087
00088 Ardusim ardusim(port);
00089
00090 int counter=0;
00091 if(scan_sensors)
00092 {
00093 while(!ardusim.sensorDiscovery(500))
00094 {
00095 counter++;
00096 if(counter==3)
00097 {
00098 ROS_FATAL("Ardusim GPnode - Failed to scan sensors for request from the Arduino!");
00099 ROS_BREAK();
00100 }
00101 ROS_WARN("Ardusim GPnode - Retrying to aquire sensors for request from the Arduino...");
00102 }
00103 }
00104 else
00105 {
00106 while(!ardusim.setAutoRequests(500))
00107 {
00108 counter++;
00109 if(counter==3)
00110 {
00111 ROS_FATAL("Ardusim GPnode - Failed to aquire sensors for request from the Arduino!");
00112 ROS_BREAK();
00113 }
00114 ROS_WARN("Ardusim GPnode - Retrying to aquire sensors for request from the Arduino...");
00115 }
00116 }
00117
00118 std::list<int> requests;
00119 std::list<int>::iterator it;
00120 ardusim.getRequests(&requests);
00121
00122 it=requests.begin();
00123 while(it!=requests.end())
00124 {
00125 if(*it==ARDUSIM_RANGE)
00126 {
00127 range_pub = n.advertise<lse_sensor_msgs::Range>("range", 1);
00128 ROS_INFO("Ardusim GPnode - Advertising lse_sensor_msgs::Range on topic /range");
00129 }
00130 else if(*it==ARDUSIM_NOSE)
00131 {
00132 nose_pub = n.advertise<lse_sensor_msgs::Nostril>("nose", 1);
00133 ROS_INFO("Ardusim GPnode - Advertising lse_sensor_msgs::Nostril on topic /nose");
00134 }
00135 else if(*it==ARDUSIM_TPA)
00136 {
00137 tpa_pub = n.advertise<lse_sensor_msgs::TPA>("tpa", 1);
00138 ROS_INFO("Ardusim GPnode - Advertising lse_sensor_msgs::TPA on topic /tpa");
00139 }
00140 else if(*it==ARDUSIM_ANEMOMETER)
00141 {
00142 if(!ardusim.loadAnemometerCalibFile(&file_path))
00143 {
00144 ROS_FATAL("Ardusim GPnode - Could not load the anemometer calibration file!");
00145 ROS_BREAK();
00146 }
00147
00148 anemometer_pub = n.advertise<lse_sensor_msgs::Anemometer>("wind", 1);
00149 ROS_INFO("Ardusim GPnode - Advertising lse_sensor_msgs::Anemometer on topic /anemometer");
00150 }
00151 else
00152 {
00153 ROS_WARN("Ardusim GPnode - Ops! Found an unknown sensor id %d", *it);
00154 }
00155 ++it;
00156 }
00157
00158 ROS_INFO("Ardusim GPnode - Initiating sensor data publishing...");
00159
00160 int i;
00161 std::string frame_id;
00162
00163 ros::Rate r(10);
00164 while(ros::ok())
00165 {
00166 if(ardusim.getSensorData(100))
00167 {
00168 if(ardusim.getRange(&range_msgs))
00169 {
00170 ROS_INFO("Got Range!!!");
00171 for(i=0 ; i<range_msgs.size() ; i++)
00172 {
00173 if(range_msgs.size()==1) range_msgs[i].header.frame_id = sonar_frame_id;
00174 else
00175 {
00176 mapFrame(&frame_id, sonar_frame_id.c_str(), i);
00177 range_msgs[i].header.frame_id = frame_id;
00178 }
00179 range_msgs[i].field_of_view = 0.523598776;
00180 range_msgs[i].min_range = 0.03;
00181 range_msgs[i].max_range = 2.00;
00182
00183 range_pub.publish(range_msgs.at(i));
00184 }
00185 }
00186 if(ardusim.getNose(&nose_msgs))
00187 {
00188 for(i=0 ; i<nose_msgs.size() ; i++)
00189 {n.param<std::string>("anemometer_frame_id", anemometer_frame_id, "/base_anemometer");
00190 if(nose_msgs.size()==1) nose_msgs[i].header.frame_id = nose_frame_id;
00191 else
00192 {
00193 mapFrame(&frame_id, nose_frame_id.c_str(), i);
00194 nose_msgs[i].header.frame_id = frame_id;
00195 }
00196 nose_msgs[i].min_reading = 0.0;
00197 nose_msgs[i].max_reading = 3300.0;
00198
00199 nose_pub.publish(nose_msgs.at(i));
00200 }
00201 }
00202 if(ardusim.getTPA(&tpa_msgs))
00203 {
00204 for(i=0 ; i<tpa_msgs.size() ; i++)
00205 {
00206 if(tpa_msgs.size()==1) tpa_msgs[i].header.frame_id = tpa_frame_id;
00207 else
00208 {
00209 mapFrame(&frame_id, tpa_frame_id.c_str(), i);
00210 tpa_msgs[i].header.frame_id = frame_id;
00211 }
00212
00213 tpa_pub.publish(tpa_msgs.at(i));
00214 }
00215 }
00216 if(ardusim.getAnemometer(&anemometer_msgs))
00217 {
00218 for(i=0 ; i<anemometer_msgs.size() ; i++)
00219 {
00220 if(anemometer_msgs.size()==1) anemometer_msgs[i].header.frame_id = anemometer_frame_id;
00221 else
00222 {
00223 mapFrame(&frame_id, anemometer_frame_id.c_str(), i);
00224 anemometer_msgs[i].header.frame_id = frame_id;
00225 }
00226
00227 anemometer_pub.publish(anemometer_msgs.at(i));
00228 }
00229 }
00230 }
00231 r.sleep();
00232 }
00233 return(0);
00234 }
00235
00236
00237