39 cout<<
"Received point cloud"<<endl;
46 pcl::PointCloud<pcl::PointXYZ> PclCloud;
48 pcl::fromROSMsg(cloud,PclCloud);
52 map<double,pair<uint,double> > grid;
53 map<double,pair<uint,double> >::iterator it;
55 double spacing = 0.5*M_PI/180.;
62 cout<<
"mtt Input size:"<<PclCloud.points.size()<<endl;
64 for(uint i=0;i<PclCloud.points.size();i++)
66 theta=atan2(PclCloud.points[i].y,PclCloud.points[i].x);
67 r=sqrt(pow(PclCloud.points[i].x,2)+pow(PclCloud.points[i].y,2));
70 rightBorder = spacing * ceil(theta/spacing);
71 leftBorder = rightBorder - spacing;
73 if(fabs(rightBorder-theta)<fabs(leftBorder-theta))
78 if(grid.find(theta)!=grid.end())
80 if(r<grid[theta].second)
93 for(it=grid.begin();it!=grid.end();it++)
96 data.
x[i]=PclCloud.points[(*it).second.first].x;
97 data.
y[i]=PclCloud.points[(*it).second.first].y;
98 data.
r[i]=sqrt(pow(data.
x[i],2)+pow(data.
y[i],2));
99 data.
t[i]=atan2(data.
y[i],data.
x[i]);
105 cout<<
"mtt Size of data:"<<data.
n_points<<endl;
111 void CreateMarkers(vector<visualization_msgs::Marker>& marker_vector,mtt::TargetList& target_msg,vector<t_listPtr>& list)
113 static map<pair<string,int>, pair<visualization_msgs::Marker,int> >
marker_map;
114 map<pair<string,int>, pair<visualization_msgs::Marker,int> >::iterator it;
118 marker_vector.clear();
120 for(it=marker_map.begin();it!=marker_map.end();it++)
124 std_msgs::ColorRGBA color;
125 class_colormap colormap(
"hsv",10, 1,
false);
127 visualization_msgs::Marker marker;
129 marker.header.frame_id =
pointData.header.frame_id;
132 marker.header.stamp = ros::Time::now();
134 marker.action = visualization_msgs::Marker::ADD;
136 marker.pose.position.z=0.3;
138 marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
140 marker.scale.x = 1.0;
141 marker.scale.y = 1.0;
142 marker.scale.z = 1.0;
151 for(uint i=0;i<list.size();i++)
153 if(list[i]->shape.lines.size()!=0)
155 marker.pose.position.x=list[i]->position.estimated_x;
156 marker.pose.position.y=list[i]->position.estimated_y;
158 marker.text = boost::lexical_cast<
string>(list[i]->id);
162 marker_map[make_pair(marker.ns,marker.id) ] = make_pair(marker,1);
166 marker.pose.position.x=0;
167 marker.pose.position.y=0;
169 marker.text =
"origin";
173 marker_map[make_pair(marker.ns,marker.id) ] = make_pair(marker,1);
177 marker.type = visualization_msgs::Marker::LINE_STRIP;
178 marker.ns =
"objects";
180 marker.pose.position.x=0;
181 marker.pose.position.y=0;
182 marker.pose.position.z=0;
184 marker.scale.x = 0.2;
185 marker.scale.y = 0.1;
186 marker.scale.z = 0.1;
188 for(uint i=0;i<list.size();i++)
190 marker.color = colormap.color(list[i]->
id);
192 geometry_msgs::Point p;
195 marker.points.clear();
198 for(l=0;l<list[i]->shape.lines.size();l++)
200 p.x = list[i]->shape.lines[l]->xi;
201 p.y = list[i]->shape.lines[l]->yi;
203 marker.points.push_back(p);
206 p.x = list[i]->shape.lines[l-1]->xf;
207 p.y = list[i]->shape.lines[l-1]->yf;
209 marker.points.push_back(p);
213 marker_map[make_pair(marker.ns,marker.id) ] = make_pair(marker,1);
250 for(it=marker_map.begin();it!=marker_map.end();it++)
252 if( it->second.second==0 )
253 it->second.first.action = visualization_msgs::Marker::DELETE;
254 else if(it->second.second<=-1)
263 marker_vector.push_back(it->second.first);
269 mtt::TargetList targetList;
275 vector<t_clustersPtr> clusters;
276 vector<t_objectPtr> object;
277 vector<t_listPtr> list;
279 visualization_msgs::MarkerArray markersMsg;
282 init(argc,argv,
"mtt");
286 Subscriber subs_points = nh.subscribe(
"/points", 1000,
PointsHandler);
287 Publisher
target_publisher = nh.advertise<mtt::TargetList>(
"/new_targets", 1000);
288 Publisher
markers_publisher= nh.advertise<visualization_msgs::MarkerArray>(
"/new_markers", 1000);
295 cout<<
"Start to spin"<<endl;
310 clustering(full_data,clusters,&config,&flags);
311 cout<<
"clusters: "<<clusters.size()<<endl;
331 targetList.Targets.clear();
337 target.header.stamp = ros::Time::now();
338 target.header.frame_id =
pointData.header.frame_id;
342 for(uint i=0;i<list.size();i++)
344 geometry_msgs::Pose pose;
345 geometry_msgs::Twist
vel;
346 geometry_msgs::Point initialpose;
347 geometry_msgs::Point finalpose;
350 target.header.seq = list[i]->id;
351 target.id = list[i]->id;
354 vel.linear.x=list[i]->velocity.velocity_x;
355 vel.linear.y=list[i]->velocity.velocity_y;
357 target.velocity =
vel;
360 double theta = atan2(vel.linear.y,vel.linear.x);
361 geometry_msgs::Quaternion target_orientation =
362 tf::createQuaternionMsgFromYaw(theta);
365 pose.position.x = list[i]->position.estimated_x;
366 pose.position.y = list[i]->position.estimated_y;
368 pose.orientation = target_orientation;
372 initialpose.x = list[i]->shape.lines[0]->xi;
373 initialpose.y = list[i]->shape.lines[0]->yi;
375 target.initialpose = initialpose;
378 finalpose.x = list[i]->shape.lines[list[i]->shape.lines.size()-1]->xf;
379 finalpose.y = list[i]->shape.lines[list[i]->shape.lines.size()-1]->yf;
381 target.finalpose = finalpose;
384 target.size = sqrt(((finalpose.x - initialpose.x) * (finalpose.x - initialpose.x))
385 + ((finalpose.y - initialpose.y) * (finalpose.y - initialpose.y)));
389 double velocity = sqrt(pow(vel.linear.x,2)+
390 pow(vel.linear.y,2));
393 targetList.Targets.push_back(target);
399 target_publisher.publish(targetList);
402 markers_publisher.publish(markersMsg);
void PointsHandler(const sensor_msgs::PointCloud2 &msg)
void clean_objets(vector< t_objectPtr > &objects)
Frees memory associated with objects.
void CreateMarkers(vector< visualization_msgs::Marker > &marker_vector, mtt::TargetList &target_msg, vector< t_listPtr > &list)
int main(int argc, char **argv)
void init_flags(t_flag *flags)
Init flags.
void calc_cluster_props(t_cluster **clusters, int size, t_data *data, t_config *config)
Calculates cluster properties.
void AssociateObjects(vector< t_listPtr > &list, vector< t_objectPtr > &objects, t_config &config, t_flag &flags)
Global include file that combines all local include files for the mtt algorithm.
void PointCloud2ToData(sensor_msgs::PointCloud2 &cloud, t_data &data)
void MotionModelsIteration(vector< t_listPtr > &list, t_config &config)
map< pair< string, int >, pair< visualization_msgs::Marker, int > > marker_map
This structure has all points coordinates.
t_cluster ** clustering(t_data *data, int *count, t_config *config, t_flag *flags)
Performs clustering operation.
bool clusters2objects(vector< t_objectPtr > &objectsPtr, vector< t_clustersPtr > &clusters, t_data &data, t_config &config)
Converts clusters of points into objects using recursive line fitting.
This structure contains global configurations parameters.
void init_config(t_config *config)
Init configuration.
void calc_object_props(vector< t_objectPtr > &objects)
Computes object properties, such as centroid and size.
ros::Publisher target_publisher
ros::Publisher markers_publisher
This structure contains global flags parameters.
sensor_msgs::PointCloud2 pointData