45 pcl::PointCloud<pcl::PointXYZ> PclCloud;
46 pcl::PCLPointCloud2 pcl_pc;
47 pcl_conversions::toPCL(cloud, pcl_pc);
48 pcl::fromPCLPointCloud2(pcl_pc, PclCloud);
52 map<double,pair<uint,double> > grid;
53 map<double,pair<uint,double> >::iterator it;
55 double spacing = 1.*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::TargetListPC& 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 = target_msg.header.frame_id;
130 cout<<
"fid:"<<target_msg.header.frame_id<<endl;
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 = 0.2;
141 marker.scale.y = 0.2;
142 marker.scale.z = 0.2;
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.02;
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);
219 for(it=marker_map.begin();it!=marker_map.end();it++)
221 if( it->second.second==0 )
222 it->second.first.action = visualization_msgs::Marker::DELETE;
223 else if(it->second.second<=-1)
232 marker_vector.push_back(it->second.first);
238 mtt::TargetListPC targetList;
244 vector<t_clustersPtr> clusters;
245 vector<t_objectPtr> object;
246 vector<t_listPtr> list;
248 visualization_msgs::MarkerArray markersMsg;
251 init(argc,argv,
"mtt");
255 Subscriber subs_points = nh.subscribe(
"/points", 1000,
PointsHandler);
256 Publisher
target_publisher = nh.advertise<mtt::TargetList>(
"/targets", 1000);
257 Publisher
markers_publisher= nh.advertise<visualization_msgs::MarkerArray>(
"/markers", 1000);
258 Publisher marker_publisher= nh.advertise<visualization_msgs::Marker>(
"/marker", 1000);
263 cout<<
"Start to spin"<<endl;
278 clustering(full_data,clusters,&config,&flags);
298 targetList.id.clear();
299 targetList.obstacle_lines.clear();
301 pcl::PointCloud<pcl::PointXYZ> target_positions;
302 pcl::PointCloud<pcl::PointXYZ> velocity;
304 target_positions.header.frame_id =
pointData.header.frame_id;
306 velocity.header.frame_id =
pointData.header.frame_id;
308 targetList.header.stamp = ros::Time::now();
309 targetList.header.frame_id =
pointData.header.frame_id;
312 for(uint i=0;i<list.size();i++)
314 targetList.id.push_back(list[i]->
id);
316 pcl::PointXYZ position;
318 position.x = list[i]->position.estimated_x;
319 position.y = list[i]->position.estimated_y;
322 target_positions.points.push_back(position);
326 vel.x=list[i]->velocity.velocity_x;
327 vel.y=list[i]->velocity.velocity_y;
330 velocity.points.push_back(vel);
332 pcl::PointCloud<pcl::PointXYZ> shape;
333 pcl::PointXYZ line_point;
336 for(j=0;j<list[i]->shape.lines.size();j++)
338 line_point.x=list[i]->shape.lines[j]->xi;
339 line_point.y=list[i]->shape.lines[j]->yi;
341 shape.points.push_back(line_point);
344 line_point.x=list[i]->shape.lines[j-1]->xf;
345 line_point.y=list[i]->shape.lines[j-1]->yf;
347 sensor_msgs::PointCloud2 shape_cloud;
348 pcl::toROSMsg(shape,shape_cloud);
349 targetList.obstacle_lines.push_back(shape_cloud);
352 pcl::toROSMsg(target_positions, targetList.position);
353 pcl::toROSMsg(velocity, targetList.velocity);
355 target_publisher.publish(targetList);
360 markers_publisher.publish(markersMsg);
void clean_objets(vector< t_objectPtr > &objects)
Frees memory associated with objects.
int main(int argc, char **argv)
void PointsHandler(const sensor_msgs::PointCloud2 &msg)
void init_flags(t_flag *flags)
Init flags.
void PointCloud2ToData(sensor_msgs::PointCloud2 &cloud, t_data &data)
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.
sensor_msgs::PointCloud2 pointData
void MotionModelsIteration(vector< t_listPtr > &list, t_config &config)
map< pair< string, int >, pair< visualization_msgs::Marker, int > > marker_map
void CreateMarkers(vector< visualization_msgs::Marker > &marker_vector, mtt::TargetListPC &target_msg, vector< t_listPtr > &list)
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.