Fixed no init new_stimated_pose_t

src/adc_landmarks_slam_solver_alg_node.cpp

@@ -109,6 +109,7 @@ 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
   {
@@ -234,6 +235,7 @@ 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_ = false;
         this->estimated_pose_mutex_exit();
         this->features_mutex_exit();
         return;
@@ -299,6 +301,7 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
           frame_data.current_odom = this->tf_odom_base_;
           if (!get_tf_map_odom(this->features_stamp_, frame_data.tf_map_odom))
           {
+            // this->new_features_data_ = false;
             this->estimated_pose_mutex_exit();
             this->features_mutex_exit();
             return;
@@ -310,6 +313,7 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
           {
             if (!calculate_robot_state(frame_data.ts, frame_data.robot_state))
             {
+              // this->new_features_data_ = false;
               this->estimated_pose_mutex_exit();
               this->features_mutex_exit();
               return;
@@ -321,6 +325,7 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
           {
             if (!update_landmarks_frame_data(frame_data))
             {
+              // this->new_features_data_ = false;
               this->estimated_pose_mutex_exit();
               this->features_mutex_exit();
               return;
@@ -491,7 +496,7 @@ bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool _update_landmarks)
 
   if (this->features_frame_ == "")
   {
-    ROS_DEBUG("AdcLandmarksSlamSolverAlgNode::check_landmarks -> Features frame id empty; AR tag filter not initialized.");
+    ROS_WARN("AdcLandmarksSlamSolverAlgNode::check_landmarks -> Features frame id empty; AR tag filter not initialized.");
     return false;
   }