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
Example #2
0
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()
Example #3
0
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()
Example #4
0
    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
Example #5
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)
Example #10
0
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()
Example #11
0
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()
Example #12
0
    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()
Example #13
0
 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()
Example #14
0
    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)
Example #15
0
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()
Example #16
0
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)
Example #18
0
    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.")
Example #19
0
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
Example #20
0
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)
Example #22
0
    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')
Example #24
0
    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)
Example #25
0
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()
Example #26
0
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)
Example #27
0
 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()
Example #28
0
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)
Example #30
0
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()