/home/joel/ros_workspace/lar3/planning/trajectory_planner/src/c_trajectory.cpp

Go to the documentation of this file.

#include <trajectory_planner/c_trajectory.h>
t_func_output c_trajectory::generate(vector<double> alpha_in, vector<double> arc_in, vector<double> speed_in, t_vehicle_description& vd)
{       
        // ________________________________
        //|                                |
        //|       Test input vectors       |
        //|________________________________|
        if((alpha_in.size()==arc_in.size()) && (arc_in.size()==speed_in.size()))
        {

                //Copy all the values to internal variables
                double arc_sum=0;
                for(size_t i=0; i<alpha_in.size(); ++i)
                {
                        if(abs(alpha_in[i])<=M_PI/2)
                        {       
                                alpha.push_back(alpha_in[i]);
                                arc.push_back(arc_in[i]);
                                speed.push_back(speed_in[i]);
                                arc_sum=arc_sum+arc[i];
                                total_arc.push_back(arc_sum);
                        }
                        else
                        {
                                ROS_ERROR("Detected alpha bigger than pi/2");
                                return FAILURE;
                        }
                }

                // ________________________________
                //|                                |
                //|   Compute local node coords    |
                //|________________________________|
                for(size_t i=0; i<alpha.size(); ++i)
                {
                        lx.push_back((D/tan(alpha[i]))*sin(arc[i]/(D/tan(alpha[i]))));
                        ly.push_back((D/tan(alpha[i]))-(D/tan(alpha[i]))*cos(arc[i]/(D/tan(alpha[i]))));                
                }

                for(size_t i=0; i<alpha.size(); ++i)
                {
                        // Local coords of IRC
                        double lcx = 0;
                        double lcy = D/tan(alpha[i]);

                        if (alpha[i]>0)
                        {
                                ltheta.push_back (M_PI/2 + atan2(ly[i]- lcy, lx[i]- lcx));
                        }
                        else
                        {
                                ltheta.push_back (-M_PI/2 + atan2(ly[i]- lcy, lx[i]- lcx));
                        }
                }

                // ________________________________
                //|                                |
                //|     Define transformations     |
                //|________________________________|
                // Compute transformation
                compute_transformation(); 
                
                
                //   ___________________________________
                //   |                                 |
                //   | Compute vhc lines for each node |
                //   |_________________________________| 
                for(size_t i=0; i<alpha.size(); ++i)
                {
                        std::vector<t_lines> v;
                        t_lines line1;
        
                        line1.x[0]=x[i]-vd.lenght_back*cos(theta[i])-(vd.width/2.0)*sin(theta[i]);
                        line1.y[0]=y[i]-vd.lenght_back*sin(theta[i])+(vd.width/2.0)*cos(theta[i]);
                        line1.x[1]=x[i]+vd.lenght_front*cos(theta[i])-(vd.width/2.0)*sin(theta[i]);
                        line1.y[1]=y[i]+vd.lenght_front*sin(theta[i])+(vd.width/2.0)*cos(theta[i]);
                        v.push_back(line1);     

                        line1.x[0]=x[i]+vd.lenght_front*cos(theta[i])-(vd.width/2.0)*sin(theta[i]);;
                        line1.y[0]=y[i]+vd.lenght_front*sin(theta[i])+(vd.width/2.0)*cos(theta[i]);
                        line1.x[1]=x[i]+vd.lenght_front*cos(theta[i])+(vd.width/2.0)*sin(theta[i]);;
                        line1.y[1]=y[i]+vd.lenght_front*sin(theta[i])-(vd.width/2.0)*cos(theta[i]);
                        v.push_back(line1);     
                        
                        line1.x[0]=x[i]+vd.lenght_front*cos(theta[i])+(vd.width/2.0)*sin(theta[i]);;
                        line1.y[0]=y[i]+vd.lenght_front*sin(theta[i])-(vd.width/2.0)*cos(theta[i]);
                        line1.x[1]=x[i]-vd.lenght_back*cos(theta[i])+(vd.width/2.0)*sin(theta[i]);;
                        line1.y[1]=y[i]-vd.lenght_back*sin(theta[i])-(vd.width/2.0)*cos(theta[i]);
                        v.push_back(line1);     
                        
                        line1.x[0]=x[i]-vd.lenght_back*cos(theta[i])+(vd.width/2.0)*sin(theta[i]);;
                        line1.y[0]=y[i]-vd.lenght_back*sin(theta[i])-(vd.width/2.0)*cos(theta[i]);
                        line1.x[1]=x[i]-vd.lenght_back*cos(theta[i])-(vd.width/2.0)*sin(theta[i]);;
                        line1.y[1]=y[i]-vd.lenght_back*sin(theta[i])+(vd.width/2.0)*cos(theta[i]);
                        v.push_back(line1);     
                
                        v_lines.push_back(v);
                }

                return SUCCESS;
        }
        else
        {
                ROS_ERROR("Input vector dimensions mismatch");
                return FAILURE;
        }
}


t_func_output c_trajectory::compute_transformation()
{
        //compute vector of local transforms. Transform at position i transforms from i-1 to i
        pcl::PointCloud<pcl::PointXYZ> ponto_teste, ponto_result;
        ltrans.clear();
        theta.push_back(ltheta[0]);
        for(size_t i=0; i<alpha.size(); ++i)
        {               
                tf::Transform transform;
                transform.setOrigin(tf::Vector3(lx[i], ly[i], 0));
                transform.setRotation(tf::createQuaternionFromRPY(0, 0, ltheta[i]));
                ltrans.push_back(transform);
                
                pcl::PointXYZ pt1(0,0,0);
                ponto_teste.points.push_back(pt1);
                
                for(int j=i;j>=0;--j)
                {
                        pcl_ros::transformPointCloud(ponto_teste, ponto_result, ltrans[j]);
                        
                        if(j==0)
                        {
                                x.push_back(ponto_result.points[0].x);
                                y.push_back(ponto_result.points[0].y);
                                ponto_result.clear();
                                ponto_teste.clear();
                        }       
                        else
                        {
                                ponto_teste.clear();
                                ponto_teste.points.push_back(ponto_result.points[0]);
                                ponto_result.clear();
                        }
                }
                
                if(i!=0)
                {
                        double theta_provi=theta[i-1]+ltheta[i];
                        while(theta_provi<-M_PI)
                        {
                                theta_provi=2*M_PI-abs(theta_provi);
                        }
                        while(theta_provi>M_PI)
                        {
                                theta_provi=theta_provi-2*M_PI;
                        }
                        theta.push_back(theta_provi);
                }

        }
        return SUCCESS;
}


void c_trajectory::create_markers(std::vector<visualization_msgs::Marker>* marker_vec, int* marker_count, int num_traj)
{
        //Create a marker
        visualization_msgs::Marker marker,marker2;
        geometry_msgs::Point p,pp;
        std_msgs::ColorRGBA color;
        
        
        // ________________________________
        //|                                |
        //|           Line List            |
        //|________________________________|
        //Line marker to trajectory
        marker.header.frame_id = "/parking_frame";
        marker.header.stamp = ros::Time::now();
        marker.ns = "lines";
        marker.id = num_traj;
        marker.action = visualization_msgs::Marker::ADD;
        marker.type = visualization_msgs::Marker::LINE_LIST;
        marker.scale.x = 0.01;
        marker.color.r = 0.0;
        marker.color.g = 0.7;
        marker.color.b = 0.0;
        marker.color.a = 1.0;
        int first_step=1;
        for(size_t i=0; i<alpha.size(); ++i)
        {
                if(first_step==1)
                {
                        p.x = 0;
                        p.y = 0;
                        p.z = 0;
                        marker.points.push_back(p);
                        p.x = x[i];
                        p.y = y[i];
                        p.z = 0;
                        marker.points.push_back(p);
                        first_step=0;
                }
                else
                {
                        p.x = x[i-1];
                        p.y = y[i-1];
                        p.z = 0;
                        marker.points.push_back(p);
                        p.x = x[i];
                        p.y = y[i];
                        p.z = 0;
                        marker.points.push_back(p);
                }               
        }
        marker_vec->push_back(marker);
        
        // ________________________________
        //|                                |
        //|          Cube Marker           |
        //|________________________________|
        // Cube marker to t nodes
        marker.header.frame_id = "/parking_frame";
        marker.header.stamp = ros::Time::now();
        marker.ns = "nodes";
        marker.action = visualization_msgs::Marker::ADD;
        marker.type = visualization_msgs::Marker::CUBE;
        marker.pose.orientation.x = 0.0;
        marker.pose.orientation.y = 0.0;
        marker.pose.orientation.z = 0.0;
        marker.pose.orientation.w = 1.0;
        marker.scale.x = 0.03;
        marker.scale.y = 0.03;
        marker.scale.z = 0.03;
        marker.color.r = 0.0;
        marker.color.g = 0.0;
        marker.color.b = 1.0;
        marker.color.a = 1.0;
        for(size_t i=0; i<alpha.size(); ++i)
        {
                marker.id = (*marker_count)++;
                marker.pose.position.x = x[i];
                marker.pose.position.y = y[i];
                marker.pose.position.z = 0;
                marker_vec->push_back(marker);
        }

        // ________________________________
        //|                                |
        //|           text nodes           |
        //|________________________________|
        // Points marker to t nodes
        marker.header.frame_id = "/parking_frame";
        marker.header.stamp = ros::Time::now();
        marker.ns = "node_number";
        marker.action = visualization_msgs::Marker::ADD;
        marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
        marker.pose.orientation.x = 0.0;
        marker.pose.orientation.y = 0.0;
        marker.pose.orientation.z = 0.0;
        marker.pose.orientation.w = 1.0;
        marker.scale.z = 0.15;
        marker.color.r = 0.0;
        marker.color.g = 0.0;
        marker.color.b = 0.2;
        marker.color.a = 1.0;
        for(size_t i=0; i<alpha.size(); ++i)
        {
                marker.id = (*marker_count)++;
                marker.pose.position.x = x[i];
                marker.pose.position.y = y[i];
                marker.pose.position.z = 0.075;
                char str[1024];
                sprintf(str,"%ld",i);
                marker.text = str;
                marker_vec->push_back(marker);  
        }
        
        // ________________________________
        //|                                |
        //|         text trajectory        |
        //|________________________________|
        // Points marker to t nodes
        marker.header.frame_id = "/parking_frame";
        marker.header.stamp = ros::Time::now();
        marker.ns = "trajectory_number";
        marker.action = visualization_msgs::Marker::ADD;
        marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
        marker.pose.orientation.x = 0.0;
        marker.pose.orientation.y = 0.0;
        marker.pose.orientation.z = 0.0;
        marker.pose.orientation.w = 1.0;
        marker.scale.z = 0.2;
        marker.color.r = 0.0;
        marker.color.g = 0.0;
        marker.color.b = 1.0;
        marker.color.a = 1.0;
        marker.id = (*marker_count)++;
        marker.pose.position.x = x[x.size()-1];
        marker.pose.position.y = y[y.size()-1];
        marker.pose.position.z = 0.2;
        marker.text = "Traj " + str( boost::format("%d") % (num_traj));
        marker_vec->push_back(marker);  
        
        // ________________________________
        //|                                |
        //|             Arrow              |
        //|________________________________|
        //Line marker to trajectory
        marker.header.frame_id = "/parking_frame";
        marker.header.stamp = ros::Time::now();
        marker.ns = "Orientation arrow";
        marker.action = visualization_msgs::Marker::ADD;
        marker.type = visualization_msgs::Marker::ARROW;
        marker.color.r = 1.0;
        marker.color.g = 0.0;
        marker.color.b = 0.0;
        marker.color.a = 1.0;
        marker.scale.x=0.3;
        marker.scale.y=0.3;
        marker.scale.z=0.3;
        for(size_t i=0; i<alpha.size(); ++i)
        {
                marker.id = (*marker_count)++;
                marker.pose.position.x = x[i];
                marker.pose.position.y = y[i];
                marker.pose.position.z = 0;
                marker.pose.orientation.z = sin(theta[i]/2);
                marker.pose.orientation.w = cos(theta[i]/2);
                marker_vec->push_back(marker);          
        }
        
        // ________________________________
        //|                                |
        //|     Rectangle (car shadow)     |
        //|________________________________|
        // Represents the form of the car in each node
        marker.header.frame_id = "/parking_frame";
        marker.header.stamp = ros::Time::now();
        marker.ns = "car shadow";
        marker.action = visualization_msgs::Marker::ADD;
        marker.type = visualization_msgs::Marker::CUBE;
        marker.scale.x = 0.8;
        marker.scale.y = 0.42;
        marker.scale.z = 0.05;
        marker.color.r = 0.25;
        marker.color.g = 0.41;
        marker.color.b = 0.88;
        marker.color.a = 0.1;
        for(size_t i=0; i<alpha.size(); ++i)
        {
                marker.id = (*marker_count)++;
                marker.pose.position.x = x[i]+0.2*cos(theta[i]);
                marker.pose.position.y = y[i]+0.2*sin(theta[i]);
                marker.pose.position.z = 0;
                marker.pose.orientation.z = sin(theta[i]/2);
                marker.pose.orientation.w = cos(theta[i]/2);
                marker_vec->push_back(marker);
        }
        
        
        // ________________________________
        //|                                |
        //|          Car contour           |
        //|________________________________|
        //Line marker to car contour
        marker2.header.frame_id = "/parking_frame";
        marker2.header.stamp = ros::Time::now();
        marker2.ns = "car contour";
        marker2.id = (*marker_count)++;
        marker2.action = visualization_msgs::Marker::ADD;
        marker2.type = visualization_msgs::Marker::LINE_LIST;
        marker2.scale.x = 0.01;
        marker2.color.r = 0.5;
        marker2.color.g = 0.5;
        marker2.color.b = 1.0;
        marker2.color.a = 0.5;
        for(size_t i=0; i<alpha.size(); ++i)
        {
                for(size_t l=0;l<v_lines[i].size();l++)
                {
                        pp.x = v_lines[i][l].x[0];
                        pp.y = v_lines[i][l].y[0];
                        pp.z = 0;
                        marker2.points.push_back(pp);
                                
                        pp.x = v_lines[i][l].x[1];
                        pp.y = v_lines[i][l].y[1];
                        pp.z = 0;
                        marker2.points.push_back(pp);
                }
                marker_vec->push_back(marker2);
        }
}