def start():
if cfg.Demonstration():
JointsBuffer1 = convert_joints()
JointsBuffer2 = convert_joints()
try:
# Init threads with jointsbuffer
t1=RobotThread("R1",JointsBuffer1)
t2=RobotThread("R2",JointsBuffer2)
# If program crashes, shut down threads
t1.setDaemon(True)
t2.setDaemon(True)
# start thread
t1.start()
t2.start()
while True:
pass # Just keeps run state going until user cancels
except KeyboardInterrupt:
t1.Set_Run_State(False)
t2.Set_Run_State(False)
print "Execution stopped"
return "USERINPUT"
else:
#================================
#========= INITIERING ===========
#================================
# initiate tasks
t=RobotTasks(["R1","R2"])
#===============================================
#============ Robot 1, RIGHT, from observer ====
#===============================================
JHome=[0,0,0,0,30,0]
JGet=[30,60,20,0,0,0]
JPlace1=[-30,60,-70,-130,60,150]
JPlace2=[-40,70,-36,42,-34,-173]
JPlace3=[-41,65,-90,8,32,150]
middle_point1 = [-24.18, 69.51, -12.3, 52.69, -69.14, -24.89]#xyz, 455.64, 36.75, 169.07 mm
outer_point1 = [31.14, 53.06, -4.54, -25.1, 24.75, 68.41] #xyz = 427, 543, 205.3 mmm
tas1 = [middle_point1, outer_point1, middle_point1 , outer_point1 , middle_point1, outer_point1]
#tas1=[JHome,JGet,JPlace1,JGet,JPlace2,JGet,JPlace3,JHome]
#tas1=[JPlace1]
#==============================================
#============ Robot 2, LEFT from observer =====
#==============================================
JHome=[0,0,0,0,30,0]
JGet=[-30,60,20,0,0,0]
JPlace1=[40,67,-73,0,52,5]
JPlace2=[37,42,-12,0,-50,5]
JPlace3=[13,42,-61,99,68,5]
middle_point2 = [22.28, 68.45, -10.05, 98.56, 69.60, 5.14]
outer_point2 = [-29.46, 55, -12.5, 45.7, -9.5] #xyz, 428,-544, 220 mm
tas2 =[outer_point2, middle_point2, outer_point2, middle_point2, outer_point2, middle_point2]
#tas2=[JHome,JGet,JPlace1,JGet,JPlace2,JGet,JPlace3,JHome]
#tas2=[JPlace1]
# test add both tasks and single task into both dicts.
t.add_task("R1",tas1)
t.add_task("R2",tas2)
t1=RobotThread("R1",[[]])
t2=RobotThread("R2",[[]])
t1.setDaemon(True)
t2.setDaemon(True)
t1.start()
t2.start()
robot=RobotCommander()
# UNsure which arm is which..................
pp=ros(Robot("left_arm"),Robot("right_arm"),plan_scene())
##UUgly again
class listener_class(threading.Thread):
def __init__(self, t1,t2):
self.t1 = t1
self.t2 = t2
threading.Thread.__init__(self)
self.listener()
def callback(self):
t1.Set_Run_State(False)
t2.Set_Run_State(False)
def listener(self):
rospy.Subscriber("/planning_scene", moveit_msgs.msg.PlanningScene, self.callback())
rospy.spin
listener_thread = listener_class(t1,t2)
listener_thread.setDaemon(True)
listener_thread.start()
#================================
# DRIFT AV ROBOTAR OCH UPPGIFTER.
#================================
try:
ir1 = 0
ir2 = 0
while t.count_tasks("R1") > 0 or t.count_tasks("R2") > 0:
# t.print_tasks()
if not t1.is_alive() and t.count_tasks("R1") > 0:
print "Planning for Robot 1"
# Update scene (ej klar än)
#print "Setting R1",
#print [i*3.141592/180.0 for i in robotComm.GetJoints("R1")],
#print [i*3.141592/180 for i in t.current_task("R1")]
start = time.time()
Joints_buffer = pp.plan([i*3.141592/180 for i in robotComm.GetJoints("R1")],[i*3.141592/180 for i in t.current_task("R1")],"R1")
# For some strange reason, [0,0,0,0,0,0,0] is added to Joints_buffer in the end.
# Joints_buffer.pop() removes the last index and in an uggly way solves it
if not Joints_buffer:
print "----------No MotionPlan found for R1"
else:
Joints_buffer.pop()
#print "Starting Robot 1"
#print Joints_buffer
t1=RobotThread("R1",Joints_buffer)
t1.setDaemon(True)
#t1.Set_Buffer(t.current_task("R1"))
#print "R1: Task set:",
#print t.current_task("R1")
t1.Set_Run_State(True)
#t1.Execute_Buffer()
t1.start()
end = time.time()
ir1 = ir1 + 1
print "------------Robot1 seq: ", str(ir1)
print "------------Robot1 Total plantime: ", str(start-end)
t.task_complete("R1")
elif not t2.is_alive() and t.count_tasks("R2") > 0:
print "Planning for Robot 2"
#print "Setting R2",
#print [i*3.141592/180.0 for i in robotComm.GetJoints("R2")],
#print [i*3.141592/180 for i in t.current_task("R2")]
start2 = time.time()
Joints_buffer = pp.plan([i*3.141592/180 for i in robotComm.GetJoints("R2")],[i*3.141592/180 for i in t.current_task("R2")],"R2")
if not Joints_buffer:
print "-------------No MotionPlan found for R2"
else:
Joints_buffer.pop()
#print "Starting Robot 2"
#print Joints_buffer
t2=RobotThread("R2",Joints_buffer)
t2.setDaemon(True)
#t2.Set_Buffer(t.current_task("R2"))
#print "R2: Task set:",
#print t.current_task("R2")
t2.Set_Run_State(True)
t2.start()
#t2.Execute_Buffer()
end2 =time.time()
ir2 = ir2 + 1
print "-------------Robot2 seq: ", str(ir2)
print "-------------Robot2 Total plantime: ", str(start2-end2)
t.task_complete("R2")
while t1.is_alive() or t2.is_alive():
pass
print "All tasks are completed"
time.sleep(1) # Let the threads settle before entering USERINPUT
except KeyboardInterrupt:
t1.Set_Run_State(False)
t2.Set_Run_State(False)
print "Execution stopped"
return "USERINPUT"
# Allt som behövs importeras
from Robots import Robot
from Planscene import plan_scene
from ROS import ros
import moveit_msgs.msg
from moveit_commander import RobotCommander
import rospy
# Debugging
# from optparse import OptionParser
# import inspect
robot=RobotCommander()
pp=ros(Robot("manipulator"),plan_scene())
print pp.plan([1.0,1.0,0,0,0,0],[0.0,0.0,0.0,0.0,0.0,0.0])
print "done 1"
rospy.sleep(1)
print pp.plan([1.0,1.0,0,0,0,0],[0.0,0.0,0.0,0.0,0.0,0.0])
print "Done 2"
#Node is used for remapping joint_states to /robot/joint_states. Solution for problem with ROS...
#Node which the different messages are published from!
#Skapar object av klassen
#Enarmad Robot
#robot1 = Robot("manipulator")
#Tvåarmad Robot
robot1 = Robot("right_arm", 1)
robot2 = Robot("left_arm", 2)
#Scene objektet
scene = plan_scene()
# Starting a prompt, One Arm
#promt(scene, robot1)
# Starting a prompt, TWO ARMS
promt(scene, robot1, robot2)
def start():
if cfg.Demonstration():
JointsBuffer1 = convert_joints()
JointsBuffer2 = convert_joints()
try:
# Init threads with jointsbuffer
t1=RobotThread("R1",JointsBuffer1)
t2=RobotThread("R2",JointsBuffer2)
# If program crashes, shut down threads
t1.setDaemon(True)
t2.setDaemon(True)
# start thread
t1.start()
t2.start()
while True:
pass # Just keeps run state going until user cancels
except KeyboardInterrupt:
t1.Set_Run_State(False)
t2.Set_Run_State(False)
print "Execution stopped"
return "USERINPUT"
else:
# initiate tasks
t=RobotTasks(["R1","R2"])
# Robot 1
JHome=[0,0,0,0,30,0]
JGet=[30,60,20,0,0,0]
JPlace1=[-30,60,-70,-130,60,150] # POSE MÅL: 450,33, -15,15, 592,10, [-1 -2 1 0]
JPlace2=[-40,70,-36,42,-34,-173]
JPlace3=[-41,65,-90,8,32,150]
#tas1=[JHome,JGet,JPlace1,JGet,JPlace2,JGet,JPlace3,JHome]
tas1=[JPlace1]
# Robot 2
JHome=[0,0,0,0,30,0]
JGet=[-30,60,20,0,0,0]
JPlace1=[40,67,-73,0,52,5] # POSE MÅL: 453,18, 80,26, 444,89, [0 0 0 0]
JPlace2=[37,42,-12,0,-50,5]
JPlace3=[13,42,-61,99,68,5]
#tas2=[JHome,JGet,JPlace1,JGet,JPlace2,JGet,JPlace3,JHome]
tas2=[JPlace1]
# test add both tasks and single task into both dicts.
#t.add_task("R1",tas1)
#t.add_task("R2",tas2)
t1=RobotThread("R1",[[]])
t2=RobotThread("R2",[[]])
t1.setDaemon(True)
t2.setDaemon(True)
t1.start()
t2.start()
robot=RobotCommander()
pp=ros(Robot("left_arm"),Robot("right_arm"),plan_scene())
# ##UUgly again
# class listener_class(threading.Thread):
# def __init__(self, t1,t2):
# self.t1 = t1
# self.t2 = t2
# threading.Thread.__init__(self)
# self.listener()
# def callback(self):
# t1.Set_Run_State(False)
# t2.Set_Run_State(False)
# def listener(self):
# rospy.Subscriber("/planning_scene", moveit_msgs.msg.PlanningScene, self.callback())
# rospy.spin
# listener_thread = listener_class(t1,t2)
# listener_thread.setDaemon(True)
# listener_thread.start()
xyz1=[]
xyz2=[]
times1=[]
times2=[]
#pose test
# Robot 1
mittpunkt=[455.64/1000.0, 36.75/1000.0, 169.07/1000.0]
yttrepunkt=[427/1000.0, 543/1000.0, 205.3/1000.0]
tas1=[yttrepunkt, mittpunkt, yttrepunkt, mittpunkt]
# Robot 2
mittpunkt=[455.64/1000.0, -36.75/1000.0, 169.07/1000.0]
yttrepunkt=[428/1000.0, -544/1000.0, 202/1000.0]
tas2=[yttrepunkt, mittpunkt, yttrepunkt, mittpunkt]
#================================
# DRIFT AV ROBOTAR OCH UPPGIFTER.
#================================
# Kör 10 testfall
for i in range(5):
robotComm.SetJoints("R1")
robotComm.SetJoints("R2")
time.sleep(2)
t.add_task("R1",tas1)
t.add_task("R2",tas2)
try:
while t.count_tasks("R1") > 0 or t.count_tasks("R2") > 0:
# t.print_tasks()
s=time.time()
if not t1.is_alive() and t.count_tasks("R1") > 0:
print "Planning for Robot 1"
Joints_buffer = pp.plan([i*3.141592/180 for i in robotComm.GetJoints("R1")],[i for i in t.current_task("R1")],"R1")
# For some strange reason, [0,0,0,0,0,0,0] is added to Joints_buffer in the end.
# Joints_buffer.pop() removes the last index and in an uggly way solves it
print "R1: Task set:",
print t.current_task("R1")
if not Joints_buffer:
print "----------No MotionPlan found for R1"
else:
Joints_buffer.pop()
#t.task_complete("R1")
#print "Starting Robot 1"
#print Joints_buffer
t1=RobotThread("R1",Joints_buffer)
t1.setDaemon(True)
#t1.Set_Buffer(t.current_task("R1"))
t1.Set_Run_State(True)
#t1.Execute_Buffer()
t1.start()
times1.append(time.time()-s)
elif not t2.is_alive() and t.count_tasks("R2") > 0:
print "Planning for Robot 2"
Joints_buffer = pp.plan([i*3.141592/180 for i in robotComm.GetJoints("R2")],[i*3.141592/180 for i in t.current_task("R2")],"R2")
print "R2: Task set:",
print t.current_task("R2")
if not Joints_buffer:
print "----------No MotionPlan found for R2"
else:
Joints_buffer.pop()
#t.task_complete("R2")
#print "Starting Robot 2"
#print Joints_buffer
t2=RobotThread("R2",Joints_buffer)
t2.setDaemon(True)
#t2.Set_Buffer(t.current_task("R2"))
t2.Set_Run_State(True)
t2.start()
times2.append(time.time()-s)
#t2.Execute_Buffer()
while t1.is_alive() or t2.is_alive():
pass
print "All tasks are completed"
#time.sleep(2) # Let the threads settle before entering USERINPUT
except KeyboardInterrupt:
t1.Set_Run_State(False)
t2.Set_Run_State(False)
print "Execution stopped"
# Let threads settle before getting data
time.sleep(2)
xyz1.append(robotComm.get_cartesian("R1"))
xyz2.append(robotComm.get_cartesian("R2"))
#raw_input()
print "============================"
print "= RESULTAT AV ROBOT 1, xyz ="
print "============================"
for i in xyz1:
print i
print "============================"
print "= RESULTAT AV ROBOT 1, tid ="
print "============================"
for i in times1:
print i
print "============================"
print "= RESULTAT AV ROBOT 1, xyz ="
print "============================"
for i in xyz2:
print i
print "============================"
print "= RESULTAT AV ROBOT 2, tid ="
print "============================"
for i in times2:
print i
f = open("2arm_noobj.csv", 'w')
f.write(f, "==================\n Resultat robot 1, xyz \n ============")
f.write(f, xyz1)
f.write(f, "\n==================\n Resultat robot 1, tid \n ============")
f.write(f, tid1)
f.write(f, "\n==================\n Resultat robot 2, xyz \n ============")
f.write(f, xyz2)
f.write(f, "\n==================\n Resultat robot 2, tid \n ============")
f.write(f, tid2)
return "USERINPUT"
self.test_scene.remove_object(id)
self.test_scene.r1_publish()
rospy.sleep(random.uniform(1,4))
except moveit_commander.exception.MoveItCommanderException as er:
print str(er)
except KeyboardInterrupt as er:
print str(er)
rospy.init_node("Plan_scene_publisher")
test_scene = plan_scene()
move_object_demo()
def add_move_remove(self):
try:
while True:
print "Iteration " , str(i)
i = i +1
id_list = self.test_scene.get_id_list()
if len(id_list) == 0:
r_action = 0
else:
r_id= randint(0,len(id_list)-1)