def __init__(self):
# print "Entered Initialize - true relative pose"
self.true_pose_in0 = rospy.Publisher('/ekf_bot/inertial_pose0',
inert_pose,
queue_size=1)
self.true_pose_in1 = rospy.Publisher('/ekf_bot/inertial_pose1',
inert_pose,
queue_size=1)
self.true_pose_in2 = rospy.Publisher('/ekf_bot/inertial_pose2',
inert_pose,
queue_size=1)
robot0_pose_sub = Subscriber("/robot0/odom_truth", Odometry)
robot1_pose_sub = Subscriber("/robot1/odom_truth", Odometry)
robot2_pose_sub = Subscriber("/robot2/odom_truth", Odometry)
imu0 = Subscriber('/robot0/mobile_base/sensors/imu_data', Imu)
imu1 = Subscriber('/robot1/mobile_base/sensors/imu_data', Imu)
imu2 = Subscriber('/robot2/mobile_base/sensors/imu_data', Imu)
self.inert_pose0 = inert_pose()
self.inert_pose1 = inert_pose()
self.inert_pose2 = inert_pose()
ats = ApproximateTimeSynchronizer([
robot0_pose_sub, robot1_pose_sub, robot2_pose_sub, imu0, imu1, imu2
],
queue_size=1,
slop=0.1,
allow_headerless=False)
ats.registerCallback(self.true_inert_publish)
self.node_no = 1
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__(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
class image_converter:
def __init__(self):
self.bridge = CvBridge()
self.tss = ApproximateTimeSynchronizer([Subscriber("/fusion/front_camera/image_raw",Image),Subscriber('/fusion/steering_report',SteeringReport),Subscriber('/fusion/throttle_report',ThrottleReport)],10, 0.2, allow_headerless=False)
self.tss.registerCallback(self.callback_img)
def callback_img(self,Image,SteeringReport,ThrottleReport):
i=0
try:
screen = self.bridge.imgmsg_to_cv2(Image, "bgr8")
screen = cv2.cvtColor(screen, cv2.COLOR_BGR2GRAY)
screen = cv2.resize(screen, (150,200))
steering=SteeringReport.steering_wheel_angle_cmd
throttle=ThrottleReport.pedal_cmd
except CvBridgeError as e:
print(e)
last_time = time.time()
output=[steering,throttle]
training_data.append([screen,output])
print len(training_data),steering,throttle
if len(training_data) % 100 == 0:
print(len(training_data))
np.save(file_name,training_data)
print 'saved'
def __init__(self):
cv2.namedWindow("Live Image", 1)
cv2.namedWindow("Base Image", 1)
cv2.namedWindow("Original Image", 1)
cv2.startWindowThread()
self.bridge = CvBridge()
image_sub = Subscriber(
"/camera/rgb/image_color",
Image,
queue_size=1
)
person_sub = Subscriber(
"/upper_body_detector/detections",
UpperBodyDetector,
queue_size=1
)
depth_sub = Subscriber(
"/camera/depth/image_rect",
Image,
queue_size=1
)
# detections_image_sub = Subscriber(
# "/upper_body_detector/image",
# Image,
# queue_size=1
# )
ts = ApproximateTimeSynchronizer([image_sub, person_sub, depth_sub], 1, 0.1)
ts.registerCallback(self.image_callback)
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 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 listener():
# Initialization
rospy.init_node('motor_node')
base_sub = Subscriber("base_motors", JointState)
ats = ApproximateTimeSynchronizer([base_sub], queue_size=2, slop=0.1)
ats.registerCallback(send_to_motors)
rospy.spin()
def __init__(self):
self.node_name = rospy.get_name()
self.pose = Pose()
self.prior_pose = Pose()
self.prior_roll = 0
self.prior_pitch = 0
self.prior_yaw = 0
self.start = False
self.covariance = np.zeros((36,), dtype=float)
self.odometry = Odometry()
self.br = tf.TransformBroadcaster()
# param
self.imu_offset = 0
self.lat_orig = rospy.get_param('~latitude', 0.0)
self.long_orig = rospy.get_param('~longitude', 0.0)
self.utm_orig = fromLatLong(self.lat_orig, self.long_orig)
# Service
self.srv_imu_offset = rospy.Service('~imu_offset', SetValue, self.cb_srv_imu_offest)
# Publisher
self.pub_odm = rospy.Publisher("~odometry", Odometry, queue_size=1)
# Subscriber
sub_imu = message_filters.Subscriber("~imu/data", Imu)
sub_gps = message_filters.Subscriber("~fix", NavSatFix)
ats = ApproximateTimeSynchronizer((sub_imu, sub_gps), queue_size = 1, slop = 0.1)
ats.registerCallback(self.cb_gps_imu)
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]
class image_converter:
def __init__(self):
self.image_pub = rospy.Publisher("image_topic_2", Image, queue_size=10)
self.bridge = CvBridge()
image_sub = Subscriber("/front_camera/image_raw", Image)
detect_sub = Subscriber("/detectnet/detections", Detection2DArray)
self.ts = ApproximateTimeSynchronizer([image_sub, detect_sub],
queue_size=10,
slop=0.5)
self.ts.registerCallback(self.callback)
def callback(self, data, detection):
rospy.loginfo(len(detection.detections))
if len(detection.detections) >= 1:
rospy.loginfo(detection.detections[0].bbox.center.x)
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
(rows, cols, channels) = cv_image.shape
if cols > 60 and rows > 60:
cv2.rectangle(cv_image, (50, 50), (65, 65), (255, 0, 0), 2)
cv_image = print_rectangle(self, data, detection, cv_image)
#la partie a custom
#cv2.circle(cv_image, (50,50), 10, 255)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
except CvBridgeError as e:
print(e)
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 __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 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 run(self):
sync = ApproximateTimeSynchronizer([
Subscriber(self.map1, OccupancyGrid),
Subscriber(self.map2, OccupancyGrid)
], 10, 1)
sync.registerCallback(self.sync_callback)
rospy.spin()
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 __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):
# 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__(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)
class Decision_Manager:
def __init__(self, safety_topic, drive_topic, drive_pub):
# Initialize subscribers
self.scan_subscriber = Subscriber(safety_topic,
StampedBool,
queue_size=100)
self.odom_subscriber = Subscriber(drive_topic,
AckermannDriveStamped,
queue_size=100)
self.drive_publisher = rospy.Publisher(drive_pub,
AckermannDriveStamped,
queue_size=10)
#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)
def master_callback(self, bool_topic, drive_topic):
msg = drive_topic
if (bool(bool_topic.data)):
msg.drive.speed = 0
self.drive_publisher.publish(msg)
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()
class RedDepthNode(object):
""" This node reads from the Kinect color stream and
publishes an image topic with the most red pixel marked.
Subscribes:
/camera/rgb/image_color
Publishes:
red_marked_image
"""
def __init__(self):
""" Construct the red-pixel finder node."""
rospy.init_node('red_depth_node')
self.image_pub = rospy.Publisher('red_marked_image',
Image,
queue_size=10)
self.marker_pub = rospy.Publisher('red_marker', Marker, queue_size=10)
self.cv_bridge = CvBridge()
# Unfortunately the depth data and image data from the kinect aren't
# perfectly time-synchronized. The code below handles this issue.
img_sub = message_filters.Subscriber('/camera/rgb/image_color', Image)
cloud_sub = message_filters.Subscriber(\
'/camera/depth_registered/points',
PointCloud2)
self.kinect_synch = ApproximateTimeSynchronizer([img_sub, cloud_sub],
queue_size=10,
slop=.02)
self.kinect_synch.registerCallback(self.image_points_callback)
rospy.spin()
def image_points_callback(self, img, cloud):
""" Handle image/point_cloud callbacks. """
# Convert the image message to a cv image object
cv_img = self.cv_bridge.imgmsg_to_cv2(img, "bgr8")
# Do the image processing
red_pos = find_reddest_pixel_fast(cv_img)
cv2.circle(cv_img, red_pos, 5, (0, 255, 0), -1)
# Extract just the point we want from the point cloud message
# as an iterable of (x,y,z) tuples
points = point_cloud2.read_points(cloud,
skip_nans=False,
uvs=[red_pos])
# Iterate through the returned points (really just one)
for p in points:
if not np.isnan(p[0]):
# Publish a marker at the point.
marker = make_sphere_marker(p[0], p[1], p[2], .05,
cloud.header.frame_id,
cloud.header.stamp)
self.marker_pub.publish(marker)
# Convert the modified image back to a message.
img_msg_out = self.cv_bridge.cv2_to_imgmsg(cv_img, "bgr8")
self.image_pub.publish(img_msg_out)
class Safety(object):
"""
The class that handles emergency braking.
"""
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 master_callback(self, scan_msg, odom_msg):
degrees_to_radians = np.pi / 180
linear_velocity = odom_msg.twist.twist.linear.x
angles_radian = (((np.arange(0, 1080) - 539) / 4)) * degrees_to_radians
projections = np.cos(angles_radian) * linear_velocity
projections = np.maximum(projections, 0.00000001)
# the velocity we get from odom is in the x direction
ranges = scan_msg.ranges
# compute the time to collision
ttc = ranges / projections
# minimum ttc
minimum_ttc = min(ttc)
msg = StampedBool()
msg.header.stamp = rospy.Time.now()
if minimum_ttc < self.THRESHOLD:
msg.data = True
rospy.loginfo("Engage AEB")
else:
msg.data = False
self.brake_bool.publish(msg)
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 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 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 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):
# 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 test_approx(self):
m0 = MockFilter()
m1 = MockFilter()
ts = ApproximateTimeSynchronizer([m0, m1], 1, 0.1)
ts.registerCallback(self.cb_collector_2msg)
if 0:
# Simple case, pairs of messages, make sure that they get combined
for t in range(10):
self.collector = []
msg0 = MockMessage(t, 33)
msg1 = MockMessage(t, 34)
m0.signalMessage(msg0)
self.assertEqual(self.collector, [])
m1.signalMessage(msg1)
self.assertEqual(self.collector, [(msg0, msg1)])
# Scramble sequences of length N. Make sure that TimeSequencer recombines them.
random.seed(0)
for N in range(1, 10):
m0 = MockFilter()
m1 = MockFilter()
seq0 = [MockMessage(rospy.Time(t), random.random()) for t in range(N)]
seq1 = [MockMessage(rospy.Time(t), random.random()) for t in range(N)]
# random.shuffle(seq0)
ts = ApproximateTimeSynchronizer([m0, m1], N, 0.1)
ts.registerCallback(self.cb_collector_2msg)
self.collector = []
for msg in random.sample(seq0, N):
m0.signalMessage(msg)
self.assertEqual(self.collector, [])
for msg in random.sample(seq1, N):
m1.signalMessage(msg)
self.assertEqual(set(self.collector), set(zip(seq0, seq1)))
# Scramble sequences of length N of headerless and header-having messages.
# Make sure that TimeSequencer recombines them.
rospy.rostime.set_rostime_initialized(True)
random.seed(0)
for N in range(1, 10):
m0 = MockFilter()
m1 = MockFilter()
seq0 = [MockMessage(rospy.Time(t), random.random()) for t in range(N)]
seq1 = [MockHeaderlessMessage(random.random()) for t in range(N)]
# random.shuffle(seq0)
ts = ApproximateTimeSynchronizer([m0, m1], N, 0.1, allow_headerless=True)
ts.registerCallback(self.cb_collector_2msg)
self.collector = []
for msg in random.sample(seq0, N):
m0.signalMessage(msg)
self.assertEqual(self.collector, [])
for i in random.sample(range(N), N):
msg = seq1[i]
rospy.rostime._set_rostime(rospy.Time(i+0.05))
m1.signalMessage(msg)
self.assertEqual(set(self.collector), set(zip(seq0, seq1)))
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 main(args):
rospy.init_node('take_depthRGB_pics', anonymous=True)
#tss = ApproximateTimeSynchronizer([Subscriber("/camera/depth/image", Image),
# Subscriber("/camera/rgb/image_rect_color", Image)], queue_size=1, slop=0.1)
#tss.registerCallback(callback)
global stream_on
while (not rospy.is_shutdown()):
get_input = 'x'
get_input = raw_input("Press 's' to start stream, 'p' to pause: \n")
tss = ApproximateTimeSynchronizer([Subscriber("/camera/depth_registered/image_raw", Image),
Subscriber("/camera/rgb/image_raw", Image)], queue_size=1, slop=0.1)
if str(get_input) == 's':
stream_on = 1
tss.registerCallback(callback)
if str(get_input) == 'p':
stream_on = 0
tss.registerCallback(callback)
def __init__(self):
# Create new windows
cv2.namedWindow("Live Image", 1)
cv2.namedWindow("Base Image", 1)
cv2.namedWindow("Original Image", 1)
cv2.startWindowThread()
self.bridge = CvBridge()
# Commented code below used for testing only
# self.match_pub = rospy.Publisher("~match_image_out", Image, queue_size=1)
# self.base_pub = rospy.Publisher("~base_image_out", Image, queue_size=1)
# self.match_string = rospy.Publisher("~match_bool", String, queue_size=1)
# Subscribe to the required topics. Using /camera/ instead of /head_xiton/ when
# not running on Linda
image_sub = Subscriber(
"/head_xtion/rgb/image_color",
Image,
queue_size=1
)
person_sub = Subscriber(
"/upper_body_detector/detections",
UpperBodyDetector,
queue_size=1
)
depth_sub = Subscriber(
"/head_xtion/depth/image_rect",
Image,
queue_size=1
)
# Time syncronizer is implimented to make sure that all of the frames match up from all of the topics.
ts = ApproximateTimeSynchronizer([image_sub, person_sub, depth_sub], 1, 0.1)
ts.registerCallback(self.image_callback)
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
