Commit 1366e3e7 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Updated parameters. Fixed bug on no updating feature candidate pose

parent c0c77ace
......@@ -13,4 +13,4 @@ ids_enabled: "0, 1"
time_persistance_filter_en: True
time_persistance_alpha_window: 0.035
time_persistance_range_window: 0.1
time_persistance_min_detections: 5
time_persistance_min_detections: 4
......@@ -68,6 +68,7 @@ class AdcTagLocalizationFilterAlgNode : public algorithm_base::IriBaseAlgorithm<
std::vector<int> ids_enabled; ///< Vector with the Tag ids enabled.
std::list<Feature_candidate> candidates; ///< List with the tag candidates to be features.
ros::Time stamp; ///< Last msg time stamp.
std::string source_frame_id; ///< Source frame id to publish with this frame id when no marker is detected.
// [publisher attributes]
ros::Publisher tag_features_publisher_;
......@@ -134,8 +135,10 @@ class AdcTagLocalizationFilterAlgNode : public algorithm_base::IriBaseAlgorithm<
* \param r The detection distance.
*
* \param theta The detection theta.
*
* \param pose The detection pose.
*/
bool update_feature_candidates(double r, double theta);
bool update_feature_candidates(double r, double theta, const geometry_msgs::Pose &pose);
/**
* \brief Function to update features msg.
......
......@@ -12,7 +12,7 @@ AdcTagLocalizationFilterAlgNode::AdcTagLocalizationFilterAlgNode(void) :
this->setRate(this->config_.rate);
// [init publishers]
this->tag_features_publisher_ = this->private_node_handle_.advertise<visualization_msgs::MarkerArray>("tag_features", 1);
this->tag_features_publisher_ = this->private_node_handle_.advertise<visualization_msgs::MarkerArray>("vis_features", 1);
this->features_publisher_ = this->private_node_handle_.advertise<iri_adc_msgs::feature_array>("features", 1);
// [init subscribers]
......@@ -42,7 +42,9 @@ AdcTagLocalizationFilterAlgNode::AdcTagLocalizationFilterAlgNode(void) :
for (unsigned int i = 0; i < this->ids_enabled.size(); i++)
std::cout << this->ids_enabled[i] << ", ";
std::cout << std::endl;
std::list<Feature_candidate>().swap(this->candidates);
this->new_data = false;
this->source_frame_id = "";
// [init services]
......@@ -61,6 +63,7 @@ AdcTagLocalizationFilterAlgNode::~AdcTagLocalizationFilterAlgNode(void)
std::vector<int>().swap(this->ids_enabled);
std::list<Feature_candidate>().swap(this->candidates);
this->new_data = false;
this->source_frame_id = "";
}
void AdcTagLocalizationFilterAlgNode::mainNodeThread(void)
......@@ -138,7 +141,7 @@ void AdcTagLocalizationFilterAlgNode::prepare_feature_candidates()
}
}
bool AdcTagLocalizationFilterAlgNode::update_feature_candidates(double r, double theta)
bool AdcTagLocalizationFilterAlgNode::update_feature_candidates(double r, double theta, const geometry_msgs::Pose &pose)
{
//Check if is a new candidate or an already seen candidate
if (!this->config_.time_persistance_filter_en)
......@@ -153,6 +156,7 @@ bool AdcTagLocalizationFilterAlgNode::update_feature_candidates(double r, double
//update feature candidate position
it->r = r;
it->th = theta;
it->pose = pose;
return false;
}
}
......@@ -188,7 +192,7 @@ void AdcTagLocalizationFilterAlgNode::publish_visualization_msg()
// this->visualization_msg_.markers[i].type = visualization_msgs::Marker::TEXT_VIEW_FACING;
this->visualization_msg_.markers[i].type = visualization_msgs::Marker::CYLINDER;
this->visualization_msg_.markers[i].action = visualization_msgs::Marker::ADD;
this->visualization_msg_.markers[i].lifetime = ros::Duration(0.1);
this->visualization_msg_.markers[i].lifetime = ros::Duration(0.2);
this->visualization_msg_.markers[i].pose.position.x = this->features_msg_.features[i].pose.position.x;
this->visualization_msg_.markers[i].pose.position.y = this->features_msg_.features[i].pose.position.y;
......@@ -222,13 +226,14 @@ void AdcTagLocalizationFilterAlgNode::ar_tag_detections_callback(const ar_track_
this->ar_tag_detections_mutex_enter();
if (msg->markers.size() == 0)
{
this->features_msg_.header.frame_id = msg->header.frame_id;
this->features_msg_.header.frame_id = this->source_frame_id;
this->features_msg_.header.stamp = msg->header.stamp;
}
else
{
this->features_msg_.header.frame_id = msg->markers[0].header.frame_id;
this->features_msg_.header.stamp = msg->markers[0].header.stamp;
this->source_frame_id = msg->markers[0].header.frame_id;
}
prepare_feature_candidates();
......@@ -245,7 +250,7 @@ void AdcTagLocalizationFilterAlgNode::ar_tag_detections_callback(const ar_track_
theta -= 2*M_PI;
while (theta < -M_PI)
theta += 2*M_PI;
if (update_feature_candidates(dist, theta))
if (update_feature_candidates(dist, theta, msg->markers[i].pose.pose))
{
Feature_candidate cand;
cand.r = dist;
......
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