service faster than topic call
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();
}