Eric, Adam, Patrick,
Here is the diff of my working camera_calibration fix to get two
independent camera timestamps "close enough" to at least test
calibration (and see that Stereo calibration is not working at all).
Index: nodes/cameracalibrator.py
===================================================================
--- nodes/cameracalibrator.py (revision 28602)
+++ nodes/cameracalibrator.py (working copy)
@@ -60,11 +60,23 @@
class CalibrationNode:
+ def mineA(self, msg):
+ self.lasttime = msg.header.stamp
+
+ def mineB(self,msg):
+ diff = msg.header.stamp-self.lasttime
+ sec = diff.to_sec()
+ if abs(sec) < 0.09:
+ msg.header.stamp = self.lasttime
+
def __init__(self, chess_size, dim):
self.chess_size = chess_size
self.dim = dim
+ self.lasttime = rospy.Time.now()
lsub = message_filters.Subscriber('left', sensor_msgs.msg.Image)
+ lsub.registerCallback(self.mineA)
rsub = message_filters.Subscriber('right', sensor_msgs.msg.Image)
+ rsub.registerCallback(self.mineB)
ts = message_filters.TimeSynchronizer([lsub, rsub], 4)
ts.registerCallback(self.queue_stereo)
Eric Perko wrote:
> Hey Adam,
>
> I'm trying to do stereo calibration as well and running into the exact
> same problem. I'm switching from boxturtle to latest to see if that
> will solve the problem. If you do 'rostopic list -v' you will probably
> see /image listed as being subscribed to, but having unknown type,
> meaning nothing is publishing it yet. At least that is the behavior I
> get. However, I was still having the same hanging behavior even after
> commenting out the line in cameracalibrator.py where it subscribes to
> /image, so it may or may not be the root problem.
>
> My understanding is that when publishing to a /camera_info topic, the
> timestamps for the camera_info and the image_raw should be synced, so
> you have to publish at whatever rate you publish your image. I could
> be wrong, but that is my understanding from the discussion surrounding
> a previous problem I was having with image_proc not working with
> camera1394.
>
> - Eric
>
> On Tue, Apr 6, 2010 at 7:24 PM, Adam Leeper <aleeper@stanford.edu> wrote:
>> Hi all-
>>
>> I'm trying to calibrate a pair of cameras for stereo. I only have the
>> following topics:
>>
>> * /my_stereo/left/image_raw [sensor_msgs/Image] 1 publisher
>> * /my_stereo/right/image_raw [sensor_msgs/Image] 1 publisher
>>
>> That is, I don't have any topics publishing mono camera info or anything.
>>
>> I do
>> rosrun camera_calibration cameracalibrator.py
>> right:=/my_stereo/right/image_raw left:=/my_stereo/left/image_raw
>> left_camera:=/my_stereo/left right_camera:=/my_stereo/right
>>
>> but it just hangs. Since mono calibration works fine, my only guess is that
>> the stereo routine requires the mono calibration info to proceed, so it is
>> waiting for more topics before continuing. If so, the tutorial should
>> probably mention that, because right now it just says "check to see that
>> there is a left and right image_raw topic".
>>
>> On a related note, what is the proper way to publish camera info? Do I write
>> a node that just publishes the message every few seconds? It seems that the
>> camera info doesn't change quickly :)
>>
>>
>> Thanks!
>> --Adam
>>
>>
>>
>>
>> Adam Leeper
>> Stanford University
>> aleeper@stanford.edu
>> 719.358.3804
>>
>> _______________________________________________
>> ros-users mailing list
>> ros-users@code.ros.org
>> https://code.ros.org/mailman/listinfo/ros-users
>>
>>
> _______________________________________________
> ros-users mailing list
> ros-users@code.ros.org
> https://code.ros.org/mailman/listinfo/ros-users