laser_height_estimation.h
Go to the documentation of this file.00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, 00005 * Ivan Dryanovski <ivan.dryanovski@gmail.com> 00006 * William Morris <morris@ee.ccny.cuny.edu> 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of CCNY Robotics Lab nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 */ 00036 00037 #ifndef LASER_HEIGHT_ESTIMATION_LASER_HEIGHT_ESTIMATION_H 00038 #define LASER_HEIGHT_ESTIMATION_LASER_HEIGHT_ESTIMATION_H 00039 00040 #include <ros/ros.h> 00041 #include <sensor_msgs/LaserScan.h> 00042 #include <sensor_msgs/Imu.h> 00043 #include <std_msgs/Float64.h> 00044 #include <mav_msgs/common.h> 00045 #include <mav_msgs/Height.h> 00046 #include <tf/transform_datatypes.h> 00047 #include <tf/transform_listener.h> 00048 #include <boost/thread/mutex.hpp> 00049 00050 const std::string scan_topic_ = "scan"; 00051 00052 namespace mav 00053 { 00054 00055 class LaserHeightEstimation 00056 { 00057 private: 00058 00059 // **** ros-related variables 00060 00061 ros::NodeHandle nh_; 00062 ros::NodeHandle nh_private_; 00063 ros::Subscriber imu_subscriber_; 00064 ros::Subscriber scan_subscriber_; 00065 ros::Publisher height_to_base_publisher_; 00066 ros::Publisher height_to_footprint_publisher_; 00067 tf::TransformListener tf_listener_; 00068 00069 // **** state variables 00070 00071 bool initialized_; 00072 double floor_height_; 00073 double prev_height_; 00074 00075 btTransform base_to_laser_; 00076 btTransform base_to_footprint_; 00077 btTransform world_to_base_; 00078 00079 sensor_msgs::Imu latest_imu_msg_; 00080 00081 ros::Time last_update_time_; 00082 00083 // **** parameters 00084 00085 std::string world_frame_; 00086 std::string base_frame_; 00087 std::string footprint_frame_; 00088 int min_values_; 00089 double max_stdev_; 00090 double max_height_jump_; 00091 bool use_imu_; 00092 00093 // **** member functions 00094 00095 void scanCallback (const sensor_msgs::LaserScanPtr& scan_msg); 00096 void imuCallback (const sensor_msgs::ImuPtr& imu_msg); 00097 bool setBaseToLaserTf(const sensor_msgs::LaserScanPtr& scan_msg); 00098 void getStats(const std::vector<double> values, double& ave, double& stdev); 00099 00100 public: 00101 00102 LaserHeightEstimation(ros::NodeHandle nh, ros::NodeHandle nh_private); 00103 virtual ~LaserHeightEstimation(); 00104 }; 00105 }; // namespace mav 00106 00107 #endif // LASER_HEIGHT_ESTIMATION_LASER_HEIGHT_ESTIMATION_H 00108