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" AND * 00018 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * 00019 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * 00020 * DISCLAIMED. IN NO EVENT SHALL Markus Bader BE LIABLE FOR ANY * 00021 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * 00022 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * 00023 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * 00024 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * 00025 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * 00026 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * * 00027 ***********************************************************************************/ 00028 00029 #ifndef MRPT_LOCALIZATION_NODE_H 00030 #define MRPT_LOCALIZATION_NODE_H 00031 00032 #include "ros/ros.h" 00033 #include <tf/transform_listener.h> 00034 #include <tf/transform_broadcaster.h> 00035 #include <nav_msgs/Odometry.h> 00036 #include <nav_msgs/GetMap.h> 00037 #include <sensor_msgs/LaserScan.h> 00038 #include <geometry_msgs/PoseWithCovarianceStamped.h> 00039 #include <nav_msgs/OccupancyGrid.h> 00040 #include <dynamic_reconfigure/server.h> 00041 #include "mrpt_localization/MotionConfig.h" 00042 #include "mrpt_localization/mrpt_localization.h" 00043 #include "nav_msgs/MapMetaData.h" 00044 00046 class PFLocalizationNode : public PFLocalization { 00047 MRPT_ROS_LOG_MACROS; 00048 public: 00049 struct Parameters : public PFLocalization::Parameters{ 00050 static const int MOTION_MODEL_GAUSSIAN = 0; 00051 static const int MOTION_MODEL_THRUN = 1; 00052 Parameters(PFLocalizationNode *p); 00053 ros::NodeHandle node; 00054 void callbackParameters (mrpt_localization::MotionConfig &config, uint32_t level ); 00055 dynamic_reconfigure::Server<mrpt_localization::MotionConfig> reconfigureServer_; 00056 dynamic_reconfigure::Server<mrpt_localization::MotionConfig>::CallbackType reconfigureFnc_; 00057 void update(const unsigned long &loop_count); 00058 double rate; 00059 int parameter_update_skip; 00060 int particlecloud_update_skip; 00061 int map_update_skip; 00062 std::string tf_prefix; 00063 std::string odom_frame_id; 00064 std::string global_frame_id; 00065 std::string base_frame_id; 00066 }; 00067 00068 PFLocalizationNode ( ros::NodeHandle &n ); 00069 ~PFLocalizationNode(); 00070 void init (); 00071 void loop (); 00072 void callbackLaser (const sensor_msgs::LaserScan&); 00073 void callbackInitialpose (const geometry_msgs::PoseWithCovarianceStamped&); 00074 void updateMap (const nav_msgs::OccupancyGrid&); 00075 void publishTF(); 00076 private: //functions 00077 Parameters *param(); 00078 void update (); 00079 void updateLaserPose (std::string frame_id); 00080 ros::Subscriber subInitPose_; 00081 ros::Subscriber subLaser0_; 00082 ros::Subscriber subLaser1_; 00083 ros::Subscriber subLaser2_; 00084 ros::Subscriber subMap_; 00085 ros::ServiceClient clientMap_; 00086 ros::Publisher pub_Particles_; 00087 ros::Publisher pub_map_; 00088 ros::Publisher pub_metadata_; 00089 ros::ServiceServer service_map_; 00090 tf::TransformListener listenerTF_; 00091 tf::TransformBroadcaster tf_broadcaster_; 00092 std::map<std::string, mrpt::poses::CPose3D> laser_poses_; 00093 ros::NodeHandle n_; 00094 unsigned long loop_count_; 00095 void publishParticles(); 00096 00097 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)); 00098 bool mapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res ); 00099 void publishMap (); 00100 virtual bool waitForMap(); 00101 nav_msgs::GetMap::Response resp_; 00102 }; 00103 00104 #endif // MRPT_LOCALIZATION_NODE_H