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

Go to the documentation of this file.

#include <trajectory_planner/c_manage_trajectory.h>

t_func_output c_manage_trajectory::compute_DLO(c_trajectoryPtr& trajectory, std::vector<t_obstacle>& vo)
{
        //delete all previous computed collision pts
        trajectory->collision_pts.erase(trajectory->collision_pts.begin(), trajectory->collision_pts.end());

        if (trajectory->closest_node<0 || trajectory->closest_node >=(int)trajectory->x.size())
        {
                ROS_ERROR("Error on node");
                return FAILURE;
        }
        //cycle all nodes until the closest node
        
        trajectory->score.DLO = 10.0;   // Equal to maximum_admissible_to_DLO
        trajectory->score.FS = 1;
        for (int n=0; n<= trajectory->closest_node; ++n)
        {
                if (trajectory->v_lines.size()!=trajectory->x.size())
                {
                        ROS_ERROR("Node lines and number of nodes not equal");
                }
        
                //cycle all vehicle lines
                for (size_t l=0; l< trajectory->v_lines[n].size(); ++l)
                {
                        double Ax = trajectory->v_lines[n][l].x[0];     
                        double Ay = trajectory->v_lines[n][l].y[0];     
                        double Bx = trajectory->v_lines[n][l].x[1];     
                        double By = trajectory->v_lines[n][l].y[1];     

                        //cycle all obstacles           
                        for (size_t o=0; o< vo.size(); ++o)
                        {
                                //cycle all lines inside each obstacle
                                for (size_t lo=1; lo< vo[o].x.size(); ++lo)
                                {
                                        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));
                                        if(trajectory->score.DLO > DLOprev)
                                        {
                                                trajectory->score.DLO=DLOprev;
                                        }
                                        double Cx = vo[o].x[lo-1];
                                        double Cy = vo[o].y[lo-1];
                                        double Dx = vo[o].x[lo];
                                        double Dy = vo[o].y[lo];
                                        double X,Y;

                                        int ret = lineSegmentIntersection( Ax, Ay,
                                                        Bx,  By,
                                                        Cx,  Cy,
                                                        Dx,  Dy,
                                                        &X,  &Y); 
                                        
                                        if (ret==DO_INTERSECT)
                                        {
                                                t_point p;
                                                p.x=X;
                                                p.y=Y;
                                                trajectory->collision_pts.push_back(p);
                                                trajectory->score.FS*=0;
                                        }
                                }
                        }
                }
        }
        return SUCCESS;
}

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) 
{
        double  distAB, theCos, theSin, newX, ABpos ;

        //  Fail if either line segment is zero-length.
        if (((Ax==Bx) && (Ay==By)) || ((Cx==Dx) && (Cy==Dy))) return DONT_INTERSECT;

        //  Fail if the segments share an end-point.
        if (((Ax==Cx) && (Ay==Cy)) || ((Bx==Cx) && (By==Cy))
                        ||  ((Ax==Dx) && (Ay==Dy)) || ((Bx==Dx) && (By==Dy))) {
                return DONT_INTERSECT; }

        //  (1) Translate the system so that point A is on the origin.
        Bx-=Ax; By-=Ay;
        Cx-=Ax; Cy-=Ay;
        Dx-=Ax; Dy-=Ay;

        //  Discover the length of segment A-B.
        distAB=sqrt(Bx*Bx+By*By);

        //  (2) Rotate the system so that point B is on the positive X axis.
        theCos=Bx/distAB;
        theSin=By/distAB;
        newX=Cx*theCos+Cy*theSin;
        Cy  =Cy*theCos-Cx*theSin; Cx=newX;
        newX=Dx*theCos+Dy*theSin;
        Dy  =Dy*theCos-Dx*theSin; Dx=newX;

        //  Fail if segment C-D doesn't cross line A-B.
        if ((Cy<0. && Dy<0.) || (Cy>=0. && Dy>=0.)) return DONT_INTERSECT;

        //  (3) Discover the position of the intersection point along line A-B.
        ABpos=Dx+(Cx-Dx)*Dy/(Dy-Cy);

        //  Fail if segment C-D crosses line A-B outside of segment A-B.
        if (ABpos<0. || ABpos>distAB) return DONT_INTERSECT;

        //  (4) Apply the discovered position to line A-B in the original coordinate system.
        *X=Ax+ABpos*theCos;
        *Y=Ay+ABpos*theSin;

        //  Success.
        return DO_INTERSECT; 
}

t_func_output c_manage_trajectory::set_obstacles(mtt::TargetList& msg)
{
        vo.erase(vo.begin(), vo.end()); 
        for (size_t i=0; i<msg.obstacle_lines.size(); ++i)
        {
                t_obstacle o;

                pcl::PointCloud<pcl::PointXYZ> pc;
                pcl::fromROSMsg(msg.obstacle_lines[i], pc);

                for (size_t j=0; j<pc.points.size(); ++j)
                {
                        o.x.push_back(pc.points[j].x);
                        o.y.push_back(pc.points[j].y);
                }

                vo.push_back(o);
        }
        return SUCCESS;
}

t_func_output c_manage_trajectory::set_vehicle_description(double w, double lb, double lf, double ht, double hb)
{
        vehicle_description.width = w;
        vehicle_description.lenght_back = lb;
        vehicle_description.lenght_front = lf;
        vehicle_description.height_top = ht;
        vehicle_description.height_bottom = hb;
        return SUCCESS;
}

t_func_output c_manage_trajectory::get_traj_info_msg_from_chosen(trajectory_planner::traj_info* info)
{
        info->arc.erase(info->arc.begin(), info->arc.end());
        info->total_arc.erase(info->total_arc.begin(), info->total_arc.end());
        info->alpha.erase(info->alpha.begin(), info->alpha.end());
        info->speed.erase(info->speed.begin(), info->speed.end());
        for(int j=0;j<=(int)vt[chosen_traj.index]->closest_node;++j)
        {
                info->arc.push_back(vt[chosen_traj.index]->arc[j]);
                info->total_arc.push_back(vt[chosen_traj.index]->total_arc[j]);
                info->alpha.push_back(vt[chosen_traj.index]->alpha[j]);
        }
        set_speed_vector(info);
        
        cout<<"ARC SIZE (when constructing) "<<info->arc.size()<<endl;
        cout<<"TOTAL ARC SIZE (when constructing) "<<info->total_arc.size()<<endl;
        cout<<"ALPHA SIZE (when constructing) "<<info->alpha.size()<<endl;
        cout<<"SPEED SIZE (when constructing) "<<info->speed.size()<<endl;
        return SUCCESS;
}


t_func_output c_manage_trajectory::set_speed_vector(trajectory_planner::traj_info* info)
{
        for(int i=0;i<=(int)vt[chosen_traj.index]->closest_node;++i)
        {
                if(i < ((int)vt[chosen_traj.index]->arc.size() - 1))
                {
                        if((info->arc[i])*(info->arc[i+1])<0.0)
                        {
                                info->speed.push_back((info->arc[i]/fabs(info->arc[i]))*_SPEED_SAFFETY_); // This is the speed set to reverse/forward or forward/reverse
                        }
                        else
                        {
                                info->speed.push_back((info->arc[i]/fabs(info->arc[i]))*_SPEED_REQUIRED_);
                        }
                }
                else
                {
                        info->speed.push_back((info->arc[i]/fabs(info->arc[i]))*_SPEED_REQUIRED_);
                }
        }
        return SUCCESS;
}


int c_manage_trajectory::set_chosen_traj(int n)
{
        chosen_traj.index=n;
        return 1;
}

t_func_output c_manage_trajectory::set_attractor_point(double x, double y, double theta)
{
AP.x = x; AP.y = y; AP.theta=theta;     
return SUCCESS;
}

t_func_output c_manage_trajectory::compute_global_traj_score(c_trajectoryPtr& trajectory)
{
        double W_DAP=0.40;
        double W_ADAP=0.35;
        double W_DLO=0.25;
        trajectory->score.overall_norm = (W_DAP*trajectory->score.DAPnorm + W_ADAP*trajectory->score.ADAPnorm + W_DLO*trajectory->score.DLOnorm)*trajectory->score.FS;
        cout<<"Overallscore= "<<trajectory->score.overall_norm<<endl;
        
        return SUCCESS;
}

t_func_output c_manage_trajectory::set_inter_axis_distance(double val)
{
        inter_axis_distance = val;
        return SUCCESS;         
}       
        
t_func_output c_manage_trajectory::create_new_trajectory(vector<double> alpha_in, vector<double> arc_in, vector<double> speed_in)
{       
        //allocate space for new traj
        c_trajectoryPtr t_ptr(new c_trajectory(inter_axis_distance));
        vt.push_back(t_ptr);

        //set the parameters of the traj
        return vt[vt.size()-1]->generate(alpha_in, arc_in, speed_in, vehicle_description);
}

t_func_output c_manage_trajectory::compute_chosen_traj(void)
{
        double max_val=-1;
        chosen_traj.index=-1;   

        for (int i=0; i< (int)vt.size(); i++)
        {
                if (vt[i]->score.overall_norm>max_val)
                {
                        chosen_traj.index=i;
                        max_val= vt[i]->score.overall_norm;
                }
        }
        
        
        if(chosen_traj.index!=-1)
                return SUCCESS;
        else
                return FAILURE;
}               

t_func_output c_manage_trajectory::create_static_markers(void)
{
        cout<<"ja create static"<<endl;
        static_marker_vec.clear();
        int marker_count=0;
        for (int i=0; i< (int)vt.size(); i++)
        {
                vt[i]->create_markers(&static_marker_vec, &marker_count, i);
        }
        return SUCCESS;
}

t_func_output c_manage_trajectory::compute_vis_marker_array(visualization_msgs::MarkerArray* marker_array)
{
        std::vector<visualization_msgs::Marker> marker_vec;

        ROS_INFO("static marker vec size=%ld",static_marker_vec.size());
        for(size_t i=0;i<static_marker_vec.size();++i)
        {
                marker_vec.push_back(static_marker_vec[i]);
        }



        int marker_count=0;
        for (int i=0; i< (int)vt.size(); ++i)
        {
                draw_on_node(vt[i],&marker_vec,&marker_count,0.15,vt[i]->score.DAP,vt[i]->score.DAPnorm,"DAP ");
                draw_on_node(vt[i],&marker_vec,&marker_count,0.30,vt[i]->score.ADAP,vt[i]->score.ADAPnorm,"ADAP ");
                draw_on_node(vt[i],&marker_vec,&marker_count,0.45,vt[i]->score.DLO,vt[i]->score.DLOnorm,"DLO ");
                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 ");
        }

        // ________________________________
        //|                                |
        //|           Line List            |
        //|________________________________|
        //Line marker to trajectory
        visualization_msgs::Marker marker2;
        geometry_msgs::Point p;
        marker2.header.frame_id = "/parking_frame";
        marker2.header.stamp = ros::Time::now();
        marker2.ns = "Chosen trajectory";
        marker2.id = 0;
        marker2.action = visualization_msgs::Marker::ADD;
        marker2.type = visualization_msgs::Marker::LINE_LIST;
        marker2.scale.x = 0.05;
        marker2.color.r = 0.00;
        marker2.color.g = 1.00;
        marker2.color.b = 0.00;
        marker2.color.a = 1.00;
        int first_step=1;
        for(size_t i=0; i<vt[chosen_traj.index]->x.size(); ++i)
        {
                if(first_step==1)
                {
                        p.x = 0;
                        p.y = 0;
                        p.z = 0.00;
                        marker2.points.push_back(p);
                        p.x = vt[chosen_traj.index]->x[i];
                        p.y = vt[chosen_traj.index]->y[i];
                        p.z = 0.00;
                        marker2.points.push_back(p);
                        first_step=0;
                }
                else
                {
                        p.x = vt[chosen_traj.index]->x[i-1];
                        p.y = vt[chosen_traj.index]->y[i-1];
                        p.z = 0.00;
                        marker2.points.push_back(p);
                        p.x = vt[chosen_traj.index]->x[i];
                        p.y = vt[chosen_traj.index]->y[i];
                        p.z = 0.00;
                        marker2.points.push_back(p);
                }               
        }
        marker_vec.push_back(marker2);
        
        // ________________________________
        //|                                |
        //|     Rectangle (actual)         |
        //|________________________________|
        // Represents the form of the car in each node
        visualization_msgs::Marker marker;
        marker.header.frame_id = "/parking_frame";
        marker.header.stamp = ros::Time::now();
        marker.ns = "car actual node";
        marker.id = 0;
        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.80;
        marker.color.g = 0.50;
        marker.color.b = 0.20;
        marker.color.a = 0.8;
        marker.pose.position.x = vt[chosen_traj.index]->x[current_node]+0.2*cos(vt[chosen_traj.index]->theta[current_node]);
        marker.pose.position.y = vt[chosen_traj.index]->y[current_node]+0.2*sin(vt[chosen_traj.index]->theta[current_node]);
        marker.pose.position.z = 0;
        marker.pose.orientation.z = sin(vt[chosen_traj.index]->theta[current_node]/2);
        marker.pose.orientation.w = cos(vt[chosen_traj.index]->theta[current_node]/2);
        marker_vec.push_back(marker);


        // ________________________________
        //|                                |
        //|        Best node (sphere)      |
        //|________________________________|
        // Represents the form of the car in each node
        visualization_msgs::Marker marker3;
        marker3.header.frame_id = "/parking_frame";
        marker3.header.stamp = ros::Time::now();
        marker3.ns = "Closest Node";
        marker3.action = visualization_msgs::Marker::ADD;
        marker3.type = visualization_msgs::Marker::SPHERE;
        marker3.scale.x = 0.2;
        marker3.scale.y = 0.2;
        marker3.scale.z = 0.2;
        marker3.color.r = 1.0;
        marker3.color.g = 0.0;
        marker3.color.b = 0.1;
        marker3.color.a = 0.6;

        for(size_t i=0; i<vt.size(); ++i)
        {
                marker3.id +=2;
                marker3.pose.position.x = vt[i]->x[vt[i]->closest_node];
                marker3.pose.position.y = vt[i]->y[vt[i]->closest_node];
                marker3.pose.position.z = 0;
                marker_vec.push_back(marker3);
        }


        // ________________________________
        //|                                |
        //|        Colision (cylinder)     |
        //|________________________________|
        // Represents the form of the car in each node
        visualization_msgs::Marker marker4;
        marker4.header.frame_id = "/parking_frame";
        marker4.header.stamp = ros::Time::now();
        marker4.ns = "Colision points";
        marker4.action = visualization_msgs::Marker::ADD;
        marker4.type = visualization_msgs::Marker::CYLINDER;
        marker4.scale.x = 0.075;
        marker4.scale.y = 0.075;
        marker4.scale.z = 0.1;
        marker4.color.r = 1.0;
        marker4.color.g = 0.84;
        marker4.color.b = 0.0;
        marker4.color.a = 0.6;
        
        
        static size_t coli_marker_total = 0;
        int total=0;
        for(size_t j=0; j<vt.size(); ++j)
        {
                ROS_INFO("Traj%ld has %ld collisions\n",j,vt[j]->collision_pts.size());

                for(size_t i=0; i<vt[j]->collision_pts.size(); ++i)
                {
                        marker4.id =total;
                        marker4.pose.position.x = vt[j]->collision_pts[i].x;
                        marker4.pose.position.y = vt[j]->collision_pts[i].y;
                        marker_vec.push_back(marker4);
                        total++;
                }
        }

        ROS_INFO("total=%d coli=%ld",total,coli_marker_total);
        //erase old colision markers
        for(size_t j=total; j<coli_marker_total; ++j)
        {
                ROS_INFO("deleting marker ");
                marker4.header.frame_id = "/parking_frame";
                marker4.id =j;
                marker4.action = visualization_msgs::Marker::DELETE;
                marker_vec.push_back(marker4);
        }

        coli_marker_total = total;


        marker_array->set_markers_vec(marker_vec);              
        
        return SUCCESS;
}

t_func_output c_manage_trajectory::compute_trajectories_scores(void)
{

        double maximum_admissible_to_DAP=8.0;// ATENTION to negative values if bigger than maximum
        double maximum_admissible_to_DLO=10.0;// ATENTION to negative values if bigger than maximum
        for (int i=0; i< (int)vt.size(); ++i)
        {
                //Compute DAP and ADAP
                compute_DAP(vt[i],AP);

                //Compute DLO
                compute_DLO(vt[i], vo);


                //normalize DAP
                vt[i]->score.DAPnorm=1-(vt[i]->score.DAP)/maximum_admissible_to_DAP;

                //normalize ADAP
                vt[i]->score.ADAPnorm=1.0-(vt[i]->score.ADAP/(M_PI));

                //normalize DLO
                vt[i]->score.DLOnorm=(vt[i]->score.DLO)/maximum_admissible_to_DLO;
        }

        //compute overall score for each traj
        for (size_t i=0; i< vt.size(); ++i)
        {
                compute_global_traj_score(vt[i]);
        }       

        compute_chosen_traj();

        return SUCCESS;
}

t_func_output c_manage_trajectory::compute_DAP(c_trajectoryPtr& trajectory,t_desired_coordinates& AP)
{       
        trajectory->score.DAP=10e6;

        trajectory->closest_node=-1;

        for(size_t i = 0; i < trajectory->x.size(); ++i)
        {
                double DAP_prev = sqrt( pow(trajectory->x[i]-AP.x,2)+ pow(trajectory->y[i]-AP.y,2));
                
                if(DAP_prev < trajectory->score.DAP)
                {
                        trajectory->score.DAP = DAP_prev;
                        trajectory->closest_node = i;
                }
        }

        if(trajectory->closest_node!=-1)
        {
                trajectory->score.ADAP=compute_ADAP(trajectory,AP,trajectory->closest_node);
                return SUCCESS;
        }
        else
        {
                return FAILURE;
        }
}

double c_manage_trajectory::compute_ADAP(c_trajectoryPtr& trajectory,t_desired_coordinates& AP, int i)
{
        double adap=abs(trajectory->theta[i]-AP.theta);
        if (adap>M_PI)
                adap=2*M_PI-adap; 
        return adap;
}

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)
{
        //Create a marker
        visualization_msgs::Marker marker;
        std_msgs::ColorRGBA color;
        // ________________________________
        //|                                |
        //|           text nodes           |
        //|________________________________|
        // Points marker to t nodes
        marker.header.frame_id = "/parking_frame";
        marker.header.stamp = ros::Time::now();
        marker.ns = "info";
        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.80;
        marker.color.g = 0.50;
        marker.color.b = 0.20;
        marker.color.a = 1.0;
        marker.id = (*marker_count)++;
        marker.pose.position.x = trajectory->x[trajectory->x.size()-1]+0.25+z_high;
        marker.pose.position.y = trajectory->y[trajectory->x.size()-1];
        marker.pose.position.z = z_high;

        marker.text = string_init + str( boost::format("%.2f") % value) + " ("+str( boost::format("%.2f") % normalized_value)+")";
        marker_vec->push_back(marker);  
}