HectorMappingRos.cpp
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 #include "HectorMappingRos.h"
00030 
00031 #include "map/GridMap.h"
00032 
00033 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00034 #include <nav_msgs/Odometry.h>
00035 
00036 #include "sensor_msgs/PointCloud2.h"
00037 
00038 #include "HectorDrawings.h"
00039 #include "HectorDebugInfoProvider.h"
00040 #include "HectorMapMutex.h"
00041 
00042 #ifndef TF_SCALAR_H
00043   typedef btScalar tfScalar;
00044 #endif
00045 
00046 HectorMappingRos::HectorMappingRos()
00047   : debugInfoProvider(0)
00048   , hectorDrawings(0)
00049   , lastGetMapUpdateIndex(-100)
00050   , tfB_(0)
00051   , map__publish_thread_(0)
00052   , initial_pose_set_(false)
00053 {
00054   ros::NodeHandle private_nh_("~");
00055 
00056   std::string mapTopic_ = "map";
00057 
00058   private_nh_.param("pub_drawings", p_pub_drawings, false);
00059   private_nh_.param("pub_debug_output", p_pub_debug_output_, false);
00060   private_nh_.param("pub_map_odom_transform", p_pub_map_odom_transform_,true);
00061   private_nh_.param("pub_odometry", p_pub_odometry_,false);
00062   private_nh_.param("advertise_map_service", p_advertise_map_service_,true);
00063   private_nh_.param("scan_subscriber_queue_size", p_scan_subscriber_queue_size_, 5);
00064 
00065   private_nh_.param("map_resolution", p_map_resolution_, 0.025);
00066   private_nh_.param("map_size", p_map_size_, 1024);
00067   private_nh_.param("map_start_x", p_map_start_x_, 0.5);
00068   private_nh_.param("map_start_y", p_map_start_y_, 0.5);
00069   private_nh_.param("map_multi_res_levels", p_map_multi_res_levels_, 3);
00070 
00071   private_nh_.param("update_factor_free", p_update_factor_free_, 0.4);
00072   private_nh_.param("update_factor_occupied", p_update_factor_occupied_, 0.9);
00073 
00074   private_nh_.param("map_update_distance_thresh", p_map_update_distance_threshold_, 0.4);
00075   private_nh_.param("map_update_angle_thresh", p_map_update_angle_threshold_, 0.9);
00076 
00077   private_nh_.param("scan_topic", p_scan_topic_, std::string("scan"));
00078   private_nh_.param("sys_msg_topic", p_sys_msg_topic_, std::string("syscommand"));
00079   private_nh_.param("pose_update_topic", p_pose_update_topic_, std::string("poseupdate"));
00080 
00081   private_nh_.param("use_tf_scan_transformation", p_use_tf_scan_transformation_,true);
00082   private_nh_.param("use_tf_pose_start_estimate", p_use_tf_pose_start_estimate_,false);
00083   private_nh_.param("map_with_known_poses", p_map_with_known_poses_, false);
00084 
00085   private_nh_.param("base_frame", p_base_frame_, std::string("base_link"));
00086   private_nh_.param("map_frame", p_map_frame_, std::string("map"));
00087   private_nh_.param("odom_frame", p_odom_frame_, std::string("odom"));
00088 
00089   private_nh_.param("pub_map_scanmatch_transform", p_pub_map_scanmatch_transform_,true);
00090   private_nh_.param("tf_map_scanmatch_transform_frame_name", p_tf_map_scanmatch_transform_frame_name_, std::string("scanmatcher_frame"));
00091 
00092   private_nh_.param("output_timing", p_timing_output_,false);
00093 
00094   private_nh_.param("map_pub_period", p_map_pub_period_, 2.0);
00095 
00096   double tmp = 0.0;
00097   private_nh_.param("laser_min_dist", tmp, 0.4);
00098   p_sqr_laser_min_dist_ = static_cast<float>(tmp*tmp);
00099 
00100   private_nh_.param("laser_max_dist", tmp, 30.0);
00101   p_sqr_laser_max_dist_ = static_cast<float>(tmp*tmp);
00102 
00103   private_nh_.param("laser_z_min_value", tmp, -1.0);
00104   p_laser_z_min_value_ = static_cast<float>(tmp);
00105 
00106   private_nh_.param("laser_z_max_value", tmp, 1.0);
00107   p_laser_z_max_value_ = static_cast<float>(tmp);
00108 
00109   if (p_pub_drawings)
00110   {
00111     ROS_INFO("HectorSM publishing debug drawings");
00112     hectorDrawings = new HectorDrawings();
00113   }
00114 
00115   if(p_pub_debug_output_)
00116   {
00117     ROS_INFO("HectorSM publishing debug info");
00118     debugInfoProvider = new HectorDebugInfoProvider();
00119   }
00120 
00121   if(p_pub_odometry_)
00122   {
00123     odometryPublisher_ = node_.advertise<nav_msgs::Odometry>("scanmatch_odom", 50);
00124   }
00125 
00126   slamProcessor = new hectorslam::HectorSlamProcessor(static_cast<float>(p_map_resolution_), p_map_size_, p_map_size_, Eigen::Vector2f(p_map_start_x_, p_map_start_y_), p_map_multi_res_levels_, hectorDrawings, debugInfoProvider);
00127   slamProcessor->setUpdateFactorFree(p_update_factor_free_);
00128   slamProcessor->setUpdateFactorOccupied(p_update_factor_occupied_);
00129   slamProcessor->setMapUpdateMinDistDiff(p_map_update_distance_threshold_);
00130   slamProcessor->setMapUpdateMinAngleDiff(p_map_update_angle_threshold_);
00131 
00132   int mapLevels = slamProcessor->getMapLevels();
00133   mapLevels = 1;
00134 
00135   for (int i = 0; i < mapLevels; ++i)
00136   {
00137     mapPubContainer.push_back(MapPublisherContainer());
00138     slamProcessor->addMapMutex(i, new HectorMapMutex());
00139 
00140     std::string mapTopicStr(mapTopic_);
00141 
00142     if (i != 0)
00143     {
00144       mapTopicStr.append("_" + boost::lexical_cast<std::string>(i));
00145     }
00146 
00147     std::string mapMetaTopicStr(mapTopicStr);
00148     mapMetaTopicStr.append("_metadata");
00149 
00150     MapPublisherContainer& tmp = mapPubContainer[i];
00151     tmp.mapPublisher_ = node_.advertise<nav_msgs::OccupancyGrid>(mapTopicStr, 1, true);
00152     tmp.mapMetadataPublisher_ = node_.advertise<nav_msgs::MapMetaData>(mapMetaTopicStr, 1, true);
00153 
00154     if ( (i == 0) && p_advertise_map_service_)
00155     {
00156       tmp.dynamicMapServiceServer_ = node_.advertiseService("dynamic_map", &HectorMappingRos::mapCallback, this);
00157     }
00158 
00159     setServiceGetMapData(tmp.map_, slamProcessor->getGridMap(i));
00160 
00161     if ( i== 0){
00162       mapPubContainer[i].mapMetadataPublisher_.publish(mapPubContainer[i].map_.map.info);
00163     }
00164   }
00165 
00166   ROS_INFO("HectorSM p_base_frame_: %s", p_base_frame_.c_str());
00167   ROS_INFO("HectorSM p_map_frame_: %s", p_map_frame_.c_str());
00168   ROS_INFO("HectorSM p_odom_frame_: %s", p_odom_frame_.c_str());
00169   ROS_INFO("HectorSM p_scan_topic_: %s", p_scan_topic_.c_str());
00170   ROS_INFO("HectorSM p_use_tf_scan_transformation_: %s", p_use_tf_scan_transformation_ ? ("true") : ("false"));
00171   ROS_INFO("HectorSM p_pub_map_odom_transform_: %s", p_pub_map_odom_transform_ ? ("true") : ("false"));
00172   ROS_INFO("HectorSM p_scan_subscriber_queue_size_: %d", p_scan_subscriber_queue_size_);
00173   ROS_INFO("HectorSM p_map_pub_period_: %f", p_map_pub_period_);
00174   ROS_INFO("HectorSM p_update_factor_free_: %f", p_update_factor_free_);
00175   ROS_INFO("HectorSM p_update_factor_occupied_: %f", p_update_factor_occupied_);
00176   ROS_INFO("HectorSM p_map_update_distance_threshold_: %f ", p_map_update_distance_threshold_);
00177   ROS_INFO("HectorSM p_map_update_angle_threshold_: %f", p_map_update_angle_threshold_);
00178   ROS_INFO("HectorSM p_laser_z_min_value_: %f", p_laser_z_min_value_);
00179   ROS_INFO("HectorSM p_laser_z_max_value_: %f", p_laser_z_max_value_);
00180 
00181   scanSubscriber_ = node_.subscribe(p_scan_topic_, p_scan_subscriber_queue_size_, &HectorMappingRos::scanCallback, this);
00182   sysMsgSubscriber_ = node_.subscribe(p_sys_msg_topic_, 2, &HectorMappingRos::sysMsgCallback, this);
00183 
00184   poseUpdatePublisher_ = node_.advertise<geometry_msgs::PoseWithCovarianceStamped>(p_pose_update_topic_, 1, false);
00185   posePublisher_ = node_.advertise<geometry_msgs::PoseStamped>("slam_out_pose", 1, false);
00186 
00187   scan_point_cloud_publisher_ = node_.advertise<sensor_msgs::PointCloud>("slam_cloud",1,false);
00188 
00189   tfB_ = new tf::TransformBroadcaster();
00190   ROS_ASSERT(tfB_);
00191 
00192   /*
00193   bool p_use_static_map_ = false;
00194 
00195   if (p_use_static_map_){
00196     mapSubscriber_ = node_.subscribe(mapTopic_, 1, &HectorMappingRos::staticMapCallback, this);
00197   }
00198   */
00199 
00200   initial_pose_sub_ = new message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>(node_, "initialpose", 2);
00201   initial_pose_filter_ = new tf::MessageFilter<geometry_msgs::PoseWithCovarianceStamped>(*initial_pose_sub_, tf_, p_map_frame_, 2);
00202   initial_pose_filter_->registerCallback(boost::bind(&HectorMappingRos::initialPoseCallback, this, _1));
00203 
00204 
00205   map__publish_thread_ = new boost::thread(boost::bind(&HectorMappingRos::publishMapLoop, this, p_map_pub_period_));
00206 
00207   map_to_odom_.setIdentity();
00208 
00209   lastMapPublishTime = ros::Time(0,0);
00210 }
00211 
00212 HectorMappingRos::~HectorMappingRos()
00213 {
00214   delete slamProcessor;
00215 
00216   if (hectorDrawings)
00217     delete hectorDrawings;
00218 
00219   if (debugInfoProvider)
00220     delete debugInfoProvider;
00221 
00222   if (tfB_)
00223     delete tfB_;
00224 
00225   if(map__publish_thread_)
00226     delete map__publish_thread_;
00227 }
00228 
00229 void HectorMappingRos::scanCallback(const sensor_msgs::LaserScan& scan)
00230 {
00231   if (hectorDrawings)
00232   {
00233     hectorDrawings->setTime(scan.header.stamp);
00234   }
00235 
00236   ros::WallTime startTime = ros::WallTime::now();
00237 
00238   if (!p_use_tf_scan_transformation_)
00239   {
00240     if (rosLaserScanToDataContainer(scan, laserScanContainer,slamProcessor->getScaleToMap()))
00241     {
00242       slamProcessor->update(laserScanContainer,slamProcessor->getLastScanMatchPose());
00243     }
00244   }
00245   else
00246   {
00247     ros::Duration dur (0.5);
00248 
00249     if (tf_.waitForTransform(p_base_frame_,scan.header.frame_id, scan.header.stamp,dur))
00250     {
00251       tf::StampedTransform laserTransform;
00252       tf_.lookupTransform(p_base_frame_,scan.header.frame_id, scan.header.stamp, laserTransform);
00253 
00254       //projector_.transformLaserScanToPointCloud(p_base_frame_ ,scan, pointCloud,tf_);
00255       projector_.projectLaser(scan, laser_point_cloud_,30.0);
00256 
00257       if (scan_point_cloud_publisher_.getNumSubscribers() > 0){
00258         scan_point_cloud_publisher_.publish(laser_point_cloud_);
00259       }
00260 
00261       Eigen::Vector3f startEstimate(Eigen::Vector3f::Zero());
00262 
00263       if(rosPointCloudToDataContainer(laser_point_cloud_, laserTransform, laserScanContainer, slamProcessor->getScaleToMap()))
00264       {
00265         if (initial_pose_set_){
00266           initial_pose_set_ = false;
00267           startEstimate = initial_pose_;
00268         }else if (p_use_tf_pose_start_estimate_){
00269 
00270           try
00271           {
00272             tf::StampedTransform stamped_pose;
00273 
00274             tf_.waitForTransform(p_map_frame_,p_base_frame_, scan.header.stamp, ros::Duration(0.5));
00275             tf_.lookupTransform(p_map_frame_, p_base_frame_,  scan.header.stamp, stamped_pose);
00276 
00277             tfScalar yaw, pitch, roll;
00278             stamped_pose.getBasis().getEulerYPR(yaw, pitch, roll);
00279 
00280             startEstimate = Eigen::Vector3f(stamped_pose.getOrigin().getX(),stamped_pose.getOrigin().getY(), yaw);
00281           }
00282           catch(tf::TransformException e)
00283           {
00284             ROS_ERROR("Transform from %s to %s failed\n", p_map_frame_.c_str(), p_base_frame_.c_str());
00285             startEstimate = slamProcessor->getLastScanMatchPose();
00286           }
00287 
00288         }else{
00289           startEstimate = slamProcessor->getLastScanMatchPose();
00290         }
00291 
00292 
00293         if (p_map_with_known_poses_){
00294           slamProcessor->update(laserScanContainer, startEstimate, true);
00295         }else{
00296           slamProcessor->update(laserScanContainer, startEstimate);
00297         }
00298       }
00299 
00300     }else{
00301       ROS_INFO("lookupTransform %s to %s timed out. Could not transform laser scan into base_frame.", p_base_frame_.c_str(), scan.header.frame_id.c_str());
00302       return;
00303     }
00304   }
00305 
00306   if (p_timing_output_)
00307   {
00308     ros::WallDuration duration = ros::WallTime::now() - startTime;
00309     ROS_INFO("HectorSLAM Iter took: %f milliseconds", duration.toSec()*1000.0f );
00310   }
00311 
00312   //If we're just building a map with known poses, we're finished now. Code below this point publishes the localization results.
00313   if (p_map_with_known_poses_)
00314   {
00315     return;
00316   }
00317 
00318   poseInfoContainer_.update(slamProcessor->getLastScanMatchPose(), slamProcessor->getLastScanMatchCovariance(), scan.header.stamp, p_map_frame_);
00319 
00320   poseUpdatePublisher_.publish(poseInfoContainer_.getPoseWithCovarianceStamped());
00321   posePublisher_.publish(poseInfoContainer_.getPoseStamped());
00322 
00323   if(p_pub_odometry_)
00324   {
00325     nav_msgs::Odometry tmp;
00326     tmp.pose = poseInfoContainer_.getPoseWithCovarianceStamped().pose;
00327 
00328     tmp.header = poseInfoContainer_.getPoseWithCovarianceStamped().header;
00329     odometryPublisher_.publish(tmp);
00330   }
00331 
00332   if (p_pub_map_odom_transform_)
00333   {
00334     tf::StampedTransform odom_to_base;
00335 
00336     try
00337     {
00338       tf_.waitForTransform(p_odom_frame_, p_base_frame_, scan.header.stamp, ros::Duration(0.5));
00339       tf_.lookupTransform(p_odom_frame_, p_base_frame_, scan.header.stamp, odom_to_base);
00340     }
00341     catch(tf::TransformException e)
00342     {
00343       ROS_ERROR("Transform failed during publishing of map_odom transform: %s",e.what());
00344       odom_to_base.setIdentity();
00345     }
00346     map_to_odom_ = tf::Transform(poseInfoContainer_.getTfTransform() * odom_to_base.inverse());
00347     tfB_->sendTransform( tf::StampedTransform (map_to_odom_, scan.header.stamp, p_map_frame_, p_odom_frame_));
00348   }
00349 
00350   if (p_pub_map_scanmatch_transform_){
00351     tfB_->sendTransform( tf::StampedTransform(poseInfoContainer_.getTfTransform(), scan.header.stamp, p_map_frame_, p_tf_map_scanmatch_transform_frame_name_));
00352   }
00353 }
00354 
00355 void HectorMappingRos::sysMsgCallback(const std_msgs::String& string)
00356 {
00357   ROS_INFO("HectorSM sysMsgCallback, msg contents: %s", string.data.c_str());
00358 
00359   if (string.data == "reset")
00360   {
00361     ROS_INFO("HectorSM reset");
00362     slamProcessor->reset();
00363   }
00364 }
00365 
00366 bool HectorMappingRos::mapCallback(nav_msgs::GetMap::Request  &req,
00367                                    nav_msgs::GetMap::Response &res)
00368 {
00369   ROS_INFO("HectorSM Map service called");
00370   res = mapPubContainer[0].map_;
00371   return true;
00372 }
00373 
00374 void HectorMappingRos::publishMap(MapPublisherContainer& mapPublisher, const hectorslam::GridMap& gridMap, ros::Time timestamp, MapLockerInterface* mapMutex)
00375 {
00376   nav_msgs::GetMap::Response& map_ (mapPublisher.map_);
00377 
00378   //only update map if it changed
00379   if (lastGetMapUpdateIndex != gridMap.getUpdateIndex())
00380   {
00381 
00382     int sizeX = gridMap.getSizeX();
00383     int sizeY = gridMap.getSizeY();
00384 
00385     int size = sizeX * sizeY;
00386 
00387     std::vector<int8_t>& data = map_.map.data;
00388 
00389     //std::vector contents are guaranteed to be contiguous, use memset to set all to unknown to save time in loop
00390     memset(&data[0], -1, sizeof(int8_t) * size);
00391 
00392     if (mapMutex)
00393     {
00394       mapMutex->lockMap();
00395     }
00396 
00397     for(int i=0; i < size; ++i)
00398     {
00399       if(gridMap.isFree(i))
00400       {
00401         data[i] = 0;
00402       }
00403       else if (gridMap.isOccupied(i))
00404       {
00405         data[i] = 100;
00406       }
00407     }
00408 
00409     lastGetMapUpdateIndex = gridMap.getUpdateIndex();
00410 
00411     if (mapMutex)
00412     {
00413       mapMutex->unlockMap();
00414     }
00415   }
00416 
00417   map_.map.header.stamp = timestamp;
00418 
00419   mapPublisher.mapPublisher_.publish(map_.map);
00420 }
00421 
00422 bool HectorMappingRos::rosLaserScanToDataContainer(const sensor_msgs::LaserScan& scan, hectorslam::DataContainer& dataContainer, float scaleToMap)
00423 {
00424   size_t size = scan.ranges.size();
00425 
00426   float angle = scan.angle_min;
00427 
00428   dataContainer.clear();
00429 
00430   dataContainer.setOrigo(Eigen::Vector2f::Zero());
00431 
00432   float maxRangeForContainer = scan.range_max - 0.1f;
00433 
00434   for (size_t i = 0; i < size; ++i)
00435   {
00436     float dist = scan.ranges[i];
00437 
00438     if ( (dist > scan.range_min) && (dist < maxRangeForContainer))
00439     {
00440       dist *= scaleToMap;
00441       dataContainer.add(Eigen::Vector2f(cos(angle) * dist, sin(angle) * dist));
00442     }
00443 
00444     angle += scan.angle_increment;
00445   }
00446 
00447   return true;
00448 }
00449 
00450 bool HectorMappingRos::rosPointCloudToDataContainer(const sensor_msgs::PointCloud& pointCloud, const tf::StampedTransform& laserTransform, hectorslam::DataContainer& dataContainer, float scaleToMap)
00451 {
00452   size_t size = pointCloud.points.size();
00453   //ROS_INFO("size: %d", size);
00454 
00455   dataContainer.clear();
00456 
00457   tf::Vector3 laserPos (laserTransform.getOrigin());
00458   dataContainer.setOrigo(Eigen::Vector2f(laserPos.x(), laserPos.y())*scaleToMap);
00459 
00460   for (size_t i = 0; i < size; ++i)
00461   {
00462 
00463     const geometry_msgs::Point32& currPoint(pointCloud.points[i]);
00464 
00465     float dist_sqr = currPoint.x*currPoint.x + currPoint.y* currPoint.y;
00466 
00467     if ( (dist_sqr > p_sqr_laser_min_dist_) && (dist_sqr < p_sqr_laser_max_dist_) ){
00468 
00469       if ( (currPoint.x < 0.0f) && (dist_sqr < 0.50f)){
00470         continue;
00471       }
00472 
00473       tf::Vector3 pointPosBaseFrame(laserTransform * tf::Vector3(currPoint.x, currPoint.y, currPoint.z));
00474 
00475       float pointPosLaserFrameZ = pointPosBaseFrame.z() - laserPos.z();
00476 
00477       if (pointPosLaserFrameZ > p_laser_z_min_value_ && pointPosLaserFrameZ < p_laser_z_max_value_)
00478       {
00479         dataContainer.add(Eigen::Vector2f(pointPosBaseFrame.x(),pointPosBaseFrame.y())*scaleToMap);
00480       }
00481     }
00482   }
00483 
00484   return true;
00485 }
00486 
00487 void HectorMappingRos::setServiceGetMapData(nav_msgs::GetMap::Response& map_, const hectorslam::GridMap& gridMap)
00488 {
00489   Eigen::Vector2f mapOrigin (gridMap.getWorldCoords(Eigen::Vector2f::Zero()));
00490   mapOrigin.array() -= gridMap.getCellLength()*0.5f;
00491 
00492   map_.map.info.origin.position.x = mapOrigin.x();
00493   map_.map.info.origin.position.y = mapOrigin.y();
00494   map_.map.info.origin.orientation.w = 1.0;
00495 
00496   map_.map.info.resolution = gridMap.getCellLength();
00497 
00498   map_.map.info.width = gridMap.getSizeX();
00499   map_.map.info.height = gridMap.getSizeY();
00500 
00501   map_.map.header.frame_id = p_map_frame_;
00502   map_.map.data.resize(map_.map.info.width * map_.map.info.height);
00503 }
00504 
00505 /*
00506 void HectorMappingRos::setStaticMapData(const nav_msgs::OccupancyGrid& map)
00507 {
00508   float cell_length = map.info.resolution;
00509   Eigen::Vector2f mapOrigin (map.info.origin.position.x + cell_length*0.5f,
00510                              map.info.origin.position.y + cell_length*0.5f);
00511 
00512   int map_size_x = map.info.width;
00513   int map_size_y = map.info.height;
00514 
00515   slamProcessor = new hectorslam::HectorSlamProcessor(cell_length, map_size_x, map_size_y, Eigen::Vector2f(0.0f, 0.0f), 1, hectorDrawings, debugInfoProvider);
00516 }
00517 */
00518 
00519 
00520 void HectorMappingRos::publishMapLoop(double map_pub_period)
00521 {
00522   ros::Rate r(1.0 / map_pub_period);
00523   while(ros::ok())
00524   {
00525     //ros::WallTime t1 = ros::WallTime::now();
00526     ros::Time mapTime (ros::Time::now());
00527     //publishMap(mapPubContainer[2],slamProcessor->getGridMap(2), mapTime);
00528     //publishMap(mapPubContainer[1],slamProcessor->getGridMap(1), mapTime);
00529     publishMap(mapPubContainer[0],slamProcessor->getGridMap(0), mapTime, slamProcessor->getMapMutex(0));
00530 
00531     //ros::WallDuration t2 = ros::WallTime::now() - t1;
00532 
00533     //std::cout << "time s: " << t2.toSec();
00534     //ROS_INFO("HectorSM ms: %4.2f", t2.toSec()*1000.0f);
00535 
00536     r.sleep();
00537   }
00538 }
00539 
00540 void HectorMappingRos::staticMapCallback(const nav_msgs::OccupancyGrid& map)
00541 {
00542 
00543 }
00544 
00545 void HectorMappingRos::initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
00546 {
00547   initial_pose_set_ = true;
00548 
00549   tf::Pose pose;
00550   tf::poseMsgToTF(msg->pose.pose, pose);
00551   initial_pose_ = Eigen::Vector3f(msg->pose.pose.position.x, msg->pose.pose.position.y, tf::getYaw(pose.getRotation()));
00552   ROS_INFO("Setting initial pose with world coords x: %f y: %f yaw: %f", initial_pose_[0], initial_pose_[1], initial_pose_[2]);
00553 }
00554 
00555 


hector_mapping
Author(s): Stefan Kohlbrecher
autogenerated on Wed Aug 26 2015 11:45:03