Commit a3492b18 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Added flag to prevent when seeing a lot of features that the problem always updates

parent 9b4fe2c0
......@@ -111,6 +111,8 @@ class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<Ad
Eigen::Vector3d estimated_sigma_; ///< Estimated main sigmas.
bool reset_estimated_pose_; ///< Boolena to set new_estimated_pose to false.
bool last_frame_min_detections_; ///< Flag to know that last frame was because a lot of features seen.
// [publisher attributes]
ros::Publisher frame_data_publisher_;
visualization_msgs::MarkerArray frame_data_MarkerArray_msg_;
......
......@@ -150,6 +150,8 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
this->features_stamp_ = ros::Time::now();
this->tf_map_odom_ << 0.0, 0.0, 0.0;
this->last_frame_min_detections_ = false;
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++)
......@@ -307,8 +309,9 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
inc_ang = std::fabs(this->tf_odom_base_(2) - this->last_update_odom_base_(2));
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_ || this->features_.size() >= this->config_.update_problem_features_detected)
(inc_ang > this->config_.update_problem_angle) || this->new_estimated_pose_ || (!this->last_frame_min_detections_ && (this->features_.size() >= this->config_.update_problem_features_detected)))
{
this->last_frame_min_detections_ = !this->last_frame_min_detections_;
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)))
......@@ -475,7 +478,7 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
ROS_ERROR("AdcLandmarksSlamSolverAlgNode::mainNodeThread -> tf_map_odom not initialized");
this->last_update_t_ = t;
}
}
}
// [fill msg structures]
// Initialize the topic message structure
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment