コード例 #1
0
ファイル: mtt_new_msg.cpp プロジェクト: procopiostein/leader
int main(int argc,char**argv)
{
    //this is now an array
    mtt::TargetList targetList;

    t_config	config;
    t_data	full_data;
    t_flag	flags;

    vector<t_clustersPtr> clusters;
    vector<t_objectPtr> object;
    vector<t_listPtr> list;

    visualization_msgs::MarkerArray markersMsg;

    // Initialize ROS
    init(argc,argv,"mtt");

    NodeHandle nh("~");

    Subscriber subs_points = nh.subscribe("/points", 1000, PointsHandler);
    Publisher target_publisher = nh.advertise<mtt::TargetList>("/targets", 1000);
    Publisher markers_publisher= nh.advertise<visualization_msgs::MarkerArray>("/markers", 1000);
    Publisher arrow_publisher= nh.advertise<visualization_msgs::MarkerArray>("/arrows", 1000);

    init_flags(&flags);		//Inits flags values
    init_config(&config);	//Inits configuration values

    cout<<"Start to spin"<<endl;
    Rate r(100);
    while(ok())
    {
        spinOnce();
        r.sleep();

        if(!new_data)
            continue;
        new_data=false;

        //Get data from PointCloud2 to full_data
        PointCloud2ToData(pointData,full_data);

        //clustering
        clustering(full_data,clusters,&config,&flags);

        //calc_cluster_props
        calc_cluster_props(clusters,full_data);

        //clusters2objects
        clusters2objects(object,clusters,full_data,config);

        calc_object_props(object);

        //AssociateObjects
        AssociateObjects(list,object,config,flags);

        //MotionModelsIteration
        MotionModelsIteration(list,config);

        //cout<<"Number of targets "<<list.size()<<endl;

        clean_objets(object);//clean current objects

        //clear target list array
        targetList.Targets.clear();

        //structure to be fed to array
        mtt::Target target;

        //build header
        target.header.stamp = ros::Time::now();
        target.header.frame_id = pointData.header.frame_id;


        //trying to draw arrows
        visualization_msgs::MarkerArray arrow_arrray;

        for(uint i=0; i<list.size(); i++)
        {
            geometry_msgs::Pose pose;
            geometry_msgs::Twist vel;

            //header
            target.header.seq = list[i]->id;
            target.id = list[i]->id;

            //velocity
            vel.linear.x=list[i]->velocity.velocity_x;
            vel.linear.y=list[i]->velocity.velocity_y;
            vel.linear.z=0;
            target.velocity = vel;

            //compute orientation based on velocity.
            double theta = atan2(vel.linear.y,vel.linear.x);
            geometry_msgs::Quaternion target_orientation =
                tf::createQuaternionMsgFromYaw(theta);

            //pose
            pose.position.x = list[i]->position.estimated_x;
            pose.position.y = list[i]->position.estimated_y;
            pose.position.z = 0;
            pose.orientation = target_orientation;
            target.pose = pose;

            //feed array with current target
            //condition to accept as valit target (procopio change)
            double velocity = sqrt(pow(vel.linear.x,2)+
                                   pow(vel.linear.y,2));

            if(/*velocity > 0.5 &&*/ velocity < 3.0)
                targetList.Targets.push_back(target);


            //////////////////////////
            //drawing arrow part

            if(list[i]->velocity.velocity_module > 0.2 &&
                    list[i]->velocity.velocity_module < 5.0) {

                visualization_msgs::Marker arrow_marker;

                arrow_marker.type = visualization_msgs::Marker::ARROW;
                arrow_marker.action = visualization_msgs::Marker::ADD;

                // Set the frame ID and timestamp.
                arrow_marker.header.frame_id = pointData.header.frame_id;
                arrow_marker.header.stamp = ros::Time::now();

                arrow_marker.color.r = 0;
                arrow_marker.color.g = 1;
                arrow_marker.color.b = 0;
                arrow_marker.color.a = 1;

//                     arrow_marker.scale.x = ;
                arrow_marker.scale.x = list[i]->velocity.velocity_module;  //length
                arrow_marker.scale.y = 0.1; //width
                arrow_marker.scale.z = 0.1; //height

                arrow_marker.lifetime = ros::Duration(1.0);
                arrow_marker.id = list[i]->id;

                // Set the pose of the arrow_marker.  This is a full 6DOF pose relative to the frame/time specified in the header
                arrow_marker.pose = pose;

                arrow_arrray.markers.push_back(arrow_marker);
                //////////////////////////
            }
        }

        target_publisher.publish(targetList);

        CreateMarkers(markersMsg.markers,targetList,list);
        markers_publisher.publish(markersMsg);
        arrow_publisher.publish(arrow_arrray);

        flags.fi=false;
    }

    return 0;
}
コード例 #2
0
ファイル: mtt.cpp プロジェクト: lardemua/old_dependencies
int main(int argc,char**argv)
{
	mtt::TargetListPC targetList;
	
	t_config	config;	
	t_data		full_data;
	t_flag		flags;
	
	vector<t_clustersPtr> clusters;
	vector<t_objectPtr> object;
	vector<t_listPtr> list;
	
	visualization_msgs::MarkerArray markersMsg;
	
	// Initialize ROS
	init(argc,argv,"mtt");
	
	NodeHandle nh("~");
	
	Subscriber subs_points = nh.subscribe("/points", 1000, PointsHandler);
	Publisher target_publisher = nh.advertise<mtt::TargetList>("/targets", 1000);
	Publisher markers_publisher= nh.advertise<visualization_msgs::MarkerArray>("/markers", 1000);
	Publisher marker_publisher= nh.advertise<visualization_msgs::Marker>("/marker", 1000);
	
	init_flags(&flags);		//Inits flags values
	init_config(&config);	//Inits configuration values
	
	cout<<"Start to spin"<<endl;
	Rate r(100);
	while(ok())
	{
		spinOnce();
		r.sleep();
		
		if(!new_data)
			continue;
		new_data=false;
		
		//Get data from PointCloud2 to full_data
		PointCloud2ToData(pointData,full_data);
		
		//clustering		
		clustering(full_data,clusters,&config,&flags);
		
		//calc_cluster_props
		calc_cluster_props(clusters,full_data);
		
		//clusters2objects		
		clusters2objects(object,clusters,full_data,config);
		
		calc_object_props(object);
		
		//AssociateObjects
		AssociateObjects(list,object,config,flags);
		
		//MotionModelsIteration
		MotionModelsIteration(list,config);

// 		cout<<"Number of targets "<<list.size()<<endl;
		
		clean_objets(object);//clean current objects
		
		targetList.id.clear();
		targetList.obstacle_lines.clear();//clear all lines
		
		pcl::PointCloud<pcl::PointXYZ> target_positions;
		pcl::PointCloud<pcl::PointXYZ> velocity;
			
		target_positions.header.frame_id = pointData.header.frame_id;
		
		velocity.header.frame_id = pointData.header.frame_id;
		
        targetList.header.stamp = ros::Time::now();
		targetList.header.frame_id = pointData.header.frame_id;
		
		
		for(uint i=0;i<list.size();i++)
		{
			targetList.id.push_back(list[i]->id);
		  
			pcl::PointXYZ position;
			
			position.x = list[i]->position.estimated_x;
			position.y = list[i]->position.estimated_y;
			position.z = 0;
			
			target_positions.points.push_back(position);
			
			pcl::PointXYZ vel;
			
			vel.x=list[i]->velocity.velocity_x;
			vel.y=list[i]->velocity.velocity_y;
			vel.z=0;
			
			velocity.points.push_back(vel);
			
			pcl::PointCloud<pcl::PointXYZ> shape;
			pcl::PointXYZ line_point;
			
			uint j;
			for(j=0;j<list[i]->shape.lines.size();j++)
			{
				line_point.x=list[i]->shape.lines[j]->xi;
				line_point.y=list[i]->shape.lines[j]->yi;
				
				shape.points.push_back(line_point);
			}
			
			line_point.x=list[i]->shape.lines[j-1]->xf;
			line_point.y=list[i]->shape.lines[j-1]->yf;
			
			sensor_msgs::PointCloud2 shape_cloud;
			pcl::toROSMsg(shape,shape_cloud);
			targetList.obstacle_lines.push_back(shape_cloud);
		}
		
		pcl::toROSMsg(target_positions, targetList.position);
		pcl::toROSMsg(velocity, targetList.velocity);
			
		target_publisher.publish(targetList);
		
		CreateMarkers(markersMsg.markers,targetList,list);
		//markersMsg.header.frame_id=targetList.header.frame_id;

		markers_publisher.publish(markersMsg);
		
		flags.fi=false;
	}
	
	return 0;
}