def __init__(self):
self.status_publisher = rospy.Publisher('/pr2_block_builder/status',
Status,
latch=True)
self.block_status_publisher = rospy.Publisher(
'/pr2_block_builder/completion', BlockStatus)
self.picker = Picker()
self.placer = Placer()
rospy.sleep(1)
self.sendStatus("ready")
def __init__(self):
self.status_publisher = rospy.Publisher('/pr2_block_builder/status', Status, latch=True)
self.block_status_publisher = rospy.Publisher('/pr2_block_builder/completion', BlockStatus)
self.picker = Picker()
self.placer = Placer()
rospy.sleep(1)
self.sendStatus("ready")
class Builder():
ready = True
picker = None
placer = None
status_publisher = None
block_status_publisher = None
def __init__(self):
self.status_publisher = rospy.Publisher('/pr2_block_builder/status',
Status,
latch=True)
self.block_status_publisher = rospy.Publisher(
'/pr2_block_builder/completion', BlockStatus)
self.picker = Picker()
self.placer = Placer()
rospy.sleep(1)
self.sendStatus("ready")
def start(self, blocks):
if (self.ready):
# Prevent further invocations
self.ready = False
# Send the start status
self.sendStatus("started")
# Sort the blocks
self.sortBlocks(blocks, 0)
# Do construction
status = self.construct(blocks)
# Send the completion status
self.sendStatus(status)
def stop(self):
self.ready = False
if self.picker != None:
self.picker.cancel()
if self.placer != None:
self.placer.cancel()
def restart(self):
self.stop()
self.ready = True
self.sendStatus("ready")
def construct(self, blocks):
for i in range(0, len(blocks)):
block = blocks[i]
# Send the status
self.sendBlockStatus(block, "started")
# Pick
status = self.picker.pick(i)
# Place
if status == "completed":
status = self.placer.place(block)
# Send the status
self.sendBlockStatus(block, status)
# Stop if we've failed or aborted
if status != "completed":
return status
return "completed"
def sendStatus(self, status, message=""):
rospy.loginfo("Status: %s", status)
statusObject = Status()
statusObject.status.data = status
statusObject.message.data = message
self.status_publisher.publish(statusObject)
def sendBlockStatus(self, block, status, message=""):
rospy.loginfo("Block (%s, %s, %s) %s", block.position.x,
block.position.y, block.position.z, status)
statusObject = BlockStatus()
statusObject.block = block
statusObject.status.data = status
statusObject.message.data = message
self.block_status_publisher.publish(statusObject)
def sortBlocks(self, blocks, start):
nextBlock = blocks[start]
nextBlockIndex = start
for i in range(start, len(blocks)):
if (blocks[i].position.z < nextBlock.position.z):
nextBlock = blocks[i]
nextBlockIndex = i
elif (blocks[i].position.z == nextBlock.position.z
and blocks[i].position.x > nextBlock.position.x):
nextBlock = blocks[i]
nextBlockIndex = i
elif (blocks[i].position.z == nextBlock.position.z
and blocks[i].position.x == nextBlock.position.x
and blocks[i].position.y > nextBlock.position.y):
nextBlock = blocks[i]
nextBlockIndex = i
blocks[nextBlockIndex] = blocks[start]
blocks[start] = nextBlock
if (start == len(blocks) - 1):
return blocks
return self.sortBlocks(blocks, start + 1)
def pp(x, y, z, num):
global picker
global p
pose = Pose()
pose.position.x = x
pose.position.y = y
pose.position.z = z
picker.pick(num)
rospy.loginfo(p.place(pose))
if __name__ == '__main__':
rospy.init_node('placer_test')
global picker
global p
picker = Picker()
p = Placer()
pp(0, 0, 0, 0)
pp(0, 1, 0, 1)
pp(0, 2, 0, 2)
pp(1, 2, 0, 3)
pp(2, 2, 0, 4)
pp(0, 1, 0, 5)
pp(0, 2, 0, 6)
pp(0, 0, 1, 7)
class Builder():
ready = True
picker = None
placer = None
status_publisher = None
block_status_publisher = None
def __init__(self):
self.status_publisher = rospy.Publisher('/pr2_block_builder/status', Status, latch=True)
self.block_status_publisher = rospy.Publisher('/pr2_block_builder/completion', BlockStatus)
self.picker = Picker()
self.placer = Placer()
rospy.sleep(1)
self.sendStatus("ready")
def start(self, blocks):
if (self.ready):
# Prevent further invocations
self.ready = False
# Send the start status
self.sendStatus("started")
# Sort the blocks
self.sortBlocks(blocks, 0)
# Do construction
status = self.construct(blocks)
# Send the completion status
self.sendStatus(status)
def stop(self):
self.ready = False
if self.picker!=None:
self.picker.cancel()
if self.placer!=None:
self.placer.cancel()
def restart(self):
self.stop()
self.ready = True
self.sendStatus("ready")
def construct(self, blocks):
for i in range(0, len(blocks)):
block = blocks[i]
# Send the status
self.sendBlockStatus(block, "started")
# Pick
status = self.picker.pick(i)
# Place
if status=="completed":
status = self.placer.place(block, false)
# Send the status
self.sendBlockStatus(block, status);
# Stop if we've failed or aborted
if status!="completed":
return status
return "completed"
def sendStatus(self, status, message=""):
rospy.loginfo("Status: %s", status)
statusObject = Status()
statusObject.status.data = status
statusObject.message.data = message
self.status_publisher.publish(statusObject)
def sendBlockStatus(self, block, status, message=""):
rospy.loginfo("Block (%s, %s, %s) %s", block.position.x, block.position.y, block.position.z, status)
statusObject = BlockStatus()
statusObject.block = block
statusObject.status.data = status
statusObject.message.data = message
self.block_status_publisher.publish(statusObject)
def sortBlocks(self, blocks, start):
dist_block = blocks[i].position.x * blocks[i].position.x + blocks[i].position.y * blocks[i].position.y
nextBlock = blocks[start]
nextBlockIndex = start
dist_nextBlock = dist_block
for i in range(start, len(blocks)):
dist_block = blocks[i].position.x * blocks[i].position.x + blocks[i].position.y * blocks[i].position.y
if (blocks[i].position.z < nextBlock.position.z):
nextBlock = blocks[i]
nextBlockIndex = i
dist_nextBlock = dist_block
elif (blocks[i].position.z == nextBlock.position.z and dist_block < dist_nextBlock):
nextBlock = blocks[i]
nextBlockIndex = i
dist_nextBlock = dist_block
elif (blocks[i].position.z == nextBlock.position.z and dist_block == dist_nextBlock and blocks[i].position.x < nextBlock.position.x):
nextBlock = blocks[i]
nextBlockIndex = i
dist_nextBlock = dist_block
blocks[nextBlockIndex] = blocks[start]
blocks[start] = nextBlock
if (start == len(blocks)-1):
return blocks
return self.sortBlocks(blocks, start + 1)
def process(self, synth=False, c1=0.01, c2=0.5, window=False):
"""
Function to bandpass filter and trim the components
Seismograms are trimmed so that they start 1 minute before the expected arrival and end 2 minutes after the arrival
c1 - [Hz] Lower corner frequency
c2 - [Hz] Upper corner frequency (N.B I have used c2 = 00.5 intially and am now trying c2 = 0.3 to see if that improves SNR without cutting oiut too mcuh singal)
By default traces will be filtered between 0.01Hz-0.5Hz
"""
# print(synth)
# De-mean and detrend each component
self.BHN.detrend(type='demean') #demeans the component
self.BHE.detrend(type='demean')
self.BHZ.detrend(type='demean')
# print(self.BHN)
# Detrend
self.BHN.detrend(type='simple') #De-trends component
self.BHE.detrend(type='simple') #De-trends component
self.BHZ.detrend(type='simple') #De-trends component
# Filter each component. Bandpass flag gives a bandpass-butterworth filter
self.BHN.filter("bandpass",
freqmin=c1,
freqmax=c2,
corners=2,
zerophase=True)
self.BHE.filter("bandpass",
freqmin=c1,
freqmax=c2,
corners=2,
zerophase=True)
self.BHZ.filter("bandpass",
freqmin=c1,
freqmax=c2,
corners=2,
zerophase=True)
# Now trim each component to the input length
if synth == False: # We only need to trim and set the window length for real data, not synthetics made with sacsplitwave
# Now set the trim
# print('Trim Traces')
t1 = (self.tt - 60) #I.e A minute before the arrival
t2 = (self.tt + 120) #I.e Two minutes after the arrival
self.BHN.trim(self.BHN[0].stats.starttime + t1,
self.BHN[0].stats.starttime + t2)
self.BHE.trim(self.BHE[0].stats.starttime + t1,
self.BHE[0].stats.starttime + t2)
self.BHZ.trim(self.BHZ[0].stats.starttime + t1,
self.BHZ[0].stats.starttime + t2)
# Add windowing ranges to sac headers user0,user1,user2,user3 [start1,start2,end1,end2]
# Set the range of window starttime (user0/user1)
user0 = self.tt - 15 #15 seconds before arrival
user1 = self.tt # t predicted arrival
# Set the raqnge of window endtime (user2/user3)
user2 = self.tt + 15 # 15 seconds after, gives a min window size of 20 seconds
user3 = self.tt + 30 # 30 seconds after, gives a max window size of 45 seconds
#
if window == True:
# Windowing code
# Combine BHN and BHE to make a stream
st = self.BHN + self.BHE
# print(user0,user1,user2,user3)
Windower = Picker.WindowPicker(st, user0, user1, user2, user3,
t1)
# Windower.pick()
if Windower.wbeg1 is None:
print("Skipping")
self.bad = True # Switch to tell sheba.py if we actually want to meausre this event
else:
print("Windower Closed, adjusting window ranges")
(
user0, user1, user2, user3
) = Windower.wbeg1, Windower.wbeg2, Windower.wend1, Windower.wend2
self.bad = False
# Set window ranges in SAC headers
self.BHN[0].stats.sac.user0, self.BHN[0].stats.sac.user1, self.BHN[
0].stats.sac.user2, self.BHN[0].stats.sac.user3 = (user0,
user1,
user2,
user3)
self.BHE[0].stats.sac.user0, self.BHE[0].stats.sac.user1, self.BHE[
0].stats.sac.user2, self.BHE[0].stats.sac.user3 = (user0,
user1,
user2,
user3)
self.BHZ[0].stats.sac.user0, self.BHZ[0].stats.sac.user1, self.BHZ[
0].stats.sac.user2, self.BHZ[0].stats.sac.user3 = (user0,
user1,
user2,
user3)
else:
# print('Synthetics Used, Windows *should* be predefined')
# print(self.BHN.stats.sac.user0,self.BHN.stats.sac.user1,self.BHN.stats.sac.user2,self.BHN.stats.sac.user3)
pass
