Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <ros/ros.h>
00031
00032 #include <actionlib/client/simple_action_client.h>
00033
00034 #include <sensor_msgs/Image.h>
00035 #include <sensor_msgs/CameraInfo.h>
00036 #include <sensor_msgs/PointCloud2.h>
00037
00038 #include <stereo_msgs/DisparityImage.h>
00039
00040 #include "pr2_gripper_click/SelectRegionAction.h"
00041
00042 int main(int argc, char **argv)
00043 {
00044 ros::init(argc, argv, "test_select_region");
00045 ros::NodeHandle nh("~");
00046 std::string topic("/select_region_ui");
00047
00048 actionlib::SimpleActionClient<pr2_gripper_click::SelectRegionAction> client(topic, true);
00049 while(!client.waitForServer(ros::Duration(2.0)) && nh.ok())
00050 {
00051 ROS_INFO("Waiting for action client on topic %s", topic.c_str());
00052 }
00053
00054 while (nh.ok())
00055 {
00056 sensor_msgs::Image::ConstPtr recent_image;
00057 stereo_msgs::DisparityImage::ConstPtr recent_disparity_image;
00058 sensor_msgs::CameraInfo::ConstPtr recent_camera_info;
00059
00060 ROS_INFO("Waiting for messages...");
00061 std::string image_topic("/narrow_stereo/left/image_rect");
00062 std::string disparity_image_topic("/narrow_stereo_textured/disparity");
00063 std::string camera_info_topic("/narrow_stereo_textured/left/camera_info");
00064 std::string point_cloud_topic("/narrow_stereo_textured/points2");
00065 while ((!recent_image || !recent_disparity_image || !recent_camera_info) && nh.ok())
00066 {
00067 if (!recent_image)
00068 {
00069 ROS_INFO_STREAM(" Waiting for message on topic " << image_topic);
00070 recent_image = ros::topic::waitForMessage<sensor_msgs::Image>(image_topic, nh, ros::Duration(0.5));
00071 }
00072 if (!recent_disparity_image)
00073 {
00074 ROS_INFO_STREAM(" Waiting for message on topic " << disparity_image_topic);
00075 recent_disparity_image = ros::topic::waitForMessage<stereo_msgs::DisparityImage>
00076 (disparity_image_topic, nh, ros::Duration(0.5));
00077 }
00078 if (!recent_camera_info)
00079 {
00080 ROS_INFO_STREAM(" Waiting for message on topic " << camera_info_topic);
00081 recent_camera_info = ros::topic::waitForMessage<sensor_msgs::CameraInfo>
00082 (camera_info_topic, nh, ros::Duration(0.5));
00083 }
00084 }
00085 if (!nh.ok()) exit(0);
00086 ROS_INFO("All required messages received; sending goal to gripper click action");
00087
00088 pr2_gripper_click::SelectRegionGoal click_goal;
00089 click_goal.sensor_data.image = *recent_image;
00090 click_goal.sensor_data.disparity_image = *recent_disparity_image;
00091 click_goal.sensor_data.camera_info = *recent_camera_info;
00092
00093 client.sendGoal(click_goal);
00094 while (!client.waitForResult(ros::Duration(0.5)) && nh.ok())
00095 {
00096 }
00097 if (!nh.ok())
00098 {
00099 client.cancelGoal();
00100 break;
00101 }
00102
00103 if (client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
00104 {
00105 ROS_INFO("The select region action has not succeeded;");
00106 }
00107 else
00108 {
00109 ROS_INFO("Select region has succeeded;");
00110 }
00111 ros::Duration(3.0).sleep();
00112 }
00113 }