[Personalrobots-commit] SF.net SVN: personalrobots:[15860] pkg/trunk

[<vij...@us...>]

Revision: 15860
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=15860&view=rev
Author:   vijaypradeep
Date:     2009-05-21 01:31:52 +0000 (Thu, 21 May 2009)

Log Message:
-----------
Now publish joint pos for beginning and end of scans. Resized viz markers

Modified Paths:
--------------
    pkg/trunk/drivers/robot/pr2/dense_laser_assembler/scripts/assembler_node.py
    pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/dense_laser_assembler/dense_laser_cache.py
    pkg/trunk/vision/stereo_checkerboard_detector/src/stereo_checkerboard_node.cpp

Modified: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/scripts/assembler_node.py
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/scripts/assembler_node.py	2009-05-21 01:31:32 UTC (rev 15859)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/scripts/assembler_node.py	2009-05-21 01:31:52 UTC (rev 15860)
@@ -37,6 +37,7 @@
 import roslib; roslib.load_manifest('dense_laser_assembler') 
 import sys
 import rospy
+import threading
 from time import sleep
 from laser_scan.msg import *
 from robot_msgs.msg import *
@@ -52,17 +53,23 @@
 image_pub = [ ]
 test_pub = [ ]
 
+
 def scan_callback(msg) :
+    req_lock.acquire()
     laser_cache.add_scan(msg)
     laser_cache.process_scans()
     laser_cache.process_interval_reqs()
+    req_lock.release()
 
 def mech_callback(msg) :
+    req_lock.acquire()
     laser_cache.add_mech_state(msg)
     laser_cache.process_scans()
     laser_cache.process_interval_reqs()
+    req_lock.release()
 
 def signal_callback(msg) :
+    req_lock.acquire()
     global first_signal
     global prev_signal
     laser_cache.process_scans()
@@ -74,6 +81,7 @@
     prev_signal = msg
     print 'Got signal message!'
     print '  Signal=%i' % msg.signal
+    req_lock.release()
 
 def interval_req_callback(scans) :
 
@@ -92,7 +100,7 @@
     print 'About to process cloud with %u scans' % len(scans)
 
     # Sort by tilting joint angle
-    sorted_scans = sorted(scans, lambda x,y:cmp(x.pos,y.pos))
+    sorted_scans = sorted(scans, lambda x,y:cmp(x.pos[0],y.pos[0]))
 
 
     msg_header = roslib.msg.Header()
@@ -114,9 +122,10 @@
 
     intensity_msg.data.layout = layout
     range_msg.data.layout     = layout
-    joint_msg.data.layout = MultiArrayLayout([ MultiArrayDimension('rows', rows, rows),
-                                               MultiArrayDimension('cols', 1, 1),
+    joint_msg.data.layout = MultiArrayLayout([ MultiArrayDimension('rows', rows, rows*2),
+                                               MultiArrayDimension('cols', 2, 2),
                                                MultiArrayDimension('elem', 1, 1) ], 0 )
+    joint_msg.data.layout = layout
 
     # Clear out data from the info message. (Keep everything except intensity and range data)
     #info_msg.ranges = [ ]
@@ -128,32 +137,36 @@
     for x in sorted_scans :
         intensity_msg.data.data.extend(x.scan.intensities)
         range_msg.data.data.extend(x.scan.ranges)
-        joint_msg.data.data.append(x.pos)
+        joint_msg.data.data.extend(x.pos)
     
     intensity_pub.publish(intensity_msg)
     range_pub.publish(range_msg)
     info_pub.publish(info_msg)
     joint_pos_pub.publish(joint_msg)
 
-    image = image_msgs.msg.Image()
+    #image = image_msgs.msg.Image()
 
-    image.header = msg_header
-    image.label  = 'intensity'
-    image.encoding = 'mono'
-    image.depth = 'uint8'
-    max_val = max(intensity_msg.data.data)
-    image.uint8_data.data   = "".join([chr(int(255*x/max_val)) for x in intensity_msg.data.data])
+    #image.header = msg_header
+    #image.label  = 'intensity'
+    #image.encoding = 'mono'
+    #image.depth = 'uint8'
+    #max_val = max(joint_msg.data.data)
+    #min_val = min(joint_msg.data.data)
+    #image.uint8_data.data   = "".join([chr(int(255*x/max_val)) for x in intensity_msg.data.data])
+    #image.uint8_data.data   = "".join([chr(int(255*(x-min_val)/(max_val-min_val))) for x in joint_msg.data.data])
     #image.uint8_data.data = ''
-    image.uint8_data.layout = intensity_msg.data.layout
-    image_pub.publish(image)
+    #image.uint8_data.layout = intensity_msg.data.layout
+    #image_pub.publish(image)
 
 
     print '  Processed cloud with %u rays' % len(intensity_msg.data.data)
 
 if __name__ == '__main__':
 
+    req_lock = threading.Lock()
+
     print 'Initializing DenseLaserCache'
-    laser_cache = DenseLaserCache(interval_req_callback, 50, 1000, 100) ;
+    laser_cache = DenseLaserCache(interval_req_callback, 200, 1000, 100) ;
     print 'Done initializing'
 
     rospy.init_node('dense_assembler', anonymous=False)
@@ -164,7 +177,7 @@
     range_pub     = rospy.Publisher("dense_tilt_scan/range",     Float32MultiArrayStamped)
     info_pub      = rospy.Publisher("dense_tilt_scan/scan_info", LaserScan)
     joint_pos_pub = rospy.Publisher("dense_tilt_scan/joint_info",Float32MultiArrayStamped)
-    image_pub = rospy.Publisher("test_image", Image)
+    #image_pub = rospy.Publisher("test_image", Image)
     
     scan_sub = rospy.Subscriber("tilt_scan", LaserScan, scan_callback)
 

Modified: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/dense_laser_assembler/dense_laser_cache.py
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/dense_laser_assembler/dense_laser_cache.py	2009-05-21 01:31:32 UTC (rev 15859)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/dense_laser_assembler/dense_laser_cache.py	2009-05-21 01:31:52 UTC (rev 15860)
@@ -90,26 +90,32 @@
     # then added to the laser_done queue
     def process_scans(self) :
         self._mech_lock.acquire()
-        local_mech_q = self._mech_q
-        self._mech_lock.release()
 
-        if len(local_mech_q) > 0 :
-            oldest_mech = local_mech_q[0]
-            newest_mech = local_mech_q[-1]
+        if len(self._mech_q) > 0 :
+            oldest_mech_time = self._mech_q[0].header.stamp
+            newest_mech_time = self._mech_q[-1].header.stamp
         else :
+            self._mech_lock.release()
             return
-        
+
         self._laser_wait_lock.acquire()
-        laser_pending_q = [x for x in self._laser_wait_q
-                             if x.header.stamp < newest_mech.header.stamp and
-                                x.header.stamp > oldest_mech.header.stamp]
+
+        scan_interval = [ (x.header.stamp,
+#                           x.header.stamp + rostime.Duration().from_seconds(0.0) )
+                           x.header.stamp+rostime.Duration().from_seconds(len(x.ranges)*x.time_increment))
+                           for x in self._laser_wait_q ]
+
+        laser_pending_q = [x for i,x in enumerate(self._laser_wait_q)
+                             if scan_interval[i][1] < newest_mech_time and
+                                scan_interval[i][0] > oldest_mech_time]
         # Purge entire queue, except scans in the future
-        self._laser_wait_q = [x for x in self._laser_wait_q
-                                if x.header.stamp >= newest_mech.header.stamp]
+        self._laser_wait_q = [x for i,x in enumerate(self._laser_wait_q)
+                                if scan_interval[i][1] >= newest_mech_time]
+
         self._laser_wait_lock.release()
 
         # Now process the pending queue
-        processed = [self._process_pending(x,local_mech_q) for x in laser_pending_q]
+        processed = [self._process_pending(x,self._mech_q) for x in laser_pending_q]
 
         #if len(processed) > 0 :
         #    print 'processed %i scans' % len(processed)
@@ -119,26 +125,41 @@
         while len(self._laser_done_q) > self._max_laser_done_len :
             self._laser_done_q.pop(0)
         self._laser_done_lock.release()
-
+        self._mech_lock.release()
     # Process a scan, assuming that we have MechansimState data available before and after the scan
     # \param scan_msg The message that we want to process
     # \param the time-order queue of mechanism_state data
     # \param The processed scan element
     def _process_pending(self, scan_msg, mech_q):
+        scan_end = scan_msg.header.stamp + rostime.Duration().from_seconds(len(scan_msg.ranges)*scan_msg.time_increment)
         mech_before = [x for x in mech_q if x.header.stamp < scan_msg.header.stamp][-1]
-        mech_after  = [x for x in mech_q if x.header.stamp > scan_msg.header.stamp][0]
+        mech_after  = [x for x in mech_q if x.header.stamp > scan_end][0]
 
+        # Get the joint position and time associated with the MechanismState before the scan
         ind = [x.name for x in mech_before.joint_states].index('laser_tilt_mount_joint')
         pos_before = mech_before.joint_states[ind].position
-        elapsed_before = (scan_msg.header.stamp - mech_before.header.stamp).to_seconds()
+        time_before= mech_before.header.stamp
 
+        # Get the joint position and time associated with the MechanismState before the scan
         ind = [x.name for x in mech_after.joint_states].index('laser_tilt_mount_joint')
         pos_after = mech_after.joint_states[ind].position
-        elapsed_after = (mech_after.header.stamp - scan_msg.header.stamp).to_seconds()
+        time_after= mech_after.header.stamp
 
-        elapsed_total = elapsed_before + elapsed_after
+        elapsed = (time_after - time_before).to_seconds()
+
         # Linear interp
-        pos_during = elapsed_after/elapsed_total*pos_before + elapsed_before/elapsed_total*pos_after
+        #pos_during = elapsed_after/elapsed_total*pos_before + elapsed_before/elapsed_total*pos_after
+
+        #key_rays = range(0, len(scan_msg.ranges))
+        key_rays = [0, len(scan_msg.ranges)-1]
+        time_during = [ scan_msg.header.stamp
+                        + rostime.Duration().from_seconds(scan_msg.time_increment*x)
+                          for x in key_rays]
+        time_normalized = [ (x - time_before).to_seconds()/elapsed for x in time_during]
+        if any( [x > 1 for x in time_normalized]) or any( [x < 0 for x in time_normalized] ) :
+            print 'Error computing normalized time'
+        pos_during = [ pos_before * (1-x) + pos_after * x for x in time_normalized ]
+
         elem = LaserCacheElem()
         elem.pos  = pos_during
         elem.scan = scan_msg

Modified: pkg/trunk/vision/stereo_checkerboard_detector/src/stereo_checkerboard_node.cpp
===================================================================
--- pkg/trunk/vision/stereo_checkerboard_detector/src/stereo_checkerboard_node.cpp	2009-05-21 01:31:32 UTC (rev 15859)
+++ pkg/trunk/vision/stereo_checkerboard_detector/src/stereo_checkerboard_node.cpp	2009-05-21 01:31:52 UTC (rev 15860)
@@ -156,8 +156,8 @@
     marker.action = visualization_msgs::Marker::ADD ;
     tf::PoseTFToMsg(pose, marker.pose) ;
     marker.scale.x = .2 ;
-    marker.scale.y = .03 ;
-    marker.scale.z = .03 ;
+    marker.scale.y = .2 ;
+    marker.scale.z = .2 ;
     marker.color.r = 1.0 ;
     marker.color.g = 0.0 ;
     marker.color.b = 0.0 ;


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.