def __init__(self, topics_to_parse, path, offset_dts, scale_factors, world_frame_transform): self.topics_to_parse = topics_to_parse self.duplicate = False if len(topics_to_parse) == 2 or topics_to_parse[2] == topics_to_parse[0]: # We use the same data stream for building and eval print 'Duplicate image streams! Not running another synchroniser!' self.duplicate = True self.path = path self.offset_dts = offset_dts self.scale_factors = scale_factors self.rs_data = [] self.kin_data = [] self.subs = [] self.img_sub = Subscriber(topics_to_parse[0], Image) self.subs.append(self.img_sub) self.pose_sub = Subscriber(topics_to_parse[1], PoseStamped) self.subs.append(self.pose_sub) self.world_frame_transform = world_frame_transform if not self.duplicate: if topics_to_parse[2] != topics_to_parse[0]: self.subs.append(Subscriber(topics_to_parse[2], Image)) else: self.subs.append(self.img_sub) if topics_to_parse[3] != topics_to_parse[1]: self.subs.append(Subscriber(topics_to_parse[3], PoseStamped)) else: self.subs.append(self.pose_sub) self.rs_synchronizer = ApproximateTimeSynchronizer([self.subs[0], self.subs[1]], 100, 0.05) self.rs_synchronizer.registerCallback(self.got_tuple, 'rs') if not self.duplicate: self.kin_synchronizer = ApproximateTimeSynchronizer([self.subs[2], self.subs[3]], 100, 0.05) self.kin_synchronizer.registerCallback(self.got_tuple, 'kin') self.bridge = cv_bridge.CvBridge() print 'Topics to parse are '+str(topics_to_parse)
def listener(): global control_pub, record_param_pub # In ROS, nodes are uniquely named. If two nodes with the same # name are launched, the previous one is kicked off. The # anonymous=True flag means that rospy will choose a unique # name for our 'listener' node so that multiple listeners can # run simultaneously. rospy.init_node('listener', anonymous=True) #rospy.Subscriber('/camera/imu', Imu, callback1) #rospy.Subscriber('/dynamixel/XM3', XM2, callback2) #rospy.Subsriber('/dynamixel/XM4', XM2, callback3) #rospy.Subscriber('/vrpn_client_node/camera1/pose', PoseStamped, callback4) ts = ApproximateTimeSynchronizer( [Subscriber('/dynamixel/XM3', XM2), Subscriber('/dynamixel/XM4', XM2)], queue_size=100, slop=0.1) ts.registerCallback(callback) control_pub = rospy.Publisher('Wheel', Float64MultiArray, queue_size=10) record_param_pub = rospy.Publisher('record_inner_param', Float64StampedMultiArray, queue_size=10) # spin() simply keeps python from exiting until this node is stopped rospy.spin()
def __init__(self, name): rospy.loginfo("[" + rospy.get_name() + "] " + "Starting ... ") self.vis_marker_topic = rospy.get_param("~vis_marker_topic", "~visualization_marker") self.vel_costmap_topic = rospy.get_param("~vel_costmap_topic", "/velocity_costmap") self.origin_topic = rospy.get_param("~origin_topic", "origin") self.base_link_tf = rospy.get_param("~base_link_tf", "base_link") self.vis_pub = rospy.Publisher(self.vis_marker_topic, Marker, queue_size=1) self.tf = TransformListener() self.cc = CostmapCreator( rospy.Publisher(self.vel_costmap_topic, OccupancyGrid, queue_size=10, latch=True), rospy.Publisher(self.origin_topic, PoseStamped, queue_size=10)) self.dyn_srv = DynServer(VelocityCostmapsConfig, self.dyn_callback) subs = [ Subscriber( rospy.get_param("~qtc_topic", "/qtc_state_predictor/prediction_array"), QTCPredictionArray), Subscriber( rospy.get_param("~ppl_topic", "/people_tracker/positions"), PeopleTracker) ] self.ts = ApproximateTimeSynchronizer(fs=subs, queue_size=60, slop=0.1) self.ts.registerCallback(self.callback) rospy.loginfo("[" + rospy.get_name() + "] " + "... all done.")
def __initMessageFilter(self): # rospy.Subscriber("/vision_manager/occipital_lobe_topic", # self.__occipitalMessageType, # self.ddd, # queue_size=1) # rospy.Subscriber("/spinalcord/sternocleidomastoid_position", # JointState, # self.eee, # queue_size=1) if self.__motorCortexMsgType is not None and self.__motorCortexTopicName is not None: rospy.Subscriber(self.__motorCortexTopicName, self.__motorCortexMsgType, self.__motorCortexCallback, queue_size=1) objSub = Subscriber("/vision_manager/occipital_lobe_topic", self.__occipitalMessageType, buff_size=2**24) panTiltSub = Subscriber("/spinalcord/sternocleidomastoid_position", JointState) self.__messageFilter = ApproximateTimeSynchronizer( [objSub, panTiltSub], 5, 0.05, allow_headerless=True) self.__messageFilter.registerCallback(self.__callback)
def __init__(self): """ One publisher should publish to the /brake topic with a AckermannDriveStamped brake message. One publisher should publish to the /brake_bool topic with a Bool message. You should also subscribe to the /scan topic to get the LaserScan messages and the /odom topic to get the current speed of the vehicle. The subscribers should use the provided odom_callback and scan_callback as callback methods NOTE that the x component of the linear velocity in odom is the speed """ self.brake_bool = rospy.Publisher('brake_bool', StampedBool, queue_size=100) # Initialize subscribers self.scan_subscriber = Subscriber('scan', LaserScan, queue_size=100) self.odom_subscriber = Subscriber('odom', Odometry, queue_size=100) #create the time synchronizer self.sub = ApproximateTimeSynchronizer( [self.scan_subscriber, self.odom_subscriber], queue_size=100, slop=0.020) #register the callback to the synchronizer self.sub.registerCallback(self.master_callback) self.THRESHOLD = 0.5
def listener(self): # In ROS, nodes are uniquely named. If two nodes with the same # name are launched, the previous one is kicked off. The # anonymous=True flag means that rospy will choose a unique # name for our 'listener' node so that multiple listeners can # run simultaneously. self.count = 0 #calib_file = "/home/robesafe/AB3DMOT/data/KITTI/resources/CARLA/calib/0102.txt" #calib_file = "/media/robesafe/videos1.2/KITTI/KITTI_dataset_tracking/data_tracking_calib/training/calib/0001.txt" calib_file = "t4ac_calib.txt" self.calib = Calibration(calib_file) # load the calibration self.bridge = CvBridge() rospy.init_node('visualizer', anonymous=True) subs_info = Subscriber("/AB3DMOT_kitti", Object_kitti_list, queue_size=5) subs_img = Subscriber("/zed/zed_node/left/image_rect_color", Image, queue_size=5) self.image_pub = rospy.Publisher("/AB3DMOT_image", Image) #subs_img = Subscriber("/carla/ego_vehicle/camera/rgb/view/image_color", Image, queue_size = 5) ats = ApproximateTimeSynchronizer([subs_info, subs_img], queue_size=5, slop=10) ats.registerCallback(self.callback) # spin() simply keeps python from exiting until this node is stopped rospy.spin()
def init_node(self, ns="~"): self.imu_pose = rospy.get_param(ns + "imu_pose") self.imu_pose = n2g(self.imu_pose, "Pose3") self.imu_rot = self.imu_pose.rotation() # Parameters for Node self.dvl_max_velocity = rospy.get_param(ns + "dvl_max_velocity") self.keyframe_duration = rospy.get_param(ns + "keyframe_duration") self.keyframe_translation = rospy.get_param(ns + "keyframe_translation") self.keyframe_rotation = rospy.get_param(ns + "keyframe_rotation") # Subscribers and caches self.imu_sub = Subscriber(IMU_TOPIC, Imu) self.dvl_sub = Subscriber(DVL_TOPIC, DVL) self.depth_sub = Subscriber(DEPTH_TOPIC, Depth) self.depth_cache = Cache(self.depth_sub, 1) # Use point cloud for visualization self.traj_pub = rospy.Publisher(LOCALIZATION_TRAJ_TOPIC, PointCloud2, queue_size=10) self.odom_pub = rospy.Publisher(LOCALIZATION_ODOM_TOPIC, Odometry, queue_size=10) # Sync self.ts = ApproximateTimeSynchronizer([self.imu_sub, self.dvl_sub], 200, 0.1) self.ts.registerCallback(self.callback) self.tf = tf.TransformBroadcaster() loginfo("Localization node is initialized")
def __init__(self): print("Verifying Estimates") self.syn_pub = rospy.Publisher('sync_estimates', SyncVerifyEstimates, queue_size=10) self.error_pub = rospy.Publisher('estimate_error', SyncEstimateError, queue_size=10) estimate_subs = Subscriber("xyz_debug_estimates", XYZDebugEstimate) truth_subs = Subscriber("mocap/velocity/body_level_frame", TwistWithCovarianceStamped) rgbd_subs = Subscriber("rgbd_velocity/body_level_frame", DeltaToVel) mocap_pose_subs = Subscriber("pose_stamped", PoseStamped) self.sync_msg = SyncVerifyEstimates() self.estimate_error = SyncEstimateError() self.multiplier = 1 self.sonar_offset = 0 use_sonar = False self.multiplier = rospy.get_param("SD_Multiplied") self.sonar_offset = rospy.get_param("Z_offset") use_sonar = rospy.get_param("use_sonar") print(use_sonar) if use_sonar: sonar_subs = Subscriber("sonar_ned", Range) approx_sync_sonar = ApproximateTimeSynchronizer([estimate_subs, truth_subs, rgbd_subs, mocap_pose_subs,sonar_subs], queue_size=5, slop=0.1) approx_sync_sonar.registerCallback(self.callbackSonar) else: approx_sync_wo_sonar = ApproximateTimeSynchronizer([estimate_subs, truth_subs, rgbd_subs, mocap_pose_subs], queue_size=5, slop=0.1) approx_sync_wo_sonar.registerCallback(self.callbackNOSonar)
def run(self): sync = ApproximateTimeSynchronizer([ Subscriber(self.map1, OccupancyGrid), Subscriber(self.map2, OccupancyGrid) ], 10, 1) sync.registerCallback(self.sync_callback) rospy.spin()
def main(): global camera_info_topic, tf_listener rospy.init_node('open3d_recorder', anonymous=True) # Create TF listener tf_listener = tf.TransformListener(rospy.Duration(500)) # Get parameters depth_image_topic = rospy.get_param('~depth_image_topic') color_image_topic = rospy.get_param('~color_image_topic') camera_info_topic = rospy.get_param('~camera_info_topic') cache_count = rospy.get_param('~cache_count', 10) slop = rospy.get_param('~slop', 0.01) # The delay (in seconds) with which messages can be synchronized. allow_headerless = False #allow storing headerless messages with current ROS time instead of timestamp rospy.loginfo(rospy.get_caller_id() + ": depth_image_topic - " + depth_image_topic) rospy.loginfo(rospy.get_caller_id() + ": color_image_topic - " + color_image_topic) rospy.loginfo(rospy.get_caller_id() + ": camera_info_topic - " + camera_info_topic) depth_sub = Subscriber(depth_image_topic, Image) rgb_sub = Subscriber(color_image_topic, Image) tss = ApproximateTimeSynchronizer([depth_sub, rgb_sub], cache_count, slop, allow_headerless) tss.registerCallback(cameraCallback) start_server = rospy.Service('start_recording', StartRecording, startRecordingCallback) stop_server = rospy.Service('stop_recording', StopRecording, stopRecordingCallback) rospy.spin()
def __init__(self, debug=False, init_ros_node=False): self.debug = debug self.rgb_raw = None self.depth_raw = None self.bridge = CvBridge() if init_ros_node: rospy.init_node('image_converter', anonymous=True) self.image_sub = Subscriber("/camera/rgb/image_rect_color", Image) self.depth_sub = Subscriber("/camera/depth_registered/image_raw", Image) self.tss = message_filters.ApproximateTimeSynchronizer([self.image_sub, self.depth_sub], queue_size=10, slop=0.5) self.tss.registerCallback(self.callback) time.sleep(0.5) if self.debug: print("Waiting for subscriber to return initial camera feed.") while self.depth_raw is None: time.sleep(0.1) if self.debug: print('Camera feed initialisation successful.')
def __init__(self): # variables self.node_name = rospy.get_name() self.odom = None self.scan = None self.vehicle_yaw = None # param self.cell_size = rospy.get_param('~cell_size', 0.5) # meter self.map_length = rospy.get_param("~map_length", 35) self.wait_time = rospy.get_param("~wait_time", 0.1) self.vehicle_size = rospy.get_param("~vehicle_size", 0.1) self.drift_x = rospy.get_param("~drift_x", 0.1) # case by odom and lidar position self.drift_y = rospy.get_param("~drift_y", 0.1) self.cell_length = int(self.map_length / self.cell_size * 2) self.frame_id = rospy.get_param("~frame_id", "odom") self.frame_period = rospy.get_param("~frame_period", 0.2) # Publisher self.pub_grid_map = rospy.Publisher("map", OccupancyGrid, queue_size=1) rospy.Timer(rospy.Duration(self.frame_period), self.create_local_map) # Subscriber sub_odometry = Subscriber("odom", Odometry) sub_laser_scan = Subscriber("scan", LaserScan) ats = ApproximateTimeSynchronizer((sub_odometry, sub_laser_scan), queue_size=1, slop=self.wait_time) ats.registerCallback(self.cb_sensor)
def init_node(self, ns="~"): self.use_slam_traj = rospy.get_param(ns + "use_slam_traj", True) self.x0, self.y0 = rospy.get_param(ns + "origin") self.width, self.height = rospy.get_param(ns + "size") self.resolution = rospy.get_param(ns + "resolution") self.inc = rospy.get_param(ns + "inc") self.pub_occupancy1 = rospy.get_param(ns + "pub_occupancy1") self.hit_prob = rospy.get_param(ns + "hit_prob") self.miss_prob = rospy.get_param(ns + "miss_prob") self.inflation_angle = rospy.get_param(ns + "inflation_angle") self.inflation_radius = rospy.get_param(ns + "inflation_range") self.pub_occupancy2 = rospy.get_param(ns + "pub_occupancy2") self.inflation_radius = rospy.get_param(ns + "inflation_radius") self.outlier_filter_radius = rospy.get_param(ns + "outlier_filter_radius") self.outlier_filter_min_points = rospy.get_param( ns + "outlier_filter_min_points") self.pub_intensity = rospy.get_param(ns + "pub_intensity") # Only update keyframe that has significant movement self.min_translation = rospy.get_param(ns + "min_translation") self.min_rotation = rospy.get_param(ns + "min_rotation") self.sonar_sub = Subscriber(SONAR_TOPIC, OculusPing) if self.use_slam_traj: self.traj_sub = Subscriber(SLAM_TRAJ_TOPIC, PointCloud2) else: self.traj_sub = Subscriber(LOCALIZATION_TRAJ_TOPIC, PointCloud2) # Method 1 if self.pub_occupancy1: self.feature_sub = Subscriber(SONAR_FEATURE_TOPIC, PointCloud2) # Method 2 if self.pub_occupancy2: self.feature_sub = Subscriber(SLAM_CLOUD_TOPIC, PointCloud2) # The time stamps for trajectory and ping have to be exactly the same # A big queue_size is required to assure no keyframe is missed especially # for offline playing. self.ts = TimeSynchronizer( [self.traj_sub, self.sonar_sub, self.feature_sub], 100) self.ts.registerCallback(self.tpf_callback) self.intensity_map_pub = rospy.Publisher(MAPPING_INTENSITY_TOPIC, OccupancyGrid, queue_size=1, latch=True) self.occupancy_map_pub = rospy.Publisher(MAPPING_OCCUPANCY_TOPIC, OccupancyGrid, queue_size=1, latch=True) self.get_map_srv = rospy.Service(ns + "get_map", GetOccupancyMap, self.get_map) self.configure() loginfo("Mapping node is initialized")
def __init__(self): # Init attributes print("start") self.pose = PoseStamped() self.pose.header.frame_id = "base_link" self.first_pose = PoseStamped() self.euler = None self.active = True self.start = True self.first = True # Init subscribers and publishers #tss = TimeSynchronizer(Subscriber("/imu/data", Imu),Subscriber("/fix", NavSatFix)) #tss.registerCallback(call_back) imu_sub = Subscriber("imu/data", Imu) gps_sub = Subscriber("/fix", NavSatFix) ats = ApproximateTimeSynchronizer((imu_sub, gps_sub), queue_size=1, slop=0.1) ats.registerCallback(self.call_back) #self.sub_imu = rospy.Subscriber("/imu/data", Imu, self.imu_cb, queue_size=1) #self.sub_gps = rospy.Subscriber("/gps/fix", NavSatFix, self.gps_cb, queue_size=1) self.pub_gazebo = rospy.Publisher('/david/cmd_vel', Twist, queue_size=1) self.pub_pose = rospy.Publisher('/pose', PoseStamped, queue_size=1)
def __init__(self): # Subscribers # self.marker_array_subscribe = rospy.Subscriber("/apriltags_kinect2/detections", AprilTagDetections, self.detection_callback) # self.rgb_image_subscribe = rospy.Subscriber("/head/kinect2/qhd/image_color_rect", Image, self.rgb_callback) # self.depth_image_subscribe = rospy.Subscriber("/head/kinect2/qhd/image_depth_rect", Image, self.depth_callback) self.camera_info = rospy.Subscriber("/head/kinect2/qhd/camera_info", CameraInfo, self.camera_callback) tss = ApproximateTimeSynchronizer([ Subscriber("/head/kinect2/qhd/image_color_rect", Image), Subscriber("/head/kinect2/qhd/image_depth_rect", Image), Subscriber("/apriltags_kinect2/detections", AprilTagDetections) ], 1, 0.5) tss.registerCallback(self.processtag_callback) # Vars self.camera_intrinsics = None self.rgb_image = None self.depth_image = None self.mark_array = None # Converters self.bridge = CvBridge() # Publisher self.marker_publish = rospy.Publisher( "/apriltags_kinect2/marker_array_fused", MarkerArray) self.detection_publish = rospy.Publisher( "/apriltags_kinect2/detections_fused", AprilTagDetections)
def __init__(self, tempSup): # Pub self.image_pub_scaled = rospy.Publisher("/dados_sync/image_scaled", Image, queue_size=10) self.image_pub_8bit = rospy.Publisher("/dados_sync/image_8bits", Image, queue_size=10) self.pub_pc = rospy.Publisher("/dados_sync/point_cloud", PointCloud2, queue_size=10) self.pub_odom = rospy.Publisher("/dados_sync/odometry", Odometry, queue_size=10) # Sub self.thermal_sub = Subscriber("/termica/thermal/image_raw", Image) self.pc_sub = Subscriber("/stereo/points2", PointCloud2) self.odom_sub = Subscriber("/stereo_odometer/odometry", Odometry) self.ats = ApproximateTimeSynchronizer( [self.thermal_sub, self.pc_sub, self.odom_sub], queue_size=5, slop=0.5) self.ats.registerCallback(self.tcallback) self.bridge = CvBridge() self.tempFundoDeEscala = tempSup
def __init__(self, name): rospy.loginfo("Starting %s..." % name) self.vis_pub = rospy.Publisher("~visualization_marker", Marker, queue_size=1) self.tf = TransformListener() self.cc = CostmapCreator( rospy.Publisher("/velocity_costmap", OccupancyGrid, queue_size=10, latch=True), rospy.Publisher("~origin", PoseStamped, queue_size=10)) self.dyn_srv = DynServer(VelocityCostmapsConfig, self.dyn_callback) subs = [ Subscriber( rospy.get_param("~qtc_topic", "/qtc_state_predictor/prediction_array"), QTCPredictionArray), Subscriber( rospy.get_param("~ppl_topic", "/people_tracker/positions"), PeopleTracker) ] ts = TimeSynchronizer(fs=subs, queue_size=60) ts.registerCallback(self.callback) rospy.loginfo("... all done.")
def main(args): rospy.init_node('bag_to_yaprofi') global outdir, br, seq outdir = args.outdir br = cv_bridge.CvBridge() seq = args.seq os.makedirs(os.path.join(outdir, 'rgb_left')) os.makedirs(os.path.join(outdir, 'rgb_right')) os.makedirs(os.path.join(outdir, 'depth')) with open(os.path.join(outdir, 'rgb_left.txt'), 'w') as f: f.write('#\n' * 3) with open(os.path.join(outdir, 'rgb_right.txt'), 'w') as f: f.write('#\n' * 3) with open(os.path.join(outdir, 'depth.txt'), 'w') as f: f.write('#\n' * 3) with open(os.path.join(outdir, 'gt_tum.txt'), 'w') as f: pass with open(os.path.join(outdir, 'gt_kitti.txt'), 'w') as f: pass sub_image_left = Subscriber('/zed_node/left/image_rect_color/compressed', CompressedImage) sub_image_right = Subscriber('/zed_node/right/image_rect_color/compressed', CompressedImage) sub_depth = Subscriber('/zed_node/depth/depth_registered', Image) sub_odom = Subscriber('/groundtruth', Odometry) ats = ApproximateTimeSynchronizer([sub_image_left, sub_image_right, sub_depth, sub_odom], queue_size=5, slop=0.05) ats.registerCallback(callback) rospy.spin()
def main(): global angle global speed global stop, y_max angle = 0.0 speed = 0.0 rospy.init_node('listener', anonymous=True) robo_angle_pub = rospy.Publisher('robo_angle', Float32, queue_size=10) robo_speed_pub = rospy.Publisher('robo_speed', Float32, queue_size=10) laser_sub = Subscriber("scan", sensor_msgs.msg.LaserScan) camera_sub = Subscriber("/darknet_ros/bounding_boxes", BoundingBoxes) ats = ApproximateTimeSynchronizer([laser_sub, camera_sub], queue_size=5, slop=0.1, allow_headerless=True) ats.registerCallback(avoid) rospy.spin() rate = rospy.Rate(1) # 10hz while not rospy.is_shutdown(): msg_angle = angle msg_speed = speed print( " s =", speed, "a =", angle, stop, ) robo_angle_pub.publish(msg_angle) robo_speed_pub.publish(msg_speed) rate.sleep()
def __init__(self): print "Entered Initialize - range measure multiple" self.sigma_rho = rospy.get_param("~sigma_rho") self.rng_pub10 = rospy.Publisher('/ekf_bot/range_meas10', sensor_rho_pairwise, queue_size=1) self.rng_pub20 = rospy.Publisher('/ekf_bot/range_meas20', sensor_rho_pairwise, queue_size=1) self.rng_pub12 = rospy.Publisher('/ekf_bot/range_meas12', sensor_rho_pairwise, queue_size=1) robot0_pose_sub = Subscriber("/ekf_bot/inertial_pose0", inert_pose) robot1_pose_sub = Subscriber("/ekf_bot/inertial_pose1", inert_pose) robot2_pose_sub = Subscriber("/ekf_bot/inertial_pose2", inert_pose) self.rng10 = sensor_rho_pairwise() self.rng20 = sensor_rho_pairwise() self.rng12 = sensor_rho_pairwise() ats = ApproximateTimeSynchronizer( [robot0_pose_sub, robot1_pose_sub, robot2_pose_sub], queue_size=1000, slop=0.1, allow_headerless=False) ats.registerCallback(self.rng_pairwise) self.node_no = 1
def execute(self, userdata): # If prempted before even getting a chance, give up. if self.preempt_requested(): self.service_preempt() return 'aborted' self._trigger_event.clear() self._height_sub = Subscriber('height', Range) self._image_sub = Subscriber('image', Image) sync = ApproximateTimeSynchronizer([self._height_sub, self._image_sub], queue_size=1, slop=0.05) finishdata = [] self._finished = False sync.registerCallback(self.callback, userdata, finishdata) # Wait until a candidate has been found. self._trigger_event.wait() del sync if self.preempt_requested(): self.service_preempt() return 'aborted' assert (len(finishdata) == 1) return finishdata[0]
def main6(): imgSub = Subscriber('camera/color/image_raw', Image) depthSub = Subscriber('camera/aligned_depth_to_color/image_raw', Image) ats = ApproximateTimeSynchronizer([imgSub, depthSub], queue_size=1, slop=0.2) ats.registerCallback(callImgDepth)
def main5(): poseSub = Subscriber('raw_odom', Odometry) lblSub = Subscriber('semantic_label', SemLabel) ats = ApproximateTimeSynchronizer([poseSub, lblSub], queue_size=1, slop=0.2) ats.registerCallback(callPoseLbl)
def main3(): poseSub = Subscriber('RosAria/pose', Odometry) imgSub = Subscriber('camera/color/image_raw', Image) ats = ApproximateTimeSynchronizer([poseSub, imgSub], queue_size=1, slop=0.2) ats.registerCallback(callPoseImg)
def __init__(self): #self.aflsync_node = AflSync() sub_gps = Subscriber("/gps/fix", NavSatFix) sub_rpy = Subscriber("/imu/data", TatBry) ts = message_filters.ApproximateTimeSynchronizer([sub_gps, sub_rpy], 30, 0.24 ) ts.registerCallback(self.cb_synco)
def execute(self): self.__height_sub = Subscriber('height', Range) self.__image_sub = Subscriber('image', Image) self.__sync = ApproximateTimeSynchronizer( [self.__height_sub, self.__image_sub], queue_size=1, slop=0.05) self.__sync.registerCallback(self.__callback) rospy.spin()
def __init__(self): self.bridge = CvBridge() self.image_sub = Subscriber("/camera/rgb/image_rect_color",Image) self.depth_sub = Subscriber("/camera/depth_registered/image_raw", Image) self.image_pub = rospy.Publisher("mouthxyz", Point, queue_size=10) tss = message_filters.ApproximateTimeSynchronizer([self.image_sub, self.depth_sub],queue_size=10, slop=0.5) tss.registerCallback(self.callback)
def __init__(self): rospy.init_node(NAME, anonymous=True) # Image detection parameters self.conf_thresh = 0.6 self.hier_thresh = 0.8 self.frame_height = 416 self.frame_width = 416 # Set up dynamic reconfigure self.srv = Server(DetectorConfig, self.reconf_cb) # Set up OpenCV Bridge self.bridge = CvBridge() # Load network rp = rospkg.RosPack() # Load object labels classfile = os.path.join(rp.get_path(NAME), rospy.get_param('~label_file_path')) self.classes = None with open(classfile, 'rt') as f: self.classes = f.read().rstrip('\n').split('\n') # Load network cfgfile = os.path.join(rp.get_path(NAME), rospy.get_param('~cfg_file_path')) weightsfile = os.path.join(rp.get_path(NAME), rospy.get_param('~weights_file_path')) self.net = cv.dnn.readNet(cfgfile, weightsfile) self.net.setPreferableTarget(cv.dnn.DNN_TARGET_OPENCL) # Set up detection image publisher det_topic = rospy.get_param('~detection_image_topic_name') self.det_pub = rospy.Publisher(det_topic, Image, queue_size=1, latch=True) # Set up prediction publisher pred_topic = rospy.get_param('~predictions_topic_name') self.pred_pub = rospy.Publisher(pred_topic, Predictions, queue_size=1, latch=True) # The first subscriber retrieves image data from the left image and passes it to the callback function image_topic = rospy.get_param('~camera_topic_name') image_sub = Subscriber(image_topic, Image, queue_size=1) # The second subscriber retrieves depth data from the camera as 32 bit floats with values in meters and maps this onto an image structure, which is passed to callback2 if rospy.has_param('~depth_topic_name'): depth_topic = rospy.get_param('~depth_topic_name') depth_sub = Subscriber(depth_topic, Image, queue_size=1) else: depth_sub = None # We want to receive both images in the same callback if depth_sub: rospy.loginfo("with depth sub") ts = ApproximateTimeSynchronizer([image_sub, depth_sub], queue_size=1, slop=0.1) else: rospy.loginfo("without depth sub") ts = ApproximateTimeSynchronizer([image_sub], queue_size=1, slop=0.1) ts.registerCallback(self.image_cb)
def __init__(self): # rospy.init_node('tl_detector') rospy.init_node('tl_detector', log_level=rospy.DEBUG) # Load configuration config_string = rospy.get_param('/traffic_light_config') self.config = yaml.load(config_string) self.is_site = self.config['is_site'] self.lookahead_waypoints = self.config['lookahead_waypoints'] self.current_pose = None self.waypoints = None self.waypoints_tree = None self.ready = False self.stop_line_positions_idx = [] self.camera_image = None self.lights = [] self.light_state = TrafficLight.UNKNOWN self.light_waypoint_idx = -1 self.light_state_count = 0 self.bridge = CvBridge() self.light_classifier = None self.base_waypoints_sub = rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb) # Synced Subscribers synced_subscribers = [Subscriber('/current_pose', PoseStamped)] # Check if we force the usage of the simulator light state, not available when on site if self.config['use_light_state'] and not self.is_site: rospy.logwarn('Classifier disabled, using simulator light state') # Note that we do not subscribe to the camera image else: # Setup the classifier self.light_classifier = TLClassifier(self.config) # When the classifier is enabled then the camera image needs to be in sync with the current_pose synced_subscribers.append(Subscriber('/image_color', Image)) # TODO Find out, this should greatly improve performance: # Subscriber('/image_color', Image, queue_size=1, buff_size=???) # buff_size should be greater than queue_size * avg_msg_byte_size self.traffic_lights_sub = rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray, self.traffic_lights_cb) self.synced_sub = ApproximateTimeSynchronizer(synced_subscribers, SYNC_QUEUE_SIZE, 0.1) self.synced_sub.registerCallback(self.synced_data_cb) # Publisher self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint', Int32, queue_size=1) rospy.spin()
def main7(): poseSub = Subscriber('rtabmap/odom', Odometry) imgSub = Subscriber('camera/color/image_raw', Image) depthSub = Subscriber('camera/aligned_depth_to_color/image_raw', Image) ats = ApproximateTimeSynchronizer([poseSub, imgSub, depthSub], queue_size=1, slop=0.2) ats.registerCallback(callPoseSE3ImgDepth)
def execute(self): self.__height_sub = Subscriber('height', Range) self.__image_sub = Subscriber('image', Image) self.__sync = ApproximateTimeSynchronizer([self.__height_sub, self.__image_sub], queue_size=1, slop=0.05) self.__sync.registerCallback(self.__callback) rospy.spin()
class RosStateMachine(StateMachine): def __init__(self, states, transitions, start, outcomes, camera=None, optical_flow=None): StateMachine.__init__(self, states, transitions, start, outcomes) self.__last_output = Userdata() self.__bridge = CvBridge() # Publishers self.__delta_pub = tf.TransformBroadcaster() self.__debug_pub = rospy.Publisher('/debug_image', Image, queue_size=1) #self.__pid_input_pub = rospy.Publisher('/pid_input', controller_msg, # queue_size=1) self.__state_pub = rospy.Publisher('/statemachine', String, queue_size=1) if camera is None: self.__camera = Camera.from_ros() else: self.__camera = camera #if optical_flow is None: # self.__optical_flow = OpticalFlow.from_ros() #else: # self.__optical_flow = optical_flow self.__config_max_height = rospy.get_param('config/max_height') def execute(self): self.__height_sub = Subscriber('height', Range) self.__image_sub = Subscriber('image', Image) self.__sync = ApproximateTimeSynchronizer([self.__height_sub, self.__image_sub], queue_size=1, slop=0.05) self.__sync.registerCallback(self.__callback) rospy.spin() def __callback(self, range_msg, image_msg): """ Receives ROS messages and advances the state machine. """ self.__state_pub.publish(self.current_state) if self.current_state == self.FINAL_STATE: rospy.loginfo('Final state reached.') rospy.signal_shutdown('Final state reached.') self.__height_sub.unregister() self.__image_sub.unregister() del self.__sync return u = self.create_userdata(range_msg=range_msg, image_msg=image_msg) self.__last_output = self(u) def publish_debug(self, userdata, image_callback, *args, **kwargs): if self.__debug_pub.get_num_connections() <= 0: return image = image_callback(userdata, *args, **kwargs) if image is None: return img_msg = self.__bridge.cv2_to_imgmsg(image, encoding='bgr8') self.__debug_pub.publish(img_msg) def publish_delta(self, x, y, z, theta): if np.isnan(x): # TODO: Fix this rospy.logerr('Publish delta : Got bad x: {0}'.format(x)) #print 'BAD X?', x, repr(x) #traceback.print_stack() return x, y = -x, -y rospy.loginfo('Publish delta : x {0}, y {1}, z {2}, theta {3}' .format(x, y, z, theta)) q = tf.transformations.quaternion_from_euler(0, 0, theta) # TODO: camera frame (down/forward) camera_frame = 'downward_cam_optical_frame' self.__delta_pub.sendTransform((x, y, z), q, rospy.Time.now(), 'waypoint', camera_frame) # TODO: remove legacy topic publish msg = controller_msg() msg.x = -y msg.y = x msg.z = z msg.t = theta #self.__pid_input_pub.publish(msg) def publish_delta__keep_height(self, userdata, x, y, theta, target_height): delta_z = target_height - userdata.height_msg.range self.publish_delta(x, y, delta_z, theta) def create_userdata(self, **kwargs): u = Userdata(self.__last_output) u.publish_debug_image = lambda cb, *args, **kwargs: self.publish_debug(u.image, cb, *args, **kwargs) u.publish_delta = self.publish_delta u.publish_delta__keep_height = lambda x, y, theta, target_height: self.publish_delta__keep_height(u, x, y, theta, target_height) u.height_msg = kwargs['range_msg'] u.max_height = self.__config_max_height u.camera = self.__camera #u.optical_flow = self.__optical_flow try: u.image = self.__bridge.imgmsg_to_cv2(kwargs['image_msg'], desired_encoding='bgr8') except CvBridgeError as error: rospy.logerror(error) return None return u