iorodeo avatar iorodeo committed 5a891ed Draft

Worked on frame_drop_tester.

Comments (0)

Files changed (7)

mct_blob_finder/manifest.xml

   <depend package="cv_bridge"/>
   <depend package="sensor_msgs"/>
   <depend package="mct_msg_and_srv"/>
+  <depend package="mct_introspection"/>
 
   <!-- Temporary  *************** -->
   <depend package="std_msgs" />

mct_blob_finder/nodes/blob_finder_node.py

 
 from cv_bridge.cv_bridge import CvBridge 
 from mct_blob_finder import BlobFinder
+import mct_introspection
 
 # Messages
 from sensor_msgs.msg import Image
+from mct_msg_and_srv.msg import SeqAndImage
 from mct_msg_and_srv.msg import BlobData
 from mct_msg_and_srv.msg import Blob
 
         self.blobFinder.filter_by_area = True 
         self.blobFinder.min_area = 0
         self.blobFinder.max_area = 200
+        self.topic_type = mct_introspection.get_topic_type(topic)
 
         rospy.init_node('blob_finder')
         self.ready = False
-        self.image_sub = rospy.Subscriber(self.topic,Image,self.image_callback)
+        if self.topic_type == 'sensor_msgs/Image':
+            self.image_sub = rospy.Subscriber(self.topic,Image,self.image_callback)
+        else:
+            self.image_sub = rospy.Subscriber(self.topic,SeqAndImage,self.seq_and_image_callback)
         self.image_pub = rospy.Publisher('image_blobs', Image)
         self.blob_data_pub = rospy.Publisher('blob_data', BlobData)
         node_name = rospy.get_name()
         resp_args = (threshold, filter_by_area, min_area, max_area)
         return  BlobFinderGetParamResponse(*resp_args)
 
-
-    def image_callback(self,data):
+    def publish_blob_data(self,blobs_list, blobs_image, image_header, image_seq):
         """
-        Callback for image topic subscription - finds blobs in image.
+        Publish image of blobs and blob data.
         """
-        if not self.ready:
-            return 
-
-        with self.lock:
-            blobs_list, blobs_image = self.blobFinder.findBlobs(data)
-
-        #print(len(blobs_list))
-
-        # Publish image of blobs
         blobs_rosimage = self.bridge.cv_to_imgmsg(blobs_image,encoding="passthrough")
         self.image_pub.publish(blobs_rosimage)
 
         # Create the blob data message and publish
         blob_data_msg = BlobData()
-        blob_data_msg.header = data.header
+        blob_data_msg.header = image_header
+        blob_data_msg.image_seq = image_seq
         blob_data_msg.number_of_blobs = len(blobs_list)
         for b in blobs_list:
             blob_msg = Blob()
             blob_data_msg.blob.append(blob_msg)
         self.blob_data_pub.publish(blob_data_msg)
 
+    def image_callback(self,data):
+        """
+        Callback for image topic subscription.
+        """
+        if not self.ready:
+            return 
+        with self.lock:
+            blobs_list, blobs_image = self.blobFinder.findBlobs(data)
+        self.publish_blob_data(blobs_list, blobs_image, data.header, data.header.seq)
+
+
+    def seq_and_image_callback(self,data):
+        """
+        Callback for SeqAndImage topic subscription.
+        """
+        if not self.ready:
+            return 
+        with self.lock:
+            blobs_list, blobs_image = self.blobFinder.findBlobs(data.image)
+        self.publish_blob_data(blobs_list, blobs_image, data.image.header, data.seq)
+
+
     def run(self):
         rospy.spin()
 

mct_frame_drop_test/manifest.xml

   <review status="unreviewed" notes=""/>
   <url>http://ros.org/wiki/mct_frame_drop_test</url>
   <depend package="rospy"/>
+  <depend package="mct_xml_tools"/>
+  <depend package="mct_msg_and_srv"/>
+  <depend package="mct_introspection"/>
 
 </package>
 

mct_frame_drop_test/nodes/frame_drop_tester.py

+#!/usr/bin/env python
+from __future__ import print_function
+import roslib
+roslib.load_manifest('mct_frame_drop_test')
+import rospy
+
+import os
+import os.path
+import tempfile
+import subprocess
+import functools
+import threading
+import mct_introspection
+
+from mct_xml_tools import launch
+from mct_msg_and_srv.msg import BlobData
+
+class Frame_Drop_Tester(object):
+
+    def __init__(self, camera_number_0, camera_number_1, output_file):
+        self.ready = False
+        self.tester_popen = None
+        self.lock =  threading.Lock()
+        self.camera_0 = 'camera_{0}'.format(camera_number_0)
+        self.camera_1 = 'camera_{0}'.format(camera_number_1)
+        self.output_file = output_file
+        self.fid = open(self.output_file,'w')
+        self.camera_list = [self.camera_0, self.camera_1]
+        self.tmp_dir = tempfile.gettempdir()
+        self.launch_file = os.path.join(self.tmp_dir,'frame_drop_test.launch')
+        rospy.on_shutdown(self.clean_up)
+        rospy.init_node('frame_drop_tester')
+        self.launch_tester()
+        self.seq_to_blob_data = {}
+        blob_data_topics = self.get_blob_data_topics()
+
+        blob_data_sub = {}
+        for topic in blob_data_topics:
+            camera = topic.split('/')[2]
+            handler = functools.partial(self.blob_data_handler,camera)
+            blob_data_sub[camera] = rospy.Subscriber(topic,BlobData,handler)
+
+        self.ready = True
+
+    def blob_data_handler(self,camera,data):
+        if not self.ready:
+            return
+        with self.lock:
+            try:
+                self.seq_to_blob_data[data.image_seq][camera] = data
+            except KeyError:
+                self.seq_to_blob_data[data.image_seq] = {camera:data}
+
+    def get_blob_data_topics(self):
+        blob_data_topics = []
+        while len(blob_data_topics) < 2:
+            blob_data_topics = mct_introspection.find_topics_w_ending('blob_data')
+            blob_data_topics = [t for t in blob_data_topics if 'frame_drop_test' in t]
+        return blob_data_topics
+
+    def launch_tester(self):
+        launch.create_frame_drop_test_launch(self.launch_file,self.camera_0,self.camera_1)
+        self.tester_popen = subprocess.Popen(['roslaunch',self.launch_file])
+
+    def kill_tester(self):
+        if self.tester_popen is not None:
+            self.tester_popen.send_signal(subprocess.signal.SIGINT)
+            self.tester_popen = None
+            try:
+                os.remove(self.launch_file)
+            except OSError, e:
+                rospy.logwarn('Error removing frame skipper launch file: {0}'.format(str(e)))
+
+    def run(self):
+        while not rospy.is_shutdown():
+            with self.lock:
+                for seq, blob_data in sorted(self.seq_to_blob_data.items()):
+                    if len(blob_data) == 2:
+                        num0 = blob_data[self.camera_0].number_of_blobs
+                        num1 = blob_data[self.camera_1].number_of_blobs
+                        print('seq:', seq)
+                        print('ok: ', num0==num1)
+                        print('number_of_blobs:')
+                        print('  {0} {1}'.format(self.camera_0,num0))
+                        print('  {0} {1}'.format(self.camera_1,num1))
+                        print('---- ')
+                        print()
+                        self.fid.write('{0} {1} {2}\n'.format(seq,num0,num1))
+                        del self.seq_to_blob_data[seq]
+
+    def clean_up(self):
+        self.kill_tester()
+        self.fid.close()
+
+# -----------------------------------------------------------------------------
+if __name__ == '__main__':
+    import sys
+    camera_number_0 = int(sys.argv[1])
+    camera_number_1 = int(sys.argv[2])
+    try:
+        output_file = sys.argv[3]
+    except IndexError:
+        output_file = 'frame_drop_data.txt'
+
+    node = Frame_Drop_Tester(camera_number_0, camera_number_1,output_file)
+    node.run()

mct_msg_and_srv/msg/BlobData.msg

 Header header
 uint32 number_of_blobs 
+uint32 image_seq
 Blob[] blob

mct_xml_tools/src/mct_xml_tools/launch.py

     with open(filename,'w') as f:
         f.write(xml_str)
 
-def create_frame_drop_test_launch(filename,camera0,camera1,display=False):
+def create_frame_drop_test_launch(filename,camera0,camera1,display=True):
     """
     Creates launch file for the blob_finder based frame drop test.
     """
     template_name = 'frame_drop_test_launch.xml'
     machine_file = mct_utilities.file_tools.machine_launch_file
 
-    image_corr_topic_list = mct_introspection.find_topics_w_ending('image_corr')
+    image_corr_topic_list = mct_introspection.find_topics_w_ending('seq_and_image_corr')
     camera_to_image_corr = {}
     for topic in image_corr_topic_list:
         for camera in (camera0, camera1):
     template = jinja2_env.get_template(template_name)
     xml_str = template.render(
             machine_file=machine_file, 
-            blob_finder_launch=blob_finder_launch
+            blob_finder_launch=blob_finder_launch,
+            display=display,
             )
     with open(filename,'w') as f:
         f.write(xml_str)
 
 
 
-    
-
-
 # -----------------------------------------------------------------------------
 if __name__ == '__main__':
 

mct_xml_tools/src/mct_xml_tools/three_point_tracker.launch

-<launch>
-    <!-- Autogenerated launch file - do not hand edit -->
-    <include file="/home/albert/ros/mct_config/machine/mct.machine" />
-    <group ns="three_point_tracker_params">
-        <rosparam command="load" file="$(env MCT_CONFIG)/tracking_2d/three_point_tracker_params.yaml"/>
-    </group>
-    
-    <!-- maze trackers -->
-    
-    <group ns="/mct_slave1/camera_1/camera">
-        <node pkg="mct_tracking_2d" type="three_point_tracker.py" name="three_point_tracker" args="/mct_slave1/camera_1/camera/seq_and_image_corr" machine="mct_slave1"/>
-    </group>
-    
-    <group ns="/mct_master/camera_2/camera">
-        <node pkg="mct_tracking_2d" type="three_point_tracker.py" name="three_point_tracker" args="/mct_master/camera_2/camera/seq_and_image_corr" machine="mct_master"/>
-    </group>
-    
-    <group ns="/mct_slave3/camera_3/camera">
-        <node pkg="mct_tracking_2d" type="three_point_tracker.py" name="three_point_tracker" args="/mct_slave3/camera_3/camera/seq_and_image_corr" machine="mct_slave3"/>
-    </group>
-    
-    <group ns="/mct_slave3/camera_4/camera">
-        <node pkg="mct_tracking_2d" type="three_point_tracker.py" name="three_point_tracker" args="/mct_slave3/camera_4/camera/seq_and_image_corr" machine="mct_slave3"/>
-    </group>
-    
-    <group ns="/mct_master/camera_5/camera">
-        <node pkg="mct_tracking_2d" type="three_point_tracker.py" name="three_point_tracker" args="/mct_master/camera_5/camera/seq_and_image_corr" machine="mct_master"/>
-    </group>
-    
-    <group ns="/mct_slave1/camera_7/camera">
-        <node pkg="mct_tracking_2d" type="three_point_tracker.py" name="three_point_tracker" args="/mct_slave1/camera_7/camera/seq_and_image_corr" machine="mct_slave1"/>
-    </group>
-    
-    <group ns="/mct_slave3/camera_8/camera">
-        <node pkg="mct_tracking_2d" type="three_point_tracker.py" name="three_point_tracker" args="/mct_slave3/camera_8/camera/seq_and_image_corr" machine="mct_slave3"/>
-    </group>
-    
-    <group ns="/mct_slave2/camera_9/camera">
-        <node pkg="mct_tracking_2d" type="three_point_tracker.py" name="three_point_tracker" args="/mct_slave2/camera_9/camera/seq_and_image_corr" machine="mct_slave2"/>
-    </group>
-    
-    <group ns="/mct_slave2/camera_10/camera">
-        <node pkg="mct_tracking_2d" type="three_point_tracker.py" name="three_point_tracker" args="/mct_slave2/camera_10/camera/seq_and_image_corr" machine="mct_slave2"/>
-    </group>
-    
-    <group ns="/mct_slave1/camera_11/camera">
-        <node pkg="mct_tracking_2d" type="three_point_tracker.py" name="three_point_tracker" args="/mct_slave1/camera_11/camera/seq_and_image_corr" machine="mct_slave1"/>
-    </group>
-    
-    <group ns="maze">
-        <node pkg="mct_tracking_2d" type="three_point_tracker_synchronizer.py" name="three_point_tracker_synchronizer" args="maze" machine="mct_master"/>
-    </group>
-    
-    <!-- sleepbox trackers -->
-    
-    <group ns="/mct_master/camera_12/camera">
-        <node pkg="mct_tracking_2d" type="three_point_tracker.py" name="three_point_tracker" args="/mct_master/camera_12/camera/seq_and_image_corr" machine="mct_master"/>
-    </group>
-    
-    <group ns="sleepbox">
-        <node pkg="mct_tracking_2d" type="three_point_tracker_synchronizer.py" name="three_point_tracker_synchronizer" args="sleepbox" machine="mct_master"/>
-    </group>
-    
-</launch>
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.