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

Adapted to name change from landmarks to features. Updated documentation.

parent 65b581e8
......@@ -33,8 +33,8 @@
// [publisher subscriber headers]
#include <visualization_msgs/MarkerArray.h>
#include <iri_adc_msgs/landmark_array.h>
#include <iri_adc_msgs/landmark.h>
#include <iri_adc_msgs/feature_array.h>
#include <iri_adc_msgs/feature.h>
#include <ar_track_alvar_msgs/AlvarMarkers.h>
// [service client headers]
......@@ -51,10 +51,10 @@ typedef struct {
geometry_msgs::Pose pose; ///< Ar tag pose.
double r; ///< Range at detection.
double th; ///< Angle at detection.
bool detected;
int count;
int id;
int confidence;
bool detected; ///< If it has been detected.
int count; ///< Number of time detected in a row
int id; ///< Tag id
int confidence; ///< Tag confidence
}Feature_candidate;
/**
......@@ -73,8 +73,8 @@ class AdcTagLocalizationFilterAlgNode : public algorithm_base::IriBaseAlgorithm<
ros::Publisher tag_features_publisher_;
visualization_msgs::MarkerArray visualization_msg_;
ros::Publisher landmarks_publisher_;
iri_adc_msgs::landmark_array landmarks_msg_;
ros::Publisher features_publisher_;
iri_adc_msgs::feature_array features_msg_;
// [subscriber attributes]
......@@ -102,27 +102,49 @@ class AdcTagLocalizationFilterAlgNode : public algorithm_base::IriBaseAlgorithm<
Config config_;
/**
* \brief config variable
* \brief Function to apply range, confidence and id filters.
*
* This variable has all the driver parameters defined in the cfg config file.
* Is updated everytime function config_update() is called.
* \param range The detection distance.
*
* \param confidence The detection confidence.
*
* \param id The Tag id.
*
* \return If is a Tag.
*/
bool is_a_tag(double range, int confidence, int id);
/**
* \brief config variable
* \brief Function to filter tag by ids if enabled
*
* This variable has all the driver parameters defined in the cfg config file.
* Is updated everytime function config_update() is called.
* \param id The Tag id.
*
* \return If is id enabled.
*/
bool check_tag_id(int id);
/**
* \brief Function to remove feature candidates not detected and reinit its variables.
*/
void prepare_feature_candidates();
/**
* \brief Function to update features candidates applying time persistance filter if enabled.
*
* \param r The detection distance.
*
* \param theta The detection theta.
*/
bool update_feature_candidates(double r, double theta);
void update_landmarks_msg();
/**
* \brief Function to update features msg.
*/
void update_features_msg();
/**
* \brief Function to publish visualization markers.
*/
void publish_visualization_msg();
public:
/**
......
......@@ -13,7 +13,7 @@ AdcTagLocalizationFilterAlgNode::AdcTagLocalizationFilterAlgNode(void) :
// [init publishers]
this->tag_features_publisher_ = this->private_node_handle_.advertise<visualization_msgs::MarkerArray>("tag_features", 1);
this->landmarks_publisher_ = this->private_node_handle_.advertise<iri_adc_msgs::landmark_array>("landmarks", 1);
this->features_publisher_ = this->private_node_handle_.advertise<iri_adc_msgs::feature_array>("features", 1);
// [init subscribers]
this->ar_tag_detections_subscriber_ = this->private_node_handle_.subscribe("ar_tag_detections", 1, &AdcTagLocalizationFilterAlgNode::ar_tag_detections_callback, this);
......@@ -72,8 +72,8 @@ void AdcTagLocalizationFilterAlgNode::mainNodeThread(void)
if (this->new_data)
{
update_landmarks_msg();
this->landmarks_publisher_.publish(this->landmarks_msg_);
update_features_msg();
this->features_publisher_.publish(this->features_msg_);
if (this->config_.publish_visualization)
publish_visualization_msg();
this->new_data = false;
......@@ -83,7 +83,7 @@ void AdcTagLocalizationFilterAlgNode::mainNodeThread(void)
//this->tag_features_MarkerArray_msg_.data = my_var;
// Initialize the topic message structure
//this->landmarks_landmark_array_msg_.data = my_var;
//this->features_feature_array_msg_.data = my_var;
// [fill srv structure and make request to the server]
......@@ -95,7 +95,7 @@ void AdcTagLocalizationFilterAlgNode::mainNodeThread(void)
//this->tag_features_publisher_.publish(this->tag_features_MarkerArray_msg_);
// Uncomment the following line to publish the topic message
//this->landmarks_publisher_.publish(this->landmarks_landmark_array_msg_);
//this->features_publisher_.publish(this->features_feature_array_msg_);
this->ar_tag_detections_mutex_exit();
// this->alg_.unlock();
......@@ -159,45 +159,45 @@ bool AdcTagLocalizationFilterAlgNode::update_feature_candidates(double r, double
return true;
}
void AdcTagLocalizationFilterAlgNode::update_landmarks_msg()
void AdcTagLocalizationFilterAlgNode::update_features_msg()
{
std::vector<iri_adc_msgs::landmark>().swap(this->landmarks_msg_.landmarks);
std::vector<iri_adc_msgs::feature>().swap(this->features_msg_.features);
for (auto it = this->candidates.begin(); it != this->candidates.end(); ++it)
{
if (!this->config_.time_persistance_filter_en || (it->count >= this->config_.time_persistance_min_detections))
{
iri_adc_msgs::landmark landmark;
landmark.pose = it->pose;
landmark.probability = it->confidence;
landmark.type = it->id;
this->landmarks_msg_.landmarks.push_back(landmark);
iri_adc_msgs::feature feature;
feature.pose = it->pose;
feature.probability = it->confidence;
feature.type = it->id;
this->features_msg_.features.push_back(feature);
}
}
}
void AdcTagLocalizationFilterAlgNode::publish_visualization_msg()
{
this->visualization_msg_.markers.resize(this->landmarks_msg_.landmarks.size());
for (unsigned int i = 0; i < this->landmarks_msg_.landmarks.size(); i++)
this->visualization_msg_.markers.resize(this->features_msg_.features.size());
for (unsigned int i = 0; i < this->features_msg_.features.size(); i++)
{
this->visualization_msg_.markers[i].header.frame_id = this->landmarks_msg_.header.frame_id;
this->visualization_msg_.markers[i].header.stamp = this->landmarks_msg_.header.stamp;
this->visualization_msg_.markers[i].header.frame_id = this->features_msg_.header.frame_id;
this->visualization_msg_.markers[i].header.stamp = this->features_msg_.header.stamp;
this->visualization_msg_.markers[i].id = i;
this->visualization_msg_.markers[i].ns = this->landmarks_msg_.header.frame_id;
this->visualization_msg_.markers[i].ns = this->features_msg_.header.frame_id;
this->visualization_msg_.markers[i].ns.append("_features");
// 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].pose.position.x = this->landmarks_msg_.landmarks[i].pose.position.x;
this->visualization_msg_.markers[i].pose.position.y = this->landmarks_msg_.landmarks[i].pose.position.y;
this->visualization_msg_.markers[i].pose.position.z = this->landmarks_msg_.landmarks[i].pose.position.z;
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;
this->visualization_msg_.markers[i].pose.position.z = this->features_msg_.features[i].pose.position.z;
this->visualization_msg_.markers[i].pose.orientation.x = this->landmarks_msg_.landmarks[i].pose.orientation.x;
this->visualization_msg_.markers[i].pose.orientation.y = this->landmarks_msg_.landmarks[i].pose.orientation.y;
this->visualization_msg_.markers[i].pose.orientation.z = this->landmarks_msg_.landmarks[i].pose.orientation.z;
this->visualization_msg_.markers[i].pose.orientation.w = this->landmarks_msg_.landmarks[i].pose.orientation.w;
this->visualization_msg_.markers[i].pose.orientation.x = this->features_msg_.features[i].pose.orientation.x;
this->visualization_msg_.markers[i].pose.orientation.y = this->features_msg_.features[i].pose.orientation.y;
this->visualization_msg_.markers[i].pose.orientation.z = this->features_msg_.features[i].pose.orientation.z;
this->visualization_msg_.markers[i].pose.orientation.w = this->features_msg_.features[i].pose.orientation.w;
this->visualization_msg_.markers[i].scale.x = 0.05;
this->visualization_msg_.markers[i].scale.y = 0.05;
......@@ -222,13 +222,13 @@ void AdcTagLocalizationFilterAlgNode::ar_tag_detections_callback(const ar_track_
this->ar_tag_detections_mutex_enter();
if (msg->markers.size() == 0)
{
this->landmarks_msg_.header.frame_id = msg->header.frame_id;
this->landmarks_msg_.header.stamp = msg->header.stamp;
this->features_msg_.header.frame_id = msg->header.frame_id;
this->features_msg_.header.stamp = msg->header.stamp;
}
else
{
this->landmarks_msg_.header.frame_id = msg->markers[0].header.frame_id;
this->landmarks_msg_.header.stamp = msg->markers[0].header.stamp;
this->features_msg_.header.frame_id = msg->markers[0].header.frame_id;
this->features_msg_.header.stamp = msg->markers[0].header.stamp;
}
prepare_feature_candidates();
......
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