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
