def GoToXOfMine(targetx):
global errorThreshold
while not rospy.is_shutdown():
result = mine.detection(originImage)
if result['find'] == True:
xError = targetx - result['Cy']
if abs(xError) < errorThreshold[0]:
break
xLength = xPid.run(xError)
mCtrl.WalkTheDistance(xLength, 0, 0)
mCtrl.WaitForWalkingDone()
def passMinefield():
while not rospy.is_shutdown():
result = wall.detection(originImage)
if result['find'] == True:
break
result = mine.detection(originImage)
if result['find'] == True:
GoToXOfMine(115)
GoToYOfMine(8)
for i in range(0, 3):
mCtrl.SendGaitCommand(0.04, 0.0, 0.0)
mCtrl.WaitForWalkingDone()
def GoToYOfMine(targety):
global errorThreshold
while not rospy.is_shutdown():
result = mine.detection(originImage)
if result['find'] == True:
w, h = originImage.shape[1], originImage.shape[0]
if result['Cx'] > w / 2:
yError = (w - targety) - result['Cx']
else:
yError = targety - result['Cx']
if abs(yError) < errorThreshold[0]:
break
yLength = yPid.run(yError)
mCtrl.WalkTheDistance(0, yLength, 0)
mCtrl.WaitForWalkingDone()
def SimPrepareKickBall(targetx, targety, targeta):
global errorThreshold
while not rospy.is_shutdown():
result1 = ball.detection(originImage)
result2 = hole.detection(originImage)
if (result1['find'] != False) and (result2['find'] != False):
xError = targetx - result1['Cy']
yError = targety - result1['Cx']
aError = targeta - result2['Cx']
if (abs(xError) < errorThreshold[0]) and (
abs(yError) < errorThreshold[1]) and (abs(aError) <
errorThreshold[2]):
break
xLength = xPid.run(xError)
yLength = yPid.run(yError)
aLength = aPid.run(aError)
mCtrl.WalkTheDistance(xLength, yLength, aLength)
mCtrl.WaitForWalkingDone()
def GoToBall(targetx, targety, targeta):
global errorThreshold
while not rospy.is_shutdown():
result = ball.detection(originImage)
if result['find'] != False:
xError = targetx - result['Cy']
yError = targety - result['Cx']
aError = targeta - result['Cx']
if (abs(xError) < errorThreshold[0]) and (
abs(yError) < errorThreshold[1]) and (abs(aError) <
errorThreshold[2]):
break
xLength = xPid.run(xError)
yLength = yPid.run(yError)
aLength = aPid.run(aError)
mCtrl.WalkTheDistance(xLength, yLength, aLength)
mCtrl.WaitForWalkingDone()
else:
rospy.logwarn('no ball found!')
time.sleep(0.5)
for i in range(0, 16):
mCtrl.SendGaitCommand(0.07, 0.0, -7.5)
for i in range(0, 16):
mCtrl.SendGaitCommand(0.07, 0.0, 7.5)
for i in range(0, 5):
mCtrl.SendGaitCommand(0.0, 0.0, -12.0)
def rosShutdownHook():
mCtrl.ResetBodyhub()
rospy.signal_shutdown('node_close')
if __name__ == '__main__':
rospy.init_node('walking_SPath_node', anonymous=True)
time.sleep(0.2)
rospy.on_shutdown(rosShutdownHook)
rospy.loginfo('node runing...')
if mCtrl.SetBodyhubTo_walking(nodeControlId) == False:
rospy.logerr('bodyhub to walking fail!')
rospy.signal_shutdown('error')
exit(1)
time.sleep(1)
while not rospy.is_shutdown():
walkingSPath()
mCtrl.WaitForWalkingDone()
mCtrl.ResetBodyhub()
rospy.signal_shutdown('exit')
def SimkickBall():
global setpLength
GoToBall(330.0, 320.0, 320.0)
SimPrepareKickBall(360.0, 280.0, 285.0)
mCtrl.SendGaitCommand(setpLength[0], 0.0, 0.0)
mCtrl.WaitForWalkingDone()
