Main MRPT website > C++ reference for MRPT 1.5.7
CBoardSonars.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 #include "hwdrivers-precomp.h" // Precompiled headers
11 
12 #include <mrpt/utils/utils_defs.h>
13 #include <mrpt/system/os.h>
14 
16 
17 using namespace mrpt::utils;
18 using namespace mrpt::hwdrivers;
19 using namespace std;
20 
22 
23 /*-------------------------------------------------------------
24  CBoardSonars
25 -------------------------------------------------------------*/
27 {
29  m_usbSerialNumber = "SONAR001";
30 
31  m_sensorLabel = "SONAR1";
32 
33  m_gain = 6;
34  m_maxRange = 4.0f;
35 
36  MRPT_END
37 }
38 
39 /*-------------------------------------------------------------
40  loadConfig_sensorSpecific
41 -------------------------------------------------------------*/
42 void CBoardSonars::loadConfig_sensorSpecific( const mrpt::utils::CConfigFileBase &configSource,
43  const std::string &iniSection )
44 {
46 
47  std::vector<double> aux; // Auxiliar vector
48 
49  // Some parameters ...
50  m_usbSerialNumber = configSource.read_string(iniSection,"USB_serialNumber",m_usbSerialNumber,true);
51  m_gain = configSource.read_int(iniSection,"gain",m_gain,true);
52  m_maxRange = configSource.read_float(iniSection,"maxRange",m_maxRange,true);
53  m_minTimeBetweenPings = configSource.read_float(iniSection,"minTimeBetweenPings",m_minTimeBetweenPings,true);
54  // ----------------------------------------------------------------------------------------------------------------------
55  ASSERT_( m_maxRange>0 && m_maxRange<=11 );
56  ASSERT_( m_gain<=16 );
57 
58 
59  // Sonar firing order ...
60  configSource.read_vector( iniSection, "firingOrder", m_firingOrder, m_firingOrder, true );
61 
62  // ----------------------------------------------------------------------------------------------------------------------
63 
64  // Individual sonar gains ...
65  configSource.read_vector( iniSection, "sonarGains", aux, aux, true );
66 
69  for( itSonar = m_firingOrder.begin(), itAux = aux.begin(); itSonar != m_firingOrder.end(); ++itSonar, ++itAux )
70  m_sonarGains[ *itSonar ] = *itAux;
71  // ----------------------------------------------------------------------------------------------------------------------
72  ASSERT_( aux.size() == m_firingOrder.size() );
73 
74  // Individual sonar poses
75  aux.clear();
76  for( itSonar = m_firingOrder.begin(); itSonar != m_firingOrder.end(); ++itSonar )
77  {
78  configSource.read_vector( iniSection, format("pose%i",*itSonar), aux, aux, true ); // Get the sonar poses
79  m_sonarPoses[ *itSonar ] = mrpt::math::TPose3D( aux[0], aux[1], aux[2], DEG2RAD( (float)aux[3]), DEG2RAD( (float)aux[4]), DEG2RAD( (float)aux[5]) );
80  }
81  // ----------------------------------------------------------------------------------------------------------------------
82  ASSERT_( m_sonarGains.size() == m_firingOrder.size() );
83 
84  MRPT_END
85 }
86 
87 /*-------------------------------------------------------------
88  queryFirmwareVersion
89 -------------------------------------------------------------*/
90 bool CBoardSonars::queryFirmwareVersion( string &out_firmwareVersion )
91 {
92  try
93  {
94  utils::CMessage msg,msgRx;
95 
96  // Try to connect to the device:
97  if (!checkConnectionAndConnect()) return false;
98 
99  msg.type = 0x10;
100  sendMessage(msg);
101 
102  if (receiveMessage(msgRx) )
103  {
104  msgRx.getContentAsString( out_firmwareVersion );
105  return true;
106  }
107  else
108  return false;
109  }
110  catch(...)
111  {
112  Close();
113  return false;
114  }
115 }
116 
117 
118 /*-------------------------------------------------------------
119  checkConnectionAndConnect
120 -------------------------------------------------------------*/
121 bool CBoardSonars::sendConfigCommands()
122 {
123  try
124  {
125  if (!isOpen()) return false;
126  utils::CMessage msg,msgRx;
127  size_t i;
128 
129  // Send cmd for firing order:
130  // ----------------------------
131  msg.type = 0x12;
132  msg.content.resize(16);
133  for (i=0;i<16;i++)
134  {
135  if (i<m_firingOrder.size())
136  msg.content[i] = m_firingOrder[i];
137  else msg.content[i] = 0xFF;
138  }
139  sendMessage(msg);
140  if (!receiveMessage(msgRx) ) return false; // Error
141 
142  // Send cmd for gain:
143  // ----------------------------
144  //msg.type = 0x13;
145  //msg.content.resize(1);
146  //msg.content[0] = m_gain;
147  //sendMessage(msg);
148  //if (!receiveMessage(msgRx) ) return false; // Error
149 
150  // Send cmd for set of gains:
151  // ----------------------------
152  msg.type = 0x16;
153  msg.content.resize(16);
154  for (i=0;i<16;i++)
155  {
156  if( m_sonarGains.find(i) != m_sonarGains.end() )
157  msg.content[i] = m_sonarGains[i];
158  else msg.content[i] = 0xFF;
159  }
160  sendMessage(msg);
161  if (!receiveMessage(msgRx) ) return false; // Error
162 
163  // Send cmd for max range:
164  // ----------------------------
165  msg.type = 0x14;
166  msg.content.resize(1);
167  msg.content[0] = (int)((m_maxRange/0.043f) - 1);
168  sendMessage(msg);
169  if (!receiveMessage(msgRx) ) return false; // Error
170 
171  // Send cmd for max range:
172  // ----------------------------
173  msg.type = 0x15;
174  msg.content.resize(2);
175  uint16_t T = (uint16_t)(m_minTimeBetweenPings * 1000.0f);
176  msg.content[0] = T >> 8;
177  msg.content[1] = T & 0x00FF;
178  sendMessage(msg);
179  if (!receiveMessage(msgRx) ) return false; // Error
180 
181  return true;
182  }
183  catch(...)
184  {
185  // Error opening device:
186  Close();
187  return false;
188  }
189 }
190 
191 /*-------------------------------------------------------------
192  getObservation
193 -------------------------------------------------------------*/
195 {
196  try
197  {
198  obs.sensorLabel = m_sensorLabel;
200  obs.minSensorDistance = 0.04f;
201  obs.maxSensorDistance = m_maxRange;
202  obs.sensorConeApperture = DEG2RAD(30.0f);
203  obs.sensedData.clear();
205 
206  utils::CMessage msg,msgRx;
207 
208  // Try to connect to the device:
209  if (!checkConnectionAndConnect()) return false;
210 
211  msg.type = 0x11;
212  sendMessage(msg);
213 
214  if (receiveMessage(msgRx) )
215  {
216  if ( msgRx.content.empty() )
217  return false;
218 
219  // For each sensor:
220  ASSERT_((msgRx.content.size() % 2) == 0);
221  vector<uint16_t> data(msgRx.content.size() / 2);
222  memcpy( &data[0], &msgRx.content[0], msgRx.content.size() );
223 
224  for (size_t i=0;i<data.size() / 2;i++)
225  {
226  uint16_t sonar_idx = data[2*i + 0];
227  uint16_t sonar_range_cm = data[2*i + 1];
228  if ( sonar_range_cm!=0xFFFF && sonar_idx<16 )
229  {
230  obsRange.sensorID = sonar_idx;
231  obsRange.sensorPose = m_sonarPoses[sonar_idx]; // mrpt::poses::CPose3D(); // sonar_idx
232  obsRange.sensedDistance = sonar_range_cm * 0.01f;
233  obs.sensedData.push_back( obsRange );
234  }
235  }
236  return true;
237  }
238  else
239  return false;
240  }
241  catch(...)
242  {
243  Close();
244  return false;
245  }
246 }
247 
248 /*-------------------------------------------------------------
249  programI2CAddress
250 -------------------------------------------------------------*/
251 bool CBoardSonars::programI2CAddress( uint8_t currentAddress, uint8_t newAddress )
252 {
253  try
254  {
255  utils::CMessage msg,msgRx;
256 
257  // Try to connect to the device:
258  if (!checkConnectionAndConnect()) return false;
259 
260  msg.type = 0x20;
261  msg.content.resize(2);
262  msg.content[0] = currentAddress;
263  msg.content[1] = newAddress;
264  sendMessage(msg);
265 
267 
268  return receiveMessage(msgRx);
269  }
270  catch(...)
271  {
272  Close();
273  return false;
274  }
275 }
276 
277 /*-------------------------------------------------------------
278  checkConnectionAndConnect
279 -------------------------------------------------------------*/
280 bool CBoardSonars::checkConnectionAndConnect()
281 {
282  if (isOpen())
283  return true;
284 
285  try
286  {
287  OpenBySerialNumber( m_usbSerialNumber );
289  Purge();
291  SetLatencyTimer(1);
292  SetTimeouts(300,100);
293 
294  return sendConfigCommands();
295  }
296  catch(...)
297  {
298  // Error opening device:
299  Close();
300  return false;
301  }
302 }
303 
304 /*-------------------------------------------------------------
305  doProcess
306 -------------------------------------------------------------*/
307 void CBoardSonars::doProcess()
308 {
309  mrpt::obs::CObservationRangePtr obs = mrpt::obs::CObservationRange::Create();
310  if (getObservation( *obs ))
311  appendObservation( obs );
312 }
void BASE_IMPEXP memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) MRPT_NO_THROWS
An OS and compiler independent version of "memcpy".
Definition: os.cpp:358
Declares a class derived from "CObservation" that encapsules a single range measurement, and associated parameters.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
unsigned __int16 uint16_t
Definition: rptypes.h:46
float minSensorDistance
The data members.
OBSERVATION_T::Ptr getObservation(mrpt::obs::CSensoryFramePtr &observations, mrpt::obs::CObservationPtr &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:32
float sensorConeApperture
Cone aperture of each ultrasonic beam, in radians.
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
mrpt::system::TTimeStamp BASE_IMPEXP getCurrentTime()
Returns the current (UTC) system time.
Definition: datetime.cpp:71
Scalar * iterator
Definition: eigen_plugins.h:23
static CObservationRangePtr Create()
Contains classes for various device interfaces.
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
STL namespace.
This class allows loading and storing values and vectors of different types from a configuration text...
unsigned char uint8_t
Definition: rptypes.h:43
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
#define MRPT_END
float sensedDistance
The measured range, in meters (or a value of 0 if there was no detected echo).
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
This "software driver" implements the communication protocol for interfacing a Ultrasonic range finde...
Definition: CBoardSonars.h:48
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 ...
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
double DEG2RAD(const double x)
Degrees to radians.
Definition: bits.h:82
GLsizei const GLchar ** string
Definition: glext.h:3919
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
#define MRPT_START
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
void getContentAsString(std::string &str)
Gets the contents of the message as a string.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp. Where available, this should contain the accurate satellite-based time...
uint16_t sensorID
Some kind of sensor ID which identifies it on the bus (if applicable, 0 otherwise) ...
math::TPose3D sensorPose
The 6D position of the sensor on the robot.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
#define ASSERT_(f)
std::vector< uint8_t > content
The contents of the message (memory is automatically handled by the std::vector object) ...
Definition: CMessage.h:33
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3520
A class that contain generic messages, that can be sent and received from a "CClientTCPSocket" object...
Definition: CMessage.h:29
TMeasurementList sensedData
All the measurements.
uint32_t type
An identifier of the message type (only the least-sig byte is typically sent)
Definition: CMessage.h:32



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019