00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00039 #include <ros/ros.h>
00040 #include <std_msgs/Header.h>
00041 #include <tf/transform_listener.h>
00042 #include "mtt/TargetList.h"
00043
00044 #include <visualization_msgs/Marker.h>
00045 #include <visualization_msgs/MarkerArray.h>
00046
00047 mtt::TargetList targetList;
00048 mtt::Target target;
00049 visualization_msgs::Marker arrow_marker;
00050 visualization_msgs::MarkerArray arrow_markerList;
00051 visualization_msgs::Marker ids_marker;
00052 visualization_msgs::MarkerArray ids_markerList;
00053
00054 ros::Publisher targets_pub;
00055 ros::Publisher arrows_pub;
00056 ros::Publisher ids_pub;
00057
00058 uint interest_id = 90;
00059
00060 void targetsCallback(const mtt::TargetList& list){
00061 targetList.Targets.clear();
00062 for(uint i = 0; i < list.Targets.size(); i++){
00063 if (list.Targets[i].id == interest_id){
00064 target = list.Targets[i];
00065 targetList.Targets.push_back(target);
00066 }
00067 }
00068 targets_pub.publish(targetList);
00069 }
00070
00071 void arrowsCallback(const visualization_msgs::MarkerArray &msg){
00072 arrow_markerList.markers.clear();
00073 for(uint i = 0; i < msg.markers.size(); i++){
00074 if(msg.markers[i].id == interest_id){
00075 arrow_marker = msg.markers[i];
00076 arrow_markerList.markers.push_back(arrow_marker);
00077 }
00078 }
00079 arrows_pub.publish(arrow_markerList);
00080 }
00081
00082 void idsCallback(const visualization_msgs::MarkerArray &msg){
00083 ids_markerList.markers.clear();
00084 for(uint i = 0; i < msg.markers.size(); i++){
00085 if(msg.markers[i].id == interest_id){
00086 ids_marker = msg.markers[i];
00087 ids_markerList.markers.push_back(ids_marker);
00088 }
00089 }
00090 ids_pub.publish(ids_markerList);
00091 }
00092
00093
00094 int main(int argc, char** argv)
00095 {
00096 ros::init(argc, argv, "bag_filter");
00097 ros::NodeHandle n;
00098
00099 ros::Rate loop_rate(10);
00100
00101 ros::Subscriber sub_targets = n.subscribe("/targetsF", 1000, targetsCallback);
00102 ros::Subscriber sub_arrows = n.subscribe("/arrowsF", 1000, arrowsCallback);
00103 ros::Subscriber sub_ids = n.subscribe("/idsF", 1000, idsCallback);
00104
00105 targets_pub = n.advertise<mtt::TargetList>("/targets", 100);
00106 arrows_pub = n.advertise<visualization_msgs::MarkerArray>("/arrows", 100);
00107 ids_pub = n.advertise<visualization_msgs::MarkerArray>("/ids", 100);
00108
00109 while (ros::ok())
00110 {
00111 ros::spinOnce();
00112 loop_rate.sleep();
00113 }
00114
00115 return(0);
00116 }
00117
00118
00119