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()