def __init__(self):
print("[INFO] initializing config...")
self.publish_images = True
self.bridge = CvBridge()
print("[INFO] Loading ROS topics...")
self.bboxes_pub = rospy.Publisher(
"/yolo/Segbboxes", Detection2DArray,
queue_size=1) #Bboxes for object_handler
self.images_pub = rospy.Publisher(
"/yolo/Segimages", Detection2DArray,
queue_size=1) #Bboxes with images for display
cloud_sub = message_filters.Subscriber(
"/zed2/zed_node/point_cloud/cloud_registered", PointCloud2)
self.cache = message_filters.Cache(cloud_sub, 30)
#boxes_sub = message_filters.Subscriber("/yolo/bboxes",Detection2DArray)
#rospy.Subscriber("/zed2/zed_node/point_cloud/cloud_registered",PointCloud2, self.callback_cloud, queue_size = 30)
rospy.Subscriber("/zed2/zed_node/point_cloud/cloud_registered",
PointCloud2,
self.callback_cloud,
queue_size=1)
rospy.Subscriber("/yolo/bboxes",
Detection2DArray,
self.callback_box,
queue_size=1)
print("[INFO] Loading complete")
self.cloud_list = []
self.cloud_list_headers = []
self.queue_lim = 60
def __init__(self, ip, joy_topic="/cmd_vel", scale = 0.005):
self.robot = robot_controller.Ur3(ip, 30003, 30002)
self.sub = message_filters.Subscriber(joy_topic, Twist)
self.cache = message_filters.Cache(self.sub, cache_size=1, allow_headerless=True)
self.scale = scale
self.last_pose = self.robot.get_pose()
self.bounds_x = (0.225, -0.225)
self.bounds_y = (0.315, -0.315)
def __init__(self, name): self.name = name self.tf = '/vicon/'+name+'/'+name self.tl = TransformListener() self.pose = np.array([0,0,0]) self.orient = np.array([0,0,0]) # for velocity: sub = message_filters.Subscriber(self.tf, TransformStamped) self.cache = message_filters.Cache(sub, 100) self.vel = np.array([0,0,0])
def listener():
rospy.init_node('numpy_listener')
#rospy.Subscriber("/mobile_base/sensors/imu_data", Imu, imu_callback)
#rospy.Subscriber("/odom", Odometry, odom_callback)
# rospy.Subscriber("/map", OccupancyGrid, map_callback)
#odom_init_once = None
#odom_init_once = rospy.Subscriber("/odom", Odometry, odom_init_once)
rospy.Subscriber("/scan", LaserScan, laser_callback)
# rospy.Subscriber("floats", Floats, callback)
# Time sync problem ... it seems that computer is too slow to process,
# so the algorithm runs with latest state but older measurements!
odom_sub = message_filters.Subscriber("/odom", Odometry)
global odom_cache
odom_cache = message_filters.Cache(odom_sub, 2)
scan_sub = message_filters.Subscriber("/scan", LaserScan)
global laser_cache
laser_cache = message_filters.Cache(scan_sub, 2)
ts = message_filters.TimeSynchronizer([odom_sub, scan_sub], 10)
#ts.registerCallback(laser_callback, 1)
rospy.spin()
def __init__(self, name, leader=False):
Mocap_object.__init__(self, name)
self.leader = leader
self.sp = np.array([0., 0., 0.])
self.near_obstacle = False
self.nearest_obstacle = None
self.rad_imp = radius_impedance_model() # Obstacle avoidance
sub_sp = message_filters.Subscriber(self.name + "_sp", PoseStamped)
self.cache_sp = message_filters.Cache(sub_sp, 100)
self.vel_sp = np.array([0, 0, 0])
def __init__(self, template_file_list_name, subscribe_topic_name,
model_file, dlib_face_predictor):
self.recognizer = face_recognizer.face_recognizer(
model_file, dlib_face_predictor)
if not self.recognizer.set_inside_templates(template_file_list_name):
print "fail"
exit()
self.bridge = CvBridge()
sub = message_filters.Subscriber(subscribe_topic_name, sensorImg)
self.cache = message_filters.Cache(sub, 100)
thread.start_new_thread(self.run, ())
def main():
rospy.init_node('segment_bag_to_video')
if len(sys.argv) < 3:
print 'Usage: segment_bag_to_video.py model.ckpt INPUT.bag OUTPUT.bag'
return
checkpoint_path = sys.argv[1]
hand_segmentation = HandSegmentation(checkpoint_path)
input_path = sys.argv[2]
output_path = sys.argv[3]
input_bag = rosbag.Bag(input_path)
output_bag = rosbag.Bag(output_path, 'w')
color_pass_to_cache = PassToCache()
depth_pass_to_cache = PassToCache()
color_cache = message_filters.Cache(color_pass_to_cache, cache_size=5)
depth_cache = message_filters.Cache(depth_pass_to_cache, cache_size=5)
demo = Demo(hand_segmentation, output_bag)
queue_size = 1
slop_seconds = 0.015
sync = message_filters.ApproximateTimeSynchronizer(
[color_cache, depth_cache], queue_size, slop_seconds)
sync.registerCallback(demo.callback)
i = 0
message_count = input_bag.get_message_count(topic_filters=[COLOR_TOPIC, DEPTH_TOPIC])
for topic, msg, t in input_bag.read_messages(topics=[COLOR_TOPIC, DEPTH_TOPIC]):
i += 1
if i % 100 == 0:
print 'Processed {} of {} messages ({}%)'.format(i, message_count, 100.0 * float(i) / message_count)
if topic == COLOR_TOPIC:
color_cache.add(msg)
elif topic == DEPTH_TOPIC:
depth_cache.add(msg)
output_bag.close()
def __init__(self,
robot_ip,
joy_topic="/joy",
gripper_topic="/gripper/command",
scale=0.001):
print('Connecting to {}'.format(robot_ip))
self.robot = robot_controller.Ur3(robot_ip, 30003, 30002)
self.sub = message_filters.Subscriber(joy_topic, Joy)
self.cache = message_filters.Cache(self.sub,
cache_size=1,
allow_headerless=False)
self.pub = rospy.Publisher(gripper_topic, String, queue_size=2)
self.scale = scale
self.orient_scale = 0.01
self.last_pose = self.robot.get_pose()
self.last_data_h_seq = None
self.dead_zone = 0.2
self.update_count = 20
self.update_counter = 0
def setupMessageInterface(self):
""" Setup ROS publisher, subscriber, and message filters """
self.__unlock()
# Topic Names
drtkTopic = rospy.get_param('~drtk_topic', "/drtk_node/drtk_output")
tdcpLocalTopic = rospy.get_param('~tdcp_local_topic',
"/tdcp_node/tdcp_output")
# Message Interface Parameters
queueSize = rospy.get_param('~queue_size', 10)
cacheSize = rospy.get_param('~cache_size', 10)
self.maxTimeOffset = rospy.get_param('~max_time_offset', 0.25)
# Publishers
self.path_pub = rospy.Publisher("~path",
PathDiagnostics,
queue_size=queueSize)
# ROS Message Filters
drtk_sub = message_filters.Subscriber(drtkTopic, DrtkOutput)
# Message Filter Caches
self.drtk_cache = message_filters.Cache(drtk_sub, cache_size=cacheSize)
# Sync Variables
self.latestTdcpLocalMsg = TdcpOutput()
self.newTdcpLocalFlag = 0
# ROS Subscriber
tdcp_local_sub = rospy.Subscriber(tdcpLocalTopic,
TdcpOutput,
self.tdcpLocalCallback,
queue_size=queueSize)
# ROS Timer
rospy.Timer(rospy.Duration(0.05), self.tick)
self.stopwatch = 0.0
def __init__(self, config):
self.config = config
self.pp = pprint.PrettyPrinter(indent=4)
self.pp.pprint(config)
rgb_image_sub = message_filters.Subscriber(
"/camera/rgb/image_color",
ImageSensor_msg,
queue_size=10000
)
rgb_info_sub = message_filters.Subscriber(
"/camera/rgb/camera_info",
CameraInfo
)
depth_info_sub = message_filters.Subscriber(
"/camera/depth/camera_info",
CameraInfo
)
depth_image_sub = message_filters.Subscriber(
"/camera/depth/image",
ImageSensor_msg,
queue_size=10000
)
self.depth_scale = 100.0
self.depth_image_cache = message_filters.Cache(depth_image_sub, 10000, allow_headerless=False)
self.cv_bridge = CvBridge()
self.output_path = config['label_gen']['output_path']
model_path = get_model_path(config['label_gen']['model_dir'], config['label_gen']['object_name'], model_type="upright")
self.initial_rotation = get_initial_rotation(config['label_gen']['object_label'])
# model_path = "/media/aditya/A69AFABA9AFA85D9/Datasets/YCB_Video_Dataset/models/004_sugar_box/textured.ply"
# self.output_path = "./bag_output/sugar_1"
# self.initial_rotation = tf.transformations.quaternion_from_euler(0, 0, -math.pi/2) #sugar_1
# self.output_path = "./bag_output/sugar_2"
# self.initial_rotation = tf.transformations.quaternion_from_euler(0, 0, -2.0 * math.pi/3) #sugar_2
# self.output_path = "./bag_output/sugar_3"
# self.initial_rotation = tf.transformations.quaternion_from_euler(0, 0, 2.0 * math.pi/3) #sugar_3
# model_path = "/media/aditya/A69AFABA9AFA85D9/Datasets/YCB_Video_Dataset/models/035_power_drill/textured_upright.ply"
# self.output_path = "./bag_output/drill_1"
# self.initial_rotation = tf.transformations.quaternion_from_euler(0, 0, math.pi) #drill_1
# self.output_path = "./bag_output/drill_2"
# self.initial_rotation = tf.transformations.quaternion_from_euler(0, 0, 2.5/3 * math.pi) #drill_2
# self.output_path = "./bag_output/drill_3"
# self.initial_rotation = tf.transformations.quaternion_from_euler(0, 0, -2.5/3 * math.pi) #drill_3
# model_path = "/media/aditya/A69AFABA9AFA85D9/Datasets/YCB_Video_Dataset/models/005_tomato_soup_can/textured.ply"
# self.output_path = "./bag_output/soup_1"
# self.initial_rotation = tf.transformations.quaternion_from_euler(0, 0, 0) #soup_1
# model_path = "/media/aditya/A69AFABA9AFA85D9/Datasets/YCB_Video_Dataset/models/006_mustard_bottle/textured.ply"
# self.output_path = "./bag_output/mustard_1"
# self.initial_rotation = tf.transformations.quaternion_from_euler(0, 0, 0) #mustard_1
# self.output_path = "./bag_output/mustard_2"
# self.initial_rotation = tf.transformations.quaternion_from_euler(0, 0, 2.5/3 * math.pi) #mustard_2
# self.output_path = "./bag_output/sugar_3"
# self.initial_rotation = tf.transformations.quaternion_from_euler(0, 0, 2.0 * math.pi/3) #sugar_3
mkdir_if_missing(self.output_path)
self.world_frame = "/base_footprint"
self.camera_frame = "/camera_rgb_optical_frame"
self.tf_listener = tf.TransformListener()
self.camera_pose = None
self.rgb_camera_instrinc_matrix = None
self.depth_camera_instrinc_matrix = None
self.counter = 0
self.MAX_COUNTER = 400
self.pub_filtered_cloud = \
rospy.Publisher(
"image_node/filtered_cloud",
PointCloud2,
queue_size=10
)
self.pub_pose_cloud = \
rospy.Publisher(
"image_node/pose_cloud",
PointCloud2,
queue_size=10
)
# Read Model
cloud = PlyData.read(model_path).elements[0].data
cloud = np.transpose(np.vstack((cloud['x'], cloud['y'], cloud['z'])))
cloud_pose = pcl.PointCloud()
cloud_pose.from_array(cloud)
sor = cloud_pose.make_voxel_grid_filter()
sor.set_leaf_size(0.015, 0.015, 0.015)
cloud_pose = sor.filter()
self.mesh_cloud = np.asarray(cloud_pose)
# Z point up
self.object_height = np.max(self.mesh_cloud[:,2]) - np.min(self.mesh_cloud[:,2])
self.cloud_filter_params = {
"xmin" : 0.1,
"xmax" : 0.6,
"ymin" : 0.0,
"ymax" : 1.7,
"zmin" : 0.75, #drill, sugar, mustard
# "zmin" : 0.76, #soup can
"object_height" : self.object_height,
"downsampling_leaf_size" : 0.015
}
print(self.cloud_filter_params)
print ("Num points after downsample and filter : {}".format(
self.mesh_cloud.shape[0]))
rgb_image_sub.registerCallback(self.image_callback)
rgb_info_sub.registerCallback(self.rgb_info_callback)
depth_info_sub.registerCallback(self.depth_info_callback)
import tf
import struct
from laser_assembler.srv import AssembleScans
from laser_geometry import LaserProjection
from sensor_msgs.msg import PointCloud, ChannelFloat32, Image, CameraInfo, LaserScan
from sensor_msgs import point_cloud2
from image_geometry import PinholeCameraModel
from cv_bridge import CvBridge, CvBridgeError
from geometry_msgs.msg import PointStamped, Point32
rospy.init_node('camera_lidar')
tf_listener = tf.TransformListener()
image_sub = message_filters.Subscriber('camera/image_raw', Image)
image_cache = message_filters.Cache(image_sub, 10)
camera_model = PinholeCameraModel()
bridge = CvBridge()
def on_camera_info(camera_info):
camera_model.fromCameraInfo(camera_info)
camera_info_sub = rospy.Subscriber('camera/camera_info',
CameraInfo,
on_camera_info,
queue_size=1)
laser_projector = LaserProjection()
def __init__(self, name, leader=False):
Mocap_object.__init__(self, name)
self.sp = np.array([0., 0., 0.])
sub_sp = message_filters.Subscriber(self.name + "_sp", PoseStamped)
self.cache_sp = message_filters.Cache(sub_sp, 100)
self.vel_sp = np.array([0, 0, 0])
def __init__(self, **kwargs):
"""
Initializes a new FireflyEnv environment.
To check any topic we need to have the simulations running, we need to do two things:
1) Unpause the simulation: without that the stream of data doesnt flow. This is for simulations
that are pause for whatever the reason
2) If the simulation was running already for some reason, we need to reset the controllers.
This has to do with the fact that some plugins with tf, dont understand the reset of the simulation
and need to be reseted to work properly.
The Sensors: The sensors accesible are the ones considered usefull for AI learning.
Sensor Topic List:
* /pose : Reads the estimated pose of the MAV. Type: uav_msgs
* /target_tracker/pose : Reads the fused estimate of the target person. Type: geometry_msgs
Actuators Topic List:
* /command, publishes the desired waypoints and velocity for the robot. Type: uav_msgs
Args:
"""
rospy.logdebug("Start FireflyEnv INIT...")
rospy.logwarn('Setting Up Firefly ENV')
# Variables that we give through the constructor.
# None in this case
# Internal Vars
# Doesnt have any accesibles
self.controllers_list = []
# It doesnt use namespace
self.robot_name_space = ""
# We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv
super(FireflyMultiAgentGTEnv,
self).__init__(controllers_list=self.controllers_list,
robot_name_space=self.robot_name_space,
reset_controls=False,
start_init_physics_parameters=False,
reset_world_or_sim="WORLD",
**kwargs)
machine_name = '/machine_' + str(self.robotID)
rotors_machine_name = '/firefly_' + str(self.robotID)
self.rotors_machine_name = rotors_machine_name
self.num_robots = 2
if self.robotID == self.num_robots:
neighbor_name = '/machine_' + str(
(self.robotID + 1) % self.num_robots)
rotors_neighbor_name = '/firefly_' + str(
(self.robotID + 1) % self.num_robots)
else:
neighbor_name = '/machine_' + str(self.robotID + 1)
rotors_neighbor_name = '/firefly_' + str(self.robotID + 1)
self.rotors_neighbor_name = rotors_neighbor_name
self.pose_topic = machine_name + "/pose"
self.velocity_topic = rotors_machine_name + "/ground_truth/odometry"
self.velocity_neighbor_topic = rotors_neighbor_name + "/ground_truth/odometry"
self.gt_pose_topic = machine_name + "/pose/groundtruth"
self.gt_neighbor_pose_topic = neighbor_name + "/pose/groundtruth"
self.firefly_pose_topic = rotors_machine_name + "/ground_truth/pose_with_covariance"
self.target_topic = machine_name + "/target_tracker/pose"
self.gt_target_topic = "/actorpose"
self.gt_target_vel_topic = "/actorvel"
self.target_velocity_topic = machine_name + "/target_tracker/twist"
self.command_topic = machine_name + "/command"
self.neighbor_command_topic = neighbor_name + "/command"
self.destination_topic = machine_name + "/destination"
self.detections_feedback_topic = machine_name + "/object_detections/feedback"
self.noisy_joints_topic = machine_name + "/noisy_joints"
self.noisy_joints_neighbor_topic = neighbor_name + "/noisy_joints"
self.noisy_bbox_topic = machine_name + "/noisy_bbox"
self.noisy_bbox_neighbor_topic = neighbor_name + "/noisy_bbox"
self.noisy_detection = machine_name + "/noisy_detection"
self.noisy_neighbor_detection = neighbor_name + "/noisy_detection"
self.alphapose_topic = machine_name + "/result_alpha"
self.alphapose_neighbor_topic = neighbor_name + "/result_alpha"
self.alphapose_bbox_topic = machine_name + "/result_bbox"
self.alphapose_bbox_neighbor_topic = neighbor_name + "/result_bbox"
self.alphapose_detection = machine_name + "/detection"
self.alphapose_neighbor_detection = neighbor_name + "/detection"
self.mhmr_topic = "/multihmr_joints"
self.camera_info = rotors_machine_name + rotors_machine_name + "/xtion/rgb/camera_info"
self.camera_info_neighbor = rotors_neighbor_name + rotors_neighbor_name + "/xtion/rgb/camera_info"
self.joints_gt = '/gt_joints'
self.mpc_command = machine_name + "/command_MPC"
import tf
self.listener = tf.TransformListener()
self.gazebo.unpauseSim()
#self.controllers_object.reset_controllers()
# We Start all the ROS related Subscribers and publishers
rospy.Subscriber(self.firefly_pose_topic, PoseWithCovarianceStamped,
self._firefly_pose_callback)
rospy.Subscriber(self.pose_topic, uav_pose, self._pose_callback)
velocity = message_filters.Subscriber(self.velocity_topic, Odometry)
self.velocity_cache = message_filters.Cache(velocity, 300)
velocity_neighbor = message_filters.Subscriber(
self.velocity_neighbor_topic, Odometry)
self.velocity_neighbor_cache = message_filters.Cache(
velocity_neighbor, 300)
rospy.Subscriber(self.gt_pose_topic, uav_pose, self._gt_pose_callback)
rospy.Subscriber(self.gt_neighbor_pose_topic, uav_pose,
self._gt_neighbor_pose_callback)
# self.gt_neighbor_cache = message_filters.Cache(gt_neighbor,300);
rospy.Subscriber(self.target_topic, PoseWithCovarianceStamped,
self._target_callback)
gt_target = message_filters.Subscriber(self.gt_target_topic, Odometry)
self.gt_target_cache = message_filters.Cache(gt_target, 300)
rospy.Subscriber(self.target_velocity_topic,
TwistWithCovarianceStamped, self._target_vel_callback)
rospy.Subscriber(self.detections_feedback_topic, NeuralNetworkFeedback,
self._detections_feedback_callback)
noisy_joints = message_filters.Subscriber(self.noisy_joints_topic,
AlphaRes)
self.noisy_joints_cache = message_filters.Cache(noisy_joints, 300)
self.noisy_joints_cache.registerCallback(self._noisy_joints_callback)
noisy_joints_neighbor = message_filters.Subscriber(
self.noisy_joints_neighbor_topic, AlphaRes)
self.noisy_joints_neighbor_cache = message_filters.Cache(
noisy_joints_neighbor, 300)
self.noisy_joints_neighbor_cache.registerCallback(
self._noisy_joints_neighbor_callback)
noisy_bbox = message_filters.Subscriber(self.noisy_bbox_topic,
AlphaRes)
self.noisy_bbox_cache = message_filters.Cache(noisy_bbox, 300)
self.noisy_bbox_cache.registerCallback(self._noisy_bbox_callback)
noisy_bbox_neighbor = message_filters.Subscriber(
self.noisy_bbox_neighbor_topic, AlphaRes)
self.noisy_bbox_neighbor_cache = message_filters.Cache(
noisy_bbox_neighbor, 300)
self.noisy_bbox_neighbor_cache.registerCallback(
self._noisy_bbox_neighbor_callback)
rospy.Subscriber(self.noisy_detection, Int16,
self._noisy_detection_callback)
rospy.Subscriber(self.noisy_neighbor_detection, Int16,
self._noisy_neighbor_detection_callback)
alphapose = message_filters.Subscriber(self.alphapose_topic, AlphaRes)
self.alphapose_cache = message_filters.Cache(alphapose, 300)
self.alphapose_cache.registerCallback(self._alphapose_callback)
alphapose_neighbor = message_filters.Subscriber(
self.alphapose_neighbor_topic, AlphaRes)
self.alphapose_neighbor_cache = message_filters.Cache(
alphapose_neighbor, 100)
self.alphapose_neighbor_cache.registerCallback(
self._alphapose_neighbor_callback)
alphapose_bbox = message_filters.Subscriber(self.alphapose_bbox_topic,
AlphaRes)
self.alphapose_bbox_cache = message_filters.Cache(alphapose_bbox, 300)
alphapose_bbox_neighbor = message_filters.Subscriber(
self.alphapose_bbox_neighbor_topic, AlphaRes)
self.alphapose_bbox_neighbor_cache = message_filters.Cache(
alphapose_bbox_neighbor, 300)
mhmr = message_filters.Subscriber(self.mhmr_topic, PoseArray)
self.mhmr_cache = message_filters.Cache(mhmr, 300)
rospy.Subscriber(self.alphapose_detection, Int16,
self._alphapose_detection_callback)
rospy.Subscriber(self.alphapose_neighbor_detection, Int16,
self._alphapose_neighbor_detection_callback)
rospy.Subscriber(self.command_topic, uav_pose, self._command_callback)
rospy.Subscriber(self.camera_info, CameraInfo,
self._camera_info_callback)
rospy.Subscriber(self.camera_info_neighbor, CameraInfo,
self._camera_info_neighbor_callback)
rospy.Subscriber(self.joints_gt, PoseArray, self._joints_gt_callback)
rospy.Subscriber(self.mpc_command, uav_pose,
self._mpc_command_callback)
self._dest_vel_pub = rospy.Publisher(self.destination_topic,
PoseStamped,
queue_size=1)
self._cmd_vel_pub = rospy.Publisher(self.command_topic,
uav_pose,
queue_size=1)
self._cmd_neighbor_vel_pub = rospy.Publisher(
self.neighbor_command_topic, uav_pose, queue_size=1)
self._target_init_pub = rospy.Publisher(self.target_topic,
PoseWithCovarianceStamped,
queue_size=1)
self.create_circle(radius=8)
# self.gazebo.pauseSim()
outPose = uav_pose()
outPose.header.stamp = rospy.Time.now()
outPose.header.frame_id = "world"
outPose.POI.x = 0
outPose.POI.y = 0
outPose.POI.z = 0
r = 8 #
t = np.random.choice(63, 1)
outPose.position.x = r * np.cos(self.theta[t[0]])
outPose.position.y = r * np.sin(self.theta[t[0]])
outPose.position.z = -r
self._cmd_vel_pub.publish(outPose)
rospy.logwarn("Finished FireflyEnv INIT...")
self._check_all_sensors_ready()
rospy.logwarn("SENSORS OK!.")
self._check_publishers_connection()
rospy.logwarn("PUBLISHERS OK!.")
#the message filters below are used to cache the messages published on each topic. This is necessary because the
#camera capture packages publish the number of cars detected on a frame to frame basis. In an instance
#where the vehicled detector missess detecting a vehicle for a few frames(false negatives), the subscriber might
#get car count of 0, indicating no vehicles on a lane which is not correct. Therefore, using a cache, we take the
#average number of cars detected in 5(or any number) of frames. This way, we increase the tolerance of our vehicle
#detector.
#In all the camera capture packages and this one, we have changed the message type of the lane counters to 'stampedInt'.
#stampedInt is a custom message type i've created in the ros_arduino_msgs folder which is basically an Integer that
#also has a time stamp. The time stamp is required to use the message filter's caching capability.
lane1 = message_filters.Subscriber("/lane1_cars", stampedInt)
cache = message_filters.Cache(
lane1, 5
) #Change number to change the cache size. A value of 1 represents the same thing as no caching.
cache.registerCallback(lane1_callback)
lane2 = message_filters.Subscriber("/lane2_cars", stampedInt)
cache2 = message_filters.Cache(lane2, 5)
cache2.registerCallback(lane2_callback)
lane3 = message_filters.Subscriber("/lane3_cars", stampedInt)
cache3 = message_filters.Cache(lane3, 5)
cache3.registerCallback(lane3_callback)
lane4 = message_filters.Subscriber("/lane4_cars", stampedInt)
cache4 = message_filters.Cache(lane4, 5)
cache4.registerCallback(lane4_callback)
