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 : public Parameters 00053 { 00054 static const int MOTION_MODEL_GAUSSIAN = 0; 00055 static const int MOTION_MODEL_THRUN = 1; 00056 ParametersNode(); 00057 ros::NodeHandle node; 00058 void callbackParameters( 00059 mrpt_rawlog::RawLogRecordConfig& config, uint32_t level); 00060 dynamic_reconfigure::Server<mrpt_rawlog::RawLogRecordConfig> 00061 reconfigureServer_; 00062 dynamic_reconfigure::Server<mrpt_rawlog::RawLogRecordConfig>::CallbackType 00063 reconfigureFnc_; 00064 void update(const unsigned long& loop_count); 00065 double rate; 00066 int parameter_update_skip; 00067 std::string tf_prefix; 00068 std::string odom_frame_id; 00069 std::string base_frame_id; 00070 double sensor_frame_sync_threshold; 00071 }; 00072 00073 RawlogRecordNode(ros::NodeHandle& n); 00074 ~RawlogRecordNode(); 00075 void init(); 00076 void loop(); 00077 void callbackLaser(const sensor_msgs::LaserScan&); 00078 void callbackMarker ( const marker_msgs::MarkerDetection& ); 00079 void callbackOdometry(const nav_msgs::Odometry&); 00080 00081 private: // functions 00082 ParametersNode* param(); 00083 void update(); 00084 bool getStaticTF(std::string source_frame, mrpt::poses::CPose3D& des); 00085 ros::Subscriber subLaser_; 00086 ros::Subscriber subMarker_; 00087 ros::Subscriber subOdometry_; 00088 tf::TransformListener listenerTF_; 00089 mrpt::obs::CObservationBearingRange::Ptr last_bearing_range_; 00090 mrpt::obs::CObservationBeaconRanges::Ptr last_beacon_range_; 00091 mrpt::obs::CObservation2DRangeScan::Ptr last_range_scan_; 00092 mrpt::obs::CObservationOdometry::Ptr last_odometry_; 00093 unsigned int sync_attempts_sensor_frame_; 00094 std::map<std::string, mrpt::poses::CPose3D> static_tf_; 00095 ros::NodeHandle n_; 00096 void addObservation(const ros::Time& time); 00097 bool waitForTransform( 00098 mrpt::poses::CPose3D& des, const std::string& target_frame, 00099 const std::string& source_frame, const ros::Time& time, 00100 const ros::Duration& timeout, 00101 const ros::Duration& polling_sleep_duration = ros::Duration(0.01)); 00102 00103 void convert(const nav_msgs::Odometry& src, mrpt::obs::CObservationOdometry &des); 00104 }; 00105 00106 #endif // MRPT_RAWLOG_RECORD_NODE_H