Merge branch 'develop' into kinetic-devel

Author: jack-digilabs

fiducial_slam/include/fiducial_slam/map.h

@@ -211,6 +211,7 @@ class Map {
 
     bool havePose;
     float tfPublishInterval;
+    bool publishPoseTf;
     ros::Time tfPublishTime;
     geometry_msgs::TransformStamped poseTf;
 

fiducial_slam/launch/fiducial_slam.launch

@@ -7,6 +7,7 @@
   <arg name="odom_frame" default="odom"/>
   <arg name="base_frame" default="base_link"/>
   <arg name="publish_tf" default="true"/>
+  <arg name="tf_publish_interval" default="0.2"/>
   <arg name="future_date_transforms" default="0.0"/>
   <arg name="publish_6dof_pose" default="false"/>
   <arg name="fiducial_len" default="0.14"/>
@@ -19,6 +20,8 @@
     <param name="map_frame" value="$(arg map_frame)" />
     <param name="odom_frame" value="$(arg odom_frame)" />
     <param name="base_frame" value="$(arg base_frame)" />
+    <param name="publish_tf" value="$(arg publish_tf)" />
+    <param name="tf_publish_interval" value="$(arg tf_publish_interval)" />
     <param name="future_date_transforms" value="$(arg future_date_transforms)" />
     <param name="publish_6dof_pose" value="$(arg publish_6dof_pose)" />
     <param name="do_pose_estimation" value="$(arg do_pose_estimation)"/>

fiducial_slam/src/map.cpp

@@ -195,6 +195,7 @@ Map::Map(ros::NodeHandle &nh) : tfBuffer(ros::Duration(30.0)){
     nh.param<std::string>("base_frame", baseFrame, "base_link");
 
     nh.param<float>("tf_publish_interval", tfPublishInterval, 1.0);
+    nh.param<bool>("publish_tf", publishPoseTf, false);
     nh.param<float>("systematic_error", systematic_error, 0.01f);
     nh.param<double>("future_date_transforms", future_date_transforms, 0.1);
     nh.param<bool>("publish_6dof_pose", publish_6dof_pose, false);
@@ -491,7 +492,11 @@ int Map::updatePose(vector<Observation>& obs, const ros::Time &time,
     poseTf = toMsg(outPose);
     poseTf.child_frame_id = outFrame;
     havePose = true;
-    publishTf();
+
+    if (publishPoseTf)
+    {
+        publishTf();
+    }
 
     ROS_INFO("Finished frame\n");
     return numEsts;
@@ -511,7 +516,7 @@ void Map::publishTf()
 void Map::update()
 {
     ros::Time now = ros::Time::now();
-    if (havePose && tfPublishInterval != 0.0 &&
+    if (publishPoseTf && havePose && tfPublishInterval != 0.0 &&
         (now - tfPublishTime).toSec() > tfPublishInterval) {
         publishTf();
         tfPublishTime = now;