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

Go to the documentation of this file.

#ifndef _trajectory_planner_nodelet_CPP_
#define _trajectory_planner_nodelet_CPP_

#include "trajectory_planner_nodelet.h"

bool plan_trajectory=false;
bool have_plan=false;
geometry_msgs::PoseStamped pose_in;
geometry_msgs::PoseStamped pose_transformed;

std::vector< pcl::PointCloud<pcl::PointXYZ> > pc_v;


void set_coordinates(trajectory_planner::coordinates msg)
{       

//      cout<<"stat Message received!!!"<<endl;
        // Change parameters
        pose_in.pose.position.x=msg.x;
        pose_in.pose.position.y=msg.y;
        pose_in.pose.orientation.w=cos(msg.theta/2.0);
        pose_in.pose.orientation.x=0.0;
        pose_in.pose.orientation.y=0.0;
        pose_in.pose.orientation.z=sin(msg.theta/2.0);

        pose_in.header.frame_id="/world";

        manage_vt->set_attractor_point(msg.x,msg.y,msg.theta);
        plan_trajectory=true;
}


void mtt_callback(mtt::TargetList msg)
{
        ROS_INFO("received mtt num obs=%ld num lines first obs =%d frame_id=%s",msg.id.size(), msg.obstacle_lines[0].width, msg.header.frame_id.c_str());

        //copy to global variable
        pc_v.erase(pc_v.begin(), pc_v.end());
        for (size_t i=0; i<msg.id.size(); ++i)
        {
                pcl::PointCloud<pcl::PointXYZ> tmp;
                pcl::fromROSMsg(msg.obstacle_lines[i], tmp);
                tmp.header.frame_id=msg.header.frame_id;
                tmp.header.stamp=msg.header.stamp;

                ROS_INFO("Obstacle %ld has %ld lines", i, tmp.points.size());
                pc_v.push_back(tmp);
        }       
}


vector<double> set_speed_vector(boost::shared_ptr<c_trajectory> t)
{
        vector<double> speed_setted;
        for(size_t i=0;i<_NUM_NODES_;++i)
        {
                if(i < (_NUM_NODES_ - 1))
                {
                        if((t->arc[i])*(t->arc[i+1])<0.0)
                        {
                                speed_setted.push_back((t->arc[i]/fabs(t->arc[i]))*_SPEED_SAFFETY_); // This is the speed set to reverse/forward or forward/reverse
                        }
                        else
                        {
                                speed_setted.push_back((t->arc[i]/fabs(t->arc[i]))*_SPEED_REQUIRED_);
                        }
                }
                else
                {
                        speed_setted.push_back((t->arc[i]/fabs(t->arc[i]))*_SPEED_REQUIRED_);
                }

        }
        return speed_setted;
}


int main(int argc, char **argv)
{
        ros::init(argc, argv, "trajectory_planner_nodelet");
        ros::NodeHandle n("~");
        p_n=&n;

        //Define the publishers and subscribers
        tf::TransformBroadcaster broadcaster;
        tf::TransformListener listener;
        p_listener = &listener;
        ros::Publisher array_pub = n.advertise<visualization_msgs::MarkerArray>( "/array_of_markers", 1 );
        ros::Subscriber sub = n.subscribe("/msg_coordinates", 1, set_coordinates);
        ros::Subscriber mtt_sub = n.subscribe("/mtt_targets", 1, mtt_callback);
        ros::Rate loop_rate(10);

        //   ___________________________________
        //   |                                 |
        //   |        Define the trajectories  |
        //   |_________________________________| 
        //Declare the traj manager class        
        manage_vt = (c_manage_trajectoryPtr) new c_manage_trajectory();

        //initialize attractor point
        t_desired_coordinates AP;
        AP.x=-1.0;
        AP.y=0.0;
        AP.theta=-M_PI/8;
        manage_vt->set_attractor_point(AP.x, AP.y, AP.theta);


        //initialize vehicle description
        manage_vt->set_vehicle_description(_VEHICLE_WIDTH_, _VEHICLE_LENGHT_BACK_, _VEHICLE_LENGHT_FRONT_, 
                        _VEHICLE_HEIGHT_TOP_, _VEHICLE_HEIGHT_BOTTOM_);

        //initialize inter axis distance
        manage_vt->set_inter_axis_distance(_D_);


        //Compute the trajectory parameters
        for (int i=0; i< _NUM_TRAJ_; i++)
        {
                vector<double> v_a;
                vector<double> v_arc;
                for (int j=0; j<_NUM_NODES_; ++j)
                {
                        v_a.push_back(M_PI/180.*a[i][j]);       
                        v_arc.push_back(arc[i][j]);     
                }
                manage_vt->create_new_trajectory(v_a,v_arc,v_a);
        }

        // Choose trajectory  (0 is trajectory 1)
        int val = 0;     // This will be commented

        if (val>=(int)manage_vt->vt.size())
        {
                ROS_ERROR("Chosen trajectory does not exist");
                exit(0);
        }
        manage_vt->set_chosen_traj(val);

        //create the static visualisation markers

        commandPublisher = n.advertise<trajectory_planner::traj_info>("/trajectory_information", 1000);

        
        
        
        
        while(ros::ok())
        {

                if(plan_trajectory==true)
                {
                        //aqui recebi um comando para defenir uma trajectoria
                        
                        plan_trajectory = false;
                        ROS_INFO("Going to plan a trajectory");
                        // _________________________________
                        //|                                 |
                        //|    Set the parking view frame   |
                        //|_________________________________| 
                        bool have_transform=true;
                        //Set the frame where to draw the trajectories
                        try
                        {
                                p_listener->lookupTransform("/world","/vehicle_odometry",ros::Time(0), transformw);
                        }
                        catch (tf::TransformException ex)
                        {
                                ROS_ERROR("%s",ex.what());
                                have_transform=false;
                        }

                        if (have_transform)
                        {
                                ros::Time time=ros::Time::now();
                                broadcaster.sendTransform(tf::StampedTransform(transformw, time,"/world", "/parking_frame"));
                                ros::spinOnce();
                                broadcaster.sendTransform(tf::StampedTransform(transformw, time+ros::Duration(5),"/world", "/parking_frame")); 
                                ros::spinOnce();                        
                                //                              cout<<"stat Publishing transform"<<endl;

                                ros::Duration(0.1).sleep();

                                //   ___________________________________
                                //   |                                 |
                                //   |        Trajectory evaluation    |
                                //   |_________________________________| 

                                //Transform attractor point to /parking_frame
                                tf::Transformer tt;
                                pose_in.header.stamp=time+ros::Duration(0.1);
                                p_listener->transformPose ("/parking_frame", pose_in, pose_transformed); 

                                ROS_INFO("pose_in frame_id=%s pose_transformed frame_id=%s",pose_in.header.frame_id.c_str(), pose_transformed.header.frame_id.c_str());
                                //Set transformed attractor point
                                manage_vt->set_attractor_point(pose_transformed.pose.position.x,pose_transformed.pose.position.y,atan2(2.0*(pose_transformed.pose.orientation.w*pose_transformed.pose.orientation.z),1-(2*(pose_transformed.pose.orientation.z*pose_transformed.pose.orientation.z))));


                                //transform mtt to /parking_frame       
                                pcl::PointCloud<pcl::PointXYZ>  pct;
                                mtt::TargetList msg_transformed;



                                for (size_t i=0; i<pc_v.size(); ++i)
                                {

                                        if (i==0)
                                        {
                                                try
                                                {
                                                        p_listener->lookupTransform(pc_v[i].header.frame_id,"/parking_frame", pc_v[i].header.stamp, transform_mtt);
                                                }
                                                catch (tf::TransformException ex)
                                                {
                                                        ROS_ERROR("%s",ex.what());
                                                }
                                        }

                                        ROS_INFO("pc_v[%ld] frame_id=%s pcp frame_id=%s",i, pc_v[i].header.frame_id.c_str(), pct.header.frame_id.c_str());
                                        pcl_ros::transformPointCloud(pc_v[i],pct,transform_mtt.inverse());

                                        sensor_msgs::PointCloud2 pc_msg;
                                        pcl::toROSMsg(pct, pc_msg);
                                        pc_msg.header.frame_id = "/parking_frame";
                                        pc_msg.header.stamp = ros::Time::now();
                                        msg_transformed.obstacle_lines.push_back(pc_msg);
                                }
                                manage_vt->set_obstacles(msg_transformed); 



                                //   ___________________________________
                                //   |                                 |
                                //   |        Draw                     |
                                //   |_________________________________|
                                manage_vt->create_static_markers();
                                
                                
                                ros::Time st=ros::Time::now();
                                
                                manage_vt->compute_trajectories_scores();

                                cout<<"Compute scores: "<<(ros::Time::now()-st).toSec();
                                
                                ROS_INFO("manage_vt chosen traj= %d",manage_vt->chosen_traj.index);


                                trajectory_planner::traj_info info;

                                manage_vt->get_traj_info_msg_from_chosen(&info);

                                commandPublisher.publish(info);


                                visualization_msgs::MarkerArray marker_array;
                                manage_vt->compute_vis_marker_array(&marker_array);
                                array_pub.publish(marker_array);
                                cout<<"ja size:"<<marker_array.markers.size()<<endl;


                                have_plan=true;

                        }
                }

                if (have_plan==true)
                {       
                        // !!!! The previous 'if' must have a weight evaluation !!!!  if ... && GlobalScore>=0.74
                        
                        broadcaster.sendTransform(tf::StampedTransform(transformw, ros::Time::now(),"/world", "/parking_frame")); 
                        cout<<"stat Publishing transform"<<endl;
                }

                //printf("CURRENT NODE-> %d\n",node);
                //printf("Distance Travelled-> %f\n",base_status.distance_traveled);

                loop_rate.sleep();
                ros::spinOnce();
        }
}
#endif