grab_result.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <pr2_tilt_laser_interface/GetSnapshotActionResult.h>
00003 #include <pr2_tilt_laser_interface/GetSnapshotActionFeedback.h>
00004 #include <sensor_msgs/PointCloud2.h>
00005 #include <pcl/io/pcd_io.h>
00006 
00007 ros::Publisher pub;
00008 void 
00009   resultCallback(const pr2_tilt_laser_interface::GetSnapshotActionResultConstPtr& action_result)
00010 {
00011   if (action_result->result.cloud.height*action_result->result.cloud.height == 0)
00012     return;
00013   pub.publish (action_result->result.cloud);
00014   ROS_INFO ("Published point cloud from action result!\n");
00015 }
00016 
00017 void 
00018   feedbackCallback(const pr2_tilt_laser_interface::GetSnapshotActionFeedbackConstPtr& action_feedback)
00019 {
00020   if (action_feedback->feedback.cloud.height*action_feedback->feedback.cloud.height == 0)
00021     return;
00022   pub.publish (action_feedback->feedback.cloud);
00023   ROS_INFO ("Published point cloud from action feedback!\n");
00024 }
00025 
00026 int 
00027   main (int argc, char** argv)
00028 {
00029   ros::init (argc, argv, "result_grabber");
00030   ros::NodeHandle nh;
00031   pub = nh.advertise<sensor_msgs::PointCloud2>("snapshot", 1, true);
00032   ros::Subscriber result_subscriber = nh.subscribe("get_laser_snapshot/result", 1, resultCallback);
00033   ros::Subscriber feedback_subscriber = nh.subscribe("get_laser_snapshot/feedback", 1, feedbackCallback);
00034   
00035   ROS_INFO ("Waiting for cloud to come in over ROS");
00036   ros::spin();
00037 
00038 
00039   //while (nh.ok ())
00040   //{
00041     //ROS_INFO ("Waiting for cloud to come in over ROS");
00042     //pr2_tilt_laser_interface::GetSnapshotActionResultConstPtr action_result = ros::topic::waitForMessage<pr2_tilt_laser_interface::GetSnapshotActionResult>("get_laser_snapshot/result");
00043     //sensor_msgs::PointCloud2 cloud = action_result->result.cloud;
00044 
00045     //pub.publish (cloud);
00046     //ROS_INFO ("Published the point cloud!\n");
00047     //ros::spinOnce ();
00048     //ros::Duration (0.5).sleep ();
00049   //}
00050 
00051   return (0);
00052 }


pr2_tilt_laser_interface
Author(s): Radu Rusu, Wim Meeussen, Vijay Pradeep
autogenerated on Sat Dec 28 2013 17:25:13