2011/ros11: stream_dragonfly2.m

File stream_dragonfly2.m, 2.8 KB (added by cteo, 7 years ago)

stream images from camera to Matlab

Line 
1% Created: Benjamin Cohen, Spring 2011
2% Modified: CL Teo, May 17, 2011 for Erratic Dragonfly - get RGB images
3% and do processing using Felzenswab's detectors
4% this version returns bounding box results back to ROS
5
6clear all;
7close all;
8
9%% Preprocessing Setup
10% setup required paths for the task
11VOC_PATH=fullfile('~', 'workspace', 'voc-release4');
12addpath(VOC_PATH);
13% we detect people here only
14load(fullfile(VOC_PATH,'VOC2007','person_final.mat'));
15do_proc = 0;
16%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
17
18%% IPC Setup -- required
19% add the ipc_bridge_matlab binaries to your path
20[a, p] = system('rospack find ipc_sensor_msgs');
21addpath(strcat(p, '/bin'));
22[a, p] = system('rospack find ipc_geometry_msgs');
23addpath(strcat(p, '/bin'));
24
25
26% 'subscribe' to the RGB image topics
27sid=sensor_msgs_Image('connect','subscriber','matlab_rgb_image_node_pub','rgb');
28mode='read'; % subscribe so we read
29
30pid=geometry_msgs_Twist('connect','publisher','matlab_proc_det_sub','det_res');
31mode2='send'; % publish
32
33% test... DOES NOT WORK
34%sid2=sensor_msgs_Image('connect','publisher','matlab_rgb_image_node_sub','rgb');
35%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
36
37
38%% Main Loop to process images from ROS %%%%%
39for i = 1:1000
40    tic
41    % get one image
42    imgout = getSensorImg_IPC(sid, mode);
43    % check if you got somthing to process
44    %% Perform all processing here %%%%%
45    if (~isempty(imgout))
46       
47        if(~do_proc)
48            % show output
49            image(imgout); title('Dragonfly RGB','FontSize',14,'FontWeight','Bold');
50            axis image;
51            drawnow;
52        else
53            % detect an object using part-based detector
54            imgout = imresize(imgout,0.2);
55            imgout = min(max(imgout,0),1);
56            bbox = process(imgout, model, 0);
57            showboxes(imgout, bbox);
58            drawnow;
59
60       
61            %% Send image back to ROS -- does not work yet! Complexity of
62            % converting images back to ROS format....
63    %         rgb_msg = sensor_msgs_Image('read',sid, 50000);
64    %         msg=sensor_msgs_Image('empty');
65    %         msg=sensor_msgs_Image(rgb_msg);
66    %         sensor_msgs_Image('send',sid2,imgout);
67
68            %% Send bounding box results back to ROS
69            % check all the /bin files in ipc_geometry_msgs for the various
70            % publishers and subscribers...
71            if(~isempty(bbox))
72                % create an empty geometry_msgs/Twist message structure
73                msg=geometry_msgs_Twist('empty');
74
75                % assign some values
76                msg.linear.x = bbox(1);
77                msg.linear.y = bbox(2);
78                msg.linear.z = bbox(3);
79                % publish to ROS
80                geometry_msgs_Twist(mode2,pid,msg);
81            end
82        end
83       
84     else
85        fprintf('no rgb image grabbed\n');
86    end
87    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
88    toc
89end
90
91
92