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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019