From 9ba09a0760b629f70a2c87ef31d7914f8ae63cc3 Mon Sep 17 00:00:00 2001
From: Alejandro Lopez Gestoso <alopez@iri.upc.edu>
Date: Tue, 18 May 2021 15:30:10 +0200
Subject: [PATCH] Added wait_feature_detected_timeout
---
cfg/AdcLandmarksSlamSolver.cfg | 1 +
config/amcl_mapping.yaml | 1 +
config/landmarks_calibration.yaml | 1 +
config/landmarks_calibration_amcl.yaml | 1 +
config/localization.yaml | 1 +
config/slam_mapping.yaml | 1 +
include/adc_landmarks_slam_solver_alg_node.h | 2 +
src/adc_landmarks_slam_solver_alg_node.cpp | 101 ++++++++++---------
8 files changed, 63 insertions(+), 46 deletions(-)
diff --git a/cfg/AdcLandmarksSlamSolver.cfg b/cfg/AdcLandmarksSlamSolver.cfg
index 46fcadf..0efb2fd 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 9674cbe..22d7182 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 8f4f0d7..7ef2a40 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 4f1900a..eda9c34 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 140d930..e3b571e 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 d3b01f1..99d51c4 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 83403d1..da87e70 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 bddb834..de16a2f 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;
}
--
GitLab