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