@@ -59,12 +59,13 @@ ros::Publisher marker_array_pub_;
5959
6060ros::Publisher box_pub;
6161
62+ // 回调函数
6263void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input){
6364
6465 PointCloud<pcl::PointXYZ>::Ptr none_ground_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
6566
6667 // Convert from ros msg to PCL::PointCloud data type
67- fromROSMsg (*input, *none_ground_cloud);
68+ fromROSMsg (*input, *none_ground_cloud); // 转为PCL数据格式PCL::PointCloud
6869
6970 // start processing pcl::pointcloud
7071
@@ -114,9 +115,9 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input){
114115
115116 visualization_msgs::MarkerArray ma;
116117
117- vector<PointCloud<PointXYZ>> bBoxes = boxFitting (none_ground_cloud, cartesianData, numCluster,ma);
118+ vector<PointCloud<PointXYZ>> bBoxes = boxFitting (none_ground_cloud, cartesianData, numCluster,ma); // bBoxes----一个数组(候选框8个坐标)
118119
119- object_tracking::trackbox boxArray;
120+ object_tracking::trackbox boxArray; // boxArray--候选框8个坐标数组 的 数组
120121
121122 boxArray.header = input->header ;
122123 boxArray.box_num = bBoxes.size ();
@@ -157,10 +158,11 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input){
157158
158159// ************************************cube visualiaztion******************************
159160
160- box_pub.publish (boxArray);
161+ box_pub.publish (boxArray); // boxArray--候选框8个坐标数组 的 数组
161162
162- cout << " size of bBoxes is " << bBoxes.size () << endl;
163- cout << " size of marker is " << ma.markers .size () << endl;
163+ // cout << "boxArray is " << boxArray<< endl; // bBoxes
164+ cout << " size of bBoxes is " << bBoxes.size () << endl; // bBoxes数量 size of bBoxes is 2
165+ cout << " size of marker is " << ma.markers .size () << endl; // marker数量 size of marker is 2
164166 marker_array_pub_.publish (ma);
165167
166168
@@ -171,8 +173,8 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input){
171173
172174// *********************************************bBoxes visualization***************************************
173175
174- visualization_msgs::Marker line_list;
175- line_list.header .frame_id = " velodyne" ;
176+ visualization_msgs::Marker line_list; // 将候选框8个点连线
177+ line_list.header .frame_id = " velodyne" ; // 定义frame_id (rviz需要设置世界坐标系为velodyne)
176178 line_list.header .stamp = ros::Time::now ();
177179 line_list.ns = " boxes" ;
178180 line_list.action = visualization_msgs::Marker::ADD;
0 commit comments