rawlog_record_node.h
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 #ifndef MRPT_RAWLOG_RECORD_NODE_H
35 #define MRPT_RAWLOG_RECORD_NODE_H
36 
37 #include <ros/ros.h>
38 #include <tf/transform_listener.h>
39 #include <nav_msgs/Odometry.h>
40 #include <sensor_msgs/LaserScan.h>
41 #include <marker_msgs/MarkerDetection.h>
42 #include <dynamic_reconfigure/server.h>
43 #include "mrpt_rawlog/RawLogRecordConfig.h"
45 
48 {
50 
51  public:
53  {
54  static const int MOTION_MODEL_GAUSSIAN = 0;
55  static const int MOTION_MODEL_THRUN = 1;
59  void callbackParameters(
60  mrpt_rawlog::RawLogRecordConfig& config, uint32_t level);
61  dynamic_reconfigure::Server<mrpt_rawlog::RawLogRecordConfig>
63  dynamic_reconfigure::Server<
64  mrpt_rawlog::RawLogRecordConfig>::CallbackType reconfigureFnc_;
65  void update(const unsigned long& loop_count);
66  double rate;
68  std::string tf_prefix;
69  std::string odom_frame_id;
70  std::string base_frame_id;
72  };
73 
76  void init();
77  void loop();
78  void callbackLaser(
79  const sensor_msgs::LaserScan&);
82  void callbackOdometry(const nav_msgs::Odometry&);
83 
84  private: // functions
86  void update();
87  bool getStaticTF(std::string source_frame, mrpt::poses::CPose3D& des);
92  mrpt::obs::CObservationBearingRange::Ptr last_bearing_range_;
93  mrpt::obs::CObservationBeaconRanges::Ptr last_beacon_range_;
94  mrpt::obs::CObservation2DRangeScan::Ptr last_range_scan_;
95  mrpt::obs::CObservationOdometry::Ptr last_odometry_;
97  std::map<std::string, mrpt::poses::CPose3D> static_tf_;
99  void addObservation(const ros::Time& time);
100  bool waitForTransform(
101  mrpt::poses::CPose3D& des, const std::string& target_frame,
102  const std::string& source_frame, const ros::Time& time,
103  const ros::Duration& timeout,
104  const ros::Duration& polling_sleep_duration = ros::Duration(0.01));
105 
106  void convert(
107  const nav_msgs::Odometry& src, mrpt::obs::CObservationOdometry& des);
108 };
109 
110 #endif // MRPT_RAWLOG_RECORD_NODE_H
unsigned int sync_attempts_sensor_frame_
ParametersNode param_
mrpt::obs::CObservationBearingRange::Ptr last_bearing_range_
bool getStaticTF(std::string source_frame, mrpt::poses::CPose3D &des)
void callbackLaser(const sensor_msgs::LaserScan &)
ParametersNode(RawlogRecord::Parameters &base_params)
dynamic_reconfigure::Server< mrpt_rawlog::RawLogRecordConfig >::CallbackType reconfigureFnc_
GLuint src
RawlogRecord::Parameters & base_param_
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))
unsigned int uint32_t
ros::Subscriber subOdometry_
GLsizei n
mrpt::obs::CObservation2DRangeScan::Ptr last_range_scan_
void convert(const nav_msgs::Odometry &src, mrpt::obs::CObservationOdometry &des)
tf::TransformListener listenerTF_
GLint level
std::map< std::string, mrpt::poses::CPose3D > static_tf_
void addObservation(const ros::Time &time)
Parameters base_param_
Definition: rawlog_record.h:82
dynamic_reconfigure::Server< mrpt_rawlog::RawLogRecordConfig > reconfigureServer_
ros::NodeHandle n_
void callbackOdometry(const nav_msgs::Odometry &)
mrpt::obs::CObservationBeaconRanges::Ptr last_beacon_range_
mrpt::obs::CObservationOdometry::Ptr last_odometry_
ros::Subscriber subLaser_
RawlogRecordNode(ros::NodeHandle &n)
void callbackMarker(const marker_msgs::MarkerDetection &)
void update(const unsigned long &loop_count)
void callbackParameters(mrpt_rawlog::RawLogRecordConfig &config, uint32_t level)
ros::Subscriber subMarker_


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