view_laser_targets.cpp
Go to the documentation of this file.
1 #include "ros/ros.h"
2 #include <visualization_msgs/Marker.h>
3 #include <visualization_msgs/MarkerArray.h>
4 #include "mtt/TargetList.h"
5 #include "sensor_msgs/LaserScan.h"
6 #include "cmath"
7 #include <iostream>
8 #include <vector>
9 #include <boost/thread.hpp>
10 #include <boost/shared_ptr.hpp>
11 
12 using namespace std;
13 
14 typedef enum
15 {
19 
20 ros::Publisher marker_publisher;
21 
27 class Markers
28 {
29  public:
37  void update(visualization_msgs::Marker& marker)
38  {
39  for(uint i=0;i<markers.size();++i)
40  if(markers[i].ns==marker.ns && markers[i].id==marker.id)//Marker found
41  {
42  markers.erase(markers.begin()+i);
43  break;
44  }
45 
46  markers.push_back(marker);
47  }
48 
52  void decrement(void)
53  {
54  for(uint i=0;i<markers.size();++i)
55  {
56  switch(markers[i].action)
57  {
58  case visualization_msgs::Marker::ADD:
59  markers[i].action = visualization_msgs::Marker::DELETE;
60  break;
61  case visualization_msgs::Marker::DELETE:
62  markers[i].action = -1;
63  break;
64  }
65  }
66  }
67 
68 
72  void clean(void)
73  {
74  vector<visualization_msgs::Marker> new_markers;
75 
76  for(uint i=0;i<markers.size();++i)
77  if(markers[i].action!=-1)
78  new_markers.push_back(markers[i]);
79 
80  markers=new_markers;
81  }
82 
88  vector<visualization_msgs::Marker> getOutgoingMarkers(void)
89  {
90  vector<visualization_msgs::Marker> m_out(markers);
91  return markers;
92  }
93  private:
95  vector<visualization_msgs::Marker> markers;
96 };
97 
104 class Candidate
105 {
106  public:
107 
108  typedef boost::shared_ptr<Candidate> Ptr;
109 
110  Candidate(double input_image, uint priority_, uint id_)
111  {
112  id = id_;
113  image = input_image;
114  priority = priority_;
115 
116  classification = UNKNOWN;
117  }
118 
119 
120  void classify()
121  {
122  t = boost::thread(boost::bind(&Candidate::do_classfication,this));
123  }
124 
126  {
127  switch(classification)
128  {
129  case PEDESTRIAN:
130  return "pedestrian";
131  case UNKNOWN:
132  return "unknown";
133  }
134 
135  return "unknown";
136  }
137 
139  {
140  //Run the heady classifcation code from pedro
141  int c = 0;
142  cout<<"Processing image"<<endl;
143  while(c<100)
144  {
145  c++;
146  boost::this_thread::sleep(boost::posix_time::milliseconds(100));
147  }
148 
149  classification = PEDESTRIAN;
150 
151  cout<<"done"<<endl;
152  }
153 
154  uint id;
155  uint priority;
157  double theta;
158  double image;
159  TypeClassification classification;
160  boost::thread t;
161 
162 };
163 
165 
166 void laserGather(const mtt::TargetList& msg)
167 {
168  markers.decrement();
169 
170  vector<Candidate::Ptr> candidate_list;
171 
172  //trying to draw markers
173  visualization_msgs::MarkerArray allmarkers;
174 
175 // cout << "Target message size: " << msg.Targets.size() << endl;
176 // cout << msg.Targets[1] << endl;
177 
178 // cout << "inicio" << endl;
179 
180  for(int i=0; i<msg.Targets.size();i++)
181  {
182 
183  if (msg.Targets[i].size > 0.01/* && msg.Targets[i].size < 0.6*/ ) // && msg.Targets[i].pose.position.x >= 0 ) //&& msg.Targets[i].pose.position.y >= 0)
184  {
185  Candidate::Ptr candidate = Candidate::Ptr(new Candidate(0.0,i,i));
186 
187 // cout << "target id" << msg.Targets[i].id << endl;
188 // cout << "target size:" << msg.Targets[i].size << endl;
189 // cout << "target position x:" << msg.Targets[i].pose.position.x << " y:" << msg.Targets[i].pose.position.y << endl;
190 
191  candidate->distance_to_target_from_center_bumper = sqrt(msg.Targets[i].pose.position.x * msg.Targets[i].pose.position.x +
192  msg.Targets[i].pose.position.y * msg.Targets[i].pose.position.y);
193 
194 // cout << "distancetotarget:" << candidate->distance_to_target_from_center_bumper << endl;
195 
196  candidate->theta = acos( msg.Targets[i].pose.position.x / candidate->distance_to_target_from_center_bumper);
197 
198 // cout << "theta:" << msg.Targets[i].velocity << endl;
199 
200  if(msg.Targets[i].velocity.linear.x > 0.01 /*candidate->distance_to_target_from_center_bumper < 20 && candidate->theta < (3.1415926/3)*/ )
201  {
202  visualization_msgs::Marker marker;
203 
204  marker.header.frame_id = "laser_1";
205  marker.header.stamp = ros::Time();
206  marker.ns = "my_namespace";
207  marker.id = msg.Targets[i].id;
208  marker.type = visualization_msgs::Marker::CUBE;
209  marker.action = visualization_msgs::Marker::ADD;
210  marker.pose.position.x = msg.Targets[i].pose.position.x;
211  marker.pose.position.y = msg.Targets[i].pose.position.y;
212  marker.pose.position.z = msg.Targets[i].pose.position.z;
213  marker.pose.orientation.x = 0.0;
214  marker.pose.orientation.y = 0.0;
215  marker.pose.orientation.z = 0.0;
216  marker.pose.orientation.w = 1.0;
217  marker.scale.x = 0.1;
218  marker.scale.y = msg.Targets[i].size * exp(-candidate->distance_to_target_from_center_bumper /100) * 2;
219  marker.scale.z = exp(-(candidate->distance_to_target_from_center_bumper /100)*2);
220  marker.color.a = 1.0;
221  marker.color.r = 0.0;
222  marker.color.g = 1.0;
223  marker.color.b = 0.0;
224 
225  markers.update(marker);
226  candidate_list.push_back(candidate);
227 
228  }
229 
230 
231  }
232  }
233 
234  sort(candidate_list.begin(), candidate_list.end(),
235  [&](const Candidate::Ptr c1,const Candidate::Ptr c2){
236  return c1->distance_to_target_from_center_bumper > c2->distance_to_target_from_center_bumper;
237  });
238 
239  markers.clean();
240  allmarkers.markers = markers.getOutgoingMarkers();
241 
242  marker_publisher.publish(allmarkers);
243 }
244 
245 // - fazer a lista de peƵes (PCU)
246 // - actualizar o blog
247 
248 // bool compareByPriority(const Candidate::Ptr& c1,const Candidate::Ptr& c2)
249 // {
250 // return c1->priority > c2->priority;
251 // }
252 
253 int main (int argc, char **argv)
254 {
255  ros::init(argc, argv, "laser_gather");
256 
257  ros::NodeHandle nh("~");
258 
259  marker_publisher = nh.advertise<visualization_msgs::MarkerArray>( "/ped_markers", 10);
260  ros::Subscriber sub = nh.subscribe ("/targets", 1000, laserGather);
261 
262  ros::spin();
263 
264  return 0;
265 }
266 
267 // void test(const sensor_msgs::LaserScan& msg)
268 // {
269 // cout << "msg:" << msg << endl;
270 // }
271 //
272 // int main (int argc, char **argv)
273 // {
274 // ros::init(argc, argv, "lascan");
275 //
276 // // cout << "msg:" << /*msg <<*/ endl;
277 //
278 // ros::NodeHandle nh("~");
279 //
280 // ros::Subscriber sub = nh.subscribe ("/laser_1/scan", 1000, test);
281 //
282 // ros::spin();
283 //
284 // return 0;
285 // }
void do_classfication()
ros::Publisher marker_publisher
double distance_to_target_from_center_bumper
Dynamic markers support class.
void laserGather(const mtt::TargetList &msg)
vector< visualization_msgs::Marker > markers
Internal storing vector of markers.
void update(visualization_msgs::Marker &marker)
Update a internal marker.
TypeClassification
Markers markers
Candidate(double input_image, uint priority_, uint id_)
Candidate validation.
TypeClassification
void decrement(void)
Mark existing markers for deletion.
string getClassification()
vector< visualization_msgs::Marker > getOutgoingMarkers(void)
Obtain the list of outgoing markers.
void clean(void)
Remove markers that should not be transmitted.
boost::shared_ptr< Candidate > Ptr
int main(int argc, char **argv)


multimodal_pedestrian_detect
Author(s): Rui Azevedo
autogenerated on Mon Mar 2 2015 01:32:27