机器人操作系统（ROS2）入门与实践 课件 第10章 ROS2的三维视觉应用.ppt

floatplane_height=points_z_sum/point_num;RCLCPP_INFO(node-get_logger(),plane_height=%.2f,plane_height);?pass.setInputCloud(cloud_src.makeShared());pass.setFilterFieldName(z);pass.setFilterLimits(plane_height+0.2,1.5);pass.filter(cloud_src);?pcl::search::KdTreepcl::PointXYZ::Ptrtree(newpcl::search::KdTreepcl::PointXYZ);tree-setInputCloud(cloud_src.makeShared());?pcl::EuclideanClusterExtractionpcl::PointXYZec;ec.setInputCloud(cloud_src.makeShared());ec.setMinClusterSize(100);ec.setMaxClusterSize(25000);ec.setClusterTolerance(0.1);ec.setSearchMethod(tree);std::vectorpcl::PointIndicescluster_indices;ec.extract(cluster_indices);intobject_num=cluster_indices.size();10.2使用PCL进行物品检测RCLCPP_INFO(node-get_logger(),object_num=%d,object_num);for(inti=0;iobject_num;i++){intpoint_num=cluster_indices[i].indices.size();floatpoints_x_sum=0;floatpoints_y_sum=0;floatpoints_z_sum=0;for(intj=0;jpoint_num;j++){intpoint_index=cluster_indices[i].indices[j];points_x_sum+=cloud_src.points[point_index].x;points_y_sum+=cloud_src.points[point_index].y;points_z_sum+=cloud_src.points[point_index].z;}floatobject_x=points_x_sum/point_num;floatobject_y=points_y_sum/point_num;floatobject_z=points_z_sum/point_num;RCLCPP_INFO(node-get_logger(),object%dpos=(%.2f,%.2f,%.2f),i,object_x,object_y,object_z);}RCLCPP_INFO(node-get_logger(),---------------------);}10.2使用PCL进行物品检测intmain(intargc,char**argv){rclcpp::init(argc,argv);?node=std::make_sharedrclcpp::Node(pointcloud_objects_node);?tf_buffer_=std::make_sharedtf2_ros::Buffer(node-get_clock());tf_listener_=std::make_sharedtf2_ros::TransformListener(*tf_buffer_);?autopc_sub=node-create_subscriptionsensor_msgs::msg::PointCloud2(