mrpt_localization_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" 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(mrpt::slam::CActionRobotMovement2D::TMotionModelOptions *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


mrpt_localization
Author(s):
autogenerated on Tue Aug 5 2014 10:58:12