MRPT  1.9.9
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-2018, 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 #include "hwdrivers-precomp.h" // Precompiled headers
12 #include <mrpt/system/datetime.h>
13 
14 #include <iostream>
15 #include <iterator>
16 #include <sstream>
17 #include <thread>
18 
19 using namespace std;
20 using namespace mrpt::hwdrivers;
21 
23 
24 /* -----------------------------------------------------
25  Constructor
26  ----------------------------------------------------- */
27 CGillAnemometer::CGillAnemometer() { m_sensorLabel = "WINDSONIC"; }
28 /* -----------------------------------------------------
29  loadConfig_sensorSpecific
30  ----------------------------------------------------- */
31 void CGillAnemometer::loadConfig_sensorSpecific(
32  const mrpt::config::CConfigFileBase& configSource,
33  const std::string& iniSection)
34 {
35 #ifdef _WIN32
36  com_port =
37  configSource.read_string(iniSection, "COM_port_WIN", "COM1", true);
38 #else
39  com_port =
40  configSource.read_string(iniSection, "COM_port_LIN", "/dev/tty0", true);
41 #endif
42 
43  com_bauds = configSource.read_int(iniSection, "COM_baudRate", 9600, false);
44 
45  pose_x = configSource.read_float(iniSection, "pose_x", 0, true);
46  pose_y = configSource.read_float(iniSection, "pose_y", 0, true);
47  pose_z = configSource.read_float(iniSection, "pose_z", 0, true);
48  pose_roll = configSource.read_float(iniSection, "pose_roll", 0, true);
49  pose_pitch = configSource.read_float(iniSection, "pose_pitch", 0, true);
50  pose_yaw = configSource.read_float(iniSection, "pose_yaw", 0, true);
51 }
52 
53 /* -----------------------------------------------------
54  tryToOpenTheCOM
55 ----------------------------------------------------- */
56 bool CGillAnemometer::tryToOpenTheCOM()
57 {
58  if (COM.isOpen()) return true; // Already open
59 
60  if (m_verbose)
61  cout << "[CGillAnemometer] Opening " << com_port << " @ " << com_bauds
62  << endl;
63 
64  try
65  {
66  COM.open(com_port);
67  COM.setConfig(com_bauds, 0, 8, 1);
68  COM.setTimeouts(50, 1, 100, 1, 20);
69  COM.purgeBuffers();
70 
71  return true; // All OK!
72  }
73  catch (std::exception& e)
74  {
75  std::cerr << "[CGillAnemometer::tryToOpenTheCOM] Error opening or "
76  "configuring the serial port:"
77  << std::endl
78  << e.what();
79  COM.close();
80  return false;
81  }
82  catch (...)
83  {
84  std::cerr << "[CGillAnemometer::tryToOpenTheCOM] Error opening or "
85  "configuring the serial port."
86  << std::endl;
87  COM.close();
88  return false;
89  }
90 }
91 
92 /* -----------------------------------------------------
93  doProcess
94 ----------------------------------------------------- */
95 void CGillAnemometer::doProcess()
96 {
97  // Is the COM open?
98  if (!tryToOpenTheCOM())
99  {
100  m_state = ssError;
101  printf("ERROR: No observation received from the Anemometer!\n");
102  THROW_EXCEPTION("Cannot open the serial port");
103  }
104 
106  mrpt::make_aligned_shared<mrpt::obs::CObservationWindSensor>();
107  bool have_reading = false;
108  std::string wind_reading;
109  bool time_out = false;
110 
111  try
112  {
113  while (!have_reading)
114  {
115  // Read info into string: (default is Polar continuous)
116  // format = <STX>Q, dir, speed, units, status, <ETX>
117  // Q -> Sensor identifier
118  // dir -> 3 digits (0-359)
119  // speed -> %3.2f
120  // units -> M (m/s), K (km/h)
121  // status -> 00 = ok, else is an Error Code
122 
123  wind_reading = COM.ReadString(500, &time_out);
124  if (time_out)
125  {
126  cout << "[CGillAnemometer] " << com_port << " @ " << com_bauds
127  << " - measurement Timed-Out" << endl;
128  std::this_thread::sleep_for(10ms);
129  }
130  else
131  have_reading = true;
132  }
133 
134  // parse format = <STX>Q, dir, speed, units, status, <ETX>
135  std::deque<std::string> list;
136  mrpt::system::tokenize(wind_reading, ",", list);
137  if (list.size() == 6)
138  {
139  // Status
140  int status = atoi(list.at(4).c_str());
141  if (status == 0)
142  {
143  // Units
144  std::string s_units = list.at(3);
145  // Module
146  std::string s_speed = list.at(2);
147  if (s_units == "M")
148  obsPtr->speed = atof(s_speed.c_str());
149  else if (s_units == "K")
150  obsPtr->speed = atof(s_speed.c_str()) * 1000 / 3600;
151  else
152  {
153  printf(
154  "ERROR: WindSonic measurement units not supported: "
155  "%s\n",
156  s_units.c_str());
157  obsPtr->speed = 0.0;
158  }
159  // angle
160  std::string s_direction = list.at(1);
161  obsPtr->direction = atof(s_direction.c_str());
162 
163  // Prepare observation
164  obsPtr->sensorLabel = m_sensorLabel;
165  obsPtr->timestamp = mrpt::system::getCurrentTime();
166  obsPtr->sensorPoseOnRobot = mrpt::poses::CPose3D(
167  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
174  // not provide a wind direction
175  {
176  // Status
177  int status = atoi(list.at(3).c_str());
178  if (status == 0)
179  {
180  // Units
181  std::string s_units = list.at(2);
182  // module
183  std::string s_speed = list.at(1);
184  if (s_units == "M")
185  obsPtr->speed = atof(s_speed.c_str());
186  else if (s_units == "K")
187  obsPtr->speed = atof(s_speed.c_str()) * 1000 / 3600;
188  else
189  {
190  printf(
191  "ERROR: WindSonic measurement units not supported: "
192  "%s\n",
193  s_units.c_str());
194  obsPtr->speed = 0.0;
195  }
196  // Angle
197  obsPtr->direction = 0.0;
198 
199  // Prepare observation
200  obsPtr->sensorLabel = m_sensorLabel;
201  obsPtr->timestamp = mrpt::system::getCurrentTime();
202  obsPtr->sensorPoseOnRobot = mrpt::poses::CPose3D(
203  pose_x, pose_y, pose_z, pose_yaw, pose_pitch, pose_roll);
204  appendObservation(obsPtr);
205  }
206  else
207  printf("ERROR: WindSonic error code %u\n", status);
208  }
209  else
210  {
211  printf(
212  "ERROR: Windsonic reading incorrect format: %s [%u]\n",
213  wind_reading.c_str(), static_cast<unsigned int>(list.size()));
214  }
215  }
216  catch (std::exception& e)
217  {
218  std::cerr << "[CGillAnemometer::doProcess] Error:" << std::endl
219  << e.what();
220  }
221  catch (...)
222  {
223  std::cerr << "[CGillAnemometer::doProcess] Unknown Error" << std::endl;
224  }
225 }
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:41
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.
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...
GLsizei const GLchar ** string
Definition: glext.h:4101
#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:86
This class implements a driver for the Gill Windsonic Option 1 Anemometer The sensor is accessed via ...
_u8 status
Definition: rplidar_cmd.h:19



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020