Commit 65b581e8 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Added marker visualization publication

parent 0c4a2125
......@@ -43,6 +43,7 @@ time_persistence_filter = gen.add_group("Detection time persistence filter param
# Name Type Reconf.level Description Default Min Max
gen.add("rate", double_t, 0, "Main loop rate (Hz)", 10.0, 0.1, 1000.0)
gen.add("publish_visualization", bool_t, 0, "Boolean to publish or not visualization markers", True)
range_filter.add("range_filter_en", bool_t, 0, "Boolean to filter detections by range", False)
range_filter.add("detection_max_range", double_t, 0, "Max range to take into account a detection", 3.0, 1.0, 12.0)
......
rate: 10
rate: 100
publish_visualization: True
range_filter_en: True
detection_max_range: 3.0
detection_max_range: 4.0
confidence_filter_en: False
detection_min_confidence: 126
......
......@@ -71,7 +71,7 @@ class AdcTagLocalizationFilterAlgNode : public algorithm_base::IriBaseAlgorithm<
// [publisher attributes]
ros::Publisher tag_features_publisher_;
visualization_msgs::MarkerArray tag_features_MarkerArray_msg_;
visualization_msgs::MarkerArray visualization_msg_;
ros::Publisher landmarks_publisher_;
iri_adc_msgs::landmark_array landmarks_msg_;
......@@ -122,6 +122,8 @@ class AdcTagLocalizationFilterAlgNode : public algorithm_base::IriBaseAlgorithm<
bool update_feature_candidates(double r, double theta);
void update_landmarks_msg();
void publish_visualization_msg();
public:
/**
* \brief Constructor
......
......@@ -38,6 +38,10 @@ AdcTagLocalizationFilterAlgNode::AdcTagLocalizationFilterAlgNode(void) :
this->ids_enabled.push_back(id);
}
}
std::cout << "Ids enabled: ";
for (unsigned int i = 0; i < this->ids_enabled.size(); i++)
std::cout << this->ids_enabled[i] << ", ";
std::cout << std::endl;
this->new_data = false;
// [init services]
......@@ -70,6 +74,8 @@ void AdcTagLocalizationFilterAlgNode::mainNodeThread(void)
{
update_landmarks_msg();
this->landmarks_publisher_.publish(this->landmarks_msg_);
if (this->config_.publish_visualization)
publish_visualization_msg();
this->new_data = false;
}
// [fill msg structures]
......@@ -169,6 +175,43 @@ void AdcTagLocalizationFilterAlgNode::update_landmarks_msg()
}
}
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[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].id = i;
this->visualization_msg_.markers[i].ns = this->landmarks_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.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].scale.x = 0.05;
this->visualization_msg_.markers[i].scale.y = 0.05;
this->visualization_msg_.markers[i].scale.z = 0.02;
this->visualization_msg_.markers[i].color.a = 1.0;
this->visualization_msg_.markers[i].color.r = 0.0;
this->visualization_msg_.markers[i].color.g = 1.0;
this->visualization_msg_.markers[i].color.b = 0.0;
}
if (this->visualization_msg_.markers.size() > 0)
this->tag_features_publisher_.publish(this->visualization_msg_);
}
/* [subscriber callbacks] */
void AdcTagLocalizationFilterAlgNode::ar_tag_detections_callback(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr& msg)
{
......@@ -197,7 +240,6 @@ void AdcTagLocalizationFilterAlgNode::ar_tag_detections_callback(const ar_track_
dist = position.norm();
if (is_a_tag(dist, msg->markers[i].confidence, msg->markers[i].id))
{
ROS_INFO("is a tag");
double theta = std::atan2(msg->markers[i].pose.pose.position.x, msg->markers[i].pose.pose.position.z);
while (theta >= M_PI)
theta -= 2*M_PI;
......@@ -217,10 +259,6 @@ void AdcTagLocalizationFilterAlgNode::ar_tag_detections_callback(const ar_track_
this->candidates.push_back(cand);
}
}
else
{
ROS_INFO("not a tag");
}
}
this->new_data = true;
......
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