Main MRPT website > C++ reference for MRPT 1.5.6
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 
14 #include <mrpt/utils/TParameters.h>
15 
17 
19 
20 
21 using namespace mrpt;
22 using namespace mrpt::utils;
23 using namespace mrpt::obs;
24 using namespace mrpt::poses;
25 using namespace mrpt::system;
26 using namespace std;
27 
28 // Read the declaration in the .h file for documentation.
30  std::istream &in_stream,
31  std::vector<mrpt::obs::CObservationPtr> &out_observations,
32  const mrpt::system::TTimeStamp &time_start_log )
33 {
34  static TParametersString 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)
43  return false; // End of file
44  std::getline(in_stream, line);
45  line = trim(line);
46  };
47 
48  // Now we have a line: analyze it:
49  if ( strStartsI(line, "ROBOTLASER") )
50  {
51  // ROBOTLASER message
52  // ---------------------------
53  std::istringstream S; // Read from the string as if it was a stream
54  S.str(line);
55 
56  CObservation2DRangeScanPtr obsLaser_ptr = CObservation2DRangeScan::Create();
57  CObservation2DRangeScan* obsLaser = obsLaser_ptr.pointer(); // Faster access
58 
59  // Parse:
60  int laser_type; // SICK_LMS = 0, SICK_PLS = 1, HOKUYO_URG = 2, 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
65  >> laser_type >> start_angle >> obsLaser->aperture >> angular_resolution >> obsLaser->maxRange >> accuracy >> remission_mode ) )
66  THROW_EXCEPTION_FMT("Error parsing line from CARMEN log (params):\n'%s'\n", line.c_str() )
67 
68  size_t nRanges;
69  S >> nRanges;
70 
71  obsLaser->resizeScan(nRanges);
72 
73  for(size_t i=0;i<nRanges;i++)
74  {
75  float range;
76  if (! (S >> range) )
77  THROW_EXCEPTION_FMT("Error parsing line from CARMEN log (ranges):\n'%s'\n", line.c_str() );
78  obsLaser->setScanRange(i, range);
79  // Valid value?
80  obsLaser->setScanRangeValidity(i, (obsLaser->scan[i]>=obsLaser->maxRange || obsLaser->scan[i]<=0 ) );
81  }
82 
83  size_t remmision_count;
84  if (! (S >> remmision_count) )
85  THROW_EXCEPTION_FMT("Error parsing line from CARMEN log (remmision_count):\n'%s'\n", line.c_str() )
86 
87  vector<double> remission;
88  remission.resize(remmision_count);
89 
90  for(size_t i=0; i<remmision_count; i++)
91  {
92  if (! (S >> remission[i]) )
93  THROW_EXCEPTION_FMT("Error parsing line from CARMEN log (remmision vals):\n'%s'\n", line.c_str() )
94  }
95 
96  mrpt::math::TPose2D globalLaserPose;
97  mrpt::math::TPose2D globalRobotPose;
98 
99  if (! ( S >> globalLaserPose.x >> globalLaserPose.y >> globalLaserPose.phi
100  >> globalRobotPose.x >> globalRobotPose.y >> globalRobotPose.phi ) )
101  THROW_EXCEPTION_FMT("Error parsing line from CARMEN log (poses):\n'%s'\n", line.c_str() )
102 
103  // Compute pose of laser on the robot:
104  obsLaser->sensorPose = CPose3D( CPose2D(globalLaserPose) - CPose2D(globalRobotPose) );
105 
106  double tv,rv,fw_dist, side_dist,turn_axis;
107  S >> tv >> rv >> fw_dist >> side_dist >> turn_axis;
108 
109  double timestamp;
110  string robotName;
111  S >> timestamp >> robotName;
112 
113  const mrpt::system::TTimeStamp obs_time = time_start_log + mrpt::system::secondsToTimestamp(timestamp); // seconds -> times
114 
115  obsLaser->timestamp = obs_time;
116 
117  // Create odometry observation:
118  {
119  CObservationOdometryPtr obsOdo_ptr = CObservationOdometry::Create();
120 
121  obsOdo_ptr->timestamp = obs_time;
122  obsOdo_ptr->odometry = CPose2D(globalRobotPose);
123  obsOdo_ptr->sensorLabel = "ODOMETRY";
124 
125  out_observations.push_back(obsOdo_ptr);
126  }
127 
128  // Send out laser observation:
129  out_observations.push_back(obsLaser_ptr);
130 
131  } // end ROBOTLASER
132  else
133  if ( strStartsI(line, "FLASER") || strStartsI(line,"RLASER") )
134  {
135  // [F,R]LASER message
136  // FLASER num_readings [range_readings] x y theta odom_x odom_y odom_theta
137  // ---------------------------
138  std::istringstream S; // Read from the string as if it was a stream
139  S.str(line);
140 
141  CObservation2DRangeScanPtr obsLaser_ptr = CObservation2DRangeScan::Create();
142  CObservation2DRangeScan* obsLaser = obsLaser_ptr.pointer(); // Faster access
143 
144  // Parse:
145  size_t nRanges;
146 
147  if (! (S >> obsLaser->sensorLabel >> nRanges) )
148  THROW_EXCEPTION_FMT("Error parsing line from CARMEN log (params):\n'%s'\n", line.c_str() )
149 
150  // Params:
151  {
152  double maxRange = 81.0;
153  double resolutionDeg = 0.5;
154 
155  if (line[0]=='F')
156  { // front:
157  maxRange = atof(global_log_params.getWithDefaultVal("robot_front_laser_max","81.0").c_str());
158  resolutionDeg = atof(global_log_params.getWithDefaultVal("laser_front_laser_resolution","0.5").c_str());
159  }
160  else if (line[0]=='R')
161  { // rear:
162  maxRange = atof(global_log_params.getWithDefaultVal("robot_rear_laser_max","81.0").c_str());
163  resolutionDeg = atof(global_log_params.getWithDefaultVal("laser_rear_laser_resolution","0.5").c_str());
164  }
165  obsLaser->maxRange = maxRange;
166  obsLaser->aperture = DEG2RAD(resolutionDeg) * nRanges;
167  }
168 
169  obsLaser->resizeScan(nRanges);
170 
171  for(size_t i=0;i<nRanges;i++)
172  {
173  float range;
174  if (! (S >> range) )
175  THROW_EXCEPTION_FMT("Error parsing line from CARMEN log (ranges):\n'%s'\n", line.c_str() );
176  obsLaser->setScanRange(i,range);
177  // Valid value?
178  obsLaser->setScanRangeValidity(i, (obsLaser->scan[i]>=obsLaser->maxRange || obsLaser->scan[i]<=0 ) );
179  }
180 
181  mrpt::math::TPose2D globalLaserPose;
182  mrpt::math::TPose2D globalRobotPose;
183  if (! ( S >> globalLaserPose.x >> globalLaserPose.y >> globalLaserPose.phi
184  >> globalRobotPose.x >> globalRobotPose.y >> globalRobotPose.phi ) )
185  THROW_EXCEPTION_FMT("Error parsing line from CARMEN log (poses):\n'%s'\n", line.c_str() )
186 
187  // Compute pose of laser on the robot:
188  obsLaser->sensorPose = CPose3D( CPose2D(globalLaserPose) - CPose2D(globalRobotPose) );
189 
190 
191  double timestamp;
192  string robotName;
193  S >> timestamp >> robotName;
194 
195  const mrpt::system::TTimeStamp obs_time = time_start_log + mrpt::system::secondsToTimestamp(timestamp); // seconds -> times
196 
197  obsLaser->timestamp = obs_time;
198 
199  // Create odometry observation:
200  {
201  CObservationOdometryPtr obsOdo_ptr = CObservationOdometry::Create();
202 
203  obsOdo_ptr->timestamp = obs_time;
204  obsOdo_ptr->odometry = CPose2D(globalRobotPose);
205  obsOdo_ptr->sensorLabel = "ODOMETRY";
206 
207  out_observations.push_back(obsOdo_ptr);
208  }
209 
210  // Send out laser observation:
211  out_observations.push_back(obsLaser_ptr);
212 
213  } // end RAWLASER
214  else
215  if ( strStartsI(line, "PARAM ") )
216  {
217  // PARAM message
218  // ---------------------------
219  std::istringstream S; // Read from the string as if it was a stream
220  S.str(line);
221 
222  string key, val;
223  S >> key; // This is "PARAM"
224 
225  if (! (S >> key >> val) )
226  THROW_EXCEPTION_FMT("Error parsing line from CARMEN log (PARAM):\n'%s'\n", line.c_str() )
227 
228  if (!key.empty() && !val.empty() )
229 
230  global_log_params[ lowerCase(key) ] = val;
231 
232 
233  } // end RAWLASER
234 
235 
236  return true; // OK
237 }
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
T getWithDefaultVal(const std::string &s, const T &defaultVal) const
A const version of the [] operator and with a default value in case the parameter is not set (for usa...
Definition: TParameters.h:88
GLenum GLenum range
Definition: glew.h:7127
For usage when passing a dynamic number of (numeric) arguments to a function, by name.
Definition: TParameters.h:51
void setScanRange(const size_t i, const float val)
bool BASE_IMPEXP strStartsI(const std::string &str, const std::string &subStr)
Return true if "str" starts with "subStr" (case insensitive)
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
float maxRange
The maximum range allowed by the device, in meters (e.g. 80m, 50m,...)
bool OBS_IMPEXP carmen_log_parse_line(std::istream &in_stream, std::vector< mrpt::obs::CObservationPtr > &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...
std::string BASE_IMPEXP lowerCase(const std::string &str)
Returns an lower-case version of a string.
double DEG2RAD(const double x)
Degrees to radians.
Definition: bits.h:82
double y
X,Y coordinates.
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp. Where available, this should contain the accurate satellite-based time...
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:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
void resizeScan(const size_t len)
Resizes all data vectors to allocate a given number of scan rays.
Lightweight 2D pose.
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
std::string BASE_IMPEXP trim(const std::string &str)
Removes leading and trailing spaces.
mrpt::system::TTimeStamp BASE_IMPEXP secondsToTimestamp(const double nSeconds)
Transform a time interval (in seconds) into TTimeStamp (e.g.
Definition: datetime.cpp:219
GLuint GLfloat * val
Definition: glew.h:7785
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< float > > scan
The range values of the scan, in meters. Must have same length than validRange.
double phi
Orientation (rads)
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
void setScanRangeValidity(const size_t i, const bool val)



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018