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

Fixed bug with two cameras always locking front_features

parent ca816cd7
......@@ -168,7 +168,7 @@ double AdcLandmarksSlamSolverAlgorithm::is_a_mapped_landmark(Eigen::Vector3d _fe
geometry_msgs::PoseStamped local_land, global_land;
global_land.header.stamp = _tf_sensor_map.header.stamp;
global_land.header.frame_id = this->config_.global_frame;
global_land.header.frame_id = _tf_sensor_map.child_frame_id;//global_frame
global_land.pose.position.z = 0.0;
global_land.pose.position.x = it->second.pose(0);
global_land.pose.position.y = it->second.pose(1);
......
......@@ -204,6 +204,7 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
{
//lock access to algorithm if necessary
// this->alg_.lock();
this->rear_features_mutex_enter();
this->front_features_mutex_enter();
this->estimated_pose_mutex_enter();
// ROS_DEBUG("AdcLandmarksSlamSolverAlgNode::mainNodeThread");
......@@ -274,6 +275,7 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
// this->new_estimated_pose_ = false;
this->estimated_pose_mutex_exit();
this->front_features_mutex_exit();
this->rear_features_mutex_exit();
return;
}
tf_odom_base_msg = this->tf2_buffer.lookupTransform(this->config_.odom_frame, this->config_.base_link_frame, this->new_estimated_pose_t_);
......@@ -351,6 +353,7 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
// this->new_front_features_data_ = false;
this->estimated_pose_mutex_exit();
this->front_features_mutex_exit();
this->rear_features_mutex_exit();
return;
}
//Get global robot_state
......@@ -363,6 +366,7 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
// this->new_front_features_data_ = false;
this->estimated_pose_mutex_exit();
this->front_features_mutex_exit();
this->rear_features_mutex_exit();
return;
}
}
......@@ -380,8 +384,10 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
if (!front_land_success || !rear_land_success)
{
// this->new_front_features_data_ = false;
this->new_features_data_ = false;
this->estimated_pose_mutex_exit();
this->front_features_mutex_exit();
this->rear_features_mutex_exit();
return;
}
}
......@@ -457,6 +463,7 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
}
this->estimated_pose_mutex_exit();
this->front_features_mutex_exit();
this->rear_features_mutex_exit();
return;
}
tf_robot = this->tf2_buffer.lookupTransform(this->config_.global_frame, this->config_.base_link_frame, t);
......@@ -539,6 +546,7 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
// this->alg_.unlock();
this->estimated_pose_mutex_exit();
this->front_features_mutex_exit();
this->rear_features_mutex_exit();
}
bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool _update_landmarks, bool _front)
......@@ -550,7 +558,7 @@ bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool _update_landmarks, bool
if ((_front && this->front_features_frame_ == "") || (!_front && this->rear_features_frame_ == ""))
{
ROS_DEBUG_STREAM("AdcLandmarksSlamSolverAlgNode::check_landmarks -> " << (_front? "front ": "rear ") << "features frame id empty; AR tag filter not initialized.");
ROS_WARN_STREAM("AdcLandmarksSlamSolverAlgNode::check_landmarks -> " << (_front? "front ": "rear ") << "features frame id empty; AR tag filter not initialized.");
return false;
}
......@@ -705,7 +713,7 @@ bool AdcLandmarksSlamSolverAlgNode::is_a_new_landmark(FeatureInfo &_feature, boo
geometry_msgs::PoseStamped local_land, global_land;
global_land.header.stamp = _tf_sensor_map.header.stamp;
global_land.header.frame_id = this->config_.global_frame;
global_land.header.frame_id = _tf_sensor_map.child_frame_id;//global
global_land.pose.position.z = 0.0;
global_land.pose.position.x = it->pose(0);
global_land.pose.position.y = it->pose(1);
......@@ -756,9 +764,17 @@ bool AdcLandmarksSlamSolverAlgNode::is_a_new_landmark(FeatureInfo &_feature, boo
bool AdcLandmarksSlamSolverAlgNode::update_landmarks_frame_data(FrameData& _frame_data, bool _front)
{
if ((_front && this->front_features_frame_ == "") || (!_front && this->rear_features_frame_ == ""))
{
ROS_WARN_STREAM("AdcLandmarksSlamSolverAlgNode::update_landmarks_frame_data -> " << (_front? "front ": "rear ") << "features frame id empty; AR tag filter not initialized.");
return false;
}
//calculate tf from base_link to sensor
geometry_msgs::TransformStamped tf;
geometry_msgs::PoseStamped from_sensor, from_base_link;
tf.header.frame_id = this->config_.base_link_frame;
tf.child_frame_id = (_front? this->front_features_frame_: this->rear_features_frame_);
tf.header.stamp = this->features_stamp_;
if (!this->tf2_buffer.canTransform(this->config_.base_link_frame, (_front? this->front_features_frame_: this->rear_features_frame_), this->features_stamp_, ros::Duration(this->config_.tf_timeout)))
{
ROS_WARN_STREAM("AdcLandmarksSlamSolverAlgNode:update_landmarks_frame_data -> Can't transform from " << this->config_.base_link_frame << " to " << (_front? this->front_features_frame_: this->rear_features_frame_) << " at " << this->features_stamp_);
......@@ -769,7 +785,7 @@ bool AdcLandmarksSlamSolverAlgNode::update_landmarks_frame_data(FrameData& _fram
from_sensor.header.frame_id = (_front? this->front_features_frame_: this->rear_features_frame_);
for (unsigned int i = 0; i < (_front? this->front_features_: this->rear_features_).size(); i++)
{
if(this->front_features_[i].landmark_key > 0.0)
if((_front? this->front_features_: this->rear_features_)[i].landmark_key > 0.0)
{
LandmarkResidualData land_res_data;
// Transform sensor info to base_link frame
......@@ -874,6 +890,7 @@ void AdcLandmarksSlamSolverAlgNode::load_landmarks(void)
void AdcLandmarksSlamSolverAlgNode::merge_features()
{
std::vector<FeatureInfo>().swap(this->features_);
this->features_.clear();
if (this->new_rear_features_data_ && (this->features_stamp_ - this->rear_features_stamp_) < ros::Duration(this->config_.old_feature_timeout))
{
this->features_.resize(this->rear_features_.size());
......@@ -886,6 +903,7 @@ void AdcLandmarksSlamSolverAlgNode::merge_features()
ROS_WARN_STREAM("AdcLandmarksSlamSolverAlgNode::merge_features -> rear features too old: last stamp " << this->rear_features_stamp_);
this->new_rear_features_data_ = false;
}
if (this->new_front_features_data_ && (this->features_stamp_ - this->front_features_stamp_) < ros::Duration(this->config_.old_feature_timeout))
{
unsigned int old_size = this->features_.size();
......@@ -1145,19 +1163,30 @@ void AdcLandmarksSlamSolverAlgNode::front_features_callback(const iri_adc_msgs::
this->features_stamp_ = msg->header.stamp;
this->new_front_features_data_ = true;
this->new_features_data_ = true;
this->front_features_.resize(msg->features.size());
for (unsigned int i = 0; i < msg->features.size(); i++)
if (msg->header.frame_id != "")
{
FeatureInfo feature;
feature.msg_pose = msg->features[i].pose;
feature.landmark_key = -1.0;
feature.type = msg->features[i].type;
Eigen::Vector3d dist;
dist << msg->features[i].pose.position.x, msg->features[i].pose.position.y, msg->features[i].pose.position.z;
feature.r = dist.norm();
feature.th = std::atan2(msg->features[i].pose.position.x, msg->features[i].pose.position.z);
feature.frame = msg->header.frame_id;
this->front_features_[i] = feature;
this->front_features_.resize(msg->features.size());
for (unsigned int i = 0; i < msg->features.size(); i++)
{
FeatureInfo feature;
feature.msg_pose = msg->features[i].pose;
feature.landmark_key = -1.0;
feature.type = msg->features[i].type;
Eigen::Vector3d dist;
dist << msg->features[i].pose.position.x, msg->features[i].pose.position.y, msg->features[i].pose.position.z;
feature.r = dist.norm();
feature.th = std::atan2(msg->features[i].pose.position.x, msg->features[i].pose.position.z);
feature.frame = msg->header.frame_id;
this->front_features_[i] = feature;
}
// ROS_INFO_STREAM("Front features size " << (int)this->front_features_.size());
}
else
{
this->new_front_features_data_ = false;
std::vector<FeatureInfo>().swap(this->front_features_);
this->front_features_.clear();
// ROS_INFO_STREAM("Front features size " << (int)this->front_features_.size());
}
//std::cout << msg->data << std::endl;
......@@ -1183,31 +1212,42 @@ void AdcLandmarksSlamSolverAlgNode::rear_features_callback(const iri_adc_msgs::f
//use appropiate mutex to shared variables if necessary
//this->alg_.lock();
this->front_features_mutex_enter();
this->rear_features_mutex_enter();
this->rear_features_frame_ = msg->header.frame_id;
this->rear_features_stamp_ = msg->header.stamp;
this->features_stamp_ = msg->header.stamp;
this->new_rear_features_data_ = true;
this->new_features_data_ = true;
this->rear_features_.resize(msg->features.size());
for (unsigned int i = 0; i < msg->features.size(); i++)
if (msg->header.frame_id != "")
{
FeatureInfo feature;
feature.msg_pose = msg->features[i].pose;
feature.landmark_key = -1.0;
feature.type = msg->features[i].type;
Eigen::Vector3d dist;
dist << msg->features[i].pose.position.x, msg->features[i].pose.position.y, msg->features[i].pose.position.z;
feature.r = dist.norm();
feature.th = std::atan2(msg->features[i].pose.position.x, msg->features[i].pose.position.z);
feature.frame = msg->header.frame_id;
this->rear_features_[i] = feature;
this->rear_features_.resize(msg->features.size());
for (unsigned int i = 0; i < msg->features.size(); i++)
{
FeatureInfo feature;
feature.msg_pose = msg->features[i].pose;
feature.landmark_key = -1.0;
feature.type = msg->features[i].type;
Eigen::Vector3d dist;
dist << msg->features[i].pose.position.x, msg->features[i].pose.position.y, msg->features[i].pose.position.z;
feature.r = dist.norm();
feature.th = std::atan2(msg->features[i].pose.position.x, msg->features[i].pose.position.z);
feature.frame = msg->header.frame_id;
this->rear_features_[i] = feature;
// ROS_INFO_STREAM("Rear features size " << (int)this->rear_features_.size());
}
}
else
{
this->new_rear_features_data_ = false;
std::vector<FeatureInfo>().swap(this->rear_features_);
this->rear_features_.clear();
// ROS_INFO_STREAM("Rear features size " << (int)this->rear_features_.size());
}
//std::cout << msg->data << std::endl;
//unlock previously blocked shared variables
//this->alg_.unlock();
this->front_features_mutex_exit();
this->rear_features_mutex_exit();
}
void AdcLandmarksSlamSolverAlgNode::rear_features_mutex_enter(void)
......@@ -1229,6 +1269,7 @@ void AdcLandmarksSlamSolverAlgNode::rear_features_mutex_exit(void)
void AdcLandmarksSlamSolverAlgNode::node_config_update(Config &config, uint32_t level)
{
this->alg_.lock();
this->rear_features_mutex_enter();
this->front_features_mutex_enter();
if (config.load_landmarks && !this->config_.load_landmarks)
load_landmarks();
......@@ -1250,6 +1291,7 @@ void AdcLandmarksSlamSolverAlgNode::node_config_update(Config &config, uint32_t
}
this->config_=config;
this->front_features_mutex_exit();
this->rear_features_mutex_exit();
this->alg_.unlock();
}
......
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