MRPT  1.9.9
carmen_log_tools.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-2019, 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 "obs-precomp.h" // Precompiled headers
11 
12 #include <mrpt/math/TPose2D.h>
18 
19 using namespace mrpt;
20 using namespace mrpt::obs;
21 using namespace mrpt::poses;
22 using namespace mrpt::system;
23 using namespace std;
24 
25 // Read the declaration in the .h file for documentation.
27  std::istream& in_stream,
28  std::vector<mrpt::obs::CObservation::Ptr>& out_observations,
29  const mrpt::system::TTimeStamp& time_start_log)
30 {
31  /** global parameters loaded in previous calls. */
32  static TParametersString global_log_params;
33 
34  out_observations.clear(); // empty output container
35 
36  // Try to get line:
37  string line;
38  while (line.empty())
39  {
40  if (!in_stream) return false; // End of file
41  std::getline(in_stream, line);
42  line = trim(line);
43  };
44 
45  // Now we have a line: analyze it:
46  if (strStartsI(line, "ROBOTLASER"))
47  {
48  // ROBOTLASER message
49  // ---------------------------
50  std::istringstream S; // Read from the string as if it was a stream
51  S.str(line);
52 
53  CObservation2DRangeScan::Ptr obsLaser_ptr =
54  std::make_shared<CObservation2DRangeScan>();
55  CObservation2DRangeScan* obsLaser =
56  obsLaser_ptr.get(); // Faster access
57 
58  // Parse:
59  int laser_type; // SICK_LMS = 0, SICK_PLS = 1, HOKUYO_URG = 2,
60  // SIMULATED_LASER = 3,
61  double start_angle, angular_resolution, accuracy;
62  int remission_mode; // OFF = 0, DIRECT = 1, NORMALIZED = 2
63 
64  if (!(S >> obsLaser->sensorLabel >> laser_type >> start_angle >>
65  obsLaser->aperture >> angular_resolution >> obsLaser->maxRange >>
66  accuracy >> remission_mode))
68  "Error parsing line from CARMEN log (params):\n'%s'\n",
69  line.c_str());
70 
71  size_t nRanges;
72  S >> nRanges;
73 
74  obsLaser->resizeScan(nRanges);
75 
76  for (size_t i = 0; i < nRanges; i++)
77  {
78  float range;
79  if (!(S >> range))
81  "Error parsing line from CARMEN log (ranges):\n'%s'\n",
82  line.c_str());
83  obsLaser->setScanRange(i, range);
84  // Valid value?
85  obsLaser->setScanRangeValidity(
86  i, (obsLaser->scan[i] >= obsLaser->maxRange ||
87  obsLaser->scan[i] <= 0));
88  }
89 
90  size_t remmision_count;
91  if (!(S >> remmision_count))
93  "Error parsing line from CARMEN log (remmision_count):\n'%s'\n",
94  line.c_str());
95 
96  vector<double> remission;
97  remission.resize(remmision_count);
98 
99  for (size_t i = 0; i < remmision_count; i++)
100  {
101  if (!(S >> remission[i]))
103  "Error parsing line from CARMEN log (remmision "
104  "vals):\n'%s'\n",
105  line.c_str());
106  }
107 
108  mrpt::math::TPose2D globalLaserPose;
109  mrpt::math::TPose2D globalRobotPose;
110 
111  if (!(S >> globalLaserPose.x >> globalLaserPose.y >>
112  globalLaserPose.phi >> globalRobotPose.x >> globalRobotPose.y >>
113  globalRobotPose.phi))
115  "Error parsing line from CARMEN log (poses):\n'%s'\n",
116  line.c_str());
117 
118  // Compute pose of laser on the robot:
119  obsLaser->sensorPose =
120  CPose3D(CPose2D(globalLaserPose) - CPose2D(globalRobotPose));
121 
122  double tv, rv, fw_dist, side_dist, turn_axis;
123  S >> tv >> rv >> fw_dist >> side_dist >> turn_axis;
124 
125  double timestamp;
126  string robotName;
127  S >> timestamp >> robotName;
128 
129  const mrpt::system::TTimeStamp obs_time =
130  time_start_log +
131  std::chrono::microseconds(static_cast<uint64_t>(1e-6 * timestamp));
132 
133  obsLaser->timestamp = obs_time;
134 
135  // Create odometry observation:
136  {
137  CObservationOdometry::Ptr obsOdo_ptr =
138  std::make_shared<CObservationOdometry>();
139 
140  obsOdo_ptr->timestamp = obs_time;
141  obsOdo_ptr->odometry = CPose2D(globalRobotPose);
142  obsOdo_ptr->sensorLabel = "ODOMETRY";
143 
144  out_observations.push_back(obsOdo_ptr);
145  }
146 
147  // Send out laser observation:
148  out_observations.push_back(obsLaser_ptr);
149 
150  } // end ROBOTLASER
151  else if (strStartsI(line, "FLASER") || strStartsI(line, "RLASER"))
152  {
153  // [F,R]LASER message
154  // FLASER num_readings [range_readings] x y theta odom_x odom_y
155  // odom_theta
156  // ---------------------------
157  std::istringstream S; // Read from the string as if it was a stream
158  S.str(line);
159 
160  CObservation2DRangeScan::Ptr obsLaser_ptr =
161  std::make_shared<CObservation2DRangeScan>();
162  CObservation2DRangeScan* obsLaser =
163  obsLaser_ptr.get(); // Faster access
164 
165  // Parse:
166  size_t nRanges;
167 
168  if (!(S >> obsLaser->sensorLabel >> nRanges))
170  "Error parsing line from CARMEN log (params):\n'%s'\n",
171  line.c_str());
172 
173  // Params:
174  {
175  double maxRange = 81.0;
176  double resolutionDeg = 0.5;
177  using namespace std::string_literals;
178 
179  if (line[0] == 'F')
180  { // front:
181  maxRange = atof(
182  global_log_params
183  .getWithDefaultVal("robot_front_laser_max", "81.0"s)
184  .c_str());
185  resolutionDeg =
186  atof(global_log_params
187  .getWithDefaultVal(
188  "laser_front_laser_resolution", "0.5"s)
189  .c_str());
190  }
191  else if (line[0] == 'R')
192  { // rear:
193  maxRange =
194  atof(global_log_params
195  .getWithDefaultVal("robot_rear_laser_max", "81.0"s)
196  .c_str());
197  resolutionDeg =
198  atof(global_log_params
199  .getWithDefaultVal(
200  "laser_rear_laser_resolution", "0.5"s)
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  std::chrono::microseconds(static_cast<uint64_t>(1e-6 * timestamp));
243 
244  obsLaser->timestamp = obs_time;
245 
246  // Create odometry observation:
247  {
248  CObservationOdometry::Ptr obsOdo_ptr =
249  std::make_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:5993
double x
X,Y coordinates.
Definition: TPose2D.h:30
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.
GLdouble s
Definition: glext.h:3682
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:40
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:957
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:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:84
Lightweight 2D pose.
Definition: TPose2D.h:22
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
For usage when passing a dynamic number of (numeric) arguments to a function, by name.
Definition: TParameters.h:54
double phi
Orientation (rads)
Definition: TPose2D.h:32



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: abb8b1a1e Fri Oct 18 14:19:12 2019 +0200 at vie oct 18 14:20:13 CEST 2019