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_trajectory.h>
00035 t_func_output c_trajectory::generate(vector<double> alpha_in, vector<double> arc_in, vector<double> speed_in, t_vehicle_description& vd)
00036 {
00037
00038
00039
00040
00041 if((alpha_in.size()==arc_in.size()) && (arc_in.size()==speed_in.size()))
00042 {
00043
00044
00045 double arc_sum=0;
00046 for(size_t i=0; i<alpha_in.size(); ++i)
00047 {
00048 if(abs(alpha_in[i])<=M_PI/2)
00049 {
00050 alpha.push_back(alpha_in[i]);
00051 arc.push_back(arc_in[i]);
00052 speed.push_back(speed_in[i]);
00053 arc_sum=arc_sum+arc[i];
00054 total_arc.push_back(arc_sum);
00055 }
00056 else
00057 {
00058 ROS_ERROR("Detected alpha bigger than pi/2");
00059 return FAILURE;
00060 }
00061 }
00062
00063
00064
00065
00066
00067 for(size_t i=0; i<alpha.size(); ++i)
00068 {
00069 lx.push_back((D/tan(alpha[i]))*sin(arc[i]/(D/tan(alpha[i]))));
00070 ly.push_back((D/tan(alpha[i]))-(D/tan(alpha[i]))*cos(arc[i]/(D/tan(alpha[i]))));
00071 }
00072
00073 for(size_t i=0; i<alpha.size(); ++i)
00074 {
00075
00076 double lcx = 0;
00077 double lcy = D/tan(alpha[i]);
00078
00079 if (alpha[i]>0)
00080 {
00081 ltheta.push_back (M_PI/2 + atan2(ly[i]- lcy, lx[i]- lcx));
00082 }
00083 else
00084 {
00085 ltheta.push_back (-M_PI/2 + atan2(ly[i]- lcy, lx[i]- lcx));
00086 }
00087 }
00088
00089
00090
00091
00092
00093
00094 compute_transformation();
00095
00096
00097
00098
00099
00100
00101 for(size_t i=0; i<alpha.size(); ++i)
00102 {
00103 std::vector<t_lines> v;
00104 t_lines line1;
00105
00106 line1.x[0]=x[i]-vd.lenght_back*cos(theta[i])-(vd.width/2.0)*sin(theta[i]);
00107 line1.y[0]=y[i]-vd.lenght_back*sin(theta[i])+(vd.width/2.0)*cos(theta[i]);
00108 line1.x[1]=x[i]+vd.lenght_front*cos(theta[i])-(vd.width/2.0)*sin(theta[i]);
00109 line1.y[1]=y[i]+vd.lenght_front*sin(theta[i])+(vd.width/2.0)*cos(theta[i]);
00110 v.push_back(line1);
00111
00112 line1.x[0]=x[i]+vd.lenght_front*cos(theta[i])-(vd.width/2.0)*sin(theta[i]);;
00113 line1.y[0]=y[i]+vd.lenght_front*sin(theta[i])+(vd.width/2.0)*cos(theta[i]);
00114 line1.x[1]=x[i]+vd.lenght_front*cos(theta[i])+(vd.width/2.0)*sin(theta[i]);;
00115 line1.y[1]=y[i]+vd.lenght_front*sin(theta[i])-(vd.width/2.0)*cos(theta[i]);
00116 v.push_back(line1);
00117
00118 line1.x[0]=x[i]+vd.lenght_front*cos(theta[i])+(vd.width/2.0)*sin(theta[i]);;
00119 line1.y[0]=y[i]+vd.lenght_front*sin(theta[i])-(vd.width/2.0)*cos(theta[i]);
00120 line1.x[1]=x[i]-vd.lenght_back*cos(theta[i])+(vd.width/2.0)*sin(theta[i]);;
00121 line1.y[1]=y[i]-vd.lenght_back*sin(theta[i])-(vd.width/2.0)*cos(theta[i]);
00122 v.push_back(line1);
00123
00124 line1.x[0]=x[i]-vd.lenght_back*cos(theta[i])+(vd.width/2.0)*sin(theta[i]);;
00125 line1.y[0]=y[i]-vd.lenght_back*sin(theta[i])-(vd.width/2.0)*cos(theta[i]);
00126 line1.x[1]=x[i]-vd.lenght_back*cos(theta[i])-(vd.width/2.0)*sin(theta[i]);;
00127 line1.y[1]=y[i]-vd.lenght_back*sin(theta[i])+(vd.width/2.0)*cos(theta[i]);
00128 v.push_back(line1);
00129
00130 v_lines.push_back(v);
00131 }
00132
00133 return SUCCESS;
00134 }
00135 else
00136 {
00137 ROS_ERROR("Input vector dimensions mismatch");
00138 return FAILURE;
00139 }
00140 }
00141
00142
00148 t_func_output c_trajectory::compute_transformation()
00149 {
00150
00151 pcl::PointCloud<pcl::PointXYZ> ponto_teste, ponto_result;
00152 ltrans.clear();
00153 theta.push_back(ltheta[0]);
00154 for(size_t i=0; i<alpha.size(); ++i)
00155 {
00156 tf::Transform transform;
00157 transform.setOrigin(tf::Vector3(lx[i], ly[i], 0));
00158 transform.setRotation(tf::createQuaternionFromRPY(0, 0, ltheta[i]));
00159 ltrans.push_back(transform);
00160
00161 pcl::PointXYZ pt1(0,0,0);
00162 ponto_teste.points.push_back(pt1);
00163
00164 for(int j=i;j>=0;--j)
00165 {
00166 pcl_ros::transformPointCloud(ponto_teste, ponto_result, ltrans[j]);
00167
00168 if(j==0)
00169 {
00170 x.push_back(ponto_result.points[0].x);
00171 y.push_back(ponto_result.points[0].y);
00172 ponto_result.clear();
00173 ponto_teste.clear();
00174 }
00175 else
00176 {
00177 ponto_teste.clear();
00178 ponto_teste.points.push_back(ponto_result.points[0]);
00179 ponto_result.clear();
00180 }
00181 }
00182
00183 if(i!=0)
00184 {
00185 double theta_provi=theta[i-1]+ltheta[i];
00186 while(theta_provi<-M_PI)
00187 {
00188 theta_provi=2*M_PI-abs(theta_provi);
00189 }
00190 while(theta_provi>M_PI)
00191 {
00192 theta_provi=theta_provi-2*M_PI;
00193 }
00194 theta.push_back(theta_provi);
00195 }
00196
00197 }
00198 return SUCCESS;
00199 }
00200
00201
00209 void c_trajectory::create_markers(std::vector<visualization_msgs::Marker>* marker_vec, int* marker_count, int num_traj)
00210 {
00211
00212 visualization_msgs::Marker marker,marker2;
00213 geometry_msgs::Point p,pp;
00214 std_msgs::ColorRGBA color;
00215
00216
00217
00218
00219
00220
00221
00222 marker.header.frame_id = "/parking_frame";
00223 marker.header.stamp = ros::Time::now();
00224 marker.ns = "lines";
00225 marker.id = num_traj;
00226 marker.action = visualization_msgs::Marker::ADD;
00227 marker.type = visualization_msgs::Marker::LINE_LIST;
00228 marker.scale.x = 0.01;
00229 marker.color.r = 0.0;
00230 marker.color.g = 0.7;
00231 marker.color.b = 0.0;
00232 marker.color.a = 1.0;
00233 int first_step=1;
00234 for(size_t i=0; i<alpha.size(); ++i)
00235 {
00236 if(first_step==1)
00237 {
00238 p.x = 0;
00239 p.y = 0;
00240 p.z = 0;
00241 marker.points.push_back(p);
00242 p.x = x[i];
00243 p.y = y[i];
00244 p.z = 0;
00245 marker.points.push_back(p);
00246 first_step=0;
00247 }
00248 else
00249 {
00250 p.x = x[i-1];
00251 p.y = y[i-1];
00252 p.z = 0;
00253 marker.points.push_back(p);
00254 p.x = x[i];
00255 p.y = y[i];
00256 p.z = 0;
00257 marker.points.push_back(p);
00258 }
00259 }
00260 marker_vec->push_back(marker);
00261
00262
00263
00264
00265
00266
00267 marker.header.frame_id = "/parking_frame";
00268 marker.header.stamp = ros::Time::now();
00269 marker.ns = "nodes";
00270 marker.action = visualization_msgs::Marker::ADD;
00271 marker.type = visualization_msgs::Marker::CUBE;
00272 marker.pose.orientation.x = 0.0;
00273 marker.pose.orientation.y = 0.0;
00274 marker.pose.orientation.z = 0.0;
00275 marker.pose.orientation.w = 1.0;
00276 marker.scale.x = 0.03;
00277 marker.scale.y = 0.03;
00278 marker.scale.z = 0.03;
00279 marker.color.r = 0.0;
00280 marker.color.g = 0.0;
00281 marker.color.b = 1.0;
00282 marker.color.a = 1.0;
00283 for(size_t i=0; i<alpha.size(); ++i)
00284 {
00285 marker.id = (*marker_count)++;
00286 marker.pose.position.x = x[i];
00287 marker.pose.position.y = y[i];
00288 marker.pose.position.z = 0;
00289 marker_vec->push_back(marker);
00290 }
00291
00292
00293
00294
00295
00296
00297 marker.header.frame_id = "/parking_frame";
00298 marker.header.stamp = ros::Time::now();
00299 marker.ns = "node_number";
00300 marker.action = visualization_msgs::Marker::ADD;
00301 marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00302 marker.pose.orientation.x = 0.0;
00303 marker.pose.orientation.y = 0.0;
00304 marker.pose.orientation.z = 0.0;
00305 marker.pose.orientation.w = 1.0;
00306 marker.scale.z = 0.15;
00307 marker.color.r = 0.0;
00308 marker.color.g = 0.0;
00309 marker.color.b = 0.2;
00310 marker.color.a = 1.0;
00311 for(size_t i=0; i<alpha.size(); ++i)
00312 {
00313 marker.id = (*marker_count)++;
00314 marker.pose.position.x = x[i];
00315 marker.pose.position.y = y[i];
00316 marker.pose.position.z = 0.075;
00317 char str[1024];
00318 sprintf(str,"%ld",i);
00319 marker.text = str;
00320 marker_vec->push_back(marker);
00321 }
00322
00323
00324
00325
00326
00327
00328 marker.header.frame_id = "/parking_frame";
00329 marker.header.stamp = ros::Time::now();
00330 marker.ns = "trajectory_number";
00331 marker.action = visualization_msgs::Marker::ADD;
00332 marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00333 marker.pose.orientation.x = 0.0;
00334 marker.pose.orientation.y = 0.0;
00335 marker.pose.orientation.z = 0.0;
00336 marker.pose.orientation.w = 1.0;
00337 marker.scale.z = 0.2;
00338 marker.color.r = 0.0;
00339 marker.color.g = 0.0;
00340 marker.color.b = 1.0;
00341 marker.color.a = 1.0;
00342 marker.id = (*marker_count)++;
00343 marker.pose.position.x = x[x.size()-1];
00344 marker.pose.position.y = y[y.size()-1];
00345 marker.pose.position.z = 0.2;
00346 marker.text = "Traj " + str( boost::format("%d") % (num_traj));
00347 marker_vec->push_back(marker);
00348
00349
00350
00351
00352
00353
00354 marker.header.frame_id = "/parking_frame";
00355 marker.header.stamp = ros::Time::now();
00356 marker.ns = "Orientation arrow";
00357 marker.action = visualization_msgs::Marker::ADD;
00358 marker.type = visualization_msgs::Marker::ARROW;
00359 marker.color.r = 1.0;
00360 marker.color.g = 0.0;
00361 marker.color.b = 0.0;
00362 marker.color.a = 1.0;
00363 marker.scale.x=0.3;
00364 marker.scale.y=0.3;
00365 marker.scale.z=0.3;
00366 for(size_t i=0; i<alpha.size(); ++i)
00367 {
00368 marker.id = (*marker_count)++;
00369 marker.pose.position.x = x[i];
00370 marker.pose.position.y = y[i];
00371 marker.pose.position.z = 0;
00372 marker.pose.orientation.z = sin(theta[i]/2);
00373 marker.pose.orientation.w = cos(theta[i]/2);
00374 marker_vec->push_back(marker);
00375 }
00376
00377
00378
00379
00380
00381
00382 marker.header.frame_id = "/parking_frame";
00383 marker.header.stamp = ros::Time::now();
00384 marker.ns = "car shadow";
00385 marker.action = visualization_msgs::Marker::ADD;
00386 marker.type = visualization_msgs::Marker::CUBE;
00387 marker.scale.x = 0.8;
00388 marker.scale.y = 0.42;
00389 marker.scale.z = 0.05;
00390 marker.color.r = 0.25;
00391 marker.color.g = 0.41;
00392 marker.color.b = 0.88;
00393 marker.color.a = 0.1;
00394 for(size_t i=0; i<alpha.size(); ++i)
00395 {
00396 marker.id = (*marker_count)++;
00397 marker.pose.position.x = x[i]+0.2*cos(theta[i]);
00398 marker.pose.position.y = y[i]+0.2*sin(theta[i]);
00399 marker.pose.position.z = 0;
00400 marker.pose.orientation.z = sin(theta[i]/2);
00401 marker.pose.orientation.w = cos(theta[i]/2);
00402 marker_vec->push_back(marker);
00403 }
00404
00405
00406
00407
00408
00409
00410
00411 marker2.header.frame_id = "/parking_frame";
00412 marker2.header.stamp = ros::Time::now();
00413 marker2.ns = "car contour";
00414 marker2.id = (*marker_count)++;
00415 marker2.action = visualization_msgs::Marker::ADD;
00416 marker2.type = visualization_msgs::Marker::LINE_LIST;
00417 marker2.scale.x = 0.01;
00418 marker2.color.r = 0.5;
00419 marker2.color.g = 0.5;
00420 marker2.color.b = 1.0;
00421 marker2.color.a = 0.5;
00422 for(size_t i=0; i<alpha.size(); ++i)
00423 {
00424 for(size_t l=0;l<v_lines[i].size();l++)
00425 {
00426 pp.x = v_lines[i][l].x[0];
00427 pp.y = v_lines[i][l].y[0];
00428 pp.z = 0;
00429 marker2.points.push_back(pp);
00430
00431 pp.x = v_lines[i][l].x[1];
00432 pp.y = v_lines[i][l].y[1];
00433 pp.z = 0;
00434 marker2.points.push_back(pp);
00435 }
00436 marker_vec->push_back(marker2);
00437 }
00438 }
