Fixed bug on no changing stamp when tf fails

cfg/AdcLandmarksSlamSolver.cfg

@@ -82,8 +82,8 @@ flags.add("amcl_localization",                    bool_t,       0,           "Bo
 flags.add("calculate_covariance",                 bool_t,       0,           "Boolean to calculate robot pose covariance",                                False)
 flags.add("publish_tf_map_odom",                  bool_t,       0,           "Boolean to publish tf from map to odom",                                    False)
 
-sensor_noise.add("sensor_sigma_th",                double_t,     0,            "Sensor angular sigma",                                                       0.035, 0.001, 0.1)
-sensor_noise.add("sensor_sigma_r",                 double_t,     0,            "Sensor radial sigma",                                                        0.05, 0.01, 0.2)
+sensor_noise.add("sensor_sigma_th",                double_t,     0,            "Sensor angular sigma",                                                       0.035, 0.001, 1.0)
+sensor_noise.add("sensor_sigma_r",                 double_t,     0,            "Sensor radial sigma",                                                        0.05, 0.01, 1.0)
 
 odom_noise.add("odom_fxy",                      double_t,     0,            "Odom linear sigma factor",                                                   0.05, 0.01, 1.0)
 odom_noise.add("odom_fth",                      double_t,     0,            "Odom angular sigma factor",                                                  0.05, 0.01, 1.0)

src/adc_landmarks_slam_solver_alg_node.cpp

@@ -109,7 +109,6 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
       this->estimated_sigma_(1) =this->estimated_sigma_(0);
       this->estimated_sigma_(2) =this->estimated_sigma_(0);
     }
-    this->new_estimated_pose_t_ = ros::Time::now();
   }
   else
   {
@@ -147,6 +146,10 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
   this->last_publish_odom_ << 0.0, 0.0, 0.0;
   this->publish_pose_ = false;
 
+  this->new_estimated_pose_t_ = ros::Time::now();
+  this->features_stamp_ = ros::Time::now();
+  this->tf_map_odom_ << 0.0, 0.0, 0.0;
+
   this->robot_pose_msg_.header.frame_id = this->config_.global_frame;
   this->robot_pose_msg_.pose.pose.position.z = 0.0;
   for (unsigned int i = 0; i < 36; i++)
@@ -235,6 +238,22 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
       if (!this->tf2_buffer.canTransform(this->config_.odom_frame, this->config_.base_link_frame, this->new_estimated_pose_t_, ros::Duration(this->config_.tf_timeout)))
       {
         ROS_WARN_STREAM("AdcLandmarksSlamSolverAlgNode:mainNodeThread -> Can't transform from " << this->config_.odom_frame << " to " << this->config_.base_link_frame << " at " << this->new_estimated_pose_t_);
+        this->new_estimated_pose_t_ = ros::Time::now();
+        if (this->config_.publish_tf_map_odom && !this->config_.amcl_localization)
+        {
+          this->tf_map_odom_msg.header.stamp = ros::Time::now();
+          this->tf_map_odom_msg.header.frame_id = this->config_.global_frame;
+          this->tf_map_odom_msg.child_frame_id = this->config_.odom_frame;
+          geometry_msgs::Transform tf;
+          tf.translation.x = this->tf_map_odom_(0);
+          tf.translation.y = this->tf_map_odom_(1);
+          tf.translation.z = 0.0;
+          tf2::Quaternion q;
+          q.setRPY(0.0, 0.0, this->tf_map_odom_(2));
+          tf.rotation = tf2::toMsg(q);
+          this->tf_map_odom_msg.transform = tf;
+          this->tf2_broadcaster.sendTransform(this->tf_map_odom_msg);
+        }
         // this->new_estimated_pose_ = false;
         this->estimated_pose_mutex_exit();
         this->features_mutex_exit();