HectorMappingRos.h
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Simulation, Systems Optimization and Robotics
00013 //       group, TU Darmstadt nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       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 THE COPYRIGHT HOLDER 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 HECTOR_MAPPING_ROS_H__
00030 #define HECTOR_MAPPING_ROS_H__
00031 
00032 #include "ros/ros.h"
00033 
00034 #include "tf/transform_listener.h"
00035 #include "tf/transform_broadcaster.h"
00036 #include "tf/message_filter.h"
00037 #include "message_filters/subscriber.h"
00038 
00039 #include "sensor_msgs/LaserScan.h"
00040 #include <std_msgs/String.h>
00041 
00042 #include "laser_geometry/laser_geometry.h"
00043 #include "nav_msgs/GetMap.h"
00044 
00045 #include "slam_main/HectorSlamProcessor.h"
00046 
00047 #include "scan/DataPointContainer.h"
00048 #include "util/MapLockerInterface.h"
00049 
00050 #include <boost/thread.hpp>
00051 
00052 #include "PoseInfoContainer.h"
00053 
00054 
00055 class HectorDrawings;
00056 class HectorDebugInfoProvider;
00057 
00058 class MapPublisherContainer
00059 {
00060 public:
00061   ros::Publisher mapPublisher_;
00062   ros::Publisher mapMetadataPublisher_;
00063   nav_msgs::GetMap::Response map_;
00064   ros::ServiceServer dynamicMapServiceServer_;
00065 };
00066 
00067 class HectorMappingRos
00068 {
00069 public:
00070   HectorMappingRos();
00071   ~HectorMappingRos();
00072 
00073 
00074   void scanCallback(const sensor_msgs::LaserScan& scan);
00075   void sysMsgCallback(const std_msgs::String& string);
00076 
00077   bool mapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res);
00078 
00079   void publishMap(MapPublisherContainer& map_, const hectorslam::GridMap& gridMap, ros::Time timestamp, MapLockerInterface* mapMutex = 0);
00080 
00081   bool rosLaserScanToDataContainer(const sensor_msgs::LaserScan& scan, hectorslam::DataContainer& dataContainer, float scaleToMap);
00082   bool rosPointCloudToDataContainer(const sensor_msgs::PointCloud& pointCloud, const tf::StampedTransform& laserTransform, hectorslam::DataContainer& dataContainer, float scaleToMap);
00083 
00084   void setServiceGetMapData(nav_msgs::GetMap::Response& map_, const hectorslam::GridMap& gridMap);
00085 
00086   void publishTransformLoop(double p_transform_pub_period_);
00087   void publishMapLoop(double p_map_pub_period_);
00088   void publishTransform();
00089 
00090   void staticMapCallback(const nav_msgs::OccupancyGrid& map);
00091   void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg);
00092 
00093   /*
00094   void setStaticMapData(const nav_msgs::OccupancyGrid& map);
00095   */
00096 protected:
00097 
00098   HectorDebugInfoProvider* debugInfoProvider;
00099   HectorDrawings* hectorDrawings;
00100 
00101   int lastGetMapUpdateIndex;
00102 
00103   ros::NodeHandle node_;
00104 
00105   ros::Subscriber scanSubscriber_;
00106   ros::Subscriber sysMsgSubscriber_;
00107 
00108   ros::Subscriber mapSubscriber_;
00109   message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>* initial_pose_sub_;
00110   tf::MessageFilter<geometry_msgs::PoseWithCovarianceStamped>* initial_pose_filter_;
00111 
00112   ros::Publisher posePublisher_;
00113   ros::Publisher poseUpdatePublisher_;
00114   ros::Publisher twistUpdatePublisher_;
00115   ros::Publisher odometryPublisher_;
00116   ros::Publisher scan_point_cloud_publisher_;
00117 
00118   std::vector<MapPublisherContainer> mapPubContainer;
00119 
00120   tf::TransformListener tf_;
00121   tf::TransformBroadcaster* tfB_;
00122 
00123   laser_geometry::LaserProjection projector_;
00124 
00125   tf::Transform map_to_odom_;
00126 
00127   boost::thread* map__publish_thread_;
00128 
00129   hectorslam::HectorSlamProcessor* slamProcessor;
00130   hectorslam::DataContainer laserScanContainer;
00131 
00132   PoseInfoContainer poseInfoContainer_;
00133 
00134   sensor_msgs::PointCloud laser_point_cloud_;
00135 
00136   ros::Time lastMapPublishTime;
00137   ros::Time lastScanTime;
00138   Eigen::Vector3f lastSlamPose;
00139 
00140   bool initial_pose_set_;
00141   Eigen::Vector3f initial_pose_;
00142 
00143 
00144   //-----------------------------------------------------------
00145   // Parameters
00146 
00147   std::string p_base_frame_;
00148   std::string p_map_frame_;
00149   std::string p_odom_frame_;
00150 
00151   //Parameters related to publishing the scanmatcher pose directly via tf
00152   bool p_pub_map_scanmatch_transform_;
00153   std::string p_tf_map_scanmatch_transform_frame_name_;
00154 
00155   std::string p_scan_topic_;
00156   std::string p_sys_msg_topic_;
00157 
00158   std::string p_pose_update_topic_;
00159   std::string p_twist_update_topic_;
00160 
00161   bool p_pub_drawings;
00162   bool p_pub_debug_output_;
00163   bool p_pub_map_odom_transform_;
00164   bool p_pub_odometry_;
00165   bool p_advertise_map_service_;
00166   int p_scan_subscriber_queue_size_;
00167 
00168   double p_update_factor_free_;
00169   double p_update_factor_occupied_;
00170   double p_map_update_distance_threshold_;
00171   double p_map_update_angle_threshold_;
00172 
00173   double p_map_resolution_;
00174   int p_map_size_;
00175   double p_map_start_x_;
00176   double p_map_start_y_;
00177   int p_map_multi_res_levels_;
00178 
00179   double p_map_pub_period_;
00180 
00181   bool p_use_tf_scan_transformation_;
00182   bool p_use_tf_pose_start_estimate_;
00183   bool p_map_with_known_poses_;
00184   bool p_timing_output_;
00185 
00186 
00187   float p_sqr_laser_min_dist_;
00188   float p_sqr_laser_max_dist_;
00189   float p_laser_z_min_value_;
00190   float p_laser_z_max_value_;
00191 };
00192 
00193 #endif


hector_mapping
Author(s): Stefan Kohlbrecher
autogenerated on Mon Oct 6 2014 00:33:51