Ask Your Question
0

Not receiving published clouds

asked 2017-01-14 20:30:52 -0600

updated 2017-01-14 20:31:59 -0600

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>
/*typedefs*/
using PairOfImages =  std::pair<pcl::PCLImage, pcl::PCLImage>;  //for the Ensenso grabber callback 
using PointT = pcl::PointXYZ;
using PointCloudT = pcl::PointCloud<PointT>;

  /*pointers*/
pcl::EnsensoGrabber::Ptr ensenso_ptr;

  /*Globals*/
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*/
  pclPub.publish(pcl2_msg);
 // imagePub.publish(msg);  
}

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

   ensenso_ptr.reset (new pcl::EnsensoGrabber);
  ensenso_ptr->openTcpPort();
  ensenso_ptr->openDevice();
  ensenso_ptr->enumDevices();

   //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);
   while(ros::ok())
   {    
     ensenso_ptr->registerCallback (f);
     ros::spin();
     rate.sleep();
   }

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

   return EXIT_SUCCESS;
 }
edit retag flag offensive close merge delete

Comments

Not an answer, but perhaps you can skip all of this and re-use crigroup/ensenso.

gvdhoorn gravatar imagegvdhoorn ( 2017-01-15 04:54:52 -0600 )edit

There is an official ROS-Driver from Ensenso in development, so no one should invest much time in a new driver.

NEngelhard gravatar imageNEngelhard ( 2017-01-15 09:26:00 -0600 )edit

I'm aware of that, hence my suggestion to use the crigroup code for now.

gvdhoorn gravatar imagegvdhoorn ( 2017-01-15 09:48:07 -0600 )edit

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

lakehanne gravatar imagelakehanne ( 2017-01-15 10:16:34 -0600 )edit

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?

gvdhoorn gravatar imagegvdhoorn ( 2017-01-15 10:38:13 -0600 )edit

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.

gvdhoorn gravatar imagegvdhoorn ( 2017-01-15 10:40:33 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2017-01-15 14:32:34 -0600

updated 2017-01-15 14:33:30 -0600

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.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2017-01-14 20:30:52 -0600

Seen: 88 times

Last updated: Jan 15 '17