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; }
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; }