diff --git a/cfg/AdcLandmarksSlamSolver.cfg b/cfg/AdcLandmarksSlamSolver.cfg
index 46fcadfd9050b5743d9b833161688c7a4633b624..0efb2fdfab4d178764fa420cb6e817e5a3ae11c7 100755
--- a/cfg/AdcLandmarksSlamSolver.cfg
+++ b/cfg/AdcLandmarksSlamSolver.cfg
@@ -62,6 +62,7 @@ landmarks.add("landmarks_min_detections", int_t, 0, "Th
ceres.add("update_problem_rate", double_t, 0, "Rate to update the ceres problem", 1.0, 0.00001, 10)
ceres.add("update_problem_distance", double_t, 0, "Distance from last robot state to update the ceres problem", 1.0, 0.1, 100000)
ceres.add("update_problem_angle", double_t, 0, "Angle inc from last robot state to update the ceres problem", 0.2, 0.05, 3.14)
+ceres.add("wait_feature_detected_timeout", double_t, 0, "Timeout on a update problem event to wait for a feature detection", 0.2, 0.01, 0.5)
ceres.add("landmarks_map_file", str_t, 0, "Input txt file path", "landmarks.txt")
ceres.add("problem_frame_window", int_t, 0, "Max number of frames to add", 30, -1, 120)
diff --git a/config/amcl_mapping.yaml b/config/amcl_mapping.yaml
index 9674cbef362804b1399c0b1c79c2db776388110b..22d7182e52ec28d89c278f2f920e99b831cfd3bb 100644
--- a/config/amcl_mapping.yaml
+++ b/config/amcl_mapping.yaml
@@ -21,6 +21,7 @@ publish_tf_map_odom : False
update_problem_rate: 0.1
update_problem_distance: 0.2
update_problem_angle: 0.1
+wait_feature_detected_timeout: 0.2
problem_frame_window: -1
landmarks_candidates_filter_en: True
diff --git a/config/landmarks_calibration.yaml b/config/landmarks_calibration.yaml
index 8f4f0d761e35958ad256f772c2937aa5aae261af..7ef2a4015e04471b0fc44affa62e9f96d50a45c4 100644
--- a/config/landmarks_calibration.yaml
+++ b/config/landmarks_calibration.yaml
@@ -21,6 +21,7 @@ publish_tf_map_odom : True
update_problem_rate: 0.1
update_problem_distance: 0.2
update_problem_angle: 0.1
+wait_feature_detected_timeout: 0.2
problem_frame_window: -1
landmarks_candidates_filter_en: True
diff --git a/config/landmarks_calibration_amcl.yaml b/config/landmarks_calibration_amcl.yaml
index 4f1900a1f9901e5133e88a9f3098b2a1e6dc91b4..eda9c34bc8fc75c63403c4b47acef28013d6abd4 100644
--- a/config/landmarks_calibration_amcl.yaml
+++ b/config/landmarks_calibration_amcl.yaml
@@ -21,6 +21,7 @@ publish_tf_map_odom : False
update_problem_rate: 0.1
update_problem_distance: 0.2
update_problem_angle: 0.1
+wait_feature_detected_timeout: 0.2
problem_frame_window: -1
landmarks_candidates_filter_en: True
diff --git a/config/localization.yaml b/config/localization.yaml
index 140d930f1949e43d4264edef7c80c4677692c09c..e3b571eb5f1adda0d3a632720febb023a0471e81 100644
--- a/config/localization.yaml
+++ b/config/localization.yaml
@@ -21,6 +21,7 @@ publish_tf_map_odom : False
update_problem_rate: 0.1
update_problem_distance: 0.2
update_problem_angle: 0.1
+wait_feature_detected_timeout: 0.2
problem_frame_window: 60
landmarks_candidates_filter_en: True
diff --git a/config/slam_mapping.yaml b/config/slam_mapping.yaml
index d3b01f1068f3baee1234bae46366d098ed2c621e..99d51c40a6f731b2ac45d1864200f133ba09d718 100644
--- a/config/slam_mapping.yaml
+++ b/config/slam_mapping.yaml
@@ -21,6 +21,7 @@ publish_tf_map_odom : True
update_problem_rate: 0.1
update_problem_distance: 0.2
update_problem_angle: 0.1
+wait_feature_detected_timeout: 0.2
problem_frame_window: -1
landmarks_candidates_filter_en: True
diff --git a/include/adc_landmarks_slam_solver_alg_node.h b/include/adc_landmarks_slam_solver_alg_node.h
index 83403d1e092814b3a080c43686ae6d6ed1e602bc..da87e70360b26eeffb37e73231d5dfeb3f0f67bc 100644
--- a/include/adc_landmarks_slam_solver_alg_node.h
+++ b/include/adc_landmarks_slam_solver_alg_node.h
@@ -97,6 +97,8 @@ class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<Ad
ros::Time last_update_t_; ///< Last frame update time.
Eigen::Vector3d last_update_odom_base_; ///< Last frame update odometry.
+ ros::Time update_event_t_; ///< Time stamp at last update problem event.
+ bool waiting_for_feature_; ///< Bool
ros::Time last_publish_t_; ///< Last publish pose time.
Eigen::Vector3d last_publish_odom_; ///< Last publish pose odometry.
diff --git a/src/adc_landmarks_slam_solver_alg_node.cpp b/src/adc_landmarks_slam_solver_alg_node.cpp
index bddb834bc7bd7f2609ff8d50f65de1a7881b05ab..de16a2fb54c6f3afc3a88560898e8b20c39f861d 100644
--- a/src/adc_landmarks_slam_solver_alg_node.cpp
+++ b/src/adc_landmarks_slam_solver_alg_node.cpp
@@ -119,6 +119,7 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
this->init_tf_map_odom_ = this->config_.amcl_localization;
this->last_update_t_ = ros::Time::now();
this->last_update_odom_base_ << 0.0, 0.0, 0.0;
+ this->waiting_for_feature_ = false;
this->last_publish_t_ = ros::Time::now();
this->last_publish_odom_ << 0.0, 0.0, 0.0;
@@ -219,65 +220,73 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
if (((this->features_stamp_ - this->last_update_t_) > ros::Duration(1/this->config_.update_problem_rate)) || (dist.norm() > this->config_.update_problem_distance) ||
(inc_ang > this->config_.update_problem_angle) || this->new_estimated_pose_)
{
- frame_data.ts = this->features_stamp_;
- frame_data.current_odom = this->tf_odom_base_;
- if (!get_tf_map_odom(this->features_stamp_, frame_data.tf_map_odom))
- {
- this->estimated_pose_mutex_exit();
- this->features_mutex_exit();
- return;
- }
- //Get global robot_state
- if (this->new_estimated_pose_)
- frame_data.robot_state = this->estimated_pose_;
+ if (!this->waiting_for_feature_)
+ this->update_event_t_ = this->features_stamp_;
+ if (this->features_.size() == 0 && (this->features_stamp_ - this->update_event_t_ < ros::Duration(this->config_.wait_feature_detected_timeout)))
+ this->waiting_for_feature_ = true;
else
{
- if (!calculate_robot_state(frame_data.ts, frame_data.robot_state))
+ this->waiting_for_feature_ = false;
+ frame_data.ts = this->features_stamp_;
+ frame_data.current_odom = this->tf_odom_base_;
+ if (!get_tf_map_odom(this->features_stamp_, frame_data.tf_map_odom))
{
this->estimated_pose_mutex_exit();
this->features_mutex_exit();
return;
}
- }
+ //Get global robot_state
+ if (this->new_estimated_pose_)
+ frame_data.robot_state = this->estimated_pose_;
+ else
+ {
+ if (!calculate_robot_state(frame_data.ts, frame_data.robot_state))
+ {
+ this->estimated_pose_mutex_exit();
+ this->features_mutex_exit();
+ return;
+ }
+ }
- // Update frame information and Ceres problem
- if (this->alg_.get_frames_data_size() != 0 && land_success)
- {
- if (!update_landmarks_frame_data(frame_data))
+ // Update frame information and Ceres problem
+ if (this->alg_.get_frames_data_size() != 0 && land_success)
{
- this->estimated_pose_mutex_exit();
- this->features_mutex_exit();
- return;
+ if (!update_landmarks_frame_data(frame_data))
+ {
+ this->estimated_pose_mutex_exit();
+ this->features_mutex_exit();
+ return;
+ }
}
- }
- this->alg_.add_frame_data(frame_data, this->new_estimated_pose_, this->estimated_sigma_);
- if (this->config_.publish_visualization)
- publish_frame_markers();
+ this->alg_.add_frame_data(frame_data, this->new_estimated_pose_, this->estimated_sigma_);
+ if (this->config_.publish_visualization)
+ publish_frame_markers();
- this->alg_.solve_problem(this->robot_pose_, this->covariance_m_, false);
+ this->alg_.solve_problem(this->robot_pose_, this->covariance_m_, false);
- //update fixed_frame to odom_frame tf
- if (!this->config_.amcl_localization)
- {
- tf2::Transform tf_map_odom, tf_odom_base, tf_map_base;
- tf_odom_base.setOrigin(tf2::Vector3(this->tf_odom_base_(0), this->tf_odom_base_(1), 0.0));
- tf2::Quaternion q;
- q.setRPY(0.0, 0.0, this->tf_odom_base_(2));
- tf_odom_base.setRotation(q);
- tf_map_base.setOrigin(tf2::Vector3(robot_pose_(0), robot_pose_(1), 0.0));
- q.setRPY(0.0, 0.0, robot_pose_(2));
- tf_map_base.setRotation(q);
- tf_map_odom = tf_map_base*tf_odom_base.inverse();
- tf2::Vector3 origin;
- origin = tf_map_odom.getOrigin();
- this->tf_map_odom_(0) = origin.getX();
- this->tf_map_odom_(1) = origin.getY();
- double yaw, pitch, roll;
- tf2::Matrix3x3(tf_map_odom.getRotation()).getEulerYPR(yaw, pitch, roll);
- this->tf_map_odom_(2) = yaw;
+ //update fixed_frame to odom_frame tf
+ if (!this->config_.amcl_localization)
+ {
+ tf2::Transform tf_map_odom, tf_odom_base, tf_map_base;
+ tf_odom_base.setOrigin(tf2::Vector3(this->tf_odom_base_(0), this->tf_odom_base_(1), 0.0));
+ tf2::Quaternion q;
+ q.setRPY(0.0, 0.0, this->tf_odom_base_(2));
+ tf_odom_base.setRotation(q);
+ tf_map_base.setOrigin(tf2::Vector3(robot_pose_(0), robot_pose_(1), 0.0));
+ q.setRPY(0.0, 0.0, robot_pose_(2));
+ tf_map_base.setRotation(q);
+ tf_map_odom = tf_map_base*tf_odom_base.inverse();
+ tf2::Vector3 origin;
+ origin = tf_map_odom.getOrigin();
+ this->tf_map_odom_(0) = origin.getX();
+ this->tf_map_odom_(1) = origin.getY();
+ double yaw, pitch, roll;
+ tf2::Matrix3x3(tf_map_odom.getRotation()).getEulerYPR(yaw, pitch, roll);
+ this->tf_map_odom_(2) = yaw;
+ }
+ this->last_update_t_ = this->features_stamp_;
+ this->last_update_odom_base_ = this->tf_odom_base_;
}
- this->last_update_t_ = this->features_stamp_;
- this->last_update_odom_base_ = this->tf_odom_base_;
}
this->new_features_data_ = false;
}