SensorNet.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2010, ISR University of Coimbra.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of the ISR University of Coimbra nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: Gonçalo Cabrita and Pedro Sousa on 25/10/2010
00036 *********************************************************************/
00037 #include "SensorNet.h"
00038 
00039 SensorNet::SensorNet(std::string port_name, int baud_rate)
00040 {
00041         serial_port = new cereal::CerealPort();
00042         
00043         try{ serial_port->open(port_name.c_str(), baud_rate); }
00044         catch(cereal::Exception& e)
00045         {
00046                 ROS_FATAL("SensorNet -- Could not connect to the network!");
00047                 ROS_BREAK();
00048         }
00049         
00050         start_time = ros::Time::now();
00051         
00052         timeout = 200; //ms
00053 }
00054 
00055 SensorNet::~SensorNet()
00056 {
00057         try{ serial_port->close(); }
00058         catch(cereal::Exception& e)
00059         { 
00060                 ROS_ERROR("SensorNet -- Could not close the serial port!");
00061         }
00062 
00063         delete serial_port;
00064 }
00065 
00066 bool SensorNet::sendCommand(char address, char command)
00067 {
00068         std::string data;
00069         
00070         data+=SN_START;
00071         data+=address;
00072         data+=command;
00073         data+=CR;
00074         
00075         if(DEBUG) ROS_INFO("SensorNet [DEBUG] - Sending command %c to address %c - %s", command, address, data.c_str());
00076         
00077         try{ serial_port->write(data.c_str()); }
00078         catch(cereal::Exception& e)
00079         {
00080                 ROS_ERROR("SensorNet - sendCommand - Error sending command %c to node %c.", command, address);
00081                 return false;
00082         }
00083         return true;
00084 }
00085 
00086 bool SensorNet::waitForIt(char address)
00087 {
00088         std::string reply;
00089 
00090         try{ serial_port->readBetween(&reply, SN_START, (char)CR, timeout); }
00091         catch(cereal::TimeoutException& te)
00092         { 
00093                 ROS_ERROR("SensorNet - waitForIt - Timeout waiting for ack from node %c.", address);
00094                 return false;
00095         }
00096         catch(cereal::Exception& e)
00097         { 
00098                 ROS_ERROR("SensorNet - waitForIt - Error reading ack from node %c: \n%s", address, e.what());
00099                 return false;
00100         }
00101         
00102         if(reply[1]==address) return true;
00103         ROS_ERROR("SensorNet - waitForIt - Wrong ack received: got %c instead of %c", reply[1], address);
00104         return false;
00105 }
00106 
00107 bool SensorNet::scanNodes(int period)
00108 {
00109         std::string reply;
00110         bool timedout;
00111         
00112         for(char i='A' ; i<SN_BROADCAST ; i++)
00113         {
00114                 timedout=false;
00115                 if(sendCommand(i, SN_GET_INFO))
00116                 {
00117                         try{ serial_port->readBetween(&reply, SN_START, (char)CR, timeout); }
00118                         catch(cereal::TimeoutException& te)
00119                         { 
00120                                 if(DEBUG) ROS_WARN("SensorNet - scanNodes - Timeout, skipping node %c.", i);
00121                                 timedout=true;
00122                         }
00123                         catch(cereal::Exception& e)
00124                         { 
00125                                 ROS_ERROR("SensorNet - scanNodes - Error reading from node %c: \n%s", i, e.what());
00126                                 return false;
00127                         }
00128                         
00129                         if(DEBUG) ROS_INFO("SensorNet [DEBUG] - Scanning... Reply from node %c - %s", i, reply.c_str());
00130                         
00131                         if(!timedout)
00132                         {
00133                                 int version = extractInt(reply.substr(4,reply.size()-4));
00134                                 if(version==SN_VERSION)
00135                                 {
00136                                         nodes.push_back(SensorNet::Node(reply[1], reply[2], period));
00137                                         ROS_INFO("SensorNet - Found %s node on address %c with software version %d.", reply[2]==SN_SENSOR ? "a sensor" : "an actuator", i, SN_VERSION);
00138                                 }
00139                                 else
00140                                 {
00141                                         ROS_WARN("SensorNet - Found node %c but its software version %d is not compatible with version %d. Discarding this node->..", i, version, SN_VERSION);
00142                                 }
00143                         }
00144                 }
00145         }
00146         return true;
00147 }
00148 
00149 bool SensorNet::syncNodes()
00150 {
00151         std::string data;
00152 
00153         data+=SN_START;
00154         data+=SN_BROADCAST;
00155         data+=SN_SYNC_TIME;
00156         appendTime(&data, ros::Time::now());
00157         data+=CR;
00158         
00159         if(DEBUG) ROS_INFO("SensorNet [DEBUG] - Syncing all nodes - %s", data.c_str());
00160 
00161         try{ serial_port->write(data.c_str()); }
00162         catch(cereal::Exception& e)
00163         {
00164                 ROS_ERROR("SensorNet - syncTime - Failed to sync nodes.");
00165                 return false;
00166         }
00167         return true;
00168 }
00169 
00170 void SensorNet::appendTime(std::string * data, ros::Time time)
00171 {
00172         ros::Duration t = time - start_time;
00173 
00174         int hh = 0;
00175         int mm = 0;
00176         int ss = (int)t.toSec();
00177         int d = (int)((t.toSec()-(double)ss)*10);
00178         
00179         if(ss>60)
00180         {
00181                 mm = (int)(ss/60);
00182                 ss -= 60*mm;
00183         }
00184         if(mm>60)
00185         {
00186                 hh = (int)(mm/60);
00187                 mm -= 60*hh;
00188         }
00189         if(hh>99)
00190         {
00191                 hh=99;
00192                 mm=99;
00193                 ss=99;
00194                 d=9;
00195         }
00196         
00197         char time_chars[8];
00198         sprintf(time_chars, "%02d%02d%02d%1d", hh, mm, ss, d);
00199         data->append(time_chars);
00200 }
00201 
00202 ros::Time SensorNet::extractTime(std::string data)
00203 {
00204         char time_chars[3] = {0,0,0};
00205         double secs = 0.0;
00206         int temp;
00207         
00208         //hh
00209         time_chars[0] = data.at(0);
00210         time_chars[1] = data.at(1);
00211         sscanf(time_chars, "%d", &temp);
00212         secs = temp*60*60;
00213         //mm
00214         time_chars[0] = data.at(2);
00215         time_chars[1] = data.at(3);
00216         sscanf(time_chars, "%d", &temp);
00217         secs += temp*60;
00218         //ss
00219         time_chars[0] = data.at(4);
00220         time_chars[1] = data.at(5);
00221         sscanf(time_chars, "%d", &temp);
00222         secs += temp;
00223         //dd
00224         time_chars[0] = data.at(6);
00225         time_chars[1] = 0;
00226         sscanf(time_chars, "%d", &temp);
00227         secs += temp/10;
00228         
00229         return start_time + ros::Duration(secs);
00230 }
00231 
00232 void SensorNet::appendInt(std::string * data, int value)
00233 {
00234         char value_str[8];
00235         sprintf(value_str, "%d", value);
00236         
00237         data->append(value_str);
00238 }
00239 
00240 int SensorNet::extractInt(std::string data)
00241 {
00242         int value;
00243 
00244         sscanf(data.c_str(), "%d", &value);
00245         
00246         return value;
00247 }
00248 
00249 // Node functions
00250 bool SensorNet::syncTime(SensorNet::Node * node)
00251 {
00252         std::string data;
00253 
00254         data+=SN_START;
00255         data+=node->address();
00256         data+=SN_SYNC_TIME;
00257         appendTime(&data, ros::Time::now());
00258         data+=CR;
00259         
00260         if(DEBUG) ROS_INFO("SensorNet [DEBUG] - Syncing time on node %c - %s", node->address(), data.c_str());
00261 
00262         try{ serial_port->write(data.c_str()); }
00263         catch(cereal::Exception& e)
00264         {
00265                 ROS_ERROR("SensorNet - syncTime - Failed to sync node %c.", node->address());
00266                 return false;
00267         }
00268         return waitForIt(node->address());
00269 }
00270 
00271 bool SensorNet::setTime(SensorNet::Node * node, ros::Time time)
00272 {
00273         std::string data;
00274 
00275         data+=SN_START;
00276         data+=node->address();
00277         data+=SN_SET_TIME;
00278         appendTime(&data, time);
00279         data+=SN_SEP;
00280         appendInt(&data, node->period());
00281         data+=CR;
00282 
00283         if(DEBUG) ROS_INFO("SensorNet [DEBUG] - Setting time on node %c - %s", node->address(), data.c_str());
00284 
00285         try{ serial_port->write(data.c_str()); }
00286         catch(cereal::Exception& e)
00287         {
00288                 ROS_ERROR("SensorNet - setTime - Failed to set time on node %c.", node->address());
00289                 return false;
00290         }
00291         return waitForIt(node->address());
00292 }
00293 
00294 bool SensorNet::getTime(SensorNet::Node * node, ros::Time * current_time, ros::Time * next_reading)
00295 {
00296         std::string reply;
00297         
00298         if(!sendCommand(node->address(), SN_GET_TIME)) return false;
00299         
00300         try{ serial_port->readBetween(&reply, SN_START, (char)CR, timeout); }
00301         catch(cereal::TimeoutException& te)
00302         { 
00303                 ROS_ERROR("SensorNet - getTime - Timeout getting time on node %c.", node->address());
00304                 return false;
00305         }
00306         catch(cereal::Exception& e)
00307         { 
00308                 ROS_ERROR("SensorNet - getTime - Error getting time from node %c: \n%s", node->address(), e.what());
00309                 return false;
00310         }
00311         
00312         if(DEBUG) ROS_INFO("SensorNet [DEBUG] - Getting time from node %c - %s", node->address(), reply.c_str());
00313         
00314         *current_time = extractTime(reply.substr(reply.find_first_of(node->address())+1,7));
00315         *next_reading = extractTime(reply.substr(reply.find_first_of(SN_SEP)+1,7));
00316         node->setPeriod(extractInt(reply.substr(reply.find_last_of(SN_SEP)+1,reply.size()-reply.find_last_of(SN_SEP)+1)));
00317         
00318         return true;
00319 }
00320 
00321 bool SensorNet::getSensorReadings(SensorNet::Node * node, std::vector<lse_sensor_msgs::Nostril> * mox, std::vector<lse_sensor_msgs::Anemometer> * thermistor)
00322 {
00323         if(!node->sensor()) return false;
00324         
00325         std::string reply;
00326         
00327         if(!sendCommand(node->address(), SN_READ_SENSORS)) return false;
00328         
00329         try{ serial_port->readBetween(&reply, SN_START, (char)CR, timeout); }
00330         catch(cereal::TimeoutException& te)
00331         { 
00332                 ROS_ERROR("SensorNet - getSensorReadings - Timeout getting sensor readings header on node %c.", node->address());
00333                 return false;
00334         }
00335         catch(cereal::Exception& e)
00336         { 
00337                 ROS_ERROR("SensorNet - getSensorReadings - Error reading sensors header from node %c: \n%s", node->address(), e.what());
00338                 return false;
00339         }
00340         
00341         if(DEBUG) ROS_INFO("SensorNet [DEBUG] - Getting sensors from node %c - %s", node->address(), reply.c_str());
00342         
00343         ros::Time first_reading = extractTime(reply.substr(reply.find_first_of(node->address())+1,7));
00344         
00345         int n = extractInt(reply.substr(reply.find_last_of(SN_SEP)+1,reply.size()-reply.find_last_of(SN_SEP)+1));
00346         
00347         if(DEBUG) ROS_INFO("SensorNet [DEBUG] - %d sensor readings coming this way!!!", n);
00348         
00349         for(int i=0 ; i<n ; i++)
00350         {
00351                 try{ serial_port->readBetween(&reply, SN_START, (char)CR, timeout); }
00352                 catch(cereal::TimeoutException& te)
00353                 { 
00354                         ROS_ERROR("SensorNet - getSensorReadings - Timeout getting sensor readings on node %c.", node->address());
00355                         return false;
00356                 }
00357                 catch(cereal::Exception& e)
00358                 { 
00359                         ROS_ERROR("SensorNet - getSensorReadings - Error reading sensors from node %c: \n%s", node->address(), e.what());
00360                         return false;
00361                 }
00362                 
00363                 if(DEBUG) ROS_INFO("SensorNet [DEBUG] - Getting sensor readings from node %c - %s", node->address(), reply.c_str());
00364         
00365                 int sep = reply.find_first_of(SN_SEP);
00366                 int next_sep = reply.find_first_of(SN_SEP,sep+1);
00367                 if(next_sep==-1) next_sep = reply.size()-1;
00368                 for(int j=0 ; j<4 ; j++)
00369                 {
00370                         if(node->sensors(j))
00371                         {
00372                                 lse_sensor_msgs::Nostril moxMsg;
00373                         
00374                                 char frame_id[32];
00375                                 sprintf(frame_id, "/base_node_%c_S%d", node->address(), j+1);
00376                                 moxMsg.header.frame_id = frame_id;
00377                                 moxMsg.header.stamp = first_reading + ros::Duration(node->period()*i);
00378                                 moxMsg.sensor_model = "Figaro 2620";
00379                                 moxMsg.gas_type.push_back(lse_sensor_msgs::Nostril::ORGANIC_SOLVENTS);
00380                                 moxMsg.reading = extractInt(reply.substr(sep+1,next_sep-sep));
00381                                 moxMsg.min_reading = 0.0;
00382                                 moxMsg.max_reading = 3300.0;
00383                         
00384                                 if(mox!=NULL) mox->push_back(moxMsg);
00385                                 
00386                                 sep=next_sep;
00387                                 next_sep=reply.find_first_of(SN_SEP,sep+1);
00388                                 if(next_sep==-1) next_sep = reply.size()-1;
00389                         }
00390                 }
00391                 for(int j=0 ; j<2 ; j++)
00392                 {
00393                         if(node->sensors(j+4))
00394                         {
00395                                 lse_sensor_msgs::Anemometer termMsg;
00396                                 
00397                                 char frame_id[32];
00398                                 sprintf(frame_id, "/base_node_%c_W%d", node->address(), j+1);
00399                                 termMsg.header.frame_id = frame_id;
00400                                 termMsg.header.stamp = first_reading + ros::Duration(node->period()/10*i);
00401                                 //TODO: Finish the wind speed calculation...
00402                                 termMsg.wind_speed = extractInt(reply.substr(sep+1,next_sep-sep));
00403                                 termMsg.wind_direction = 0.0;
00404                                 
00405                                 if(thermistor!=NULL) thermistor->push_back(termMsg);
00406                                 
00407                                 sep=next_sep;
00408                                 next_sep=reply.find_first_of(SN_SEP,sep+1);
00409                                 if(next_sep==-1) next_sep = reply.size()-1;
00410                         }
00411                 }
00412                 
00413         }
00414         return true;
00415 }
00416 
00417 bool SensorNet::setAllPWM(SensorNet::Node * node, int pwm1, int pwm2, int pwm3, int pwm4)
00418 {
00419         if(!node->actuator()) return false;
00420         
00421         if(pwm1>100) pwm1=100;
00422         if(pwm1<0) pwm1=0;
00423         if(pwm2>100) pwm2=100;
00424         if(pwm2<0) pwm2=0;
00425         if(pwm3>100) pwm3=100;
00426         if(pwm3<0) pwm3=0;
00427         if(pwm4>100) pwm4=100;
00428         if(pwm4<0) pwm4=0;
00429         
00430         std::string data;
00431 
00432         data+=SN_START;
00433         data+=node->address();
00434         data+=SN_SET_ALL_PWM;
00435         appendInt(&data, pwm1);
00436         data+=SN_SEP;
00437         appendInt(&data, pwm2);
00438         data+=SN_SEP;
00439         appendInt(&data, pwm3);
00440         data+=SN_SEP;
00441         appendInt(&data, pwm4);
00442         data+=CR;
00443         
00444         if(DEBUG) ROS_INFO("SensorNet [DEBUG] - Setting all PWMs on node %c - %s", node->address(), data.c_str());
00445 
00446         try{ serial_port->write(data.c_str()); }
00447         catch(cereal::Exception& e)
00448         {
00449                 ROS_ERROR("SensorNet - setAllPWM - Failed to set pwm on node %c.", node->address());
00450                 return false;
00451         }
00452         return waitForIt(node->address());
00453 }
00454 
00455 bool SensorNet::setPWM(SensorNet::Node * node, ros::Time time, unsigned int id, int pwm)
00456 {
00457         if(!node->actuator()) return false;
00458         
00459         if(pwm>100) pwm=100;
00460         if(pwm<0) pwm=0;
00461         
00462         std::string data;
00463 
00464         data+=SN_START;
00465         data+=node->address();
00466         data+=SN_SET_ALL_PWM;
00467         appendTime(&data, time);
00468         data+=SN_SEP;
00469         appendInt(&data, id);
00470         data+=SN_SEP;
00471         appendInt(&data, pwm);
00472         data+=CR;
00473 
00474         if(DEBUG) ROS_INFO("SensorNet [DEBUG] - Setting PWM on node %c - %s", node->address(), data.c_str());
00475 
00476         try{ serial_port->write(data.c_str()); }
00477         catch(cereal::Exception& e)
00478         {
00479                 ROS_ERROR("SensorNet - setPWM - Failed to set pwm on node %c.", node->address());
00480                 return false;
00481         }
00482         return waitForIt(node->address());
00483 }
00484 
00485 
00486 bool SensorNet::getInfo(SensorNet::Node * node)
00487 {
00488         std::string reply;
00489         
00490         char version[8];
00491         sprintf(version, "%d", SN_VERSION);
00492         
00493         if(!sendCommand(node->address(), SN_GET_INFO)) return false;
00494         
00495         try{ serial_port->readBetween(&reply, SN_START, (char)CR, timeout); }
00496         catch(cereal::TimeoutException& te)
00497         { 
00498                 ROS_ERROR("SensorNet - getInfo - Timeout getting info on node %c.", node->address());
00499                 return false;
00500         }
00501         catch(cereal::Exception& e)
00502         { 
00503                 ROS_ERROR("SensorNet - getInfo - Error getting info from node %c: \n%s", node->address(), e.what());
00504                 return false;
00505         }
00506         
00507         if(node->address()!=reply[1])
00508         {
00509                 ROS_ERROR("SensorNet - getInfo - Node %c came back with a different address: %c!", node->address(), reply[1]);
00510                 return false;
00511         }
00512         
00513         if(DEBUG) ROS_INFO("SensorNet [DEBUG] - Getting info on node %c - %s", node->address(), reply.c_str());
00514         
00515         node->setType(reply[2]);
00516         
00517         if(reply.compare(reply.size()-3,3,version)==0)
00518         {       
00519                 ROS_WARN("SensorNet - getInfo - Node %c seems to have an incompatible version of the software.", node->address());
00520         }
00521         
00522         return true;
00523 }
00524 
00525 bool SensorNet::setSensors(SensorNet::Node * node, int s1, int s2, int s3, int s4, int w1, int w2)
00526 {
00527         if(!node->sensor()) return false;
00528         
00529         std::string data;
00530 
00531         data+=SN_START;
00532         data+=node->address();
00533         data+=SN_SET_SENSORS;
00534         appendInt(&data, s1==0 ? 0 : 1);
00535         appendInt(&data, s2==0 ? 0 : 1);
00536         appendInt(&data, s3==0 ? 0 : 1);
00537         appendInt(&data, s4==0 ? 0 : 1);
00538         appendInt(&data, w1==0 ? 0 : 1);
00539         appendInt(&data, w2==0 ? 0 : 1);
00540         data+=CR;
00541         
00542         if(DEBUG) ROS_INFO("SensorNet [DEBUG] - Setting sensors on node %c - %s", node->address(), data.c_str());
00543 
00544         try{ serial_port->write(data.c_str()); }
00545         catch(cereal::Exception& e)
00546         {
00547                 ROS_ERROR("SensorNet - setSensors - Failed to set sensors on node %c.", node->address());
00548                 return false;
00549         }
00550         
00551         if(!waitForIt(node->address())) return false;
00552         
00553         node->setSensors(s1,s2,s3,s4,w1,w2);
00554         return true;
00555 }
00556 
00557 bool SensorNet::getSensors(SensorNet::Node * node)
00558 {
00559         if(!node->sensor()) return false;
00560 
00561         std::string reply;
00562         
00563         if(!sendCommand(node->address(), SN_GET_SENSORS)) return false;
00564         
00565         try{ serial_port->readBetween(&reply, SN_START, (char)CR, timeout); }
00566         catch(cereal::TimeoutException& te)
00567         { 
00568                 ROS_ERROR("SensorNet - getSensors - Timeout getting sensors on node %c.", node->address());
00569                 return false;
00570         }
00571         catch(cereal::Exception& e)
00572         { 
00573                 ROS_ERROR("SensorNet - getSensors - Error getting sensors from node %c: \n%s", node->address(), e.what());
00574                 return false;
00575         }
00576         
00577         if(DEBUG) ROS_INFO("SensorNet [DEBUG] - Getting sensors from node %c - %s", node->address(), reply.c_str());
00578         
00579         node->setSensors((int)(reply[2]-48),(int)(reply[3]-48),(int)(reply[4]-48),(int)(reply[5]-48),(int)(reply[6]-48),(int)(reply[7]-48));
00580         
00581         return true;
00582 }
00583 
00584 bool SensorNet::setAddress(SensorNet::Node * node, char new_address)
00585 {
00586         std::string data;
00587 
00588         data+=SN_START;
00589         data+=node->address();
00590         data+=SN_SET_ADDRESS;
00591         data+=new_address;
00592         data+=CR;
00593         
00594         if(DEBUG) ROS_INFO("SensorNet [DEBUG] - Setting a new address %c on node %c - %s", new_address, node->address(), data.c_str());
00595         
00596         try{ serial_port->write(data.c_str()); }
00597         catch(cereal::Exception& e)
00598         {
00599                 ROS_ERROR("SensorNet - setAddress - Failed to set a new address %c on node %c.", new_address, node->address());
00600                 return false;
00601         }
00602 
00603         if(!waitForIt(new_address)) return false;
00604         node->setAddress(new_address);
00605         return true;                    
00606 }
00607 
00608 // EOF


sensornet
Author(s): Gonçalo Cabrita and Pedro Sousa
autogenerated on Mon Jan 6 2014 11:27:58