Commits

iorodeo committed e3c7710

Worked on launch file for homography calibrator. Added launch for active target node.
Put homography calibrator node parameters on the paratmeter server.

  • Participants
  • Parent commits ff84e64

Comments (0)

Files changed (12)

mct_active_target/launch/active_target.launch

+<launch>
+    <include file="$(env MCT_CONFIG)/machine/mct.machine" />
+    <node pkg="mct_active_target" type="active_target_node.py" name="active_target" machine="mct_master" required="true"/>
+</launch>

mct_active_target/nodes/active_target_node.py

         target_info = file_tools.read_target_info('active')
         self.dev = ActiveTargetDev(target_info)
         self.dev.off()
+
         rospy.init_node('active_target')
         rospy.on_shutdown(self.shutdown)
 
         n = self.dev.ledArraySize[0]
         m = self.dev.ledArraySize[1]
         max_power = self.dev.maxPowerInt
-        return ActiveTargetInfoResponse(n,m,max_power)
+        square = self.dev.square
+        return ActiveTargetInfoResponse(n,m,max_power,square)
 
     def handle_cmd(self,req):
         """

mct_active_target/src/mct_active_target/active_target.py

     rospy.wait_for_service('active_target_info')
     proxy = rospy.ServiceProxy('active_target_info',ActiveTargetInfo)
     resp = proxy()
-    return resp.array_size_n, resp.array_size_m, resp.max_power
+    return resp.array_size_n, resp.array_size_m, resp.max_power, resp.square
 
 def set_pattern():
     active_target_cmd('pattern')
 
 # -----------------------------------------------------------------------------
 if __name__ == '__main__':
+
     import time
 
+    print()
     print('active_target_info')
-    led_n, led_m, max_power = active_target_info()
+    led_n, led_m, max_power, square = active_target_info()
     print('led_n:     {0}'.format(led_n))
     print('led_m:     {0}'.format(led_m))
     print('max_power: {0}'.format(max_power))
+    print('sqaure:    {0}'.format(square))
     print()
     time.sleep(1.0)
 
-    print('set_led')
-    for i in range(0,led_n):
-        for j in range(0,led_m):
-            print(' (i,j) = ({0},{1})'.format(i,j))
-            set_led(i,j,20)
-            time.sleep(0.25)
-    print()
+    #print('set_led')
+    #for i in range(0,led_n):
+    #    for j in range(0,led_m):
+    #        print(' (i,j) = ({0},{1})'.format(i,j))
+    #        set_led(i,j,20)
+    #        time.sleep(0.25)
+    #print()
 
-    print('set_pattern')
-    set_pattern()
-    print()
+    #print('set_pattern')
+    #set_pattern()
+    #print()
 
-    time.sleep(5)
+    #time.sleep(5)
 
     print('off')
     off()

mct_active_target/src/mct_active_target/active_target_dev.py

     def extract_info(self,target_info):
         port = target_info['port']
         baudrate = target_info['baudrate']
-        square = target_info['square']
+        square = float(target_info['square'])
         ledArraySize = tuple([int(x) for x in target_info['size'].split('x')])
-        maxPowerInt = target_info['max_power']
+        maxPowerInt = int(target_info['max_power'])
         return port, baudrate, square, ledArraySize, maxPowerInt
 
         

mct_homography/manifest.xml

   <depend package="mct_blob_finder"/>
   <depend package="mct_active_target"/>
   <depend package="mct_msg_and_srv"/>
+  <depend package="mct_utilities"/>
 
 </package>
 

mct_homography/nodes/homography_calibrator.py

 import threading
 import mct_active_target
 from mct_blob_finder import BlobFinder
+from mct_utilities import file_tools
 from cv_bridge.cv_bridge import CvBridge 
 
 # Services
         self.bridge = CvBridge()
         self.lock = threading.Lock()
 
+        rospy.init_node('homography_calibrator', log_level=rospy.DEBUG)
+        node_name = rospy.get_name()
+
         # Initialize data lists
         self.blobs_list = [] 
         self.image_points = [] 
         self.world_points = []
 
         # Set active target information and turn off leds
+        rospy.logdebug('running active_target_info'.format(node_name))
         target_info = mct_active_target.active_target_info()
+        rospy.logdebug('done'.format(node_name))
         self.led_n_max = target_info[0] 
         self.led_m_max = target_info[1]
+        self.number_of_leds = self.led_n_max*self.led_m_max
         self.led_max_power = target_info[2]
-        self.led_space_x = 2.5*0.0254
-        self.led_space_y = 2.5*0.0254
+        self.led_space_x = target_info[3] 
+        self.led_space_y = target_info[3] 
+        mct_active_target.off()
+
+        # Current led indices
         self.led_n = 0
         self.led_m = 0
-        self.led_power = 10 
-        self.number_of_leds = self.led_n_max*self.led_m_max
-        mct_active_target.off()
+
+        # Led power setting
+        self.led_power = rospy.get_param('{0}/target/led_power'.format(node_name),10)  
 
         # Wait count for image acquisition
-        self.image_wait_number = 4
+        #self.image_wait_number = int(calibrator_params['image_wait_number'])
+        self.image_wait_number = rospy.get_param('{0}/image_wait_number',4) 
         self.image_wait_cnt = 0
 
         # Sleep periods for idle and wait count loops
 
         # Initialize blob finder
         self.blobFinder = BlobFinder()
-        self.blobFinder.threshold = 200 
-        self.blobFinder.filter_by_area = False
-        self.blobFinder.min_area = 0
-        self.blobFinder.max_area = 200
+        self.blobFinder.threshold = rospy.get_param(
+                '{0}/blob_finder/threshold'.format(node_name),
+                200
+                ) 
+        self.blobFinder.filter_by_area = rospy.get_param(
+                '{0}/blob_finder/filter_by_area'.format(node_name), 
+                False
+                ) 
+        self.blobFinder.min_area = rospy.get_param(
+                '{0}/blob_finder/min_area'.format(node_name), 
+                0
+                )
+        self.blobFinder.max_area = rospy.get_param(
+                '{0}/blob_finder/max_area'.format(node_name),
+                200
+                ) 
 
         # Initialize homography matrix and number of points required to solve for it
         self.homography_matrix = None
-        self.num_points_required = 10
+        self.num_points_required = rospy.get_param('{0}/num_points_required'.format(node_name), 10) 
 
         # Set font and initial image information
         self.cv_text_font = cv.InitFont(cv.CV_FONT_HERSHEY_TRIPLEX, 0.8, 0.8,thickness=1)
         self.image_info= 'no data'
 
-        rospy.init_node('homography_calibrator')
 
         # Subscription to image topic
         self.image_sub = rospy.Subscriber(self.topic,Image,self.image_callback)
         self.image_calib_pub = rospy.Publisher('image_homography_calibration', Image)
 
         # Services
-        node_name = rospy.get_name()
         self.start_srv = rospy.Service(
                 '{0}/start'.format(node_name),
                 Empty,

mct_launch/launch/homography_calibration.launch

+<launch>
+    <include file="$(find mct_active_target)/launch/active_target.launch" />
+    <include file="$(find mct_camera_tools)/launch/camera1394_master.launch" />
+    <include file="$(find mct_camera_tools)/launch/image_proc_master.launch" /> 
+    <include file="$(find mct_camera_tools)/launch/mjpeg_manager.launch" />
+</launch>

mct_msg_and_srv/srv/ActiveTargetInfo.srv

 int32 array_size_n 
 int32 array_size_m
 int32 max_power
+float32 square

mct_utilities/src/mct_utilities/file_tools.py

 camera_calibration_dir = os.path.join(cameras_dir, 'calibrations')
 machine_dir = os.path.join(config_dir,'machine')
 targets_dir = os.path.join(config_dir, 'targets')
+tracking_2d_dir = os.path.join(config_dir, 'tracking_2d')
+homographies_dir = os.path.join(tracking_2d_dir, 'homographies')
 
 # Configuration files
 camera_assignment_file  = os.path.join(config_dir, 'cameras', 'camera_assignment.yaml')
 frame_rates_file = os.path.join(cameras_dir, 'frame_rates.yaml')
 machine_def_file = os.path.join(machine_dir,'machine_def.yaml')
 machine_launch_file = os.path.join(machine_dir,'mct.machine')
+homography_calibrator_params_file = os.path.join(homographies_dir, 'calibrator_params.yaml')
 
 def read_machine_def():
     """
     target_info['square'] = str(target_info['square'])
     return target_info
 
+def read_homography_calibrator_params():
+    """
+    Reads the homography calibrator parameters file
+    """
+    with open(homography_calibrator_params_file,'r') as f:
+        calibrator_params = yaml.load(f)
+    return calibrator_params
+
 def get_last_modified_time(filename):
     """
     Returns the last modified time for the given file.
         caldata = read_camera_calibration('camera_1')
         print(caldata)
 
-    if 1:
+    if 0:
         rsync_camera_calibrations(verbose=True)
 
+    if 1:
+        read_homography_calibrator_params()
+

mct_xml_tools/src/mct_xml_tools/homography_calibrator.launch

+<launch>
+    <!-- Autogenerated launch file - do not hand edit -->
+    <include file="/home/albert/ros/mct_config/machine/mct.machine" />
+    
+    <group ns="/mct_master/camera_8/camera">
+        <node pkg="mct_homography" type="homography_calibrator.py" name="homography_calibrator" args="/mct_master/camera_8/camera/image_rect" >
+            <rosparam command="load" file="/home/albert/ros/mct_config/tracking_2d/homographies/calibrator_params.yaml"/>
+        </node>
+    </group>
+    
+    <group ns="/mct_slave1/camera_4/camera">
+        <node pkg="mct_homography" type="homography_calibrator.py" name="homography_calibrator" args="/mct_slave1/camera_4/camera/image_rect" >
+            <rosparam command="load" file="/home/albert/ros/mct_config/tracking_2d/homographies/calibrator_params.yaml"/>
+        </node>
+    </group>
+    
+    <group ns="/mct_slave1/camera_9/camera">
+        <node pkg="mct_homography" type="homography_calibrator.py" name="homography_calibrator" args="/mct_slave1/camera_9/camera/image_rect" >
+            <rosparam command="load" file="/home/albert/ros/mct_config/tracking_2d/homographies/calibrator_params.yaml"/>
+        </node>
+    </group>
+    
+    <group ns="/mct_slave2/camera_7/camera">
+        <node pkg="mct_homography" type="homography_calibrator.py" name="homography_calibrator" args="/mct_slave2/camera_7/camera/image_rect" >
+            <rosparam command="load" file="/home/albert/ros/mct_config/tracking_2d/homographies/calibrator_params.yaml"/>
+        </node>
+    </group>
+    
+    <group ns="/mct_slave1/camera_2/camera">
+        <node pkg="mct_homography" type="homography_calibrator.py" name="homography_calibrator" args="/mct_slave1/camera_2/camera/image_rect" >
+            <rosparam command="load" file="/home/albert/ros/mct_config/tracking_2d/homographies/calibrator_params.yaml"/>
+        </node>
+    </group>
+    
+    <group ns="/mct_master/camera_3/camera">
+        <node pkg="mct_homography" type="homography_calibrator.py" name="homography_calibrator" args="/mct_master/camera_3/camera/image_rect" >
+            <rosparam command="load" file="/home/albert/ros/mct_config/tracking_2d/homographies/calibrator_params.yaml"/>
+        </node>
+    </group>
+    
+    <group ns="/mct_slave1/camera_1/camera">
+        <node pkg="mct_homography" type="homography_calibrator.py" name="homography_calibrator" args="/mct_slave1/camera_1/camera/image_rect" >
+            <rosparam command="load" file="/home/albert/ros/mct_config/tracking_2d/homographies/calibrator_params.yaml"/>
+        </node>
+    </group>
+    
+    <group ns="/mct_master/camera_11/camera">
+        <node pkg="mct_homography" type="homography_calibrator.py" name="homography_calibrator" args="/mct_master/camera_11/camera/image_rect" >
+            <rosparam command="load" file="/home/albert/ros/mct_config/tracking_2d/homographies/calibrator_params.yaml"/>
+        </node>
+    </group>
+    
+    <group ns="/mct_slave2/camera_5/camera">
+        <node pkg="mct_homography" type="homography_calibrator.py" name="homography_calibrator" args="/mct_slave2/camera_5/camera/image_rect" >
+            <rosparam command="load" file="/home/albert/ros/mct_config/tracking_2d/homographies/calibrator_params.yaml"/>
+        </node>
+    </group>
+    
+    <group ns="/mct_slave2/camera_10/camera">
+        <node pkg="mct_homography" type="homography_calibrator.py" name="homography_calibrator" args="/mct_slave2/camera_10/camera/image_rect" >
+            <rosparam command="load" file="/home/albert/ros/mct_config/tracking_2d/homographies/calibrator_params.yaml"/>
+        </node>
+    </group>
+     
+</launch>

mct_xml_tools/src/mct_xml_tools/launch.py

     """
     template_name = 'homography_calibrator_launch.xml'
     machine_file = mct_utilities.file_tools.machine_launch_file
+    params_file = mct_utilities.file_tools.homography_calibrator_params_file
 
     # Get list of pairs (namespace, rectified images)
     image_rect_list = mct_introspection.find_camera_image_topics(transport='image_rect')
         namespace = '/'.join(topic_split[:4])
         launch_list.append((namespace,topic))
 
-
     # 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)
+    xml_str = template.render(
+            machine_file=machine_file, 
+            params_file=params_file,
+            launch_list=launch_list
+            )
     with open(filename,'w') as f:
         f.write(xml_str)
     

mct_xml_tools/src/mct_xml_tools/templates/homography_calibrator_launch.xml

     <include file="{{machine_file}}" />
     {% for namespace, image_topic in launch_list %}
     <group ns="{{namespace}}">
-        <node pkg="mct_homography" type="homography_calibrator.py" name="homography_calibrator" args="{{image_topic}}" />
+        <node pkg="mct_homography" type="homography_calibrator.py" name="homography_calibrator" args="{{image_topic}}" >
+            <rosparam command="load" file="{{params_file}}"/>
+        </node>
     </group>
     {% endfor %} 
 </launch>