is_start = False
rospy.Subscriber('scan_black/dualarm_start_1',
Int32,
start_callback,
queue_size=1)
pub = rospy.Publisher('scan_black/strategy_behavior', Int32, queue_size=1)
left = disposingTask('left') # Set up left arm controller
rospy.sleep(.3)
while not rospy.is_shutdown() and not is_start:
rospy.loginfo('waiting for start signal')
rospy.sleep(.5)
SuctionTask.switch_mode(True)
rate = rospy.Rate(30) # 30hz
while not rospy.is_shutdown() and not left.finish:
left.process()
rate.sleep()
# robot arm back home
if left.arm.is_stop is not True:
rospy.loginfo('back home')
left.arm.wait_busy()
left.arm.jointMove(0, (0, -1, 0, 2, 0, -0.7, 0))
left.arm.wait_busy()
left.arm.jointMove(0, (0, 0, 0, 0, 0, 0, 0))
class CDualArmTask:
def __init__(self, _name = '/robotis'):
"""Initial object."""
en_sim = False
if len(sys.argv) >= 2:
rospy.set_param('en_sim', sys.argv[1])
en_sim = rospy.get_param('en_sim')
self.name = _name
self.arm = ArmTask(self.name + '_arm')
self.pick_list = 2
self.pos = (0, 0, 0)
self.euler = (0, 0, 0)
self.phi = 0
if en_sim:
self.suction = SuctionTask(self.name + '_gazebo')
print "aa"
else:
self.suction = SuctionTask(self.name)
print "bb"
def SetSpeed(self, spd):
self.arm.set_speed(spd)
def IdelPos(self):
self.arm.set_speed(50)
self.arm.jointMove(0, (0, -0.5, 0, 1, 0, -0.5, 0))
self.suction.gripper_suction_deg(0)
def InitialPos(self):
self.arm.set_speed(50)
self.arm.jointMove(0, (0, 0, 0, 0, 0, 0, 0))
self.suction.gripper_suction_deg(0)
def MoveAbs(self, Line_PtP, Pos, Euler, Redun):
if(Line_PtP == 'line'):
Line_PtP = 'line'
else:
Line_PtP = 'p2p'
self.arm.ikMove(Line_PtP, Pos, Euler, Redun)
def MoveRelPos(self, Line_PtP, Pos):
if(Line_PtP == 'line'):
Line_PtP = 'line'
else:
Line_PtP = 'p2p'
print('RelMove1')
self.arm.relative_move_pose(Line_PtP, Pos)
print('RelMove1')
def SuctionEnable(self, On_Off):
if(On_Off == True):
self.suction.gripper_vaccum_on()
elif(On_Off == False):
self.suction.gripper_vaccum_off()
def SetSuctionDeg(self, Deg):
self.suction.gripper_suction_deg(Deg)
def SuckSuccess(self):
print('suckflag')
return self.suction.is_grip
def StopRobot_and_ClearCmd(self):
# Force stop robot
print('StopRobot')
self.arm.clear_cmd()
def PauseRobot(self, PauseFlag):
# True : Pause
# False: Continue
self.arm.freeze(PauseFlag)
def GetArmPos(self):
print('Pos1')
GetArmInfo = self.arm.get_fb()
print('Pos2')
ArmPos = GetArmInfo.group_pose.position
print('Pos3')
return ArmPos # .x .y .z
def ChangeModeToGetSuckState(self, Flag):
# True : Get suck success state
# False: Arduino for RFID
self.suction.switch_mode(Flag)