MRPT  2.0.4
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-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 "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, (range >= obsLaser->maxRange || range <= 0));
87  }
88 
89  size_t remmision_count;
90  if (!(S >> remmision_count))
92  "Error parsing line from CARMEN log (remmision_count):\n'%s'\n",
93  line.c_str());
94 
95  vector<double> remission;
96  remission.resize(remmision_count);
97 
98  for (size_t i = 0; i < remmision_count; i++)
99  {
100  if (!(S >> remission[i]))
102  "Error parsing line from CARMEN log (remmision "
103  "vals):\n'%s'\n",
104  line.c_str());
105  }
106 
107  mrpt::math::TPose2D globalLaserPose;
108  mrpt::math::TPose2D globalRobotPose;
109 
110  if (!(S >> globalLaserPose.x >> globalLaserPose.y >>
111  globalLaserPose.phi >> globalRobotPose.x >> globalRobotPose.y >>
112  globalRobotPose.phi))
114  "Error parsing line from CARMEN log (poses):\n'%s'\n",
115  line.c_str());
116 
117  // Compute pose of laser on the robot:
118  obsLaser->sensorPose =
119  CPose3D(CPose2D(globalLaserPose) - CPose2D(globalRobotPose));
120 
121  double tv, rv, fw_dist, side_dist, turn_axis;
122  S >> tv >> rv >> fw_dist >> side_dist >> turn_axis;
123 
124  double timestamp;
125  string robotName;
126  S >> timestamp >> robotName;
127 
128  const mrpt::system::TTimeStamp obs_time =
129  time_start_log +
130  std::chrono::microseconds(static_cast<uint64_t>(1e-6 * timestamp));
131 
132  obsLaser->timestamp = obs_time;
133 
134  // Create odometry observation:
135  {
136  CObservationOdometry::Ptr obsOdo_ptr =
137  std::make_shared<CObservationOdometry>();
138 
139  obsOdo_ptr->timestamp = obs_time;
140  obsOdo_ptr->odometry = CPose2D(globalRobotPose);
141  obsOdo_ptr->sensorLabel = "ODOMETRY";
142 
143  out_observations.push_back(obsOdo_ptr);
144  }
145 
146  // Send out laser observation:
147  out_observations.push_back(obsLaser_ptr);
148 
149  } // end ROBOTLASER
150  else if (strStartsI(line, "FLASER") || strStartsI(line, "RLASER"))
151  {
152  // [F,R]LASER message
153  // FLASER num_readings [range_readings] x y theta odom_x odom_y
154  // odom_theta
155  // ---------------------------
156  std::istringstream S; // Read from the string as if it was a stream
157  S.str(line);
158 
159  CObservation2DRangeScan::Ptr obsLaser_ptr =
160  std::make_shared<CObservation2DRangeScan>();
161  CObservation2DRangeScan* obsLaser =
162  obsLaser_ptr.get(); // Faster access
163 
164  // Parse:
165  size_t nRanges;
166 
167  if (!(S >> obsLaser->sensorLabel >> nRanges))
169  "Error parsing line from CARMEN log (params):\n'%s'\n",
170  line.c_str());
171 
172  // Params:
173  {
174  double maxRange = 81.0;
175  double resolutionDeg = 0.5;
176  using namespace std::string_literals;
177 
178  if (line[0] == 'F')
179  { // front:
180  maxRange = atof(
181  global_log_params
182  .getWithDefaultVal("robot_front_laser_max", "81.0"s)
183  .c_str());
184  resolutionDeg =
185  atof(global_log_params
186  .getWithDefaultVal(
187  "laser_front_laser_resolution", "0.5"s)
188  .c_str());
189  }
190  else if (line[0] == 'R')
191  { // rear:
192  maxRange =
193  atof(global_log_params
194  .getWithDefaultVal("robot_rear_laser_max", "81.0"s)
195  .c_str());
196  resolutionDeg =
197  atof(global_log_params
198  .getWithDefaultVal(
199  "laser_rear_laser_resolution", "0.5"s)
200  .c_str());
201  }
202  obsLaser->maxRange = d2f(maxRange);
203  obsLaser->aperture = d2f(DEG2RAD(resolutionDeg) * nRanges);
204  }
205 
206  obsLaser->resizeScan(nRanges);
207 
208  for (size_t i = 0; i < nRanges; i++)
209  {
210  float range;
211  if (!(S >> range))
213  "Error parsing line from CARMEN log (ranges):\n'%s'\n",
214  line.c_str());
215  obsLaser->setScanRange(i, range);
216  // Valid value?
217  obsLaser->setScanRangeValidity(
218  i, (obsLaser->getScanRange(i) >= obsLaser->maxRange ||
219  obsLaser->getScanRange(i) <= 0));
220  }
221 
222  mrpt::math::TPose2D globalLaserPose;
223  mrpt::math::TPose2D globalRobotPose;
224  if (!(S >> globalLaserPose.x >> globalLaserPose.y >>
225  globalLaserPose.phi >> globalRobotPose.x >> globalRobotPose.y >>
226  globalRobotPose.phi))
228  "Error parsing line from CARMEN log (poses):\n'%s'\n",
229  line.c_str());
230 
231  // Compute pose of laser on the robot:
232  obsLaser->sensorPose =
233  CPose3D(CPose2D(globalLaserPose) - CPose2D(globalRobotPose));
234 
235  double timestamp;
236  string robotName;
237  S >> timestamp >> robotName;
238 
239  const mrpt::system::TTimeStamp obs_time =
240  time_start_log +
241  std::chrono::microseconds(static_cast<uint64_t>(1e-6 * timestamp));
242 
243  obsLaser->timestamp = obs_time;
244 
245  // Create odometry observation:
246  {
247  CObservationOdometry::Ptr obsOdo_ptr =
248  std::make_shared<CObservationOdometry>();
249 
250  obsOdo_ptr->timestamp = obs_time;
251  obsOdo_ptr->odometry = CPose2D(globalRobotPose);
252  obsOdo_ptr->sensorLabel = "ODOMETRY";
253 
254  out_observations.push_back(obsOdo_ptr);
255  }
256 
257  // Send out laser observation:
258  out_observations.push_back(obsLaser_ptr);
259 
260  } // end RAWLASER
261  else if (strStartsI(line, "PARAM "))
262  {
263  // PARAM message
264  // ---------------------------
265  std::istringstream S; // Read from the string as if it was a stream
266  S.str(line);
267 
268  string key, val;
269  S >> key; // This is "PARAM"
270 
271  if (!(S >> key >> val))
273  "Error parsing line from CARMEN log (PARAM):\n'%s'\n",
274  line.c_str());
275 
276  if (!key.empty() && !val.empty())
277 
278  global_log_params[lowerCase(key)] = val;
279 
280  } // end RAWLASER
281 
282  return true; // OK
283 }
double x
X,Y coordinates.
Definition: TPose2D.h:30
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.
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
float d2f(const double d)
shortcut for static_cast<float>(double)
std::string lowerCase(const std::string &str)
Returns an lower-case version of a string.
constexpr double DEG2RAD(const double x)
Degrees to radians.
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:85
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 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020