def bigArmL_move(self, amp=9, speed=1, blockMode=False):
print('Info: Left big Arm moving...speed = %d' % (speed))
goal = servosGoal(servoID=mc.bigArm_L,
goalAngle=(amp - 5) * 300,
goalTime=300 / speed)
res = self.servo_req(goal, self.act_client_bigArm_L)
return res
def armR_move(self, amp=9, speed=1, blockMode=False):
print('Info: Right hand moving...speed = %d' % (speed))
goal = servosGoal(servoID=mc.arm_R,
goalAngle=(amp - 5) * 300,
goalTime=300 / speed)
res = self.servo_req(goal, self.act_client_arm_R)
return res
def shoulderL_move(self, amp=9, speed=1, blockMode=False):
goal = servosGoal(servoID=mc.shoulder_L,
goalAngle=(amp - 5) * 300,
goalTime=300 / speed)
res = self.servo_req(goal, self.act_client_shoulder_L)
return res
def head_move(self, dest=-30, speed=1, blockMode=False): # speed: 1-9
""" dest: 0-320-640 =-goalAngle-=> -1500 -> 0 -> +1500 | 200/1=200, 200/10=20 ---> 200=2000ms, 20=200ms"""
goal = servosGoal(servoID=mc.head_H,
goalAngle=(dest - 320) * 10 / 4,
goalTime=300 / speed)
res = self.servo_req(goal, self.act_client_head_H)
return res
def bigArmR_move(self, amp=9, speed=1, blockMode=False):
print('Info:right big Arm moving.....speed = %d' % (speed))
goal = servosGoal(servoID=mc.bigArm_R,
goalAngle=(amp - 5) * 300,
goalTime=300 / speed)
res = self.servo_req(goal)
return res
def shoulderR_move(self, amp=9, speed=1, blockMode=False):
print('Info: Right shoulder moving...speed = %d' % (speed))
goal = servosGoal(servoID=mc.shoulder_R,
goalAngle=(amp - 5) * 300,
goalTime=300 / speed)
res = self.servo_req(goal)
return res
def shoulderL_move(self, amp=9, speed=1, blockMode=False):
'''print ("---: mc..shoulderL_move.<wait_for_server>.....")
self.act_client.wait_for_server( )
print ("---: mc shoulderL_move. Got server!")
goal = servosGoal(servoID = mc.shoulder_L, goalAngle = amp*120, goalTime = 200/speed)
self.act_client.send_goal(goal)
print ('left hand uping...Sent goal. goal is:', goal )
self.act_client.wait_for_result(rospy.Duration.from_sec(5.0) )
return self.act_client.get_result( ) '''
goal = servosGoal(servoID=mc.shoulder_L,
goalAngle=(amp - 5) * 300,
goalTime=300 / speed)
res = self.servo_req(goal)
return res
def head_move(self, dest=-30, speed=1): # speed: 1-9
block = False
# dest: 0-320-640 = -1500 -> 0 -> +1500 | 10*12=120*, 200/10=20ms ->200ms
goal = servosGoal(servoID=head_H,
goalAngle=(dest - 320) * 10 / 4,
goalTime=200 / speed)
res = None
try_count = 0
while not rospy.is_shutdown():
if not block:
self.act_client.send_goal(goal)
block = True
elif self.servo_max_wait_sec == try_count:
rospy.logwarn(
"Wait too long time for the servo action result success! will break."
)
break
else:
self.act_client.wait_for_result(rospy.Duration.from_sec(1.0))
res = self.act_client.get_result()
if res is not None:
print(
"@@@@@current act_client.get_result, types are:, result.result is, type is,",
res, type(res), res.result, type(res.result))
if (servosResult.SUCCEEDED == res.result):
block = False
print(
"~~~~~~~~~~~goal achieved. break. goal.goalAngle & res.result is:",
goal.goalAngle, res.result)
break
else:
try_count += 1
print(
"~~~~~~~~~~~~waiting for goal achieved. second is",
try_count)
return self.act_client.get_result()