Commit 33e8a6a5 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Solved some bugs. Imrpoved launch files

parent 6b9c5200
rate: 10
range_filter_en: true
range_filter_en: True
detection_max_range: 3.0
confidence_filter_en: false
confidence_filter_en: False
detection_min_confidence: 126
id_filter_en: true
ids_enabled: 0
id_filter_en: True
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
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<arg name="node_name" default="iri_adc_tag_localization_filter"/>
<arg name="output" default="screen"/>
<arg name="launch_prefix" default=""/>
<arg name="config_file" default="$(find iri_adc_tag_localization_filter)/config/params.yaml"/>
<arg name="node_name" default="iri_adc_tag_localization_filter"/>
<arg name="output" default="screen"/>
<arg name="launch_prefix" default=""/>
<arg name="config_file" default="$(find iri_adc_tag_localization_filter)/config/params.yaml"/>
<arg name="robot_name" default="model_car"/>
<arg name="ar_input_topic" default="/$(arg robot_name)/ar_pose_marker"/>
<!-- <arg name="topic_name" default="new_topic_name"/> -->
<node name="$(arg node_name)"
......@@ -13,7 +15,8 @@
output="$(arg output)"
launch-prefix="$(arg launch_prefix)">
<rosparam file="$(arg config_file)" command="load"/>
<remap from="~/ar_tag_detections" to="$(arg ar_input_topic)"/>
<!--<remap from="~/topic" to="$(arg topic_name)"/>-->
</node>
</launch>
\ No newline at end of file
</launch>
......@@ -9,6 +9,7 @@
<arg name="node_name" value="iri_adc_tag_localization_filter"/>
<arg name="output" value="$(arg output)"/>
<arg name="launch_prefix" value="$(arg launch_prefix)"/>
<arg name="config_file" value="$(find iri_adc_tag_localization_filter)/config/params.yaml"/>
</include>
<node name="rqt_reconfigure_iri_adc_tag_localization_filter"
......@@ -18,4 +19,4 @@
args="iri_adc_tag_localization_filter">
</node>
</launch>
\ No newline at end of file
</launch>
......@@ -54,6 +54,7 @@
<exec_depend>iri_base_algorithm</exec_depend>
<depend>iri_adc_msgs</depend>
<depend>ar_track_alvar_msgs</depend>
<depend>ar_track_alvar</depend>
<!-- The export tag contains other, unspecified, tags -->
......
......@@ -69,6 +69,7 @@ void AdcTagLocalizationFilterAlgNode::mainNodeThread(void)
{
update_landmarks_msg();
this->landmarks_publisher_.publish(this->landmarks_msg_);
this->new_data = false;
}
// [fill msg structures]
// Initialize the topic message structure
......@@ -150,7 +151,7 @@ void AdcTagLocalizationFilterAlgNode::update_landmarks_msg()
std::vector<iri_adc_msgs::landmark>().swap(this->landmarks_msg_.landmarks);
for (auto it = this->candidates.begin(); it != this->candidates.end(); ++it)
{
if (it->count >= this->config_.time_persistance_min_detections)
if (!this->config_.time_persistance_filter_en || (it->count >= this->config_.time_persistance_min_detections))
{
iri_adc_msgs::landmark landmark;
landmark.pose = it->pose;
......@@ -164,13 +165,22 @@ void AdcTagLocalizationFilterAlgNode::update_landmarks_msg()
/* [subscriber callbacks] */
void AdcTagLocalizationFilterAlgNode::ar_tag_detections_callback(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr& msg)
{
ROS_INFO("AdcTagLocalizationFilterAlgNode::ar_tag_detections_callback: New Message Received");
// ROS_INFO("AdcTagLocalizationFilterAlgNode::ar_tag_detections_callback: New Message Received");
//use appropiate mutex to shared variables if necessary
//this->alg_.lock();
this->ar_tag_detections_mutex_enter();
this->landmarks_msg_.header.frame_id = msg->header.frame_id;
this->landmarks_msg_.header.stamp = msg->header.stamp;
if (msg->markers.size() == 0)
{
this->landmarks_msg_.header.frame_id = msg->header.frame_id;
this->landmarks_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;
}
prepare_feature_candidates();
for (unsigned int i = 0; i < msg->markers.size(); i++)
{
......@@ -180,7 +190,8 @@ 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))
{
double theta = std::atan2(msg->markers[i].pose.pose.position.y, msg->markers[i].pose.pose.position.x);
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;
while (theta < -M_PI)
......@@ -199,7 +210,12 @@ void AdcTagLocalizationFilterAlgNode::ar_tag_detections_callback(const ar_track_
this->candidates.push_back(cand);
}
}
else
{
ROS_INFO("not a tag");
}
}
this->new_data = true;
//std::cout << msg->data << std::endl;
//unlock previously blocked shared variables
......
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