def handle_footstep_plan(self, channel, msg): if isinstance(msg, str): msg = drc.footstep_plan_t.decode(msg) footsteps, opts = decode_footstep_plan(msg) self.last_params = msg.params if len(footsteps) <= 2: # the first two footsteps are always just the positions of the robot's feet, so a plan of two or fewer footsteps is a no-op print 'BDI step translator: Empty plan recieved. Not executing.' return behavior = opts['behavior'] #if behavior == Behavior.BDI_WALKING: # # duration = 0.6 # if len(footsteps) < NUM_REQUIRED_WALK_STEPS+2: # print 'ERROR: Footstep plan must be at least 4 steps for BDI walking translation' # return #elif behavior != Behavior.BDI_STEPPING: # print "BDI step translator: Ignoring footstep plan without BDI_WALKING or BDI_STEPPING behavior" # return self.behavior = behavior now = time.time() if now - self.last_footstep_plan_time > PLAN_UPDATE_TIMEOUT: self.executing = False self.last_footstep_plan_time = now if self.mode == Mode.plotting: self.draw(footsteps) else: #if not self.executing: print "Starting new footstep plan" self.bdi_step_queue_in = footsteps self.send_params(1) if not self.safe: m = "BDI step translator: Steps received; transitioning to {:s}".format( "BDI_STEP" if self.behavior == Behavior.BDI_STEPPING else "BDI_WALK") print m ut.send_status(6, 0, 0, m) time.sleep(1) self.executing = True self.send_behavior() else: m = "BDI step translator: Steps received; in SAFE mode; not transitioning to {:s}".format( "BDI_STEP" if self.behavior == Behavior.BDI_STEPPING else "BDI_WALK") print m ut.send_status(6, 0, 0, m)
def handle_footstep_plan(self, channel, msg): if isinstance(msg, str): msg = drc.footstep_plan_t.decode(msg) footsteps, opts = decode_footstep_plan(msg) self.last_params = msg.params if len(footsteps) <= 2: # the first two footsteps are always just the positions of the robot's feet, so a plan of two or fewer footsteps is a no-op print 'BDI step translator: Empty plan recieved. Not executing.' return behavior = opts['behavior'] #if behavior == Behavior.BDI_WALKING: # # duration = 0.6 # if len(footsteps) < NUM_REQUIRED_WALK_STEPS+2: # print 'ERROR: Footstep plan must be at least 4 steps for BDI walking translation' # return #elif behavior != Behavior.BDI_STEPPING: # print "BDI step translator: Ignoring footstep plan without BDI_WALKING or BDI_STEPPING behavior" # return self.behavior = behavior now = time.time() if now - self.last_footstep_plan_time > PLAN_UPDATE_TIMEOUT: self.executing = False self.last_footstep_plan_time = now if self.mode == Mode.plotting: self.draw(footsteps) else: #if not self.executing: print "Starting new footstep plan" self.bdi_step_queue_in = footsteps self.send_params(1) if not self.safe: m = "BDI step translator: Steps received; transitioning to {:s}".format("BDI_STEP" if self.behavior == Behavior.BDI_STEPPING else "BDI_WALK") print m ut.send_status(6,0,0,m) time.sleep(1) self.executing = True self.send_behavior() else: m = "BDI step translator: Steps received; in SAFE mode; not transitioning to {:s}".format("BDI_STEP" if self.behavior == Behavior.BDI_STEPPING else "BDI_WALK") print m ut.send_status(6,0,0,m)
def handle_footstep_plan(self, channel, msg): if isinstance(msg, str): try: msg = drc.deprecated_footstep_plan_t.decode(msg) except ValueError: msg = drc.footstep_plan_t.decode(msg) if isinstance(msg, drc.deprecated_footstep_plan_t): footsteps, opts = decode_deprecated_footstep_plan(msg) elif isinstance(msg, drc.footstep_plan_t): footsteps, opts = decode_footstep_plan(msg) self.last_params = msg.params else: raise ValueError( "Can't decode footsteps: not a drc.footstep_plan_t or drc.deprecated_footstep_plan_t" ) if len(footsteps) <= 2: # the first two footsteps are always just the positions of the robot's feet, so a plan of two or fewer footsteps is a no-op print 'BDI step translator: Empty plan recieved. Not executing.' return behavior = opts['behavior'] if behavior == Behavior.BDI_WALKING: # duration = 0.6 if len(footsteps) < NUM_REQUIRED_WALK_STEPS + 2: print 'ERROR: Footstep plan must be at least 4 steps for BDI walking translation' return elif behavior != Behavior.BDI_STEPPING: print "BDI step translator: Ignoring footstep plan without BDI_WALKING or BDI_STEPPING behavior" return self.behavior = behavior now = time.time() if now - self.last_footstep_plan_time > PLAN_UPDATE_TIMEOUT: self.executing = False self.last_footstep_plan_time = now if self.mode == Mode.plotting: self.draw(footsteps) else: if not self.executing: print "Starting new footstep plan" self.bdi_step_queue_in = footsteps self.send_params(1) if not self.safe: m = "BDI step translator: Steps received; transitioning to {:s}".format( "BDI_STEP" if self.behavior == Behavior.BDI_STEPPING else "BDI_WALK") print m ut.send_status(6, 0, 0, m) time.sleep(1) self.executing = True self.send_behavior() else: m = "BDI step translator: Steps received; in SAFE mode; not transitioning to {:s}".format( "BDI_STEP" if self.behavior == Behavior.BDI_STEPPING else "BDI_WALK") print m ut.send_status(6, 0, 0, m) else: print "Got updated footstep plan" if self.bdi_step_queue_in[ self.delivered_index - 1].is_right_foot == footsteps[0].is_right_foot: print "Re-aligning new footsteps to current plan" self.bdi_step_queue_in = self.bdi_step_queue_in[:self. delivered_index - 1] + footsteps else: print "Can't align the updated plan to the current plan" return
def handle_footstep_plan(self, channel, msg): if isinstance(msg, str): try: msg = drc.deprecated_footstep_plan_t.decode(msg) except ValueError: msg = drc.footstep_plan_t.decode(msg) if isinstance(msg, drc.deprecated_footstep_plan_t): footsteps, opts = decode_deprecated_footstep_plan(msg) elif isinstance(msg, drc.footstep_plan_t): footsteps, opts = decode_footstep_plan(msg) self.last_params = msg.params else: raise ValueError("Can't decode footsteps: not a drc.footstep_plan_t or drc.deprecated_footstep_plan_t") if len(footsteps) <= 2: # the first two footsteps are always just the positions of the robot's feet, so a plan of two or fewer footsteps is a no-op print 'BDI step translator: Empty plan recieved. Not executing.' return behavior = opts['behavior'] if behavior == Behavior.BDI_WALKING: # duration = 0.6 if len(footsteps) < NUM_REQUIRED_WALK_STEPS+2: print 'ERROR: Footstep plan must be at least 4 steps for BDI walking translation' return elif behavior != Behavior.BDI_STEPPING: print "BDI step translator: Ignoring footstep plan without BDI_WALKING or BDI_STEPPING behavior" return self.behavior = behavior now = time.time() if now - self.last_footstep_plan_time > PLAN_UPDATE_TIMEOUT: self.executing = False self.last_footstep_plan_time = now if self.mode == Mode.plotting: self.draw(footsteps) else: if not self.executing: print "Starting new footstep plan" self.bdi_step_queue_in = footsteps self.send_params(1) if not self.safe: m = "BDI step translator: Steps received; transitioning to {:s}".format("BDI_STEP" if self.behavior == Behavior.BDI_STEPPING else "BDI_WALK") print m ut.send_status(6,0,0,m) time.sleep(1) self.executing = True self.send_behavior() else: m = "BDI step translator: Steps received; in SAFE mode; not transitioning to {:s}".format("BDI_STEP" if self.behavior == Behavior.BDI_STEPPING else "BDI_WALK") print m ut.send_status(6,0,0,m) else: print "Got updated footstep plan" if self.bdi_step_queue_in[self.delivered_index-1].is_right_foot == footsteps[0].is_right_foot: print "Re-aligning new footsteps to current plan" self.bdi_step_queue_in = self.bdi_step_queue_in[:self.delivered_index-1] + footsteps else: print "Can't align the updated plan to the current plan" return