rawlog_record_node.h
Go to the documentation of this file.
00001 /***********************************************************************************
00002  * Revised BSD License *
00003  * Copyright (c) 2014, Markus Bader <markus.bader@tuwien.ac.at> *
00004  * All rights reserved. *
00005  *                                                                                 *
00006  * Redistribution and use in source and binary forms, with or without *
00007  * modification, are permitted provided that the following conditions are met: *
00008  *     * Redistributions of source code must retain the above copyright *
00009  *       notice, this list of conditions and the following disclaimer. *
00010  *     * Redistributions in binary form must reproduce the above copyright *
00011  *       notice, this list of conditions and the following disclaimer in the *
00012  *       documentation and/or other materials provided with the distribution. *
00013  *     * Neither the name of the Vienna University of Technology nor the *
00014  *       names of its contributors may be used to endorse or promote products *
00015  *       derived from this software without specific prior written permission. *
00016  *                                                                                 *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  *AND *
00019  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00020  **
00021  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE *
00022  * DISCLAIMED. IN NO EVENT SHALL Markus Bader BE LIABLE FOR ANY *
00023  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES *
00024  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025  **
00026  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND *
00027  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT *
00028  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00029  **
00030  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00031  **                       *
00032  ***********************************************************************************/
00033 
00034 #ifndef MRPT_RAWLOG_RECORD_NODE_H
00035 #define MRPT_RAWLOG_RECORD_NODE_H
00036 
00037 #include <ros/ros.h>
00038 #include <tf/transform_listener.h>
00039 #include <nav_msgs/Odometry.h>
00040 #include <sensor_msgs/LaserScan.h>
00041 #include <marker_msgs/MarkerDetection.h>
00042 #include <dynamic_reconfigure/server.h>
00043 #include "mrpt_rawlog/RawLogRecordConfig.h"
00044 #include "mrpt_rawlog_record/rawlog_record.h"
00045 
00047 class RawlogRecordNode : public RawlogRecord
00048 {
00049         MRPT_ROS_LOG_MACROS;
00050 
00051    public:
00052         struct ParametersNode
00053         {
00054                 static const int MOTION_MODEL_GAUSSIAN = 0;
00055                 static const int MOTION_MODEL_THRUN = 1;
00056                 ParametersNode(RawlogRecord::Parameters& base_params);
00057                 ros::NodeHandle node;
00058                 RawlogRecord::Parameters& base_param_;
00059                 void callbackParameters(
00060                         mrpt_rawlog::RawLogRecordConfig& config, uint32_t level);
00061                 dynamic_reconfigure::Server<mrpt_rawlog::RawLogRecordConfig>
00062                         reconfigureServer_;
00063                 dynamic_reconfigure::Server<
00064                     mrpt_rawlog::RawLogRecordConfig>::CallbackType reconfigureFnc_;
00065                 void update(const unsigned long& loop_count);
00066                 double rate;
00067                 int parameter_update_skip;
00068                 std::string tf_prefix;
00069                 std::string odom_frame_id;
00070                 std::string base_frame_id;
00071                 double sensor_frame_sync_threshold;
00072         };
00073 
00074         RawlogRecordNode(ros::NodeHandle& n);
00075         ~RawlogRecordNode();
00076         void init();
00077         void loop();
00078         void callbackLaser(
00079             const sensor_msgs::LaserScan&);  
00080 
00081         void callbackMarker(const marker_msgs::MarkerDetection&);
00082         void callbackOdometry(const nav_msgs::Odometry&);
00083 
00084    private:  // functions
00085         ParametersNode param_{RawlogRecord::base_param_};
00086         void update();
00087         bool getStaticTF(std::string source_frame, mrpt::poses::CPose3D& des);
00088         ros::Subscriber subLaser_;
00089         ros::Subscriber subMarker_;
00090         ros::Subscriber subOdometry_;
00091         tf::TransformListener listenerTF_;
00092         mrpt::obs::CObservationBearingRange::Ptr last_bearing_range_;
00093         mrpt::obs::CObservationBeaconRanges::Ptr last_beacon_range_;
00094         mrpt::obs::CObservation2DRangeScan::Ptr last_range_scan_;
00095         mrpt::obs::CObservationOdometry::Ptr last_odometry_;
00096         unsigned int sync_attempts_sensor_frame_;
00097         std::map<std::string, mrpt::poses::CPose3D> static_tf_;
00098         ros::NodeHandle n_;
00099         void addObservation(const ros::Time& time);
00100         bool waitForTransform(
00101                 mrpt::poses::CPose3D& des, const std::string& target_frame,
00102                 const std::string& source_frame, const ros::Time& time,
00103                 const ros::Duration& timeout,
00104                 const ros::Duration& polling_sleep_duration = ros::Duration(0.01));
00105 
00106         void convert(
00107             const nav_msgs::Odometry& src, mrpt::obs::CObservationOdometry& des);
00108 };
00109 
00110 #endif  // MRPT_RAWLOG_RECORD_NODE_H


mrpt_rawlog
Author(s):
autogenerated on Thu Jun 6 2019 21:53:23