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



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020