Main MRPT website > C++ reference for MRPT 1.5.6
CGillAnemometer.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 
11 
12 #include "hwdrivers-precomp.h" // Precompiled headers
14 #include <mrpt/system/threads.h>
15 #include <mrpt/system/datetime.h>
16 
17 #include <iostream>
18 #include <iterator>
19 #include <sstream>
20 
21 using namespace std;
22 using namespace mrpt::utils;
23 using namespace mrpt::hwdrivers;
24 
25 
27 
28 /* -----------------------------------------------------
29  Constructor
30  ----------------------------------------------------- */
32 {
33  m_sensorLabel = "WINDSONIC";
34 }
35 
36 
37 /* -----------------------------------------------------
38  loadConfig_sensorSpecific
39  ----------------------------------------------------- */
40 void CGillAnemometer::loadConfig_sensorSpecific(
41  const mrpt::utils::CConfigFileBase &configSource,
42  const std::string &iniSection )
43 {
44 #ifdef MRPT_OS_WINDOWS
45  com_port = configSource.read_string(iniSection, "COM_port_WIN", "COM1", true ) ;
46 #else
47  com_port = configSource.read_string(iniSection, "COM_port_LIN", "/dev/tty0", true );
48 #endif
49 
50  com_bauds = configSource.read_int( iniSection, "COM_baudRate",9600, false );
51 
52  pose_x = configSource.read_float(iniSection,"pose_x",0,true);
53  pose_y = configSource.read_float(iniSection,"pose_y",0,true);
54  pose_z = configSource.read_float(iniSection,"pose_z",0,true);
55  pose_roll = configSource.read_float(iniSection,"pose_roll",0,true);
56  pose_pitch = configSource.read_float(iniSection,"pose_pitch",0,true);
57  pose_yaw = configSource.read_float(iniSection,"pose_yaw",0,true);
58 }
59 
60 
61 /* -----------------------------------------------------
62  tryToOpenTheCOM
63 ----------------------------------------------------- */
64 bool CGillAnemometer::tryToOpenTheCOM()
65 {
66  if (COM.isOpen())
67  return true; // Already open
68 
69  if (m_verbose) cout << "[CGillAnemometer] Opening " << com_port << " @ " <<com_bauds << endl;
70 
71  try
72  {
73  COM.open(com_port);
74  // Config:
75  COM.setConfig( com_bauds, 0, 8, 1 );
76  //COM.setTimeouts( 1, 0, 1, 1, 1 );
77  COM.setTimeouts(50,1,100, 1,20);
78  //mrpt::system::sleep(10);
79  COM.purgeBuffers();
80  //mrpt::system::sleep(10);
81 
82  return true; // All OK!
83  }
84  catch (std::exception &e)
85  {
86  std::cerr << "[CGillAnemometer::tryToOpenTheCOM] Error opening or configuring the serial port:" << std::endl << e.what();
87  COM.close();
88  return false;
89  }
90  catch (...)
91  {
92  std::cerr << "[CGillAnemometer::tryToOpenTheCOM] Error opening or configuring the serial port." << std::endl;
93  COM.close();
94  return false;
95  }
96 }
97 
98 /* -----------------------------------------------------
99  doProcess
100 ----------------------------------------------------- */
101 void CGillAnemometer::doProcess()
102 {
103  // Is the COM open?
104  if (!tryToOpenTheCOM())
105  {
106  m_state = ssError;
107  printf("ERROR: No observation received from the Anemometer!\n");
108  THROW_EXCEPTION("Cannot open the serial port");
109  }
110 
111  mrpt::obs::CObservationWindSensorPtr obsPtr = mrpt::obs::CObservationWindSensor::Create();
112  bool have_reading = false;
113  std::string wind_reading;
114  bool time_out = false;
115 
116  try
117  {
118  while (!have_reading)
119  {
120  // Read info into string: (default is Polar continuous)
121  // format = <STX>Q, dir, speed, units, status, <ETX>
122  // Q -> Sensor identifier
123  // dir -> 3 digits (0-359)
124  // speed -> %3.2f
125  // units -> M (m/s), K (km/h)
126  // status -> 00 = ok, else is an Error Code
127 
128  wind_reading = COM.ReadString(500,&time_out);
129  if (time_out)
130  {
131  cout << "[CGillAnemometer] " << com_port << " @ " <<com_bauds << " - measurement Timed-Out" << endl;
133  }
134  else
135  have_reading = true;
136  }
137 
138  // parse format = <STX>Q, dir, speed, units, status, <ETX>
139  std::deque<std::string> list;
140  mrpt::system::tokenize(wind_reading, ",", list);
141  if (list.size() == 6)
142  {
143  //Status
144  int status = atoi(list.at(4).c_str());
145  if (status == 0)
146  {
147  //Units
148  std::string s_units = list.at(3);
149  //Module
150  std::string s_speed = list.at(2);
151  if (s_units == "M")
152  obsPtr->speed = atof(s_speed.c_str());
153  else if (s_units == "K")
154  obsPtr->speed = atof(s_speed.c_str())*1000/3600;
155  else
156  {
157  printf("ERROR: WindSonic measurement units not supported: %s\n", s_units.c_str());
158  obsPtr->speed = 0.0;
159  }
160  //angle
161  std::string s_direction = list.at(1);
162  obsPtr->direction = atof(s_direction.c_str());
163 
164  //Prepare observation
165  obsPtr->sensorLabel = m_sensorLabel;
166  obsPtr->timestamp = mrpt::system::getCurrentTime();
167  obsPtr->sensorPoseOnRobot = mrpt::poses::CPose3D(pose_x, pose_y, pose_z, pose_yaw, pose_pitch, pose_roll);
168  appendObservation(obsPtr);
169  }
170  else
171  printf("ERROR: WindSonic error code %u\n", status);
172  }
173  else if (list.size() == 5) //when there is no wind, the sensor does not provide a wind direction
174  {
175  //Status
176  int status = atoi(list.at(3).c_str());
177  if (status == 0)
178  {
179  //Units
180  std::string s_units = list.at(2);
181  //module
182  std::string s_speed = list.at(1);
183  if (s_units == "M")
184  obsPtr->speed = atof(s_speed.c_str());
185  else if (s_units == "K")
186  obsPtr->speed = atof(s_speed.c_str()) * 1000 / 3600;
187  else
188  {
189  printf("ERROR: WindSonic measurement units not supported: %s\n", s_units.c_str());
190  obsPtr->speed = 0.0;
191  }
192  //Angle
193  obsPtr->direction = 0.0;
194 
195  //Prepare observation
196  obsPtr->sensorLabel = m_sensorLabel;
197  obsPtr->timestamp = mrpt::system::getCurrentTime();
198  obsPtr->sensorPoseOnRobot = mrpt::poses::CPose3D(pose_x, pose_y, pose_z, pose_yaw, pose_pitch, pose_roll);
199  appendObservation(obsPtr);
200  }
201  else
202  printf("ERROR: WindSonic error code %u\n", status);
203  }
204  else
205  {
206  printf("ERROR: Windsonic reading incorrect format: %s [%u]\n", wind_reading.c_str(),static_cast<unsigned int>(list.size()) );
207  }
208 
209  }
210  catch (std::exception &e)
211  {
212  std::cerr << "[CGillAnemometer::doProcess] Error:" << std::endl << e.what();
213  }
214  catch (...)
215  {
216  std::cerr << "[CGillAnemometer::doProcess] Unknown Error" << std::endl;
217  }
218 }
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
static CObservationWindSensorPtr Create()
mrpt::system::TTimeStamp BASE_IMPEXP getCurrentTime()
Returns the current (UTC) system time.
Definition: datetime.cpp:71
#define THROW_EXCEPTION(msg)
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
This class allows loading and storing values and vectors of different types from a configuration text...
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
void BASE_IMPEXP sleep(int time_ms) MRPT_NO_THROWS
An OS-independent method for sending the current thread to "sleep" for a given period of time...
Definition: threads.cpp:57
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
GLsizei const GLcharARB ** string
Definition: glew.h:3293
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
This class implements a driver for the Gill Windsonic Option 1 Anemometer The sensor is accessed via ...
_u8 status
Definition: rplidar_cmd.h:21
void BASE_IMPEXP tokenize(const std::string &inString, const std::string &inDelimiters, std::deque< std::string > &outTokens, bool skipBlankTokens=true) MRPT_NO_THROWS
Tokenizes a string according to a set of delimiting characters.



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018