MRPT  1.9.9
CBoardSonars.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 
15 #include <mrpt/system/os.h>
16 
17 #include <thread>
18 
19 using namespace mrpt::hwdrivers;
20 using namespace mrpt::serialization;
21 using namespace std;
22 
24 
25 /*-------------------------------------------------------------
26  CBoardSonars
27 -------------------------------------------------------------*/
29 {
31  m_usbSerialNumber = "SONAR001";
32 
33  m_sensorLabel = "SONAR1";
34 
35  m_gain = 6;
36  m_maxRange = 4.0f;
37 
38  MRPT_END
39 }
40 
41 /*-------------------------------------------------------------
42  loadConfig_sensorSpecific
43 -------------------------------------------------------------*/
45  const mrpt::config::CConfigFileBase& configSource,
46  const std::string& iniSection)
47 {
49 
50  std::vector<double> aux; // Auxiliar vector
51 
52  // Some parameters ...
53  m_usbSerialNumber = configSource.read_string(
54  iniSection, "USB_serialNumber", m_usbSerialNumber, true);
55  m_gain = configSource.read_int(iniSection, "gain", m_gain, true);
56  m_maxRange =
57  configSource.read_float(iniSection, "maxRange", m_maxRange, true);
58  m_minTimeBetweenPings = configSource.read_float(
59  iniSection, "minTimeBetweenPings", m_minTimeBetweenPings, true);
60  // ----------------------------------------------------------------------------------------------------------------------
61  ASSERT_(m_maxRange > 0 && m_maxRange <= 11);
62  ASSERT_(m_gain <= 16);
63 
64  // Sonar firing order ...
65  configSource.read_vector(
66  iniSection, "firingOrder", m_firingOrder, m_firingOrder, true);
67 
68  // ----------------------------------------------------------------------------------------------------------------------
69 
70  // Individual sonar gains ...
71  configSource.read_vector(iniSection, "sonarGains", aux, aux, true);
72 
73  std::vector<int32_t>::iterator itSonar;
74  std::vector<double>::iterator itAux;
75  for (itSonar = m_firingOrder.begin(), itAux = aux.begin();
76  itSonar != m_firingOrder.end(); ++itSonar, ++itAux)
77  m_sonarGains[*itSonar] = *itAux;
78  // ----------------------------------------------------------------------------------------------------------------------
79  ASSERT_(aux.size() == m_firingOrder.size());
80 
81  // Individual sonar poses
82  aux.clear();
83  for (itSonar = m_firingOrder.begin(); itSonar != m_firingOrder.end();
84  ++itSonar)
85  {
86  configSource.read_vector(
87  iniSection, format("pose%i", *itSonar), aux, aux,
88  true); // Get the sonar poses
89  m_sonarPoses[*itSonar] = mrpt::math::TPose3D(
90  aux[0], aux[1], aux[2], DEG2RAD((float)aux[3]),
91  DEG2RAD((float)aux[4]), DEG2RAD((float)aux[5]));
92  }
93  // ----------------------------------------------------------------------------------------------------------------------
94  ASSERT_(m_sonarGains.size() == m_firingOrder.size());
95 
96  MRPT_END
97 }
98 
99 /*-------------------------------------------------------------
100  queryFirmwareVersion
101 -------------------------------------------------------------*/
102 bool CBoardSonars::queryFirmwareVersion(string& out_firmwareVersion)
103 {
104  try
105  {
106  CMessage msg, msgRx;
107 
108  // Try to connect to the device:
109  if (!checkConnectionAndConnect()) return false;
110 
111  msg.type = 0x10;
112  auto arch = mrpt::serialization::archiveFrom(*this);
113  arch.sendMessage(msg);
114 
115  if (arch.receiveMessage(msgRx))
116  {
117  msgRx.getContentAsString(out_firmwareVersion);
118  return true;
119  }
120  else
121  return false;
122  }
123  catch (...)
124  {
125  Close();
126  return false;
127  }
128 }
129 
130 /*-------------------------------------------------------------
131  checkConnectionAndConnect
132 -------------------------------------------------------------*/
134 {
135  try
136  {
137  if (!isOpen()) return false;
139  size_t i;
140 
141  // Send cmd for firing order:
142  // ----------------------------
143  msg.type = 0x12;
144  msg.content.resize(16);
145  for (i = 0; i < 16; i++)
146  {
147  if (i < m_firingOrder.size())
148  msg.content[i] = m_firingOrder[i];
149  else
150  msg.content[i] = 0xFF;
151  }
152  auto arch = mrpt::serialization::archiveFrom(*this);
153  arch.sendMessage(msg);
154  if (!arch.receiveMessage(msgRx)) return false; // Error
155 
156  // Send cmd for gain:
157  // ----------------------------
158  // msg.type = 0x13;
159  // msg.content.resize(1);
160  // msg.content[0] = m_gain;
161  // sendMessage(msg);
162  // if (!receiveMessage(msgRx) ) return false; // Error
163 
164  // Send cmd for set of gains:
165  // ----------------------------
166  msg.type = 0x16;
167  msg.content.resize(16);
168  for (i = 0; i < 16; i++)
169  {
170  if (m_sonarGains.find(i) != m_sonarGains.end())
171  msg.content[i] = m_sonarGains[i];
172  else
173  msg.content[i] = 0xFF;
174  }
175  arch.sendMessage(msg);
176  if (!arch.receiveMessage(msgRx)) return false; // Error
177 
178  // Send cmd for max range:
179  // ----------------------------
180  msg.type = 0x14;
181  msg.content.resize(1);
182  msg.content[0] = (int)((m_maxRange / 0.043f) - 1);
183  arch.sendMessage(msg);
184  if (!arch.receiveMessage(msgRx)) return false; // Error
185 
186  // Send cmd for max range:
187  // ----------------------------
188  msg.type = 0x15;
189  msg.content.resize(2);
190  auto T = (uint16_t)(m_minTimeBetweenPings * 1000.0f);
191  msg.content[0] = T >> 8;
192  msg.content[1] = T & 0x00FF;
193  arch.sendMessage(msg);
194  if (!arch.receiveMessage(msgRx)) return false; // Error
195 
196  return true;
197  }
198  catch (...)
199  {
200  // Error opening device:
201  Close();
202  return false;
203  }
204 }
205 
206 /*-------------------------------------------------------------
207  getObservation
208 -------------------------------------------------------------*/
210 {
211  try
212  {
213  obs.sensorLabel = m_sensorLabel;
215  obs.minSensorDistance = 0.04f;
216  obs.maxSensorDistance = m_maxRange;
217  obs.sensorConeApperture = DEG2RAD(30.0f);
218  obs.sensedData.clear();
220 
222 
223  // Try to connect to the device:
224  if (!checkConnectionAndConnect()) return false;
225 
226  auto arch = mrpt::serialization::archiveFrom(*this);
227 
228  msg.type = 0x11;
229  arch.sendMessage(msg);
230 
231  if (arch.receiveMessage(msgRx))
232  {
233  if (msgRx.content.empty()) return false;
234 
235  // For each sensor:
236  ASSERT_((msgRx.content.size() % 2) == 0);
237  vector<uint16_t> data(msgRx.content.size() / 2);
238  memcpy(&data[0], &msgRx.content[0], msgRx.content.size());
239 
240  for (size_t i = 0; i < data.size() / 2; i++)
241  {
242  uint16_t sonar_idx = data[2 * i + 0];
243  uint16_t sonar_range_cm = data[2 * i + 1];
244  if (sonar_range_cm != 0xFFFF && sonar_idx < 16)
245  {
246  obsRange.sensorID = sonar_idx;
247  obsRange.sensorPose =
248  m_sonarPoses[sonar_idx]; // mrpt::poses::CPose3D(); //
249  // sonar_idx
250  obsRange.sensedDistance = sonar_range_cm * 0.01f;
251  obs.sensedData.push_back(obsRange);
252  }
253  }
254  return true;
255  }
256  else
257  return false;
258  }
259  catch (...)
260  {
261  Close();
262  return false;
263  }
264 }
265 
266 /*-------------------------------------------------------------
267  programI2CAddress
268 -------------------------------------------------------------*/
269 bool CBoardSonars::programI2CAddress(uint8_t currentAddress, uint8_t newAddress)
270 {
271  try
272  {
274 
275  // Try to connect to the device:
276  if (!checkConnectionAndConnect()) return false;
277  auto arch = mrpt::serialization::archiveFrom(*this);
278 
279  msg.type = 0x20;
280  msg.content.resize(2);
281  msg.content[0] = currentAddress;
282  msg.content[1] = newAddress;
283  arch.sendMessage(msg);
284 
285  std::this_thread::sleep_for(10ms);
286 
287  return arch.receiveMessage(msgRx);
288  }
289  catch (...)
290  {
291  Close();
292  return false;
293  }
294 }
295 
296 /*-------------------------------------------------------------
297  checkConnectionAndConnect
298 -------------------------------------------------------------*/
300 {
301  if (isOpen()) return true;
302 
303  try
304  {
305  OpenBySerialNumber(m_usbSerialNumber);
306  std::this_thread::sleep_for(10ms);
307  Purge();
308  std::this_thread::sleep_for(10ms);
309  SetLatencyTimer(1);
310  SetTimeouts(300, 100);
311 
312  return sendConfigCommands();
313  }
314  catch (...)
315  {
316  // Error opening device:
317  Close();
318  return false;
319  }
320 }
321 
322 /*-------------------------------------------------------------
323  doProcess
324 -------------------------------------------------------------*/
326 {
329  if (getObservation(*obs)) appendObservation(obs);
330 }
void doProcess() override
This method will be invoked at a minimum rate of "process_rate" (Hz)
bool queryFirmwareVersion(std::string &out_firmwareVersion)
Query the firmware version on the device (can be used to test communications).
Declares a class derived from "CObservation" that encapsules a single range measurement, and associated parameters.
#define MRPT_START
Definition: exceptions.h:241
bool checkConnectionAndConnect()
Tries to connect to the USB device (if disconnected).
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
float minSensorDistance
The data members.
float sensorConeApperture
Cone aperture of each ultrasonic beam, in radians.
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
mrpt::system::TTimeStamp getCurrentTime()
Returns the current (UTC) system time.
Definition: datetime.h:82
uint32_t type
An identifier of the message type (only the least-sig byte is typically sent)
Definition: CMessage.h:32
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection) override
Loads specific configuration for the device from a given source of configuration parameters, for example, an ".ini" file, loading from the section "[iniSection]" (see config::CConfigFileBase and derived classes) See hwdrivers::CBoardSonars for the possible parameters.
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.
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT&#39;s CStream, std::istream, std::ostream, std::stringstream.
Definition: CArchive.h:592
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
static Ptr Create(Args &&... args)
This class allows loading and storing values and vectors of different types from a configuration text...
float sensedDistance
The measured range, in meters (or a value of 0 if there was no detected echo).
constexpr double DEG2RAD(const double x)
Degrees to radians.
This "software driver" implements the communication protocol for interfacing a Ultrasonic range finde...
Definition: CBoardSonars.h:53
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
Definition: CObservation.h:62
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
uint16_t sensorID
Some kind of sensor ID which identifies it on the bus (if applicable, 0 otherwise) ...
A class that contain generic messages, that can be sent and received from a "CClientTCPSocket" object...
Definition: CMessage.h:27
void getContentAsString(std::string &str)
Gets the contents of the message as a string.
Definition: CMessage.cpp:96
OBSERVATION_T::Ptr getObservation(mrpt::obs::CSensoryFrame::Ptr &observations, mrpt::obs::CObservation::Ptr &observation, bool priority_to_sf=true)
Given an mrpt::obs::CSensoryFrame and a mrpt::obs::CObservation pointer if a OBSERVATION_T type obser...
Definition: obs_utils.h:31
math::TPose3D sensorPose
The 6D position of the sensor on the robot.
bool sendConfigCommands()
Sends the configuration (max range, gain,...) to the USB board.
#define MRPT_END
Definition: exceptions.h:245
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
std::vector< uint8_t > content
The contents of the message (memory is automatically handled by the std::vector object) ...
Definition: CMessage.h:35
bool programI2CAddress(uint8_t currentAddress, uint8_t newAddress)
Requests a command of "change address" for a given SRF10 device.
TMeasurementList sensedData
All the measurements.
void read_vector(const std::string &section, const std::string &name, const VECTOR_TYPE &defaultValue, VECTOR_TYPE &outValues, bool failIfNotFound=false) const
Reads a configuration parameter of type vector, stored in the file as a string: "[v1 v2 v3 ...
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
static struct FontData data
Definition: gltext.cpp:144
bool getObservation(mrpt::obs::CObservationRange &obs)
Request the latest range measurements.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020