ROS中订阅两个消息,发布一个数据。

来源:互联网 发布:淘宝 客服 代码 编辑:程序博客网 时间:2024/06/07 05:29

今天测试转换出来的点云数据,利用Rviz显示不出来结果。提示错误如下:

no message received in Rviz


Status:Error.Points Showing [0] points from [0] messages.Topic No messages received.Transform For frame[]: Frame[] does not exist.


在网上Google了一下,就把网友的例子和回答,在博客里面备份一下吧!

代码如下:通过订阅两个消息,发布一个数据。

typedef pcl::PointCloud<pcl::PointCloud<int> > PointCloud;typedef pcl::PointXYZ PointType;ros::Publisher pub;pcl::PointCloud<int> keypoint_indices;pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_ptr (new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>& keypoints = *keypoints_ptr;float angular_resolution_;float support_size_ =0.2f;//-0.1fimage_geometry::PinholeCameraModel model_;void cameraModelCallback(const sensor_msgs::CameraInfoConstPtr &info_msg){    model_.fromCameraInfo(info_msg);}void depthimage_cb(const sensor_msgs::ImageConstPtr &depth_msg){    const uint16_t* depthImage = reinterpret_cast<const uint16_t*>(&depth_msg->data[0]);    pcl::RangeImagePlanar range_image_;    pcl::RangeImageBorderExtractor range_image_border_extractor_;    // convert the depth-image to a pcl::rangeImage    angular_resolution_ = 0.5f(float)(-1);    /*range_image_.setDepthImage(depthImage,depth_msg->width, depth_msg->height, (depth_msg->width)/2, (depth_msg->height)/2, 600.0, 600.0, angular_resolution_);*/    range_image_.setDepthImage(depthImage,depth_msg->width, depth_msg->height, model_.cx(), model_.cy(),model_.fx(), model_.fy(), angular_resolution_);    range_image_.setUnseenToMaxRange();    pcl::NarfKeypoint keypoint_det;    // extract the indices of the NARF-Keypoints    keypoint_det.getParameters ().support_size =  support_size_;    keypoint_det.setRangeImageBorderExtractor (&range_image_border_extractor_);    keypoint_det.setRangeImage (&range_image_);    keypoint_det.compute (keypoint_indices);    keypoints.points.resize (keypoint_indices.points.size ());    for (size_t i=0; i<keypoint_indices.points.size (); ++i)    keypoints.points[i].getVector3fMap () = range_image_.points[keypoint_indices.points[i]].getVector3fMap ();    pub.publish (keypoints);} intmain (int argc, char** argv){  // Initialize ROS  ros::init (argc, argv, "keypointsplanar");  ros::NodeHandle nh1;  // Create a ROS subscriber for the input point cloud  ros::Subscriber sub1 = nh1.subscribe ("/camera/depth/image", 1, depthimage_cb);     ros::Subscriber sub2 = nh1.subscribe ("/camera/depth/camera_info", 1, cameraModelCallback);     // Create a ROS publisher for the output point cloud  pub = nh1.advertise<pcl::PointCloud<pcl::PointXYZ> > ("keypoints", 1);  // Spin  ros::spin ();}


解决方法: 参考   参考2   参考3

所发布数据,需要给定frame_id和stamp标签。因为Rviz显示数据,需要知道数据的frame_id和时间戳。

keypoints.header.frame_id = depth_msg->header.frame_id;keypoints.header.stamp = depth_msg->header.stamp;



0 0