rawlog_record_node.cpp
Go to the documentation of this file.
1 /***********************************************************************************
2  * Revised BSD License *
3  * Copyright (c) 2014, Markus Bader <markus.bader@tuwien.ac.at> *
4  * All rights reserved. *
5  * *
6  * Redistribution and use in source and binary forms, with or without *
7  * modification, are permitted provided that the following conditions are met: *
8  * * Redistributions of source code must retain the above copyright *
9  * notice, this list of conditions and the following disclaimer. *
10  * * Redistributions in binary form must reproduce the above copyright *
11  * notice, this list of conditions and the following disclaimer in the *
12  * documentation and/or other materials provided with the distribution. *
13  * * Neither the name of the Vienna University of Technology nor the *
14  * names of its contributors may be used to endorse or promote products *
15  * derived from this software without specific prior written permission. *
16  * *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  *AND *
19  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
20  **
21  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE *
22  * DISCLAIMED. IN NO EVENT SHALL Markus Bader BE LIABLE FOR ANY *
23  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES *
24  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
25  **
26  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND *
27  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT *
28  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
29  **
30  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31  ** *
32  ***********************************************************************************/
33 
34 #include "rawlog_record_node.h"
35 #include <boost/interprocess/sync/scoped_lock.hpp>
36 
37 #include <mrpt_bridge/pose.h>
38 #include <mrpt_bridge/laser_scan.h>
40 #include <mrpt_bridge/time.h>
41 
42 #include <mrpt/version.h>
43 using namespace mrpt::maps;
44 using namespace mrpt::obs;
45 
46 int main(int argc, char** argv)
47 {
48  ros::init(argc, argv, "rawlog_record");
50  RawlogRecordNode my_node(n);
51  my_node.init();
52  my_node.loop();
53  return 0;
54 }
55 
58 
60 {
62  ROS_INFO("rawlog file: %s", base_param_.raw_log_name.c_str());
64  {
65  subLaser_ =
66  n_.subscribe("scan", 1, &RawlogRecordNode::callbackLaser, this);
67  }
69  {
70  subMarker_ =
71  n_.subscribe("marker", 1, &RawlogRecordNode::callbackMarker, this);
72  }
73  subOdometry_ =
75 }
76 
78 
80  mrpt::poses::CPose3D& des, const std::string& target_frame,
81  const std::string& source_frame, const ros::Time& time,
82  const ros::Duration& timeout, const ros::Duration& polling_sleep_duration)
83 {
85  try
86  {
87  if (base_param_.debug)
88  ROS_INFO(
89  "debug: waitForTransform(): target_frame='%s' "
90  "source_frame='%s'",
91  target_frame.c_str(), source_frame.c_str());
92 
94  target_frame, source_frame, time, polling_sleep_duration);
96  target_frame, source_frame, time, transform);
97  }
99  {
100  ROS_INFO(
101  "Failed to get transform target_frame (%s) to source_frame (%s)",
102  target_frame.c_str(), source_frame.c_str());
103  return false;
104  }
105  mrpt_bridge::convert(transform, des);
106  return true;
107 }
108 
110  const nav_msgs::Odometry& src, mrpt::obs::CObservationOdometry& des)
111 {
112  mrpt_bridge::convert(src.header.stamp, des.timestamp);
113  mrpt_bridge::convert(src.pose.pose, des.odometry);
114  std::string odom_frame_id =
116  des.sensorLabel = odom_frame_id;
117  des.hasEncodersInfo = false;
118  des.hasVelocities = false;
119 }
120 
121 void RawlogRecordNode::callbackOdometry(const nav_msgs::Odometry& _msg)
122 {
123  // ROS_INFO("callbackOdometry");
124  if (!last_odometry_)
125  {
126  last_odometry_ = CObservationOdometry::Create();
127  }
128  convert(_msg, *last_odometry_);
129  addObservation(_msg.header.stamp);
130 }
131 
133 {
134  // ROS_INFO("callbackLaser");
135  if (!last_range_scan_)
136  {
137  last_range_scan_ = CObservation2DRangeScan::Create();
138  }
139  mrpt::poses::CPose3D sensor_pose_on_robot;
140 
141  if (getStaticTF(_msg.header.frame_id, sensor_pose_on_robot))
142  {
143  mrpt_bridge::convert(_msg, sensor_pose_on_robot, *last_range_scan_);
144 
145  addObservation(_msg.header.stamp);
146  }
147 }
148 
150 {
151  // ROS_INFO("callbackMarker");
152  if (!last_bearing_range_)
153  {
154  last_bearing_range_ = CObservationBearingRange::Create();
155  }
156  if (!last_beacon_range_)
157  {
158  last_beacon_range_ = CObservationBeaconRanges::Create();
159  }
160  mrpt::poses::CPose3D sensor_pose_on_robot;
161 
162  if (getStaticTF(_msg.header.frame_id, sensor_pose_on_robot))
163  {
164  mrpt_bridge::convert(_msg, sensor_pose_on_robot, *last_bearing_range_);
165  last_bearing_range_->sensor_std_range =
168  last_bearing_range_->sensor_std_pitch =
170 
171  mrpt_bridge::convert(_msg, sensor_pose_on_robot, *last_beacon_range_);
173  addObservation(_msg.header.stamp);
174  }
175 }
176 
178 {
181  ROS_INFO("Problem to syn data for sensor frame!");
182 
183  if (!last_odometry_) return;
184  auto odometry = CObservationOdometry::Create();
185  *odometry = *last_odometry_;
187 
188  CObservation2DRangeScan::Ptr range_scan;
189  CObservationBearingRange::Ptr bearing_range;
190  CObservationBeaconRanges::Ptr beacon_range;
191 
193  {
194  if (!last_range_scan_) return;
196  last_odometry_->timestamp, last_range_scan_->timestamp)) >
198  {
199  return;
200  }
201  range_scan = CObservation2DRangeScan::Create();
202  *range_scan = *last_range_scan_;
204  }
205 
207  {
208  if (!last_bearing_range_) return;
210  last_odometry_->timestamp, last_bearing_range_->timestamp)) >
212  {
213  return;
214  }
215  bearing_range = CObservationBearingRange::Create();
216  *bearing_range = *last_bearing_range_;
217  pRawLog.addObservationMemoryReference(bearing_range);
218  }
220  {
221  if (!last_beacon_range_) return;
223  last_odometry_->timestamp, last_beacon_range_->timestamp)) >
225  {
226  return;
227  }
228  beacon_range = CObservationBeaconRanges::Create();
229  *beacon_range = *last_beacon_range_;
231  }
232 
233  static std::shared_ptr<mrpt::poses::CPose2D> lastOdomPose;
234  if (!lastOdomPose)
235  {
236  lastOdomPose = std::make_shared<mrpt::poses::CPose2D>();
237  *lastOdomPose = odometry->odometry;
238  }
239 
240  mrpt::poses::CPose2D incOdoPose = odometry->odometry - *lastOdomPose;
241 
242  CActionRobotMovement2D odom_move;
243  odom_move.timestamp = odometry->timestamp;
244  odom_move.computeFromOdometry(incOdoPose, base_param_.motionModelOptions);
245  auto action = CActionCollection::Create();
246  action->insert(odom_move);
248 
249  auto sf = CSensoryFrame::Create();
251  {
252  CObservation::Ptr obs_range_scan = CObservation::Ptr(range_scan);
253  sf->insert(obs_range_scan);
254  }
255 
257  {
258  CObservation::Ptr obs_bearing_range = CObservation::Ptr(bearing_range);
259  sf->insert(obs_bearing_range);
260  }
262  {
263  CObservation::Ptr obs_bearing_range = CObservation::Ptr(beacon_range);
264  sf->insert(obs_bearing_range);
265  }
267 
268  *lastOdomPose = odometry->odometry;
269 
271 }
272 
274  std::string source_frame, mrpt::poses::CPose3D& des)
275 {
276  std::string target_frame_id =
278  std::string source_frame_id = source_frame;
279  std::string key = target_frame_id + "->" + source_frame_id;
282 
283  if (static_tf_.find(key) == static_tf_.end())
284  {
285  try
286  {
287  if (base_param_.debug)
288  ROS_INFO(
289  "debug: updateLaserPose(): target_frame_id='%s' "
290  "source_frame_id='%s'",
291  target_frame_id.c_str(), source_frame_id.c_str());
292 
294  target_frame_id, source_frame_id, ros::Time(0), transform);
295  tf::Vector3 translation = transform.getOrigin();
296  tf::Quaternion quat = transform.getRotation();
297  pose.x() = translation.x();
298  pose.y() = translation.y();
299  pose.z() = translation.z();
300  tf::Matrix3x3 Rsrc(quat);
302  for (int c = 0; c < 3; c++)
303  for (int r = 0; r < 3; r++) Rdes(r, c) = Rsrc.getRow(r)[c];
304  pose.setRotationMatrix(Rdes);
305  static_tf_[key] = pose;
306  ROS_INFO(
307  "Static tf '%s' with '%s'", key.c_str(),
308  pose.asString().c_str());
309  }
310  catch (tf::TransformException ex)
311  {
312  ROS_INFO("getStaticTF");
313  ROS_ERROR("%s", ex.what());
314  ros::Duration(1.0).sleep();
315  return false;
316  }
317  }
318  des = static_tf_[key];
319  return true;
320 }
unsigned int sync_attempts_sensor_frame_
CRawlog pRawLog
Definition: rawlog_record.h:84
mrpt::system::TTimeStamp timestamp
void addObservationMemoryReference(const CObservationPtr &observation)
ParametersNode param_
mrpt::obs::CObservationBearingRange::Ptr last_bearing_range_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
const GLfloat * c
void addActionsMemoryReference(const CActionCollectionPtr &action)
bool getStaticTF(std::string source_frame, mrpt::poses::CPose3D &des)
bool sleep() const
mrpt::system::TTimeStamp now()
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void callbackLaser(const sensor_msgs::LaserScan &)
GLuint src
int main(int argc, char **argv)
bool waitForTransform(mrpt::poses::CPose3D &des, const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01))
GLuint GLenum GLenum transform
ros::Subscriber subOdometry_
ROSCPP_DECL void spin(Spinner &spinner)
TFSIMD_FORCE_INLINE const Vector3 & getRow(int i) const
bool waitForTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
mrpt::poses::CPose2D odometry
std::string resolve(const std::string &prefix, const std::string &frame_name)
GLsizei n
CActionRobotMovement2D::TMotionModelOptions motionModelOptions
Definition: rawlog_record.h:76
TFSIMD_FORCE_INLINE const tfScalar & x() const
TFSIMD_FORCE_INLINE const tfScalar & z() const
mrpt::obs::CObservation2DRangeScan::Ptr last_range_scan_
#define ROS_INFO(...)
TFSIMD_FORCE_INLINE const tfScalar & y() const
CRawlog pRawLogASF
Definition: rawlog_record.h:85
void convert(const nav_msgs::Odometry &src, mrpt::obs::CObservationOdometry &des)
tf::TransformListener listenerTF_
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
action
void convert(const ros::Time &src, mrpt::system::TTimeStamp &des)
std::map< std::string, mrpt::poses::CPose3D > static_tf_
void addObservation(const ros::Time &time)
void asString(std::string &s) const
Parameters base_param_
Definition: rawlog_record.h:82
ros::NodeHandle n_
void updateRawLogName(const mrpt::system::TTimeStamp &t)
void addObservationsMemoryReference(const CSensoryFramePtr &observations)
Quaternion getRotation() const
GLdouble GLdouble GLdouble r
void callbackOdometry(const nav_msgs::Odometry &)
mrpt::obs::CObservationBeaconRanges::Ptr last_beacon_range_
mrpt::obs::CObservationOdometry::Ptr last_odometry_
ros::Subscriber subLaser_
double BASE_IMPEXP timeDifference(const mrpt::system::TTimeStamp t_first, const mrpt::system::TTimeStamp t_later)
void computeFromOdometry(const mrpt::poses::CPose2D &odometryIncrement, const TMotionModelOptions &options)
#define ROS_ERROR(...)
RawlogRecordNode(ros::NodeHandle &n)
void callbackMarker(const marker_msgs::MarkerDetection &)
void setRotationMatrix(const mrpt::math::CMatrixDouble33 &ROT)
ros::Subscriber subMarker_


mrpt_rawlog
Author(s):
autogenerated on Thu Mar 12 2020 03:22:04