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
00027 #include <trajectory_planner/c_manage_trajectory.h>
00028
00039 t_func_output c_manage_trajectory::compute_DLO(c_trajectoryPtr& trajectory, std::vector<t_obstacle>& vo)
00040 {
00041
00042 trajectory->collision_pts.erase(trajectory->collision_pts.begin(), trajectory->collision_pts.end());
00043
00044 if (trajectory->closest_node<0 || trajectory->closest_node >=(int)trajectory->x.size())
00045 {
00046 ROS_ERROR("Error on node");
00047 return FAILURE;
00048 }
00049
00050
00051 trajectory->score.DLO = 10.0;
00052 trajectory->score.FS = 1;
00053 for (int n=0; n<= trajectory->closest_node; ++n)
00054 {
00055 if (trajectory->v_lines.size()!=trajectory->x.size())
00056 {
00057 ROS_ERROR("Node lines and number of nodes not equal");
00058 }
00059
00060
00061 for (size_t l=0; l< trajectory->v_lines[n].size(); ++l)
00062 {
00063 double Ax = trajectory->v_lines[n][l].x[0];
00064 double Ay = trajectory->v_lines[n][l].y[0];
00065 double Bx = trajectory->v_lines[n][l].x[1];
00066 double By = trajectory->v_lines[n][l].y[1];
00067
00068
00069 for (size_t o=0; o< vo.size(); ++o)
00070 {
00071
00072 for (size_t lo=1; lo< vo[o].x.size(); ++lo)
00073 {
00074 double DLOprev = sqrt(pow(trajectory->v_lines[n][l].x[0]-vo[o].x[lo-1],2)+pow(trajectory->v_lines[n][l].y[0]-vo[o].y[lo-1],2));
00075 if(trajectory->score.DLO > DLOprev)
00076 {
00077 trajectory->score.DLO=DLOprev;
00078 }
00079 double Cx = vo[o].x[lo-1];
00080 double Cy = vo[o].y[lo-1];
00081 double Dx = vo[o].x[lo];
00082 double Dy = vo[o].y[lo];
00083 double X,Y;
00084
00085 int ret = lineSegmentIntersection( Ax, Ay,
00086 Bx, By,
00087 Cx, Cy,
00088 Dx, Dy,
00089 &X, &Y);
00090
00091 if (ret==DO_INTERSECT)
00092 {
00093 t_point p;
00094 p.x=X;
00095 p.y=Y;
00096 trajectory->collision_pts.push_back(p);
00097 trajectory->score.FS*=0;
00098 }
00099 }
00100 }
00101 }
00102 }
00103 return SUCCESS;
00104 }
00105
00121 int c_manage_trajectory::lineSegmentIntersection(double Ax, double Ay,double Bx, double By,double Cx, double Cy,double Dx, double Dy,double *X, double *Y)
00122 {
00123 double distAB, theCos, theSin, newX, ABpos ;
00124
00125
00126 if (((Ax==Bx) && (Ay==By)) || ((Cx==Dx) && (Cy==Dy))) return DONT_INTERSECT;
00127
00128
00129 if (((Ax==Cx) && (Ay==Cy)) || ((Bx==Cx) && (By==Cy))
00130 || ((Ax==Dx) && (Ay==Dy)) || ((Bx==Dx) && (By==Dy))) {
00131 return DONT_INTERSECT; }
00132
00133
00134 Bx-=Ax; By-=Ay;
00135 Cx-=Ax; Cy-=Ay;
00136 Dx-=Ax; Dy-=Ay;
00137
00138
00139 distAB=sqrt(Bx*Bx+By*By);
00140
00141
00142 theCos=Bx/distAB;
00143 theSin=By/distAB;
00144 newX=Cx*theCos+Cy*theSin;
00145 Cy =Cy*theCos-Cx*theSin; Cx=newX;
00146 newX=Dx*theCos+Dy*theSin;
00147 Dy =Dy*theCos-Dx*theSin; Dx=newX;
00148
00149
00150 if ((Cy<0. && Dy<0.) || (Cy>=0. && Dy>=0.)) return DONT_INTERSECT;
00151
00152
00153 ABpos=Dx+(Cx-Dx)*Dy/(Dy-Cy);
00154
00155
00156 if (ABpos<0. || ABpos>distAB) return DONT_INTERSECT;
00157
00158
00159 *X=Ax+ABpos*theCos;
00160 *Y=Ay+ABpos*theSin;
00161
00162
00163 return DO_INTERSECT;
00164 }
00165
00171 t_func_output c_manage_trajectory::set_obstacles(mtt::TargetListPC& msg)
00172 {
00173 vo.erase(vo.begin(), vo.end());
00174 for (size_t i=0; i<msg.obstacle_lines.size(); ++i)
00175 {
00176 t_obstacle o;
00177
00178 pcl::PointCloud<pcl::PointXYZ> pc;
00179 pcl::PCLPointCloud2 pcl_pc;
00180 pcl_conversions::toPCL(msg.obstacle_lines[i], pcl_pc);
00181 pcl::fromPCLPointCloud2(pcl_pc, pc);
00182
00183 for (size_t j=0; j<pc.points.size(); ++j)
00184 {
00185 o.x.push_back(pc.points[j].x);
00186 o.y.push_back(pc.points[j].y);
00187 }
00188
00189 vo.push_back(o);
00190 }
00191 return SUCCESS;
00192 }
00193
00204 t_func_output c_manage_trajectory::set_vehicle_description(double w, double lb, double lf, double ht, double hb)
00205 {
00206 vehicle_description.width = w;
00207 vehicle_description.lenght_back = lb;
00208 vehicle_description.lenght_front = lf;
00209 vehicle_description.height_top = ht;
00210 vehicle_description.height_bottom = hb;
00211 return SUCCESS;
00212 }
00213
00220 t_func_output c_manage_trajectory::get_traj_info_msg_from_chosen(trajectory_planner::traj_info* info)
00221 {
00222 info->arc.erase(info->arc.begin(), info->arc.end());
00223 info->total_arc.erase(info->total_arc.begin(), info->total_arc.end());
00224 info->alpha.erase(info->alpha.begin(), info->alpha.end());
00225 info->speed.erase(info->speed.begin(), info->speed.end());
00226 for(int j=0;j<=(int)vt[chosen_traj.index]->closest_node;++j)
00227 {
00228 info->arc.push_back(vt[chosen_traj.index]->arc[j]);
00229 info->total_arc.push_back(vt[chosen_traj.index]->total_arc[j]);
00230 info->alpha.push_back(vt[chosen_traj.index]->alpha[j]);
00231 }
00232 set_speed_vector(info);
00233
00234 cout<<"ARC SIZE (when constructing) "<<info->arc.size()<<endl;
00235 cout<<"TOTAL ARC SIZE (when constructing) "<<info->total_arc.size()<<endl;
00236 cout<<"ALPHA SIZE (when constructing) "<<info->alpha.size()<<endl;
00237 cout<<"SPEED SIZE (when constructing) "<<info->speed.size()<<endl;
00238 return SUCCESS;
00239 }
00240
00241
00248 t_func_output c_manage_trajectory::set_speed_vector(trajectory_planner::traj_info* info)
00249 {
00250 for(int i=0;i<=(int)vt[chosen_traj.index]->closest_node;++i)
00251 {
00252 if(i < ((int)vt[chosen_traj.index]->arc.size() - 1))
00253 {
00254 if((info->arc[i])*(info->arc[i+1])<0.0)
00255 {
00256 info->speed.push_back((info->arc[i]/fabs(info->arc[i]))*_SPEED_SAFFETY_);
00257 }
00258 else
00259 {
00260 info->speed.push_back((info->arc[i]/fabs(info->arc[i]))*_SPEED_REQUIRED_);
00261 }
00262 }
00263 else
00264 {
00265 info->speed.push_back((info->arc[i]/fabs(info->arc[i]))*_SPEED_REQUIRED_);
00266 }
00267 }
00268 return SUCCESS;
00269 }
00270
00271
00277 int c_manage_trajectory::set_chosen_traj(int n)
00278 {
00279 chosen_traj.index=n;
00280 return 1;
00281 }
00282
00291 t_func_output c_manage_trajectory::set_attractor_point(double x, double y, double theta)
00292 {
00293 AP.x = x; AP.y = y; AP.theta=theta;
00294 return SUCCESS;
00295 }
00296
00303 t_func_output c_manage_trajectory::compute_global_traj_score(c_trajectoryPtr& trajectory)
00304 {
00305 double W_DAP=0.40;
00306 double W_ADAP=0.35;
00307 double W_DLO=0.25;
00308 trajectory->score.overall_norm = (W_DAP*trajectory->score.DAPnorm + W_ADAP*trajectory->score.ADAPnorm + W_DLO*trajectory->score.DLOnorm)*trajectory->score.FS;
00309 cout<<"Overallscore= "<<trajectory->score.overall_norm<<endl;
00310
00311 return SUCCESS;
00312 }
00313
00320 t_func_output c_manage_trajectory::set_inter_axis_distance(double val)
00321 {
00322 inter_axis_distance = val;
00323 return SUCCESS;
00324 }
00325
00333 t_func_output c_manage_trajectory::create_new_trajectory(vector<double> alpha_in, vector<double> arc_in, vector<double> speed_in)
00334 {
00335
00336 c_trajectoryPtr t_ptr(new c_trajectory(inter_axis_distance));
00337 vt.push_back(t_ptr);
00338
00339
00340 return vt[vt.size()-1]->generate(alpha_in, arc_in, speed_in, vehicle_description);
00341 }
00342
00348 t_func_output c_manage_trajectory::compute_chosen_traj(void)
00349 {
00350 double max_val=-1;
00351 chosen_traj.index=-1;
00352
00353 for (int i=0; i< (int)vt.size(); i++)
00354 {
00355 if (vt[i]->score.overall_norm>max_val)
00356 {
00357 chosen_traj.index=i;
00358 max_val= vt[i]->score.overall_norm;
00359 }
00360 }
00361
00362
00363 if(chosen_traj.index!=-1)
00364 return SUCCESS;
00365 else
00366 return FAILURE;
00367 }
00368
00374 t_func_output c_manage_trajectory::create_static_markers(void)
00375 {
00376 cout<<"ja create static"<<endl;
00377 static_marker_vec.clear();
00378 int marker_count=0;
00379 for (int i=0; i< (int)vt.size(); i++)
00380 {
00381 vt[i]->create_markers(&static_marker_vec, &marker_count, i);
00382 }
00383 return SUCCESS;
00384 }
00385
00392 t_func_output c_manage_trajectory::compute_vis_marker_array(visualization_msgs::MarkerArray* marker_array)
00393 {
00394 std::vector<visualization_msgs::Marker> marker_vec;
00395
00396 ROS_INFO("static marker vec size=%ld",static_marker_vec.size());
00397 for(size_t i=0;i<static_marker_vec.size();++i)
00398 {
00399 marker_vec.push_back(static_marker_vec[i]);
00400 }
00401
00402
00403
00404 int marker_count=0;
00405 for (int i=0; i< (int)vt.size(); ++i)
00406 {
00407 draw_on_node(vt[i],&marker_vec,&marker_count,0.15,vt[i]->score.DAP,vt[i]->score.DAPnorm,"DAP ");
00408 draw_on_node(vt[i],&marker_vec,&marker_count,0.30,vt[i]->score.ADAP,vt[i]->score.ADAPnorm,"ADAP ");
00409 draw_on_node(vt[i],&marker_vec,&marker_count,0.45,vt[i]->score.DLO,vt[i]->score.DLOnorm,"DLO ");
00410 draw_on_node(vt[i],&marker_vec,&marker_count,0.60,(vt[i]->score.DAP+vt[i]->score.ADAP+vt[i]->score.DLO)*vt[i]->score.FS,vt[i]->score.overall_norm,"Overall ");
00411 }
00412
00413
00414
00415
00416
00417
00418 visualization_msgs::Marker marker2;
00419 geometry_msgs::Point p;
00420 marker2.header.frame_id = "/parking_frame";
00421 marker2.header.stamp = ros::Time::now();
00422 marker2.ns = "Chosen trajectory";
00423 marker2.id = 0;
00424 marker2.action = visualization_msgs::Marker::ADD;
00425 marker2.type = visualization_msgs::Marker::LINE_LIST;
00426 marker2.scale.x = 0.05;
00427 marker2.color.r = 0.00;
00428 marker2.color.g = 1.00;
00429 marker2.color.b = 0.00;
00430 marker2.color.a = 1.00;
00431 int first_step=1;
00432 for(size_t i=0; i<vt[chosen_traj.index]->x.size(); ++i)
00433 {
00434 if(first_step==1)
00435 {
00436 p.x = 0;
00437 p.y = 0;
00438 p.z = 0.00;
00439 marker2.points.push_back(p);
00440 p.x = vt[chosen_traj.index]->x[i];
00441 p.y = vt[chosen_traj.index]->y[i];
00442 p.z = 0.00;
00443 marker2.points.push_back(p);
00444 first_step=0;
00445 }
00446 else
00447 {
00448 p.x = vt[chosen_traj.index]->x[i-1];
00449 p.y = vt[chosen_traj.index]->y[i-1];
00450 p.z = 0.00;
00451 marker2.points.push_back(p);
00452 p.x = vt[chosen_traj.index]->x[i];
00453 p.y = vt[chosen_traj.index]->y[i];
00454 p.z = 0.00;
00455 marker2.points.push_back(p);
00456 }
00457 }
00458 marker_vec.push_back(marker2);
00459
00460
00461
00462
00463
00464
00465 visualization_msgs::Marker marker;
00466 marker.header.frame_id = "/parking_frame";
00467 marker.header.stamp = ros::Time::now();
00468 marker.ns = "car actual node";
00469 marker.id = 0;
00470 marker.action = visualization_msgs::Marker::ADD;
00471 marker.type = visualization_msgs::Marker::CUBE;
00472 marker.scale.x = 0.8;
00473 marker.scale.y = 0.42;
00474 marker.scale.z = 0.05;
00475 marker.color.r = 0.80;
00476 marker.color.g = 0.50;
00477 marker.color.b = 0.20;
00478 marker.color.a = 0.8;
00479 marker.pose.position.x = vt[chosen_traj.index]->x[current_node]+0.2*cos(vt[chosen_traj.index]->theta[current_node]);
00480 marker.pose.position.y = vt[chosen_traj.index]->y[current_node]+0.2*sin(vt[chosen_traj.index]->theta[current_node]);
00481 marker.pose.position.z = 0;
00482 marker.pose.orientation.z = sin(vt[chosen_traj.index]->theta[current_node]/2);
00483 marker.pose.orientation.w = cos(vt[chosen_traj.index]->theta[current_node]/2);
00484 marker_vec.push_back(marker);
00485
00486
00487
00488
00489
00490
00491
00492 visualization_msgs::Marker marker3;
00493 marker3.header.frame_id = "/parking_frame";
00494 marker3.header.stamp = ros::Time::now();
00495 marker3.ns = "Closest Node";
00496 marker3.action = visualization_msgs::Marker::ADD;
00497 marker3.type = visualization_msgs::Marker::SPHERE;
00498 marker3.scale.x = 0.2;
00499 marker3.scale.y = 0.2;
00500 marker3.scale.z = 0.2;
00501 marker3.color.r = 1.0;
00502 marker3.color.g = 0.0;
00503 marker3.color.b = 0.1;
00504 marker3.color.a = 0.6;
00505
00506 for(size_t i=0; i<vt.size(); ++i)
00507 {
00508 marker3.id +=2;
00509 marker3.pose.position.x = vt[i]->x[vt[i]->closest_node];
00510 marker3.pose.position.y = vt[i]->y[vt[i]->closest_node];
00511 marker3.pose.position.z = 0;
00512 marker_vec.push_back(marker3);
00513 }
00514
00515
00516
00517
00518
00519
00520
00521 visualization_msgs::Marker marker4;
00522 marker4.header.frame_id = "/parking_frame";
00523 marker4.header.stamp = ros::Time::now();
00524 marker4.ns = "Colision points";
00525 marker4.action = visualization_msgs::Marker::ADD;
00526 marker4.type = visualization_msgs::Marker::CYLINDER;
00527 marker4.scale.x = 0.075;
00528 marker4.scale.y = 0.075;
00529 marker4.scale.z = 0.1;
00530 marker4.color.r = 1.0;
00531 marker4.color.g = 0.84;
00532 marker4.color.b = 0.0;
00533 marker4.color.a = 0.6;
00534
00535
00536 static size_t coli_marker_total = 0;
00537 int total=0;
00538 for(size_t j=0; j<vt.size(); ++j)
00539 {
00540 ROS_INFO("Traj%ld has %ld collisions\n",j,vt[j]->collision_pts.size());
00541
00542 for(size_t i=0; i<vt[j]->collision_pts.size(); ++i)
00543 {
00544 marker4.id =total;
00545 marker4.pose.position.x = vt[j]->collision_pts[i].x;
00546 marker4.pose.position.y = vt[j]->collision_pts[i].y;
00547 marker_vec.push_back(marker4);
00548 total++;
00549 }
00550 }
00551
00552 ROS_INFO("total=%d coli=%ld",total,coli_marker_total);
00553
00554 for(size_t j=total; j<coli_marker_total; ++j)
00555 {
00556 ROS_INFO("deleting marker ");
00557 marker4.header.frame_id = "/parking_frame";
00558 marker4.id =j;
00559 marker4.action = visualization_msgs::Marker::DELETE;
00560 marker_vec.push_back(marker4);
00561 }
00562
00563 coli_marker_total = total;
00564
00565
00566 marker_array->markers=marker_vec;
00567
00568 return SUCCESS;
00569 }
00570
00576 t_func_output c_manage_trajectory::compute_trajectories_scores(void)
00577 {
00578
00579 double maximum_admissible_to_DAP=8.0;
00580 double maximum_admissible_to_DLO=10.0;
00581 for (int i=0; i< (int)vt.size(); ++i)
00582 {
00583
00584 compute_DAP(vt[i],AP);
00585
00586
00587 compute_DLO(vt[i], vo);
00588
00589
00590
00591 vt[i]->score.DAPnorm=1-(vt[i]->score.DAP)/maximum_admissible_to_DAP;
00592
00593
00594 vt[i]->score.ADAPnorm=1.0-(vt[i]->score.ADAP/(M_PI));
00595
00596
00597 vt[i]->score.DLOnorm=(vt[i]->score.DLO)/maximum_admissible_to_DLO;
00598 }
00599
00600
00601 for (size_t i=0; i< vt.size(); ++i)
00602 {
00603 compute_global_traj_score(vt[i]);
00604 }
00605
00606 compute_chosen_traj();
00607
00608 return SUCCESS;
00609 }
00610
00618 t_func_output c_manage_trajectory::compute_DAP(c_trajectoryPtr& trajectory,t_desired_coordinates& AP)
00619 {
00620 trajectory->score.DAP=10e6;
00621
00622 trajectory->closest_node=-1;
00623
00624 for(size_t i = 0; i < trajectory->x.size(); ++i)
00625 {
00626 double DAP_prev = sqrt( pow(trajectory->x[i]-AP.x,2)+ pow(trajectory->y[i]-AP.y,2));
00627
00628 if(DAP_prev < trajectory->score.DAP)
00629 {
00630 trajectory->score.DAP = DAP_prev;
00631 trajectory->closest_node = i;
00632 }
00633 }
00634
00635 if(trajectory->closest_node!=-1)
00636 {
00637 trajectory->score.ADAP=compute_ADAP(trajectory,AP,trajectory->closest_node);
00638 return SUCCESS;
00639 }
00640 else
00641 {
00642 return FAILURE;
00643 }
00644 }
00645
00654 double c_manage_trajectory::compute_ADAP(c_trajectoryPtr& trajectory,t_desired_coordinates& AP, int i)
00655 {
00656 double adap=abs(trajectory->theta[i]-AP.theta);
00657 if (adap>M_PI)
00658 adap=2*M_PI-adap;
00659 return adap;
00660 }
00661
00673 void c_manage_trajectory::draw_on_node(c_trajectoryPtr& trajectory, std::vector<visualization_msgs::Marker>* marker_vec, int* marker_count, double z_high, double value, double normalized_value, string string_init)
00674 {
00675
00676 visualization_msgs::Marker marker;
00677 std_msgs::ColorRGBA color;
00678
00679
00680
00681
00682
00683 marker.header.frame_id = "/parking_frame";
00684 marker.header.stamp = ros::Time::now();
00685 marker.ns = "info";
00686 marker.action = visualization_msgs::Marker::ADD;
00687 marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00688 marker.pose.orientation.x = 0.0;
00689 marker.pose.orientation.y = 0.0;
00690 marker.pose.orientation.z = 0.0;
00691 marker.pose.orientation.w = 1.0;
00692 marker.scale.z = 0.15;
00693 marker.color.r = 0.80;
00694 marker.color.g = 0.50;
00695 marker.color.b = 0.20;
00696 marker.color.a = 1.0;
00697 marker.id = (*marker_count)++;
00698 marker.pose.position.x = trajectory->x[trajectory->x.size()-1]+0.25+z_high;
00699 marker.pose.position.y = trajectory->y[trajectory->x.size()-1];
00700 marker.pose.position.z = z_high;
00701
00702 marker.text = string_init + str( boost::format("%.2f") % value) + " ("+str( boost::format("%.2f") % normalized_value)+")";
00703 marker_vec->push_back(marker);
00704 }