c_trajectory.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 ***************************************************************************************************/
27 #include <trajectory_planner/c_trajectory.h>
35 t_func_output c_trajectory::generate(vector<double> alpha_in, vector<double> arc_in, vector<double> speed_in, t_vehicle_description& vd)
36 {
37  // ________________________________
38  //| |
39  //| Test input vectors |
40  //|________________________________|
41  if((alpha_in.size()==arc_in.size()) && (arc_in.size()==speed_in.size()))
42  {
43 
44  //Copy all the values to internal variables
45  double arc_sum=0;
46  for(size_t i=0; i<alpha_in.size(); ++i)
47  {
48  if(abs(alpha_in[i])<=M_PI/2)
49  {
50  alpha.push_back(alpha_in[i]);
51  arc.push_back(arc_in[i]);
52  speed.push_back(speed_in[i]);
53  arc_sum=arc_sum+arc[i];
54  total_arc.push_back(arc_sum);
55  }
56  else
57  {
58  ROS_ERROR("Detected alpha bigger than pi/2");
59  return FAILURE;
60  }
61  }
62 
63  // ________________________________
64  //| |
65  //| Compute local node coords |
66  //|________________________________|
67  for(size_t i=0; i<alpha.size(); ++i)
68  {
69  lx.push_back((D/tan(alpha[i]))*sin(arc[i]/(D/tan(alpha[i]))));
70  ly.push_back((D/tan(alpha[i]))-(D/tan(alpha[i]))*cos(arc[i]/(D/tan(alpha[i]))));
71  }
72 
73  for(size_t i=0; i<alpha.size(); ++i)
74  {
75  // Local coords of IRC
76  double lcx = 0;
77  double lcy = D/tan(alpha[i]);
78 
79  if (alpha[i]>0)
80  {
81  ltheta.push_back (M_PI/2 + atan2(ly[i]- lcy, lx[i]- lcx));
82  }
83  else
84  {
85  ltheta.push_back (-M_PI/2 + atan2(ly[i]- lcy, lx[i]- lcx));
86  }
87  }
88 
89  // ________________________________
90  //| |
91  //| Define transformations |
92  //|________________________________|
93  // Compute transformation
94  compute_transformation();
95 
96 
97  // ___________________________________
98  // | |
99  // | Compute vhc lines for each node |
100  // |_________________________________|
101  for(size_t i=0; i<alpha.size(); ++i)
102  {
103  std::vector<t_lines> v;
104  t_lines line1;
105 
106  line1.x[0]=x[i]-vd.lenght_back*cos(theta[i])-(vd.width/2.0)*sin(theta[i]);
107  line1.y[0]=y[i]-vd.lenght_back*sin(theta[i])+(vd.width/2.0)*cos(theta[i]);
108  line1.x[1]=x[i]+vd.lenght_front*cos(theta[i])-(vd.width/2.0)*sin(theta[i]);
109  line1.y[1]=y[i]+vd.lenght_front*sin(theta[i])+(vd.width/2.0)*cos(theta[i]);
110  v.push_back(line1);
111 
112  line1.x[0]=x[i]+vd.lenght_front*cos(theta[i])-(vd.width/2.0)*sin(theta[i]);;
113  line1.y[0]=y[i]+vd.lenght_front*sin(theta[i])+(vd.width/2.0)*cos(theta[i]);
114  line1.x[1]=x[i]+vd.lenght_front*cos(theta[i])+(vd.width/2.0)*sin(theta[i]);;
115  line1.y[1]=y[i]+vd.lenght_front*sin(theta[i])-(vd.width/2.0)*cos(theta[i]);
116  v.push_back(line1);
117 
118  line1.x[0]=x[i]+vd.lenght_front*cos(theta[i])+(vd.width/2.0)*sin(theta[i]);;
119  line1.y[0]=y[i]+vd.lenght_front*sin(theta[i])-(vd.width/2.0)*cos(theta[i]);
120  line1.x[1]=x[i]-vd.lenght_back*cos(theta[i])+(vd.width/2.0)*sin(theta[i]);;
121  line1.y[1]=y[i]-vd.lenght_back*sin(theta[i])-(vd.width/2.0)*cos(theta[i]);
122  v.push_back(line1);
123 
124  line1.x[0]=x[i]-vd.lenght_back*cos(theta[i])+(vd.width/2.0)*sin(theta[i]);;
125  line1.y[0]=y[i]-vd.lenght_back*sin(theta[i])-(vd.width/2.0)*cos(theta[i]);
126  line1.x[1]=x[i]-vd.lenght_back*cos(theta[i])-(vd.width/2.0)*sin(theta[i]);;
127  line1.y[1]=y[i]-vd.lenght_back*sin(theta[i])+(vd.width/2.0)*cos(theta[i]);
128  v.push_back(line1);
129 
130  v_lines.push_back(v);
131  }
132 
133  return SUCCESS;
134  }
135  else
136  {
137  ROS_ERROR("Input vector dimensions mismatch");
138  return FAILURE;
139  }
140 }
141 
142 
148 t_func_output c_trajectory::compute_transformation()
149 {
150  //compute vector of local transforms. Transform at position i transforms from i-1 to i
151  pcl::PointCloud<pcl::PointXYZ> ponto_teste, ponto_result;
152  ltrans.clear();
153  theta.push_back(ltheta[0]);
154  for(size_t i=0; i<alpha.size(); ++i)
155  {
156  tf::Transform transform;
157  transform.setOrigin(tf::Vector3(lx[i], ly[i], 0));
158  transform.setRotation(tf::createQuaternionFromRPY(0, 0, ltheta[i]));
159  ltrans.push_back(transform);
160 
161  pcl::PointXYZ pt1(0,0,0);
162  ponto_teste.points.push_back(pt1);
163 
164  for(int j=i;j>=0;--j)
165  {
166  pcl_ros::transformPointCloud(ponto_teste, ponto_result, ltrans[j]);
167 
168  if(j==0)
169  {
170  x.push_back(ponto_result.points[0].x);
171  y.push_back(ponto_result.points[0].y);
172  ponto_result.clear();
173  ponto_teste.clear();
174  }
175  else
176  {
177  ponto_teste.clear();
178  ponto_teste.points.push_back(ponto_result.points[0]);
179  ponto_result.clear();
180  }
181  }
182 
183  if(i!=0)
184  {
185  double theta_provi=theta[i-1]+ltheta[i];
186  while(theta_provi<-M_PI)
187  {
188  theta_provi=2*M_PI-abs(theta_provi);
189  }
190  while(theta_provi>M_PI)
191  {
192  theta_provi=theta_provi-2*M_PI;
193  }
194  theta.push_back(theta_provi);
195  }
196 
197  }
198  return SUCCESS;
199 }
200 
201 
209 void c_trajectory::create_markers(std::vector<visualization_msgs::Marker>* marker_vec, int* marker_count, int num_traj)
210 {
211  //Create a marker
212  visualization_msgs::Marker marker,marker2;
213  geometry_msgs::Point p,pp;
214  std_msgs::ColorRGBA color;
215 
216 
217  // ________________________________
218  //| |
219  //| Line List |
220  //|________________________________|
221  //Line marker to trajectory
222  marker.header.frame_id = "/parking_frame";
223  marker.header.stamp = ros::Time::now();
224  marker.ns = "lines";
225  marker.id = num_traj;
226  marker.action = visualization_msgs::Marker::ADD;
227  marker.type = visualization_msgs::Marker::LINE_LIST;
228  marker.scale.x = 0.01;
229  marker.color.r = 0.0;
230  marker.color.g = 0.7;
231  marker.color.b = 0.0;
232  marker.color.a = 1.0;
233  int first_step=1;
234  for(size_t i=0; i<alpha.size(); ++i)
235  {
236  if(first_step==1)
237  {
238  p.x = 0;
239  p.y = 0;
240  p.z = 0;
241  marker.points.push_back(p);
242  p.x = x[i];
243  p.y = y[i];
244  p.z = 0;
245  marker.points.push_back(p);
246  first_step=0;
247  }
248  else
249  {
250  p.x = x[i-1];
251  p.y = y[i-1];
252  p.z = 0;
253  marker.points.push_back(p);
254  p.x = x[i];
255  p.y = y[i];
256  p.z = 0;
257  marker.points.push_back(p);
258  }
259  }
260  marker_vec->push_back(marker);
261 
262  // ________________________________
263  //| |
264  //| Cube Marker |
265  //|________________________________|
266  // Cube marker to t nodes
267  marker.header.frame_id = "/parking_frame";
268  marker.header.stamp = ros::Time::now();
269  marker.ns = "nodes";
270  marker.action = visualization_msgs::Marker::ADD;
271  marker.type = visualization_msgs::Marker::CUBE;
272  marker.pose.orientation.x = 0.0;
273  marker.pose.orientation.y = 0.0;
274  marker.pose.orientation.z = 0.0;
275  marker.pose.orientation.w = 1.0;
276  marker.scale.x = 0.03;
277  marker.scale.y = 0.03;
278  marker.scale.z = 0.03;
279  marker.color.r = 0.0;
280  marker.color.g = 0.0;
281  marker.color.b = 1.0;
282  marker.color.a = 1.0;
283  for(size_t i=0; i<alpha.size(); ++i)
284  {
285  marker.id = (*marker_count)++;
286  marker.pose.position.x = x[i];
287  marker.pose.position.y = y[i];
288  marker.pose.position.z = 0;
289  marker_vec->push_back(marker);
290  }
291 
292  // ________________________________
293  //| |
294  //| text nodes |
295  //|________________________________|
296  // Points marker to t nodes
297  marker.header.frame_id = "/parking_frame";
298  marker.header.stamp = ros::Time::now();
299  marker.ns = "node_number";
300  marker.action = visualization_msgs::Marker::ADD;
301  marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
302  marker.pose.orientation.x = 0.0;
303  marker.pose.orientation.y = 0.0;
304  marker.pose.orientation.z = 0.0;
305  marker.pose.orientation.w = 1.0;
306  marker.scale.z = 0.15;
307  marker.color.r = 0.0;
308  marker.color.g = 0.0;
309  marker.color.b = 0.2;
310  marker.color.a = 1.0;
311  for(size_t i=0; i<alpha.size(); ++i)
312  {
313  marker.id = (*marker_count)++;
314  marker.pose.position.x = x[i];
315  marker.pose.position.y = y[i];
316  marker.pose.position.z = 0.075;
317  char str[1024];
318  sprintf(str,"%ld",i);
319  marker.text = str;
320  marker_vec->push_back(marker);
321  }
322 
323  // ________________________________
324  //| |
325  //| text trajectory |
326  //|________________________________|
327  // Points marker to t nodes
328  marker.header.frame_id = "/parking_frame";
329  marker.header.stamp = ros::Time::now();
330  marker.ns = "trajectory_number";
331  marker.action = visualization_msgs::Marker::ADD;
332  marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
333  marker.pose.orientation.x = 0.0;
334  marker.pose.orientation.y = 0.0;
335  marker.pose.orientation.z = 0.0;
336  marker.pose.orientation.w = 1.0;
337  marker.scale.z = 0.2;
338  marker.color.r = 0.0;
339  marker.color.g = 0.0;
340  marker.color.b = 1.0;
341  marker.color.a = 1.0;
342  marker.id = (*marker_count)++;
343  marker.pose.position.x = x[x.size()-1];
344  marker.pose.position.y = y[y.size()-1];
345  marker.pose.position.z = 0.2;
346  marker.text = "Traj " + str( boost::format("%d") % (num_traj));
347  marker_vec->push_back(marker);
348 
349  // ________________________________
350  //| |
351  //| Arrow |
352  //|________________________________|
353  //Line marker to trajectory
354  marker.header.frame_id = "/parking_frame";
355  marker.header.stamp = ros::Time::now();
356  marker.ns = "Orientation arrow";
357  marker.action = visualization_msgs::Marker::ADD;
358  marker.type = visualization_msgs::Marker::ARROW;
359  marker.color.r = 1.0;
360  marker.color.g = 0.0;
361  marker.color.b = 0.0;
362  marker.color.a = 1.0;
363  marker.scale.x=0.3;
364  marker.scale.y=0.3;
365  marker.scale.z=0.3;
366  for(size_t i=0; i<alpha.size(); ++i)
367  {
368  marker.id = (*marker_count)++;
369  marker.pose.position.x = x[i];
370  marker.pose.position.y = y[i];
371  marker.pose.position.z = 0;
372  marker.pose.orientation.z = sin(theta[i]/2);
373  marker.pose.orientation.w = cos(theta[i]/2);
374  marker_vec->push_back(marker);
375  }
376 
377  // ________________________________
378  //| |
379  //| Rectangle (car shadow) |
380  //|________________________________|
381  // Represents the form of the car in each node
382  marker.header.frame_id = "/parking_frame";
383  marker.header.stamp = ros::Time::now();
384  marker.ns = "car shadow";
385  marker.action = visualization_msgs::Marker::ADD;
386  marker.type = visualization_msgs::Marker::CUBE;
387  marker.scale.x = 0.8;
388  marker.scale.y = 0.42;
389  marker.scale.z = 0.05;
390  marker.color.r = 0.25;
391  marker.color.g = 0.41;
392  marker.color.b = 0.88;
393  marker.color.a = 0.1;
394  for(size_t i=0; i<alpha.size(); ++i)
395  {
396  marker.id = (*marker_count)++;
397  marker.pose.position.x = x[i]+0.2*cos(theta[i]);
398  marker.pose.position.y = y[i]+0.2*sin(theta[i]);
399  marker.pose.position.z = 0;
400  marker.pose.orientation.z = sin(theta[i]/2);
401  marker.pose.orientation.w = cos(theta[i]/2);
402  marker_vec->push_back(marker);
403  }
404 
405 
406  // ________________________________
407  //| |
408  //| Car contour |
409  //|________________________________|
410  //Line marker to car contour
411  marker2.header.frame_id = "/parking_frame";
412  marker2.header.stamp = ros::Time::now();
413  marker2.ns = "car contour";
414  marker2.id = (*marker_count)++;
415  marker2.action = visualization_msgs::Marker::ADD;
416  marker2.type = visualization_msgs::Marker::LINE_LIST;
417  marker2.scale.x = 0.01;
418  marker2.color.r = 0.5;
419  marker2.color.g = 0.5;
420  marker2.color.b = 1.0;
421  marker2.color.a = 0.5;
422  for(size_t i=0; i<alpha.size(); ++i)
423  {
424  for(size_t l=0;l<v_lines[i].size();l++)
425  {
426  pp.x = v_lines[i][l].x[0];
427  pp.y = v_lines[i][l].y[0];
428  pp.z = 0;
429  marker2.points.push_back(pp);
430 
431  pp.x = v_lines[i][l].x[1];
432  pp.y = v_lines[i][l].y[1];
433  pp.z = 0;
434  marker2.points.push_back(pp);
435  }
436  marker_vec->push_back(marker2);
437  }
438 }
c_trajectory::y
vector< double > y
Definition: c_trajectory.h:115
c_trajectory::total_arc
vector< double > total_arc
Definition: c_trajectory.h:138
t_lines::y
double y[2]
Definition: c_trajectory.h:77
c_trajectory::compute_transformation
t_func_output compute_transformation()
Compute the transformations.
Definition: c_trajectory.cpp:148
c_trajectory::speed
vector< double > speed
Definition: c_trajectory.h:134
t_lines
Definition: c_trajectory.h:75
c_trajectory::ltrans
vector< tf::Transform > ltrans
Definition: c_trajectory.h:111
c_trajectory::x
vector< double > x
Definition: c_trajectory.h:114
c_trajectory::v_lines
vector< vector< t_lines > > v_lines
Definition: c_trajectory.h:102
c_trajectory::ly
vector< double > ly
Definition: c_trajectory.h:109
c_trajectory::generate
t_func_output generate(vector< double > alpha_in, vector< double > arc_in, vector< double > speed_in, t_vehicle_description &vd)
Test the input vectors and compute the local node coordinates.
Definition: c_trajectory.cpp:35
c_trajectory::theta
vector< double > theta
Definition: c_trajectory.h:116
t_vehicle_description::lenght_back
double lenght_back
Definition: c_trajectory.h:68
c_trajectory.h
class which generates a few number of trajectories from an input file
c_trajectory::arc
vector< double > arc
Definition: c_trajectory.h:133
c_trajectory::lx
vector< double > lx
Definition: c_trajectory.h:108
t_vehicle_description::lenght_front
double lenght_front
Definition: c_trajectory.h:69
t_vehicle_description::width
double width
Definition: c_trajectory.h:67
c_trajectory::D
double D
Definition: c_trajectory.h:135
marker
visualization_msgs::Marker marker
Definition: mtt_target_generator.cpp:52
FAILURE
Definition: c_trajectory.h:55
c_trajectory::create_markers
void create_markers(std::vector< visualization_msgs::Marker > *marker_vec, int *marker_count, int num_traj)
Add markers to marker array.
Definition: c_trajectory.cpp:209
c_trajectory::ltheta
vector< double > ltheta
Definition: c_trajectory.h:110
t_vehicle_description
Definition: c_trajectory.h:66
t_func_output
t_func_output
Definition: c_trajectory.h:55
SUCCESS
Definition: c_trajectory.h:55
c_trajectory::alpha
vector< double > alpha
Definition: c_trajectory.h:132
t_lines::x
double x[2]
Definition: c_trajectory.h:76

trajectory_planner
Author(s): Joel Pereira
autogenerated on Mon Mar 2 2015 01:32:54