Added visualization array markers

CMakeLists.txt

@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
 # ******************************************************************** 
 #                 Add catkin additional components here
 # ******************************************************************** 
-find_package(catkin REQUIRED COMPONENTS iri_base_algorithm iri_adc_msgs ar_track_alvar_msgs)
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm visualization_msgs iri_adc_msgs ar_track_alvar_msgs)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -61,7 +61,7 @@ catkin_package(
 # ******************************************************************** 
 #            Add ROS and IRI ROS run time dependencies
 # ******************************************************************** 
- CATKIN_DEPENDS iri_base_algorithm iri_adc_msgs ar_track_alvar_msgs
+ CATKIN_DEPENDS iri_base_algorithm visualization_msgs iri_adc_msgs ar_track_alvar_msgs
 # ******************************************************************** 
 #      Add system and labrobotica run time dependencies here
 # ******************************************************************** 
@@ -96,6 +96,7 @@ target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
 #               Add message headers dependencies 
 # ******************************************************************** 
 # add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} visualization_msgs_generate_messages_cpp)
 add_dependencies(${PROJECT_NAME} iri_adc_msgs_generate_messages_cpp)
 add_dependencies(${PROJECT_NAME} ar_track_alvar_msgs_generate_messages_cpp)
 # ******************************************************************** 

README.md

@@ -4,6 +4,7 @@ The iri_adc_tag_localization_filter project description
 
 # ROS Interface
 ### Topic publishers
+  - ~**tag_features** (visualization_msgs/MarkerArray.msg)
   - ~**landmarks** (iri_adc_msgs/landmark_array.msg)
 ### Topic subscribers
   - ~**ar_tag_detections** (ar_track_alvar_msgs/AlvarMarkers.msg)

include/adc_tag_localization_filter_alg_node.h

@@ -32,6 +32,7 @@
 #include <iostream>
 
 // [publisher subscriber headers]
+#include <visualization_msgs/MarkerArray.h>
 #include <iri_adc_msgs/landmark_array.h>
 #include <iri_adc_msgs/landmark.h>
 #include <ar_track_alvar_msgs/AlvarMarkers.h>
@@ -69,6 +70,9 @@ class AdcTagLocalizationFilterAlgNode : public algorithm_base::IriBaseAlgorithm<
     ros::Time stamp; ///< Last msg time stamp.
 
     // [publisher attributes]
+    ros::Publisher tag_features_publisher_;
+    visualization_msgs::MarkerArray tag_features_MarkerArray_msg_;
+
     ros::Publisher landmarks_publisher_;
     iri_adc_msgs::landmark_array landmarks_msg_;
 

package.xml

@@ -52,6 +52,7 @@
   <build_depend>iri_base_algorithm</build_depend>
   <build_export_depend>iri_base_algorithm</build_export_depend>
   <exec_depend>iri_base_algorithm</exec_depend>
+  <depend>visualization_msgs</depend>
   <depend>iri_adc_msgs</depend>
   <depend>ar_track_alvar_msgs</depend>
   <depend>ar_track_alvar</depend>

src/adc_tag_localization_filter_alg_node.cpp

@@ -12,6 +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->landmarks_publisher_ = this->private_node_handle_.advertise<iri_adc_msgs::landmark_array>("landmarks", 1);
   
   // [init subscribers]
@@ -72,6 +73,9 @@ void AdcTagLocalizationFilterAlgNode::mainNodeThread(void)
     this->new_data = false;
   }
   // [fill msg structures]
+  // Initialize the topic message structure
+  //this->tag_features_MarkerArray_msg_.data = my_var;
+
   // Initialize the topic message structure
   //this->landmarks_landmark_array_msg_.data = my_var;
 
@@ -81,6 +85,9 @@ void AdcTagLocalizationFilterAlgNode::mainNodeThread(void)
   // [fill action structure and make request to the action server]
 
   // [publish messages]
+  // Uncomment the following line to publish the topic message
+  //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_);