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