Rosanswers logo

Hi everyone,

I adapted the ensenso_grabber example from here and in a an attept to publish the clouds for onward processing in another node, I decided to publish the processed clouds and images. However, I find that I can only retrieve the images in another node while the point_clouds themselves are not streaming out. I am at a loss on what I could possibly be doing wrong.

Would appreciate your feedback in this code:

#include <memory>
#include <algorithm>

#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/image_encodings.h>

#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <pcl_conversions/pcl_conversions.h>

 /*pcl and cv headers*/
  #include <ensenso/ensenso_headers.h>
using PairOfImages =  std::pair<pcl::PCLImage, pcl::PCLImage>;  //for the Ensenso grabber callback 
using PointT = pcl::PointXYZ;
using PointCloudT = pcl::PointCloud<PointT>;

pcl::EnsensoGrabber::Ptr ensenso_ptr;

sensor_msgs::PointCloud2 pcl2_msg;   //msg to be displayed in rviz

void callback (const PointCloudT::Ptr& cloud, \
      const boost::shared_ptr<PairOfImages>& images)
  ros::NodeHandle nh;
  // image_transport::ImageTransport it(nh);
  // image_transport::Publisher imagePub = it.advertise("/camera/image", 10);
 ros::Publisher pclPub = nh.advertise<sensor_msgs::PointCloud2>("/ensenso_cloudy", 10);

  //prepare cloud for rospy publishing
  pcl::toROSMsg(*cloud, pcl2_msg);
  pcl2_msg.header.stamp = ros::Time::now();
  pcl2_msg.header.frame_id = "ensenso_cloud";

  /*Process Image and prepare for publishing*/
  unsigned char *l_image_array = reinterpret_cast<unsigned char *> (&images->first.data[0]);
  unsigned char *r_image_array = reinterpret_cast<unsigned char *> (&images->second.data[0]);

  ROS_INFO_STREAM("Encoding: " << images->first.encoding);
  ROS_INFO_STREAM("Cloud size: " << cloud->height * cloud->width);

  int type = getOpenCVType (images->first.encoding);
  cv::Mat l_image (images->first.height, images->first.width, type, l_image_array);
  cv::Mat r_image (images->first.height, images->first.width, type, r_image_array);
  cv::Mat im (images->first.height, images->first.width * 2, type);

  im.adjustROI (0, 0, 0, -0.5*images->first.width);
  l_image.copyTo (im);
  im.adjustROI (0, 0, -0.5*images->first.width, 0.5*images->first.width);
  r_image.copyTo (im);
  im.adjustROI (0, 0, 0.5*images->first.width, 0);
  //prepare image and pcl to be published for rospy
  sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), images->first.encoding, im).toImageMsg();

  /*Publish the image and cloud*/
 // imagePub.publish(msg);  

int main (int argc, char** argv)
  ros::init(argc, argv, "ensensor_bridge_node"); 
   ROS_INFO("Started node %s", ros::this_node::getName().c_str());

   ensenso_ptr.reset (new pcl::EnsensoGrabber);

   //ensenso_ptr->initExtrinsicCalibration (5); // Disable projector if you want good looking images.
   boost::function<void(const PointCloudT::Ptr&, const boost::shared_ptr<PairOfImages>&)> f \
                            = boost::bind(&callback, _1, _2);

   ensenso_ptr->start ();

   ros::Rate rate(5);
     ensenso_ptr->registerCallback (f);

    ensenso_ptr->stop ();
   ensenso_ptr->closeDevice ();
   ensenso_ptr->closeTcpPort ();

   return EXIT_SUCCESS;

Originally posted by lakehanne on ROS Answers with karma: 152 on 2017-01-14

Post score: 0

Original comments

Comment by gvdhoorn on 2017-01-15:
Not an answer, but perhaps you can skip all of this and re-use crigroup/ensenso.

Comment by NEngelhard on 2017-01-15:
There is an official ROS-Driver from Ensenso in development, so no one should invest much time in a new driver.

Comment by gvdhoorn on 2017-01-15:
I'm aware of that, hence my suggestion to use the crigroup code for now.

Comment by lakehanne on 2017-01-15:
I am aware of crigroup's work but it's not without its own issues. For one, the codebase is kind of big for me to learn before my impending deadline. I know not receiving point clouds is a simple issue that is somehow eluding me. I can subscribe to images but not pcls. Would still appreciate help

Comment by gvdhoorn on 2017-01-15:
I may be misunderstanding you, but seems to me that you just want to get clouds from an Ensenso device and publish them. Why would you need to understand what it is doing other than what you'd need to start the driver?

Comment by gvdhoorn on 2017-01-15:
As to your problem: in the while in main(..), you appear to registerCallback(..) every iteration, which in turn creates your pclPub every iteration. That could be a problem, as such things need time to register properly. It is always better to create pubs once, and then reuse them.


1 Answer 1


Rosanswers logo

Thanks to @gvdhoorn! Your suggestion seems to have fixed my problem. The publishers should be moved outside of the callback function to allow for time to process the publishers.

However, when subscribing to the image topic, I get spurious stuff on the screen such as

     [ERROR] [1484512025.632229919]: Tried to advertise a service that is already advertised in this node           [/camera/ir_image/compressed/set_parameters]
     [ERROR] [1484512025.682142849]: Tried to advertise a service that is already advertised in this node [/camera/ir_image/theora/set_parameters].

Now, I know this is well-documented here as is the case of openni launch files. It does not affect what I am trying to do but it would be good to know why ensenso generates this error as well.

Originally posted by lakehanne with karma: 152 on 2017-01-15

This answer was ACCEPTED on the original site

Post score: 0


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.