00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 #include <ros/ros.h>
00011 #include <opencv/cv.h>
00012 #include <opencv/highgui.h>
00013 #include "cv_bridge/CvBridge.h"
00014 #include "sensor_msgs/Image.h"
00015 #include "sensor_msgs/CameraInfo.h"
00016 #include "camera_self_filter/mask.h"
00017
00018
00019
00020
00021
00022 int main(int argc, char **argv) {
00023 ros::init(argc, argv, "robot_self_filter_test");
00024
00025 ros::NodeHandle nh;
00026 ros::ServiceClient svc = nh.serviceClient<camera_self_filter::mask>("self_mask");
00027
00028 sensor_msgs::CvBridge bridge_mask;
00029
00030
00031 cvNamedWindow("alpha",1);
00032
00033 while (nh.ok()){
00034
00035 camera_self_filter::mask servicecall;
00036 servicecall.request.header.frame_id = "narrow_stereo_optical_frame";
00037 servicecall.request.header.stamp = ros::Time::now();
00038 svc.call(servicecall);
00039
00040 sensor_msgs::Image mask = servicecall.response.mask_image;
00041
00042 printf("width %d height %d", mask.width, mask.height);
00043
00044
00045
00046
00047
00048
00049
00050 sensor_msgs::ImageConstPtr maskptr = boost::make_shared<sensor_msgs::Image>(boost::ref(servicecall.response.mask_image));
00051
00052 IplImage* iplimagemask = bridge_mask.imgMsgToCv(maskptr, "passthrough");
00053
00054 cvShowImage("alpha", iplimagemask);
00055
00056
00057 cvWaitKey();
00058
00059
00060
00061
00062
00063 }
00064
00065
00066 }