classify_features.cpp
Go to the documentation of this file.
1 /**************************************************************************************************
2  Software License Agreement (BSD License)
3 
4  Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
5  All rights reserved.
6 
7  Redistribution and use in source and binary forms, with or without modification, are permitted
8  provided that the following conditions are met:
9 
10  *Redistributions of source code must retain the above copyright notice, this list of
11  conditions and the following disclaimer.
12  *Redistributions in binary form must reproduce the above copyright notice, this list of
13  conditions and the following disclaimer in the documentation and/or other materials provided
14  with the distribution.
15  *Neither the name of the University of Aveiro nor the names of its contributors may be used to
16  endorse or promote products derived from this software without specific prior written permission.
17 
18  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
19  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
20  FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
21  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
22  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
24  IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
25  OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 ***************************************************************************************************/
39 #include "ros/ros.h"
40 #include "std_msgs/String.h"
41 #include "geometry_msgs/PoseWithCovariance.h"
42 #include "tf/transform_listener.h"
43 #include "mtt/TargetList.h"
44 #include <visualization_msgs/Marker.h>
45 #include <visualization_msgs/MarkerArray.h>
46 #include "opencv/cv.h"
47 #include "opencv/ml.h"
48 #include <vector>
49 #include <iostream>
50 
51 // using namespace std;
52 // using namespace cv;
53 
54 CvBoost adaboost;
55 CvMat* sample_to_classify = 0;
56 CvMat* weak_responses = 0;
57 
58 // static ros::Time start_time;
59 ros::Time time_last_msg(0);
60 ros::Duration time_elapsed;
61 ros::Duration duration_btw_msg;
62 ros::Publisher nfeatures_pub;
63 ros::Publisher marker_pub;
64 
65 tf::TransformListener *p_listener;
66 tf::StampedTransform transform;
67 
68 geometry_msgs::Twist twist;
69 geometry_msgs::Twist features;
70 geometry_msgs::PoseWithCovariance nfeatures;
71 visualization_msgs::MarkerArray ma;
72 
73 std::list<geometry_msgs::PoseWithCovariance> matlab_list;
74 
77 double robot_x, robot_y;
82 double lateral_disp;
83 double label;
84 uint badLeader = 0;
85 uint target_id = 0;
86 uint single_target = 0;
87 uint counter = 0;
88 
89 void drawPose(const geometry_msgs::Pose& input, uint target, double classification)
90 {
91 
92  visualization_msgs::Marker marker;
93 
94  marker.header.frame_id = "/map";
95  marker.header.stamp = ros::Time::now();
96 
97  marker.ns = "quality";
98  marker.id = target;
99 
100  marker.type = visualization_msgs::Marker::CYLINDER;
101  marker.action = visualization_msgs::Marker::ADD;
102 
103  // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header
104  marker.pose.position.x = input.position.x;
105  marker.pose.position.y = input.position.y;
106  marker.pose.position.z = 0;
107  marker.pose.orientation.x = 0.0;
108  marker.pose.orientation.y = 0.0;
109  marker.pose.orientation.z = 0.0;
110  marker.pose.orientation.w = 1.0;
111 
112  // Set the scale of the marker
113  marker.scale.x = 0.5;
114  marker.scale.y = 0.5;
115  marker.scale.z = 0.5;
116 
117  //good or bad leader
118  if(classification == 1){
119  marker.color.r = 0.0f;
120  marker.color.g = 1.0f;
121  }
122  else if (classification == 2){
123  marker.color.r = 1.0f;
124  marker.color.g = 0.0f;
125  }
126 
127  marker.color.b = 0.0f;
128  marker.color.a = 1.0;
129 
130  marker.lifetime = ros::Duration(1.0);
131 
132 // ROS_INFO("created marker, pushing back");
133  ma.markers.push_back(marker);
134 // ROS_INFO("created marker end");
135  // Publish the marker
136 // marker_pub.publish(marker);
137 }
138 
139 void targetsCallback(const mtt::TargetList& list)
140 {
141  static ros::Time start_time = ros::Time::now();
142  time_elapsed = ros::Time::now() - start_time;
143  matlab_list.clear();
144 
146  //use transformations to extract robot features
147 // try{
148 // p_listener->lookupTransform("/map", "/robot_0/base_link", ros::Time(0), transform);
149 // p_listener->lookupTwist("/map", "/robot_0/base_link", ros::Time(0), ros::Duration(0.5), twist);
150 // }
151  try{
152  p_listener->lookupTransform("/map", "/base_link", ros::Time(0), transform);
153  p_listener->lookupTwist("/map", "/base_link", ros::Time(0), ros::Duration(0.5), twist);
154  }
155  catch (tf::TransformException ex){
156  ROS_ERROR("%s",ex.what());
157  }
158 
159  robot_x = transform.getOrigin().x();
160  robot_y = transform.getOrigin().y();
161  robot_theta = tf::getYaw(transform.getRotation());
162  robot_vel = sqrt(pow(twist.linear.x, 2) + pow(twist.linear.y, 2));
163 
165  //sweeps target list and extract features
166  for(uint i = 0; i < list.Targets.size(); i++){
167  target_id = list.Targets[i].id;
168  target_x = list.Targets[i].pose.position.x;
169  target_y = list.Targets[i].pose.position.y;
170  target_theta = tf::getYaw(list.Targets[i].pose.orientation);
171  target_vel = sqrt(pow(list.Targets[i].velocity.linear.x,2)+
172  pow(list.Targets[i].velocity.linear.y,2));
173  position_diff = sqrt(pow(robot_x - target_x,2)+
174  pow(robot_y - target_y,2));
177  target_x - robot_x );
178  velocity_diff = robot_vel - target_vel;
180 
181 
182 // if(position_diff < 6.0 && target_vel > 0.5){
183  sample_to_classify->data.fl[1] = target_vel;
184  sample_to_classify->data.fl[2] = lateral_disp;
185  sample_to_classify->data.fl[3] = heading_diff;
186  sample_to_classify->data.fl[4] = angle_to_robot;
187  sample_to_classify->data.fl[5] = position_diff;
188 
189 // ROS_INFO("classifying ");
191 // ROS_INFO("label:%f, now will publish",label);
192  drawPose(list.Targets[i].pose, target_id, label);
193 // }
194  }
195 
196  marker_pub.publish(ma);
197  ma.markers.clear();
198 }
199 
200 
201 
202 int main(int argc, char **argv)
203 {
204  ros::init(argc, argv, "process_target");
205  ros::NodeHandle n;
206 
207  tf::TransformListener listener;
208  p_listener=&listener;
209 
210 // adaboost.load("trained_boost.xml");
211  std::cout << "loaded file: " << argv[1] << std::endl;
212  adaboost.load(argv[1]);
213  sample_to_classify = cvCreateMat( 1, 6, CV_32F );
214  weak_responses = cvCreateMat( 1, adaboost.get_weak_predictors()->total, CV_32F );
215 
216  ros::Subscriber sub = n.subscribe("/targets", 1000, targetsCallback);
217 // ros::Subscriber sub_tag = n.subscribe("/timetag", 1000, tagCallback);
218 // ros::Subscriber sub_draw = n.subscribe("/pose_to_draw", 1000, drawCallback);
219  nfeatures_pub = n.advertise<geometry_msgs::PoseWithCovariance>("/example_topic", 1000);
220  marker_pub = n.advertise<visualization_msgs::MarkerArray>("/leader_quality", 1000);
221 
222  ros::spin();
223 
224  return 0;
225 }
double heading_diff
double target_x
visualization_msgs::MarkerArray ma
mtt::Target target
Definition: bag_filter.cpp:48
double target_y
uint badLeader
CvMat * sample_to_classify
ros::Duration duration_btw_msg
double target_theta
uint target_id
void drawPose(const geometry_msgs::Pose &input, uint target, double classification)
std::list< geometry_msgs::PoseWithCovariance > matlab_list
double velocity_diff
geometry_msgs::Twist features
double lateral_disp
uint single_target
double label
ros::Publisher nfeatures_pub
double angle_to_robot
double robot_y
tf::TransformListener * p_listener
void targetsCallback(const mtt::TargetList &list)
tf::StampedTransform transform
CvMat * weak_responses
double robot_vel
double position_diff
geometry_msgs::PoseWithCovariance nfeatures
ros::Time time_last_msg(0)
int main(int argc, char **argv)
CvBoost adaboost
Code for feature extraction.
double robot_theta
ros::Duration time_elapsed
ros::Publisher marker_pub
double robot_x
geometry_msgs::Twist twist
uint counter
double target_vel


human_leader
Author(s): Procopio Stein
autogenerated on Mon Mar 2 2015 01:31:41