Commits

iorodeo committed 91b10ca

Created frame skipper package.

Comments (0)

Files changed (14)

mct_frame_skipper/CMakeLists.txt

+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})

mct_frame_skipper/Makefile

+include $(shell rospack find mk)/cmake.mk

mct_frame_skipper/mainpage.dox

+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b mct_frame_skipper is ... 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/

mct_frame_skipper/manifest.xml

+<package>
+  <description brief="mct_frame_skipper">
+
+     mct_frame_skipper
+
+  </description>
+  <author>Albert Lee</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/mct_frame_skipper</url>
+  <depend package="rospy"/>
+
+</package>
+
+

mct_frame_skipper/nodes/frame_skipper.py

+#!/usr/bin/env python
+from __future__ import print_function
+import roslib
+roslib.load_manifest('mct_tracking_2d')
+import rospy
+import sys
+import threading
+
+import cv
+from cv_bridge.cv_bridge import CvBridge 
+from mct_blob_finder import BlobFinder
+from mct_utilities import file_tools
+
+# Messages
+from sensor_msgs.msg import Image
+from sensor_msgs.msg import CameraInfo
+
+
+class FrameSkipper(object):
+
+    """
+    Frame skipper node - subscribes to the given image topic and re-published only 
+    those frames which are divisible by the frame skip parameter.
+    """
+
+    def __init__(self, topic=None, skip_param=3, max_stamp_age=1.5):
+        self.ready = False
+        self.topic = topic
+        self.skip_param = skip_param
+        self.lock = threading.Lock() 
+        self.camera_info_topic = get_camera_info_from_image_topic(self.topic)
+        self.max_stamp_age = max_stamp_age
+        self.latest_stamp = None
+
+
+        # Data dictionareis for synchronizing tracking data with image seq number
+        self.stamp_to_image = {}
+        self.stamp_to_seq = {}
+        self.seq_to_stamp_and_image = {}
+
+        rospy.init_node('frame_skipper')
+
+        # Subscribe to image and camera info topics
+        self.image_sub = rospy.Subscriber(
+                self.topic, 
+                Image, 
+                self.image_callback
+                )
+        self.info_sub = rospy.Subscriber(
+                self.camera_info_topic, 
+                CameraInfo, 
+                self.camera_info_callback
+                )
+
+        # Set up image publisher
+        repub_topic = '{0}_skip'.format(self.topic)
+        print(repub_topic)
+        self.image_repub = rospy.Publisher(repub_topic,Image)
+
+        self.ready = True
+
+
+    def camera_info_callback(self,data):
+        """
+        Callback for camera info topic subscription - used to associate stamp
+        with the image seq number.
+        """
+        if not self.ready:
+            return
+        stamp_tuple = data.header.stamp.secs, data.header.stamp.nsecs
+        with self.lock:
+            self.latest_stamp = stamp_tuple
+            self.stamp_to_seq[stamp_tuple] = data.header.seq
+
+    def image_callback(self,data):
+        """
+        Callback for image topic subscription - used to associate the stamp tuple with
+        the imcomming image.
+        """
+        if not self.ready:
+            return 
+        stamp_tuple = data.header.stamp.secs, data.header.stamp.nsecs
+        with self.lock:
+            self.stamp_to_image[stamp_tuple] = data
+
+
+    def run(self):
+        """
+        Main loop - associates images and time stamps  w/ image sequence
+        numbers and re-publishes images whose sequence numbers are divisible by
+        the frame skip parameter.
+        """
+        while not rospy.is_shutdown():
+
+            with self.lock:
+
+                # Associate data with image seq numbers
+                for stamp, image in self.stamp_to_image.items():
+                    try:
+                        seq = self.stamp_to_seq[stamp]
+                        seq_found = True
+                    except KeyError:
+                        seq_found = False
+
+                    if seq_found:
+                        self.seq_to_stamp_and_image[seq] = stamp, image 
+                        try:
+                            del self.stamp_to_image[stamp]
+                        except KeyError:
+                            pass
+                        try:
+                            del self.stamp_to_seq[stamp]
+                        except KeyError:
+                            pass
+                       
+                    else:
+                        # Throw away data greater than the maximum allowed age
+                        if self.latest_stamp is not None:
+                            latest_stamp_secs = stamp_tuple_to_secs(self.latest_stamp)
+                            stamp_secs = stamp_tuple_to_secs(stamp)
+                            stamp_age = latest_stamp_secs - stamp_secs
+                            if stamp_age > self.max_stamp_age:
+                                try:
+                                    del self.stamp_to_image[stamp]
+                                except KeyError:
+                                    pass
+
+                # Re-publish image 
+                for seq, stamp_and_image in sorted(self.seq_to_stamp_and_image.items()):
+                    stamp_tuple, image = stamp_and_image 
+                    if seq%self.skip_param == 0:
+                        self.image_repub.publish(image)
+                    del self.seq_to_stamp_and_image[seq]
+
+
+def get_camera_info_from_image_topic(topic):
+    """
+    Returns the camera info topic given an image topic from that camera
+    """
+    topic_split = topic.split('/')
+    info_topic = topic_split[:-1]
+    info_topic.append('camera_info')
+    info_topic = '/'.join(info_topic)
+    return info_topic
+
+def stamp_tuple_to_secs(stamp):
+    """
+    Converts a stamp tuple (secs,nsecs) to seconds.
+    """
+    return stamp[0] + stamp[1]*1.0e-9
+
+# -----------------------------------------------------------------------------
+if __name__ == '__main__':
+    topic = sys.argv[1]
+    skip_param = int(sys.argv[2])
+    node = FrameSkipper(topic,skip_param)
+    node.run()
+
+
+
+

mct_frame_skipper/nodes/frame_skipper_master.py

+#!/usr/bin/env python
+from __future__ import print_function
+import roslib
+roslib.load_manifest('mct_image_stitcher')
+import rospy
+
+import os
+import os.path
+import tempfile
+import subprocess
+
+from mct_xml_tools import launch
+
+# Services
+from mct_msg_and_srv.srv import CommandString 
+from mct_msg_and_srv.srv import CommandStringResponse
+
+class Frame_Skipper_Master(object):
+
+    def __init__(self):
+        self.tmp_dir = tempfile.gettempdir()
+        self.launch_file = os.path.join(self.tmp_dir,'frame_skipper.launch')
+        self.skipper_popen = None
+
+        rospy.on_shutdown(self.clean_up)
+        rospy.init_node('frame_skipper_master')
+
+        self.camera_srv = rospy.Service(
+                'frame_skipper_master',
+                CommandString,
+                self.handle_skipper_srv,
+                )
+
+    def handle_skipper_srv(self,req):
+        """
+        Handles requests to launch/kill the image stitcher nodes.
+        """
+        command = req.command.lower()
+        response = True
+        message = ''
+        if command == 'start':
+            if self.skipper_popen is None:
+                self.launch_frame_skippers()
+            else:
+                response = False
+                message = 'frame skippers already running'
+        elif command == 'stop':
+            if self.skipper_popen is not None:
+                self.kill_frame_skippers()
+            else:
+                response = False
+                message = 'frame skippers not running'
+        return CommandStringResponse(response,message)
+
+    def launch_frame_skippers(self):
+        """
+        Launches the image stitcher  nodes.
+        """
+        if self.skipper_popen is None:
+            launch.create_image_stitcher_launch(self.launch_file)
+            self.skipper_popen = subprocess.Popen(['roslaunch',self.launch_file])
+
+    def kill_frame_skippers(self):
+        """
+        Kills the image stitchers nodes.
+        """
+        if self.skipper_popen is not None:
+            self.skipper_popen.send_signal(subprocess.signal.SIGINT)
+            self.skipper_popen = None
+            try:
+                os.remove(self.launch_file)
+            except OSError, e:
+                rospy.logwarn('Error removing image stitcher launch file: {0}'.format(str(e)))
+
+    def run(self):
+        rospy.spin()
+
+    def clean_up(self):
+        self.kill_frame_skippers()
+
+# -----------------------------------------------------------------------------
+if __name__ == '__main__':
+
+    node = Frame_Skipper_Master()
+    node.run()

mct_image_stitcher/nodes/image_stitcher.py

             # Throw away any stale data in seq to images buffer
             seq_age = self.get_seq_age(seq)
             if seq_age > self.max_seq_age:
-                del self.seq_to_images[seq]
+                try:
+                    del self.seq_to_images[seq]
+                except KeyError:
+                    pass
 
 
     def run(self):

mct_introspection/src/mct_introspection/introspection.py

     return camera_topic_base
             
 
-
-
 # -----------------------------------------------------------------------------
 if __name__ == '__main__':
     
         topic_list = find_camera_info_topics()
         print(topic_list)
 
-    if 0:
+    if 1:
         node_list = get_camera_nodes()
         print(node_list)
 
         topics = find_topics_w_name('tracking_pts')
         print(topics,len(topics))
 
-    if 1:
+    if 0:
         base = get_camera_fullpath_topic('camera_1')
         print(base)
 

mct_tracking_2d/nodes/stitched_image_labeler.py

                         # Remove any elements seq_to_tracking_pts dict older than maximum allowed age
                         seq_age = self.latest_seq - seq
                         if seq_age > self.max_seq_age:
-                            del self.seq_to_trackig_pts[seq]
+                            del self.seq_to_tracking_pts[seq]
 
                 # Remove and elements form seq_to_stitched_image dict older than maximum allowed age
                 for seq in self.seq_to_stitched_image.keys():

mct_tracking_2d/nodes/three_point_tracker_synchronizer.py

         self.tracking_pts_pub = rospy.Publisher('tracking_pts', ThreePointTracker)
         self.image_tracking_pts = None
         self.image_tracking_pts_pub = rospy.Publisher('image_tracking_pts', Image)
-
         self.ready = True
         
-
     def create_camera_to_tracking_dict(self):
         """
         Creates a dictionary relating the cameras in the tracking region to their
             camera_fullpath_topic = mct_introspection.get_camera_fullpath_topic(camera)
             tracking_pts_topic = '{0}/tracking_pts'.format(camera_fullpath_topic)
             self.camera_to_tracking[camera] = tracking_pts_topic
-        
-
-        #tracking_pts_topics = mct_introspection.find_topics_w_name('tracking_pts')
-        #for topic in tracking_pts_topics:
-        #    topic_split = topic.split('/')
-        #    for camera in self.camera_list:
-        #        if camera in topic_split:
-        #            self.camera_to_tracking[camera] = topic
 
     def tracking_pts_handler(self,camera,msg):
         """

mct_xml_tools/src/mct_xml_tools/frame_skipper.launch

+<launch>
+    <!-- Autogenerated launch file - do not hand edit -->
+    <include file="/home/albert/ros/mct_config/machine/mct.machine" />
+    
+    <group ns="/mct_master/camera_11/camera">
+        <node pkg="mct_frame_skipper" type="frame_skipper.py" name="frame_skipper" args="/mct_master/camera_11/camera/image_rect 3" machine="mct_master" />
+    </group>
+    
+    <group ns="/mct_master/camera_3/camera">
+        <node pkg="mct_frame_skipper" type="frame_skipper.py" name="frame_skipper" args="/mct_master/camera_3/camera/image_rect 3" machine="mct_master" />
+    </group>
+    
+    <group ns="/mct_master/camera_6/camera">
+        <node pkg="mct_frame_skipper" type="frame_skipper.py" name="frame_skipper" args="/mct_master/camera_6/camera/image_raw 3" machine="mct_master" />
+    </group>
+    
+    <group ns="/mct_master/camera_8/camera">
+        <node pkg="mct_frame_skipper" type="frame_skipper.py" name="frame_skipper" args="/mct_master/camera_8/camera/image_rect 3" machine="mct_master" />
+    </group>
+    
+    <group ns="/mct_slave1/camera_1/camera">
+        <node pkg="mct_frame_skipper" type="frame_skipper.py" name="frame_skipper" args="/mct_slave1/camera_1/camera/image_rect 3" machine="mct_slave1" />
+    </group>
+    
+    <group ns="/mct_slave1/camera_2/camera">
+        <node pkg="mct_frame_skipper" type="frame_skipper.py" name="frame_skipper" args="/mct_slave1/camera_2/camera/image_rect 3" machine="mct_slave1" />
+    </group>
+    
+    <group ns="/mct_slave1/camera_4/camera">
+        <node pkg="mct_frame_skipper" type="frame_skipper.py" name="frame_skipper" args="/mct_slave1/camera_4/camera/image_rect 3" machine="mct_slave1" />
+    </group>
+    
+    <group ns="/mct_slave1/camera_9/camera">
+        <node pkg="mct_frame_skipper" type="frame_skipper.py" name="frame_skipper" args="/mct_slave1/camera_9/camera/image_rect 3" machine="mct_slave1" />
+    </group>
+    
+    <group ns="/mct_slave2/camera_10/camera">
+        <node pkg="mct_frame_skipper" type="frame_skipper.py" name="frame_skipper" args="/mct_slave2/camera_10/camera/image_rect 3" machine="mct_slave2" />
+    </group>
+    
+    <group ns="/mct_slave2/camera_12/camera">
+        <node pkg="mct_frame_skipper" type="frame_skipper.py" name="frame_skipper" args="/mct_slave2/camera_12/camera/image_rect 3" machine="mct_slave2" />
+    </group>
+    
+    <group ns="/mct_slave2/camera_5/camera">
+        <node pkg="mct_frame_skipper" type="frame_skipper.py" name="frame_skipper" args="/mct_slave2/camera_5/camera/image_rect 3" machine="mct_slave2" />
+    </group>
+    
+    <group ns="/mct_slave2/camera_7/camera">
+        <node pkg="mct_frame_skipper" type="frame_skipper.py" name="frame_skipper" args="/mct_slave2/camera_7/camera/image_rect 3" machine="mct_slave2" />
+    </group>
+     
+</launch>

mct_xml_tools/src/mct_xml_tools/launch.py

             )
     with open(filename,'w') as f:
         f.write(xml_str)
+
+
+def create_stitched_image_labeler_launch(filename):
+    """
+    Creates launch file for the stitched image labeler nodes based on the regions 
+    in the regions.yaml file.
+    """
+    template_name = 'stitched_image_labeler_launch.xml'
+    machine_file = mct_utilities.file_tools.machine_launch_file
+    regions_dict = mct_utilities.file_tools.read_tracking_2d_regions()
+
+    launch_dict = {}
+    for region in regions_dict:
+        arg0 = '/{0}/seq_and_image_stitched'.format(region)
+        arg1 = '/{0}/tracking_pts'.format(region)
+        launch_dict[region] = (arg0, arg1)
+
+    # Create xml launch file
+    jinja2_env = jinja2.Environment(loader=jinja2.FileSystemLoader(template_dir))
+    template = jinja2_env.get_template(template_name)
+    xml_str = template.render(
+            machine_file=machine_file, 
+            launch_dict=launch_dict,
+            )
+    with open(filename,'w') as f:
+        f.write(xml_str)
     
 
+def create_frame_skipper_launch(filename):
+    """
+    Creates launch file for frame skipper nodes. One for every camera.
+    """
+    template_name = 'frame_skipper_launch.xml'
+    machine_file = mct_utilities.file_tools.machine_launch_file
+
+
+    # Get frame skip parameter
+    stitching_params = mct_utilities.file_tools.read_tracking_2d_stitching_params()
+    skip_param = stitching_params['frame_skip']
+
+    # Get list of currently running camera names
+    camera_node_list = mct_introspection.get_camera_nodes()
+    camera_list = [node.split('/')[2] for node in camera_node_list]
+
+    # Get lists of rectified and raw image topics
+    image_raw_list = mct_introspection.find_camera_image_topics(transport='image_raw')
+    image_rect_list = mct_introspection.find_camera_image_topics(transport='image_rect')
+    
+    # Create dictionaries mapping cameras to raw and rectified images
+    camera_to_image_raw = {}
+    camera_to_image_rect = {}
+    camera_to_camera_topic = {}
+    for camera in camera_list:
+        for image_raw in image_raw_list:
+            if camera in image_raw.split('/'):
+                camera_to_image_raw[camera] = image_raw
+        for image_rect in image_rect_list:
+            if camera in image_rect.split('/'):
+                camera_to_image_rect[camera] = image_rect
+
+    # Assign image topic to frame skipper for each camera.  Use rectified image
+    # if it exists otherwise use raw image.
+    launch_list = []
+    for camera in camera_list:
+        try:
+            topic = camera_to_image_rect[camera]
+        except KeyError:
+            topic = camera_to_image_raw[camera]
+        topic_split = topic.split('/')
+        namespace = '/'.join(topic_split[:4])
+        machine = topic_split[1]
+        launch_list.append((namespace, topic, machine))
+
+
+    # Create xml launch file
+    jinja2_env = jinja2.Environment(loader=jinja2.FileSystemLoader(template_dir))
+    template = jinja2_env.get_template(template_name)
+    xml_str = template.render(
+            machine_file=machine_file, 
+            launch_list=launch_list,
+            skip_param = skip_param,
+            )
+    with open(filename,'w') as f:
+        f.write(xml_str)
 
 
 # -----------------------------------------------------------------------------
         filename = 'static_tf_publisher_2d.launch'
         create_static_tf_publisher_2d_launch(filename)
 
-    if 1:
+    if 0:
         filename = 'three_point_tracker.launch'
         create_three_point_tracker_launch(filename)
 
         filename = 'image_stitcher.launch'
         create_image_stitcher_launch(filename)
 
+    if 0:
+        filename = 'stitched_image_labeler.launch'
+        create_stitched_image_labeler_launch(filename)
+
+    if 1:
+        filename = 'frame_skipper.launch'
+        create_frame_skipper_launch(filename)
+
 
        
 

mct_xml_tools/src/mct_xml_tools/templates/frame_skipper_launch.xml

+<launch>
+    <!-- Autogenerated launch file - do not hand edit -->
+    <include file="{{machine_file}}" />
+    {% for namespace, topic, machine in launch_list %}
+    <group ns="{{namespace}}">
+        <node pkg="mct_frame_skipper" type="frame_skipper.py" name="frame_skipper" args="{{topic}} {{skip_param}}" machine="{{machine}}" />
+    </group>
+    {% endfor %} 
+</launch>

mct_xml_tools/src/mct_xml_tools/templates/stitched_image_labeler_launch.xml

+<launch>
+    <!-- Autogenerated launch file - do not hand edit -->
+    <include file="{{machine_file}}" />
+    {% for region, args in launch_dict.iteritems() %}
+    <group ns="{{region}}">
+        <node pkg="mct_tracking_2d" type="stitched_image_labeler.py" name="stitched_image_labeler" args="{{args[0]}} {{args[1]}} " machine="mct_master" />
+    </group>
+    {% endfor %} 
+</launch>