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
