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