Merge pull request #1 from ekumenlabs/fix-integration-with-move-base

Add param to shift pose publication ts
ekumenlabs · Jul 1, 2020 · 223c611 · 223c611
2 parents 3f63834 + 05497e7
commit 223c611
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 1 deletion.
diff --git a/hector_mapping/src/HectorMappingRos.cpp b/hector_mapping/src/HectorMappingRos.cpp
@@ -92,6 +92,10 @@ HectorMappingRos::HectorMappingRos()
   private_nh_.param("output_timing", p_timing_output_,false);
 
   private_nh_.param("map_pub_period", p_map_pub_period_, 2.0);
+  // Offset (ROS Duration) to add to pose data when publishing it. A
+  // positive value shifts the pose ts to the future and enables using
+  // move base to move around while mapping.
+  private_nh_.param("transform_tolerance", p_transform_tolerance_, 1.0);
 
   double tmp = 0.0;
   private_nh_.param("laser_min_dist", tmp, 0.4);
@@ -177,6 +181,7 @@ HectorMappingRos::HectorMappingRos()
   ROS_INFO("HectorSM p_map_update_angle_threshold_: %f", p_map_update_angle_threshold_);
   ROS_INFO("HectorSM p_laser_z_min_value_: %f", p_laser_z_min_value_);
   ROS_INFO("HectorSM p_laser_z_max_value_: %f", p_laser_z_max_value_);
+  ROS_INFO("HectorSM p_transform_tolerance_: %f", p_transform_tolerance_);
 
   scanSubscriber_ = node_.subscribe(p_scan_topic_, p_scan_subscriber_queue_size_, &HectorMappingRos::scanCallback, this);
   sysMsgSubscriber_ = node_.subscribe(p_sys_msg_topic_, 2, &HectorMappingRos::sysMsgCallback, this);
@@ -345,7 +350,7 @@ void HectorMappingRos::scanCallback(const sensor_msgs::LaserScan& scan)
       odom_to_base.setIdentity();
     }
     map_to_odom_ = tf::Transform(poseInfoContainer_.getTfTransform() * odom_to_base.inverse());
-    tfB_->sendTransform( tf::StampedTransform (map_to_odom_, scan.header.stamp, p_map_frame_, p_odom_frame_));
+    tfB_->sendTransform( tf::StampedTransform (map_to_odom_, scan.header.stamp + ros::Duration(p_transform_tolerance_), p_map_frame_, p_odom_frame_));
   }
 
   if (p_pub_map_scanmatch_transform_){

diff --git a/hector_mapping/src/HectorMappingRos.h b/hector_mapping/src/HectorMappingRos.h
@@ -177,6 +177,7 @@ class HectorMappingRos
   int p_map_multi_res_levels_;
 
   double p_map_pub_period_;
+  double p_transform_tolerance_;
 
   bool p_use_tf_scan_transformation_;
   bool p_use_tf_pose_start_estimate_;