41 if((alpha_in.size()==arc_in.size()) && (arc_in.size()==speed_in.size()))
46 for(
size_t i=0; i<alpha_in.size(); ++i)
48 if(abs(alpha_in[i])<=M_PI/2)
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];
58 ROS_ERROR(
"Detected alpha bigger than pi/2");
67 for(
size_t i=0; i<
alpha.size(); ++i)
73 for(
size_t i=0; i<
alpha.size(); ++i)
77 double lcy =
D/tan(
alpha[i]);
81 ltheta.push_back (M_PI/2 + atan2(
ly[i]- lcy,
lx[i]- lcx));
85 ltheta.push_back (-M_PI/2 + atan2(
ly[i]- lcy,
lx[i]- lcx));
101 for(
size_t i=0; i<
alpha.size(); ++i)
103 std::vector<t_lines> v;
137 ROS_ERROR(
"Input vector dimensions mismatch");
151 pcl::PointCloud<pcl::PointXYZ> ponto_teste, ponto_result;
154 for(
size_t i=0; i<
alpha.size(); ++i)
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);
161 pcl::PointXYZ pt1(0,0,0);
162 ponto_teste.points.push_back(pt1);
164 for(
int j=i;j>=0;--j)
166 pcl_ros::transformPointCloud(ponto_teste, ponto_result,
ltrans[j]);
170 x.push_back(ponto_result.points[0].x);
171 y.push_back(ponto_result.points[0].y);
172 ponto_result.clear();
178 ponto_teste.points.push_back(ponto_result.points[0]);
179 ponto_result.clear();
186 while(theta_provi<-M_PI)
188 theta_provi=2*M_PI-abs(theta_provi);
190 while(theta_provi>M_PI)
192 theta_provi=theta_provi-2*M_PI;
194 theta.push_back(theta_provi);
212 visualization_msgs::Marker
marker,marker2;
213 geometry_msgs::Point p,pp;
214 std_msgs::ColorRGBA color;
222 marker.header.frame_id =
"/parking_frame";
223 marker.header.stamp = ros::Time::now();
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;
234 for(
size_t i=0; i<
alpha.size(); ++i)
241 marker.points.push_back(p);
245 marker.points.push_back(p);
253 marker.points.push_back(p);
257 marker.points.push_back(p);
260 marker_vec->push_back(marker);
267 marker.header.frame_id =
"/parking_frame";
268 marker.header.stamp = ros::Time::now();
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)
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);
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)
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;
318 sprintf(str,
"%ld",i);
320 marker_vec->push_back(marker);
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);
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;
366 for(
size_t i=0; i<
alpha.size(); ++i)
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);
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)
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);
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)
424 for(
size_t l=0;l<
v_lines[i].size();l++)
429 marker2.points.push_back(pp);
434 marker2.points.push_back(pp);
436 marker_vec->push_back(marker2);
vector< double > total_arc
t_func_output compute_transformation()
Compute the transformations.
vector< tf::Transform > ltrans
vector< vector< t_lines > > v_lines
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.
class which generates a few number of trajectories from an input file
visualization_msgs::Marker marker
void create_markers(std::vector< visualization_msgs::Marker > *marker_vec, int *marker_count, int num_traj)
Add markers to marker array.