Main MRPT website > C++ reference for MRPT 1.9.9
carmen_log_tools.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 "obs-precomp.h" // Precompiled headers
11 
12 #include <mrpt/obs/CObservation2DRangeScan.h>
13 #include <mrpt/obs/CObservationOdometry.h>
14 #include <mrpt/utils/TParameters.h>
15 
16 #include <mrpt/system/string_utils.h>
17 
18 #include <mrpt/obs/carmen_log_tools.h>
19 
20 using namespace mrpt;
21 using namespace mrpt::utils;
22 using namespace mrpt::obs;
23 using namespace mrpt::poses;
24 using namespace mrpt::system;
25 using namespace std;
26 
27 // Read the declaration in the .h file for documentation.
28 bool mrpt::obs::carmen_log_parse_line(
29  std::istream& in_stream,
30  std::vector<mrpt::obs::CObservation::Ptr>& out_observations,
31  const mrpt::system::TTimeStamp& time_start_log)
32 {
33  static TParametersString
34  global_log_params; // global parameters loaded in previous calls.
35 
36  out_observations.clear(); // empty output container
37 
38  // Try to get line:
39  string line;
40  while (line.empty())
41  {
42  if (!in_stream) return false; // End of file
43  std::getline(in_stream, line);
44  line = trim(line);
45  };
46 
47  // Now we have a line: analyze it:
48  if (strStartsI(line, "ROBOTLASER"))
49  {
50  // ROBOTLASER message
51  // ---------------------------
52  std::istringstream S; // Read from the string as if it was a stream
53  S.str(line);
54 
55  CObservation2DRangeScan::Ptr obsLaser_ptr =
56  mrpt::make_aligned_shared<CObservation2DRangeScan>();
57  CObservation2DRangeScan* obsLaser =
58  obsLaser_ptr.get(); // Faster access
59 
60  // Parse:
61  int laser_type; // SICK_LMS = 0, SICK_PLS = 1, HOKUYO_URG = 2,
62  // SIMULATED_LASER = 3,
63  double start_angle, angular_resolution, accuracy;
64  int remission_mode; // OFF = 0, DIRECT = 1, NORMALIZED = 2
65 
66  if (!(S >> obsLaser->sensorLabel >> laser_type >> start_angle >>
67  obsLaser->aperture >> angular_resolution >> obsLaser->maxRange >>
68  accuracy >> remission_mode))
69  THROW_EXCEPTION_FMT(
70  "Error parsing line from CARMEN log (params):\n'%s'\n",
71  line.c_str())
72 
73  size_t nRanges;
74  S >> nRanges;
75 
76  obsLaser->resizeScan(nRanges);
77 
78  for (size_t i = 0; i < nRanges; i++)
79  {
80  float range;
81  if (!(S >> range))
82  THROW_EXCEPTION_FMT(
83  "Error parsing line from CARMEN log (ranges):\n'%s'\n",
84  line.c_str());
85  obsLaser->setScanRange(i, range);
86  // Valid value?
87  obsLaser->setScanRangeValidity(
88  i, (obsLaser->scan[i] >= obsLaser->maxRange ||
89  obsLaser->scan[i] <= 0));
90  }
91 
92  size_t remmision_count;
93  if (!(S >> remmision_count))
94  THROW_EXCEPTION_FMT(
95  "Error parsing line from CARMEN log (remmision_count):\n'%s'\n",
96  line.c_str())
97 
98  vector<double> remission;
99  remission.resize(remmision_count);
100 
101  for (size_t i = 0; i < remmision_count; i++)
102  {
103  if (!(S >> remission[i]))
104  THROW_EXCEPTION_FMT(
105  "Error parsing line from CARMEN log (remmision "
106  "vals):\n'%s'\n",
107  line.c_str())
108  }
109 
110  mrpt::math::TPose2D globalLaserPose;
111  mrpt::math::TPose2D globalRobotPose;
112 
113  if (!(S >> globalLaserPose.x >> globalLaserPose.y >>
114  globalLaserPose.phi >> globalRobotPose.x >> globalRobotPose.y >>
115  globalRobotPose.phi))
116  THROW_EXCEPTION_FMT(
117  "Error parsing line from CARMEN log (poses):\n'%s'\n",
118  line.c_str())
119 
120  // Compute pose of laser on the robot:
121  obsLaser->sensorPose =
122  CPose3D(CPose2D(globalLaserPose) - CPose2D(globalRobotPose));
123 
124  double tv, rv, fw_dist, side_dist, turn_axis;
125  S >> tv >> rv >> fw_dist >> side_dist >> turn_axis;
126 
127  double timestamp;
128  string robotName;
129  S >> timestamp >> robotName;
130 
131  const mrpt::system::TTimeStamp obs_time =
132  time_start_log +
133  mrpt::system::secondsToTimestamp(timestamp); // seconds -> times
134 
135  obsLaser->timestamp = obs_time;
136 
137  // Create odometry observation:
138  {
139  CObservationOdometry::Ptr obsOdo_ptr =
140  mrpt::make_aligned_shared<CObservationOdometry>();
141 
142  obsOdo_ptr->timestamp = obs_time;
143  obsOdo_ptr->odometry = CPose2D(globalRobotPose);
144  obsOdo_ptr->sensorLabel = "ODOMETRY";
145 
146  out_observations.push_back(obsOdo_ptr);
147  }
148 
149  // Send out laser observation:
150  out_observations.push_back(obsLaser_ptr);
151 
152  } // end ROBOTLASER
153  else if (strStartsI(line, "FLASER") || strStartsI(line, "RLASER"))
154  {
155  // [F,R]LASER message
156  // FLASER num_readings [range_readings] x y theta odom_x odom_y
157  // odom_theta
158  // ---------------------------
159  std::istringstream S; // Read from the string as if it was a stream
160  S.str(line);
161 
162  CObservation2DRangeScan::Ptr obsLaser_ptr =
163  mrpt::make_aligned_shared<CObservation2DRangeScan>();
164  CObservation2DRangeScan* obsLaser =
165  obsLaser_ptr.get(); // Faster access
166 
167  // Parse:
168  size_t nRanges;
169 
170  if (!(S >> obsLaser->sensorLabel >> nRanges))
171  THROW_EXCEPTION_FMT(
172  "Error parsing line from CARMEN log (params):\n'%s'\n",
173  line.c_str())
174 
175  // Params:
176  {
177  double maxRange = 81.0;
178  double resolutionDeg = 0.5;
179 
180  if (line[0] == 'F')
181  { // front:
182  maxRange = atof(
183  global_log_params
184  .getWithDefaultVal("robot_front_laser_max", "81.0")
185  .c_str());
186  resolutionDeg = atof(
187  global_log_params
188  .getWithDefaultVal(
189  "laser_front_laser_resolution", "0.5")
190  .c_str());
191  }
192  else if (line[0] == 'R')
193  { // rear:
194  maxRange = atof(
195  global_log_params
196  .getWithDefaultVal("robot_rear_laser_max", "81.0")
197  .c_str());
198  resolutionDeg = atof(
199  global_log_params
200  .getWithDefaultVal(
201  "laser_rear_laser_resolution", "0.5")
202  .c_str());
203  }
204  obsLaser->maxRange = maxRange;
205  obsLaser->aperture = DEG2RAD(resolutionDeg) * nRanges;
206  }
207 
208  obsLaser->resizeScan(nRanges);
209 
210  for (size_t i = 0; i < nRanges; i++)
211  {
212  float range;
213  if (!(S >> range))
214  THROW_EXCEPTION_FMT(
215  "Error parsing line from CARMEN log (ranges):\n'%s'\n",
216  line.c_str());
217  obsLaser->setScanRange(i, range);
218  // Valid value?
219  obsLaser->setScanRangeValidity(
220  i, (obsLaser->scan[i] >= obsLaser->maxRange ||
221  obsLaser->scan[i] <= 0));
222  }
223 
224  mrpt::math::TPose2D globalLaserPose;
225  mrpt::math::TPose2D globalRobotPose;
226  if (!(S >> globalLaserPose.x >> globalLaserPose.y >>
227  globalLaserPose.phi >> globalRobotPose.x >> globalRobotPose.y >>
228  globalRobotPose.phi))
229  THROW_EXCEPTION_FMT(
230  "Error parsing line from CARMEN log (poses):\n'%s'\n",
231  line.c_str())
232 
233  // Compute pose of laser on the robot:
234  obsLaser->sensorPose =
235  CPose3D(CPose2D(globalLaserPose) - CPose2D(globalRobotPose));
236 
237  double timestamp;
238  string robotName;
239  S >> timestamp >> robotName;
240 
241  const mrpt::system::TTimeStamp obs_time =
242  time_start_log +
243  mrpt::system::secondsToTimestamp(timestamp); // seconds -> times
244 
245  obsLaser->timestamp = obs_time;
246 
247  // Create odometry observation:
248  {
249  CObservationOdometry::Ptr obsOdo_ptr =
250  mrpt::make_aligned_shared<CObservationOdometry>();
251 
252  obsOdo_ptr->timestamp = obs_time;
253  obsOdo_ptr->odometry = CPose2D(globalRobotPose);
254  obsOdo_ptr->sensorLabel = "ODOMETRY";
255 
256  out_observations.push_back(obsOdo_ptr);
257  }
258 
259  // Send out laser observation:
260  out_observations.push_back(obsLaser_ptr);
261 
262  } // end RAWLASER
263  else if (strStartsI(line, "PARAM "))
264  {
265  // PARAM message
266  // ---------------------------
267  std::istringstream S; // Read from the string as if it was a stream
268  S.str(line);
269 
270  string key, val;
271  S >> key; // This is "PARAM"
272 
273  if (!(S >> key >> val))
274  THROW_EXCEPTION_FMT(
275  "Error parsing line from CARMEN log (PARAM):\n'%s'\n",
276  line.c_str())
277 
278  if (!key.empty() && !val.empty())
279 
280  global_log_params[lowerCase(key)] = val;
281 
282  } // end RAWLASER
283 
284  return true; // OK
285 }
mrpt::system::TTimeStamp
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:32
range
GLsizei range
Definition: glext.h:5907
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: CParticleFilter.h:223
mrpt::utils::TParameters
For usage when passing a dynamic number of (numeric) arguments to a function, by name.
Definition: TParameters.h:57
mrpt::system
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:30
mrpt::math::TPose2D::x
double x
X,Y coordinates.
Definition: lightweight_geom_data.h:201
mrpt::obs::carmen_log_parse_line
bool carmen_log_parse_line(std::istream &in_stream, std::vector< mrpt::obs::CObservation::Ptr > &out_imported_observations, const mrpt::system::TTimeStamp &time_start_log)
Parse one line from an text input stream and interpret it as a CARMEN log entry, returning its MRPT o...
Definition: carmen_log_tools.cpp:28
mrpt::system::strStartsI
bool strStartsI(const std::string &str, const std::string &subStr)
Return true if "str" starts with "subStr" (case insensitive)
Definition: string_utils.cpp:326
THROW_EXCEPTION_FMT
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: CColouredPointsMap.h:122
std
STL namespace.
mrpt::obs::CObservation2DRangeScan::Ptr
std::shared_ptr< CObservation2DRangeScan > Ptr
Definition: CObservation2DRangeScan.h:57
mrpt::obs::CObservationOdometry::Ptr
std::shared_ptr< CObservationOdometry > Ptr
Definition: CObservationOdometry.h:34
string_utils.h
carmen_log_tools.h
mrpt::system::lowerCase
std::string lowerCase(const std::string &str)
Returns an lower-case version of a string.
Definition: string_utils.cpp:27
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:19
val
int val
Definition: mrpt_jpeglib.h:955
mrpt::utils::DEG2RAD
double DEG2RAD(const double x)
Degrees to radians.
Definition: bits.h:96
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
CObservation2DRangeScan.h
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:17
TParameters.h
mrpt::obs::CObservation2DRangeScan
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Definition: CObservation2DRangeScan.h:55
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
mrpt::math::TPose2D
Lightweight 2D pose.
Definition: lightweight_geom_data.h:194
CObservationOdometry.h
mrpt::system::trim
std::string trim(const std::string &str)
Removes leading and trailing spaces.
Definition: string_utils.cpp:275
mrpt::system::secondsToTimestamp
mrpt::system::TTimeStamp secondsToTimestamp(const double nSeconds)
Transform a time interval (in seconds) into TTimeStamp (e.g.
Definition: datetime.cpp:223
mrpt::math::TPose2D::phi
double phi
Orientation (rads)
Definition: lightweight_geom_data.h:203


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019