def __init__(self):
dynamic_reconfigure.server.Server(ImageTimeDiffConfig,
self._cb_dyn_reconfig)
sub_hue = message_filters.Subscriber('~input/hue', Image)
sub_saturation = message_filters.Subscriber('~input/saturation', Image)
ts = message_filters.TimeSynchronizer([sub_hue, sub_saturation], 10)
ts.registerCallback(self._cb_input)
rospy.Subscriber('~start', Header, self._cb_start)
rospy.Subscriber('~stop', Header, self._cb_stop)
self.pub_stored = None
self.input = None
def listenerHardware():
rospy.init_node('listenerHardware', anonymous=True)
subRefle = message_filters.Subscriber('refletancia', RefletanciaMsg)
subDistancia = message_filters.Subscriber('distancia', SensoresDistanciaMsg)
subBtns = message_filters.Subscriber('botoes', BotoesMsg)
ts = message_filters.TimeSynchronizer([subRefle, subDistancia, subBtns], 10)
ts.registerCallback(callbackHardware)
rospy.spin()
def rundetection():
rospy.init_node('feature_detection', anonymous=True)
right_sub = message_filters.Subscriber("/image_raw_throttled", Image, queue_size=10)#,heyo1)#,queue_size=4)
left_sub = message_filters.Subscriber("/image_raw_throttled", Image, queue_size=10)#,heyo2)#,queue_size=4)
rospy.Subscriber('/bebop/odom', Odometry, writeOdom)
ts = message_filters.TimeSynchronizer([left_sub,right_sub],10)
# ts.registerCallback(OpticalFlow)
ts.registerCallback(PoseEstimate)
rospy.spin()
def __init__(self):
self.bridge = CvBridge()
self.rimage_sub = message_filters.Subscriber("/camera/right/image_raw",
Image)
self.limage_sub = message_filters.Subscriber("/camera/left/image_raw",
Image)
self.ts = message_filters.TimeSynchronizer(
[self.limage_sub, self.rimage_sub],
1).registerCallback(self.callback)
self.counter = 0
def main():
rospy.init_node('gbestcalc')
rospy.Rate(10)
bot1_sub = mf.Subscriber('/Bot_1/pbest', swarm)
bot2_sub = mf.Subscriber('/Bot_2/pbest', swarm)
bot3_sub = mf.Subscriber('/Bot_3/pbest', swarm)
bot4_sub = mf.Subscriber('/Bot_4/pbest', swarm)
bot5_sub = mf.Subscriber('/Bot_5/pbest', swarm)
ats = mf.TimeSynchronizer([bot1_sub, bot2_sub,bot3_sub,bot4_sub,bot5_sub], 10)
ats.registerCallback(callback)
gbest_calculator()
def subscribe(self):
approximate_sync = rospy.get_param("~approximate_sync", False)
queue_size = rospy.get_param("~queue_size", 100)
slop = rospy.get_param("~slop", 0.1)
sub_cls = MF.Subscriber(
"~input/classes", ClassificationResult, queue_size=1)
sub_box = MF.Subscriber(
"~input/boxes", BoundingBoxArray, queue_size=1)
sub_pose = MF.Subscriber(
"~input/poses", PoseArray, queue_size=1)
sub_people = MF.Subscriber(
"~input/people", PeoplePoseArray, queue_size=1)
sub_od = MF.Subscriber(
"~input/ObjectDetection", ObjectDetection, queue_size=1)
if approximate_sync:
sync_box = MF.ApproximateTimeSynchronizer(
[sub_box, sub_cls], queue_size=queue_size, slop=slop)
sync_pose = MF.ApproximateTimeSynchronizer(
[sub_pose, sub_cls], queue_size=queue_size, slop=slop)
sync_people = MF.ApproximateTimeSynchronizer(
[sub_people, sub_cls], queue_size=queue_size, slop=slop)
sync_od = MF.ApproximateTimeSynchronizer(
[sub_od, sub_cls], queue_size=queue_size, slop=slop)
else:
sync_box = MF.TimeSynchronizer(
[sub_box, sub_cls], queue_size=queue_size)
sync_pose = MF.TimeSynchronizer(
[sub_pose, sub_cls], queue_size=queue_size)
sync_people = MF.TimeSynchronizer(
[sub_people, sub_cls], queue_size=queue_size)
sync_od = MF.TimeSynchronizer(
[sub_od, sub_cls], queue_size=queue_size)
sync_box.registerCallback(self.box_msg_callback)
sync_pose.registerCallback(self.pose_msg_callback)
sync_people.registerCallback(self.people_msg_callback)
sync_od.registerCallback(self.od_msg_callback)
self.subscribers = [sub_cls, sub_box, sub_pose, sub_people, sub_od]
def subscribe(self):
self._sub_feature = message_filters.Subscriber('~input', Feature0D)
self._sub_label = message_filters.Subscriber('~input/label', Image)
use_async = rospy.get_param('~approximate_sync', False)
if use_async:
sync = message_filters.ApproximateTimeSynchronizer(
[self._sub_feature, self._sub_label], queue_size=10, slop=0.1)
else:
sync = message_filters.TimeSynchronizer(
[self._sub_feature, self._sub_feature], queue_size=10)
jsk_logdebug('~approximate_sync: {}'.format(use_async))
sync.registerCallback(self._apply)
def setUp(self):
self.start_time = time.time()
self.received_synced_topics = False
self.sync_ref_objects = message_filters.Subscriber(
'/object_tracks', DetectedObjectArray)
self.sync_homework_objects = message_filters.Subscriber(
'/homework4/object_tracks', DetectedObjectArray)
self.time_syncer = message_filters.TimeSynchronizer(
[self.sync_ref_objects, self.sync_homework_objects], 100)
self.time_syncer.registerCallback(self.recv_synced_topics)
self.sub_clock = rospy.Subscriber('/clock', Clock, self.recv_clock)
def __init__(self):
self.bridge = CvBridge()
# self.image_sub = rospy.Subscriber('/carla/ego_vehicle/camera/depth/view/image_depth', Image, self.callback)
self.image_sub = message_filters.Subscriber(
'/carla/ego_vehicle/camera/rgb/front/image_color', Image)
self.depth_sub = message_filters.Subscriber(
'/carla/ego_vehicle/camera/depth/view/image_depth', Image)
ts = message_filters.TimeSynchronizer([self.image_sub, self.depth_sub],
10)
# ts = message_filters.ApproximateTimeSynchronizer([self.image_sub, self.depth_sub], 10, 0.1, allow_headerless=True)
ts.registerCallback(self.dataCallback)
def listener():
#da jedan callback obraduje dva topica koristim message_filters
#image_sub = rospy.Subscriber('cnn_frame', Image, callback)
image_sub = message_filters.Subscriber('/cnn_image', Image)
feature_sub = message_filters.Subscriber('/cnn_features', CnnFeatures)
ts = message_filters.TimeSynchronizer([image_sub, feature_sub],
queue_size=10)
ts.registerCallback(callback)
rospy.spin()
def listener():
rospy.init_node('image_listenor', anonymous=True)
# synchronize three streams
image_sub = message_filters.Subscriber('/camera/color/image_raw', Image)
depth_sub = message_filters.Subscriber(
'/camera/aligned_depth_to_color/image_raw', Image)
cls_sub = message_filters.Subscriber('/seg/raw', Image)
ts = message_filters.TimeSynchronizer([image_sub, depth_sub, cls_sub], 30)
ts.registerCallback(callback)
rospy.spin()
def __init__(self):
rospy.init_node('mapping_node')
# Mapping Constants
self.origin = Pose()
self.origin.position.x = -25.
self.origin.position.y = -25.
self.origin.orientation.w = 1.
self.map_info = MapMetaData()
self.map_info.map_load_time = rospy.Time.now()
self.map_info.resolution = .1
self.map_info.height = 1000
self.map_info.width = 1000
self.map_info.origin = self.origin
# Sensor Model
self.alpha = 0.9
self.beta = 0.7
# Transform(s)
# sensor to robot
self.T_sensor = np.array([[np.cos(0), -1*np.sin(0), 0.27],
[np.sin(0), np.cos(0), 0.0],
[0.0, 0.0, 1.0]])
# robot to world
self.T_robot = np.array([[np.cos(0), -1*np.sin(0), 0.0],
[np.sin(0), np.cos(0), 0.0],
[0.0, 0.0, 1]])
# Occupancy grid variable (array)
# occ_grid is a numpy matrix
# Prefill with 0.5 for unknown occupancy
self.occ_grid = np.repeat(np.matrix(np.repeat(.5, self.map_info.width)), self.map_info.height, axis=0)
# ros_occ_grid is a ROS OccupancyGrid instance
self.ros_occ_grid = OccupancyGrid()
self.ros_occ_grid.info = self.map_info
# Publishers
self.map_pub = rospy.Publisher('/map', OccupancyGrid, queue_size=1)
# Subscribers
# pose_sub and scan_sub are synchronized.
# when the updateMap callback is executed it receives a message from both.
pose_sub = message_filters.Subscriber('/groundtruth', Odometry)
scan_sub = message_filters.Subscriber('/r2d2/laser/scan', LaserScan)
ts = message_filters.TimeSynchronizer([pose_sub, scan_sub], 10)
ts.registerCallback(self.updateMap)
rospy.spin()
def __init__(self):
self.bridge = CvBridge()
# self.subscriber = rospy.Subscriber(topicName, Image, self.callback)
self.image_sub = message_filters.Subscriber("/camera/color/image_raw",
Image)
# self.depth_sub = message_filters.Subscriber("/camera/depth/color/points", PointCloud2, self.callaback)
self.depth_sub = message_filters.Subscriber(
"/camera/aligned_depth_to_color/image_raw", Image)
self.ts = message_filters.TimeSynchronizer(
[self.image_sub, self.depth_sub], 10)
self.ts.registerCallback(self.callback)
self.vision = Vision()
def __init__(self, *args, **kwds):
wx.Frame.__init__(self, *args, **kwds)
self.image = OpenCVImage(self, -1, style=wx.TAB_TRAVERSAL)
self.SetSize((640, 480))
self.SetTitle("Displayer")
rospy.init_node('display_gui', anonymous=True, disable_signals=True)
self.bridge = CvBridge()
image_sub = message_filters.Subscriber("image", Image)
masks_sub = message_filters.Subscriber("masks", MaskArray)
self.ts = message_filters.TimeSynchronizer([image_sub, masks_sub], 100)
self.ts.registerCallback(self.callback)
def callback(self, data1, data2, data3, data4):
pass
imu1 = message_filters.Subscriber('/ns01/imu', YOUR_IMU_DATA_TYPE)
imu2 = message_filters.Subscriber('/ns02/imu', YOUR_IMU_DATA_TYPE)
imu3 = message_filters.Subscriber('/ns03/imu', YOUR_IMU_DATA_TYPE)
imu4 = message_filters.Subscriber('/ns04/imu', YOUR_IMU_DATA_TYPE)
ts = message_filters.TimeSynchronizer(imu1, imu2, imu3, imu4, 10)
ts.registerCallback(callback)
rospy.spin()
def controller():
print('ccccc')
rospy.init_node('controller',anonymous=True)
pub = rospy.Publisher('cmd_vel',Twist,queue_size=10)
x_sub = message_filters.Subscriber('/odom', Odometry)
vel_sub = message_filters.Subscriber('/rs_vect', vect_msg)
ts = message_filters.TimeSynchronizer([vel_sub,x_sub], 10)
ts.registerCallback(callback,pub)
# rate=rospy.Rate(30)
# rate.sleep()
print('dddddd')
rospy.spin()
def subscribe(self):
sub_rgb = message_filters.Subscriber("~input/rgb", Image)
sub_ins = message_filters.Subscriber("~input/label_ins", Image)
sub_lbl = message_filters.Subscriber("~input/class", ObjectClassArray)
self._subscribers = [sub_rgb, sub_ins, sub_lbl]
if rospy.get_param("approximate_sync", False):
sync = message_filters.ApproximateTimeSynchronizer(
self._subscribers, queue_size=100, slop=0.1)
else:
sync = message_filters.TimeSynchronizer(self._subscribers,
queue_size=100)
sync.registerCallback(self._callback)
def __init__(self):
self.bridge = CvBridge()
self.sub_image = message_filters.Subscriber("/zed/rgb/image_raw_color",
Image,
queue_size=1)
self.sub_depth = message_filters.Subscriber(
"/zed/depth/depth_registered", Image, queue_size=1)
self.ts = message_filters.TimeSynchronizer(
[self.sub_image, self.sub_depth], 10)
self.ts.registerCallback(self.depthSyncCallback)
rospy.loginfo("ImageGrabber initialized.")
def sensor_model():
rospy.init_node('sensor_model_ideal', anonymous=False) # Start node
# Subscriber the data in callback function
ego_data = message_filters.Subscriber("/ego_data", TrafficUpdateMovingObject)
osi_objs = message_filters.Subscriber("/osi3_moving_obj", GroundTruthMovingObjects)
#ts = message_filters.ApproximateTimeSynchronizer([ego_data, osi_objs], 10, 0.1)
ts = message_filters.TimeSynchronizer([ego_data, osi_objs], 3)
ts.registerCallback(callback)
rospy.spin() # spin() simply keeps python from exiting until this node is stopped
def arduino_cam():
rospy.init_node('arduino_cam', anonymous=True)
subRefle = message_filters.Subscriber('refletancia', RefletanciaMsg)
subDistancia = message_filters.Subscriber('distancia',
SensoresDistanciaMsg)
subCam = message_filters.Subscriber('tem_circulos', BoolStamped)
ts = message_filters.TimeSynchronizer([subRefle, subDistancia, subCam], 20)
ts.registerCallback(arduinoCamCb)
rospy.spin()
def __init__(self):
rospy.init_node("k4a_vision", anonymous=False)
self.bridge = cv_bridge.CvBridge()
self.rgb_topic = '/rgb/image_raw'
self.depth_topic = '/depth_to_rgb/image_raw'
self.cloud_topic = '/points2'
self.image_sub = message_filters.Subscriber(self.rgb_topic, Image)
self.depth_sub = message_filters.Subscriber(self.depth_topic, Image)
# self.cloud_sub = message_filters.Subscriber(self.cloud_topic, PointCloud2)
self.ts = message_filters.TimeSynchronizer(
[self.image_sub, self.depth_sub], 10)
self.ts.registerCallback(image_cloud_callback)
def __init__(self):
self.i = 0
def topic(lr, sub):
return rospy.resolve_name("stereo") + "/%s/%s" % ({'l' : 'left', 'r' : 'right'}[lr], sub)
self.tracking = {}
if 0:
self.cams = {}
for lr in "lr":
rospy.Subscriber(topic(lr, "camera_info"), sensor_msgs.msg.CameraInfo, partial(self.got_info, lr))
rospy.Subscriber(topic("l", "image_rect_color"), sensor_msgs.msg.Image, self.gotimage)
else:
tosync_stereo = [
(topic('l', "image_rect_color"), sensor_msgs.msg.Image),
(topic('l', "camera_info"), sensor_msgs.msg.CameraInfo),
(topic('r', "image_rect_color"), sensor_msgs.msg.Image),
(topic('r', "camera_info"), sensor_msgs.msg.CameraInfo)
]
tosync_mono = [
("image_stream", sensor_msgs.msg.Image),
("camera_info", sensor_msgs.msg.CameraInfo),
]
self.q_stereo = Queue.Queue()
tss = message_filters.TimeSynchronizer([message_filters.Subscriber(topic, type) for (topic, type) in tosync_stereo], 10)
tss.registerCallback(self.queue_stereo)
sth = ConsumerThread(self.q_stereo, self.handle_stereo)
sth.setDaemon(True)
sth.start()
self.q_mono = Queue.Queue()
tss = message_filters.TimeSynchronizer([message_filters.Subscriber(topic, type) for (topic, type) in tosync_mono], 10)
tss.registerCallback(self.queue_mono)
mth = ConsumerThread(self.q_mono, self.handle_mono)
mth.setDaemon(True)
mth.start()
self.pub_tf = rospy.Publisher("/tf", tf.msg.tfMessage)
self.nf = 0
def subscribe(self):
self.sub = message_filters.Subscriber('~input', Image)
self.sub_mask = message_filters.Subscriber('~input/mask', Image)
self.use_async = rospy.get_param('~approximate_sync', False)
rospy.loginfo('~approximate_sync: {}'.format(self.use_async))
if self.use_async:
sync = message_filters.ApproximateTimeSynchronizer(
[self.sub, self.sub_mask], queue_size=1000, slop=0.1)
else:
sync = message_filters.TimeSynchronizer([self.sub, self.sub_mask],
queue_size=1000)
sync.registerCallback(self._apply)
warn_no_remap('~input', '~input/mask')
def __init__(self):
self.last_score_time = rospy.get_rostime() - rospy.Duration(20)
rospy.loginfo("waiting for score service")
rospy.wait_for_service('rpc_score')
rospy.loginfo("Found score service")
image_sub = message_filters.Subscriber('/camera/rgb/image_color/compressed', CompressedImage)
info_sub = message_filters.Subscriber('/camera/rgb/camera_info', CameraInfo)
self.score_notification = rospy.Publisher("/scored_observation", Header)
apriltag_sub = message_filters.Subscriber("/tag_detections", AprilTagDetectionArray)
ts = message_filters.TimeSynchronizer([image_sub, info_sub, apriltag_sub], 10)
ts.registerCallback(self.callback)
def sensor_rotate():
# Node initialization
rospy.init_node('KFtesting', anonymous=False) # Start node
rate = rospy.Rate(rospy.get_param("freq"))
# subscribe to sensor data and ego data with time synchronization
objs1 = message_filters.Subscriber('/sensor0/obj_list_egoframe', ObjectsList)
objsI = message_filters.Subscriber('/sensor5/obj_list_egoframe', ObjectsList)
ts = message_filters.TimeSynchronizer([ego_data, objs_list],10)
#ts = message_filters.TimeSynchronizer([ego_data, objs_list], 10)
ts.registerCallback(callback)
rospy.spin()
def filter_example(
): #THIS COULD WORK, BUT DOESN'T BECAUSE TIMESTAMPS DO NOT MATCH
rospy.init_node('rosSegmentCloud')
pos_pub = rospy.Publisher("face_position", PointStamped)
print 'Filter the image topic and point cloud topic to come in together'
image_sub = message_filters.Subscriber(
"/wide_stereo/left/image_rect_color", Image)
cloud_sub = message_filters.Subscriber("/tilt_scan_cloud", PointCloud)
camera_sub = message_filters.Subscriber("/wide_stereo/left/camera_info",
CameraInfo)
ts = message_filters.TimeSynchronizer([image_sub, camera_sub], 10)
ts.registerCallback(cs.callback)
def HumansPub(self):
rospy.init_node('Morse_Humans', anonymous=True)
hum_marker_sub = []
for human_id in range(1, self.num_hum + 1):
name = 'human' + str(human_id)
hum_marker_sub.append(
message_filters.Subscriber("/" + name, HumanMarkerStamped))
self.tracked_humans_pub = rospy.Publisher("/tracked_humans",
TrackedHumans,
queue_size=1)
pose_msg = message_filters.TimeSynchronizer(hum_marker_sub, 10)
pose_msg.registerCallback(self.HumansCB)
rospy.spin()
def rundetection():
rospy.init_node('feature_detection', anonymous=True)
right_sub = message_filters.Subscriber(
"/duo3d/right/image_rect", Image,
queue_size=10) #,heyo1)#,queue_size=4)
left_sub = message_filters.Subscriber(
"/duo3d/left/image_rect", Image,
queue_size=10) #,heyo2)#,queue_size=4)
rospy.Subscriber('/bebop/odom', Odometry, writeOdom)
#ts = message_filters.ApproximateTimeSynchronizer([left_sub,right_sub],1,.05e9)
ts = message_filters.TimeSynchronizer([left_sub, right_sub], 10)
ts.registerCallback(OpticalFlow)
rospy.spin()
def subscribe(self):
self.sub_bof = message_filters.Subscriber('~input/bof',
ClassificationResult)
self.sub_ch = message_filters.Subscriber('~input/ch',
ClassificationResult)
queue_size = rospy.get_param('~queue_size', 100)
if rospy.get_param('~approximate_sync', False):
sync = message_filters.ApproximateTimeSynchronizer(
[self.sub_bof, self.sub_ch], queue_size=queue_size, slop=1)
else:
sync = message_filters.TimeSynchronizer(
[self.sub_bof, self.sub_ch], queue_size=queue_size)
sync.registerCallback(self._apply)
def main():
rospy.init_node('estimate_odom')
# create subscribers to messages from beacons
b1_sub = message_filters.Subscriber("/beacon_0/range", Position)
b2_sub = message_filters.Subscriber('/beacon_1/range', Position)
b3_sub = message_filters.Subscriber('/beacon_2/range', Position)
# syncrhonize the messages
ts = message_filters.TimeSynchronizer([b1_sub, b2_sub, b3_sub], 10)
ts.registerCallback(callback)
rospy.spin()
