def sendWalkingPlanRequest(self, footstepPlan, startPose, waitForResponse=False, waitTimeout=5000, req_type='traj'):
msg = lcmdrc.walking_plan_request_t()
msg.utime = getUtime()
state_msg = robotstate.drakePoseToRobotState(startPose)
msg.initial_state = state_msg
msg.new_nominal_state = msg.initial_state
msg.use_new_nominal_state = True
msg.footstep_plan = footstepPlan
if req_type == 'traj':
requestChannel = 'WALKING_TRAJ_REQUEST'
responseChannel = 'WALKING_TRAJ_RESPONSE'
response_type = lcmdrc.robot_plan_t
elif req_type == 'controller':
requestChannel = 'WALKING_CONTROLLER_PLAN_REQUEST'
responseChannel = 'WALKING_CONTROLLER_PLAN_RESPONSE'
response_type = lcmdrc.walking_plan_t
elif req_type == 'simulate_drake':
requestChannel = 'WALKING_SIMULATION_DRAKE_REQUEST'
responseChannel = 'WALKING_SIMULATION_TRAJ_RESPONSE'
response_type = lcmdrc.robot_plan_t
else:
raise ValueError("Invalid request type: {:s}".format(req_type))
if waitForResponse:
if waitTimeout == 0:
helper = lcmUtils.MessageResponseHelper(responseChannel, response_type)
lcmUtils.publish(requestChannel, msg)
return helper
return lcmUtils.MessageResponseHelper.publishAndWait(requestChannel, msg,
responseChannel, response_type, waitTimeout)
else:
lcmUtils.publish(requestChannel, msg)
def sendFootstepPlanCheckRequest(self, request, waitForResponse=False, waitTimeout=5000):
assert isinstance(request, lcmdrc.footstep_check_request_t)
requestChannel = 'FOOTSTEP_CHECK_REQUEST'
responseChannel = 'FOOTSTEP_PLAN_RESPONSE'
if waitForResponse:
if waitTimeout == 0:
helper = lcmUtils.MessageResponseHelper(responseChannel, lcmdrc.footstep_plan_t)
lcmUtils.publish(requestChannel, request)
return helper
return lcmUtils.MessageResponseHelper.publishAndWait(requestChannel, request,
responseChannel, lcmdrc.footstep_plan_t, waitTimeout)
else:
lcmUtils.publish(requestChannel, request)
def run(self):
responseMessageClass = lcmdrc.boolean_t
grasping_state = lcmUtils.MessageResponseHelper(
self.properties.getProperty('Channel name'),
responseMessageClass).waitForResponse(timeout=7000)
if grasping_state is not None and self.properties.getPropertyEnumValue(
'Action name') == 'Open':
if grasping_state.data == 0:
# print "Hand opening successful"
self.statusMessage = "Hand opening successful"
else:
self.fail("Could not open hand")
elif grasping_state is not None and self.properties.getPropertyEnumValue(
'Action name') == 'Grasp':
if grasping_state.data == 1:
# print "Grasping successful"
self.statusMessage = "Grasping successful"
else:
self.fail("No object in hand")
else:
self.fail("Grasping state timeout")