ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

service faster than topic call

asked 2013-05-21 10:26:52 -0500

xelda1988 gravatar image

Hi all

I have the following Code, which I use to subscribe to openni pointcloud stream, which i get a pointcloud from and then want to pass this pointcloud to a service - but the service executes before the pointcloud is fully obtained. I think it has something todo with the ros::spinOnce loop, but I'm new to ROS, so yould you please help me:

//Subscribing to openni-node and save PCD to file

  #include <ros/ros.h>
  // PCL specific includes
  #include <pcl17/ros/conversions.h>
  #include <pcl17/point_cloud.h>
  #include <pcl17/point_types.h>

  #include <pcl17/io/pcd_io.h>
  #include <pcl17/filters/voxel_grid.h>

  #include <local_recognition_pose/msg.h>

  typedef local_recognition_pose::msg PEMsgT;

  sensor_msgs::PointCloud2 output;
  void callback(const sensor_msgs::PointCloud2ConstPtr& input)
  {
//Inside the callback should be all the process that you want to do with your point cloud and at the end publish the results.
printf ("Cloudsize = %d Points\n", input->width * input->height);

  pcl17::VoxelGrid<sensor_msgs::PointCloud2> sor;
sor.setInputCloud (input);
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (output);
printf ("After filtering Cloud = %d Points.\n", output.width * output.height);

//pcl17::io::savePCDFile("/home/alexander/fuerte_workspace/grab_recognize/data/test.pcd", output);
  }

  int main (int argc, char** argv)
  {
  // Initialize ROS
  ros::init (argc, argv, "grab_recognize");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe ("/camera/depth_registered/points", 1, callback);
  ros::spinOnce();

  ros::NodeHandle nh2;

// Subscribe to local recognition pose services
ROS_INFO("Subscribing to local recogniton pose service...");
ros::service::waitForService("/local_recognition_pose/estimate_pose");
ros::ServiceClient clientPE = nh2.serviceClient<PEMsgT>("/local_recognition_pose/estimate_pose");

ROS_INFO("Calling local recogniton pose service for first scene...");
PEMsgT msg1;
      msg1.request.objects_path = "/data/PLY-MODELS";
msg1.request.scene = output;
if(!clientPE.call(msg1)) {
    ROS_ERROR("Something went wrong when calling local recognition pose estimation service");
    return 1;
}

  // Spin
     ros::spinOnce();
  }
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2013-05-21 11:09:41 -0500

Bill Smart gravatar image

The spinOnce() will not wait for a message to come in. In fact, unless the timing is very lucky, then it will never see a message. If you want to do something when a point cloud comes in, you should put your service call in the point cloud callback.

edit flag offensive delete link more

Question Tools

Stats

Asked: 2013-05-21 10:26:52 -0500

Seen: 548 times

Last updated: May 21 '13