def main_func():
#img = Image()
rospy.init_node('tag_select', anonymous=False)
rate = rospy.Rate(10) # 10hz
count = 0
first_count = True
listener = tf.TransformListener()
trans = []
rot = []
dist = []
id_list = []
tag_pose = Pose()
tag_found = False
state = 1
rtime = rospy.Time(0)
rospy.Subscriber('state',Int16,set_state_callback)
# while listener.frameExists('base') == False:
# try:
# (transWC,rotWC) = listener.lookupTransform('base', 'right_hand_camera', rospy.Time(0))
# except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
# continue
rospy.sleep(1)
while not rospy.is_shutdown():
#if count%10==0:
rospy.Subscriber("/cameras/right_hand_camera/image", Image,xdisplay_pub,count)
#tag_found, tag_transform, WCtransform = multi_marker(listener,tag_found)
while tag_found == False:
#posCB, quatCB, posWC, quatWC = multi_marker(listener)
for n in range(1,7):
marker_id = 'ar_marker_%d'%n
marker_found = listener.frameExists(marker_id)
if marker_found == True:
#print "looking at tag:",n
#(transWC,rotWC) = listener.lookupTransform('base', 'right_hand_camera', rtime)
posCB,quatCB = listener.lookupTransform('right_hand_camera', marker_id, rtime)
print "posCB:", posCB
print "quatCB", quatCB
trans.append(posCB)
rot.append(quatCB)
dist.append(np.linalg.norm(posCB))
id_list.append(n)
print marker_id, ":", posCB
if len(dist)>0:
tag_index = np.argmin(dist)
print "tag index is", tag_index
tag_selected = id_list[tag_index]
tag_transform = (trans[tag_index],rot[tag_index])
tag_found = True
#listener.waitForTransform('base', 'right_hand_camera', rospy.Time(0), rospy.Duration(.5))
posCB, quatCB = tag_transform
posWC,quatWC = listener.lookupTransform('base', 'right_hand_camera', rospy.Time(0))
print "posCB after if:", posCB
print "quatCB acter if:", quatCB
print "posWC", posWC
print "quatWC", quatWC
posCB = np.array([posCB[0], posCB[1], posCB[2]])
posWC = np.array([posWC[0], posWC[1], posWC[2]])
quatCB = np.array([quatCB[3],quatCB[0],quatCB[1],quatCB[2]])
quatWC = np.array([quatWC[3],quatWC[0],quatWC[1],quatWC[2]])
# print tag_transform
print "Distance is:", dist[np.argmin(dist)]
count += 1
#if tag_found == True:
#pub_tag_pose(tag_transform, WCtransform)
#print tag_transform
rotWC = quat_to_so3(quatWC)
rotCB = quat_to_so3(quatCB)
gWC = RpToTrans(rotWC,posWC)
print "gWC is: ", gWC
gCB = RpToTrans(rotCB,posCB)
gWB = gWC.dot(gCB)
beta = pi
gpick = gWB.dot(np.array([[cos(beta),0, -sin(beta),0],[0,1,0,0],[sin(beta),0, cos(beta),0],[0,0,0,1]]))
print "gWB is: ", gWB
print "gCB is: ", gCB
rotWB, posWB = TransToRp(gpick)
quatWB = so3_to_quat(rotWB)
print "quatWB", quatWB
print "PosWB:", posWB
bpos = Pose()
bpos.position.x = posWB[0]
bpos.position.y = posWB[1]
bpos.position.z = posWB[2]
bpos.orientation.x = quatWB[1]
bpos.orientation.y = quatWB[2]
bpos.orientation.z = quatWB[3]
bpos.orientation.w = quatWB[0]
tag_found = False
state_pub = rospy.Publisher('state', Int16, queue_size=10)
tag_pub = rospy.Publisher('block_position', Pose, queue_size = 10)
for i in range(10):
# state_pub = rospy.Publisher('state', Int16, queue_size=10)
state_pub.publish(2)
# tag_pub = rospy.Publisher('block_position', Pose, queue_size = 10)
tag_pub.publish(bpos)
print "tag selected", tag_selected
#rospy.sleep(1)
#break
print state
rate.sleep()
def main_func():
#Initialise Node and ROS variables
rospy.init_node('tag_select', anonymous=False)
rate = rospy.Rate(10) # 10hz
listener = tf.TransformListener()
#rtime = rospy.Time(0)
#Initialize variables and empty lists
trans = []
rot = []
dist = []
id_list = []
#tag_pose = Pose()
tag_found = False
global state
###Define Subsribers and Publishers
#set callback for state
rospy.Subscriber('state',Int16,set_state_callback)
rospy.Subscriber("/cameras/right_hand_camera/image", Image,xdisplay_pub)
state_pub = rospy.Publisher('state', Int16, queue_size=10, latch=True)
tag_pub = rospy.Publisher('block_position', Pose, queue_size = 10, latch=True)
goal_pub = rospy.Publisher('goal', Int16, queue_size = 10, latch=True)
#Sleep Before entering loop
rospy.sleep(.5)
'''
try:
(transWC,rotWC) = listener.lookupTransform('base', 'right_hand_camera', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
'''
####Main ROS loop to run node until shutdown
while not rospy.is_shutdown():
#IF statement to print current state
if state ==1:
print "Entered State :", state
print "Is tag_found?:", tag_found
###Main State Machine Loop - Only run while state_callback is 1
while state == 1:# and tag_found == False:
rospy.loginfo("Entered State 1")
rtime = rospy.Time.now()
trans = []
rot = []
dist = []
id_list = []
tag_found = False
rospy.sleep(.15)
print state
##For loop to search through tf frames of markers
for n in range(1,7):
marker_id = 'ar_marker_%d'%n
#tag_found = listener.frameExists(marker_id)
#print "Marker found is:", 'ar_marker_%d'%n
#Check to see if the frames exists for the marker
if listener.frameExists("/right_hand_camera") and listener.frameExists(marker_id):
#set the time for the latest lookup
rtime1 = listener.getLatestCommonTime("/right_hand_camera", marker_id)
#check to see if this time is greater that time at beginning of loop
if rtime1 > rtime:
#set the tag_found to true
tag_found = True
rtime1 = rtime
print "Marker list: AR_Marker", n, "exist:", tag_found
##when a frame is found for a particular Marker "n" add to list
##only 1 addition per "n" in for
if tag_found == True:
#print "looking at tag:",n
#(transWC,rotWC) = listener.lookupTransform('base', 'right_hand_camera', rtime)
posCB,quatCB = listener.lookupTransform('right_hand_camera', marker_id, rospy.Time(0))
#print "posCB:", posCB
#print "quatCB", quatCB
trans.append(posCB)
rot.append(quatCB)
dist.append(np.linalg.norm(posCB))
id_list.append(n)
#print marker_id, ":", posCB
tag_found = False
##IF Tag list is not empty we have a tag
if len(dist)>0:
#set tag_found
tag_found = True
#choose the closest tag found (if more than one)
tag_index = np.argmin(dist)
tag_selected = id_list[tag_index]
tag_transform = (trans[tag_index],rot[tag_index])
print "tag index is", tag_index
#get tag and world frames into correct variables
posCB, quatCB = tag_transform
posWC,quatWC = listener.lookupTransform('base', 'right_hand_camera', rospy.Time(0))
print "posCB from tf:", posCB
#create proper format for pos and quat variables for functions
posCB = np.array([posCB[0], posCB[1], posCB[2]])
posWC = np.array([posWC[0], posWC[1], posWC[2]])
quatCB = np.array([quatCB[3],quatCB[0],quatCB[1],quatCB[2]])
quatWC = np.array([quatWC[3],quatWC[0],quatWC[1],quatWC[2]])
#convert quat to Rotations
rotWC = quat_to_so3(quatWC)
rotCB = quat_to_so3(quatCB)
#Build Transform SE3 matrix
gWC = RpToTrans(rotWC,posWC)
gCB = RpToTrans(rotCB,posCB)
gWB = gWC.dot(gCB)
#Reorient Orientation so that gripper is correct
beta = pi
gpick = gWB.dot(np.array([[cos(beta),0, -sin(beta),0],[0,1,0,0],[sin(beta),0, cos(beta),0],[0,0,0,1]]))
#Recreate quatWB and rotWB (Block in world frame) from pickup Transform
rotWB, posWB = TransToRp(gpick)
quatWB = so3_to_quat(rotWB)
#Create poses and orientations for block in world
bpos = Pose()
bpos.position.x = posWB[0]
bpos.position.y = posWB[1]
bpos.position.z = -.1489 #posWB[2]
bpos.orientation.x = .99
bpos.orientation.y = -.024
bpos.orientation.z = .024
bpos.orientation.w = .0133
#print statements
print "PosWB:", posWB
print "Distance is:", dist[np.argmin(dist)]
##PUBLISH TAG if it Exists
if tag_found == True:
#Set locally and publish new state
if tag_selected < 3:
goal_pub.publish(1)
else:
goal_pub.publish(2)
rospy.loginfo(2)
state_pub.publish(2)
#publish tage Pose
tag_pub.publish(bpos)
print "tag selected", tag_selected
print state
del posCB,quatCB,posWC,quatWC,posWB,quatWB
###Part of Main While
rate.sleep()
def main_func():
#Initialise Node and ROS variables
rospy.init_node('tag_select', anonymous=False)
rate = rospy.Rate(10) # 10hz
listener = tf.TransformListener()
#rtime = rospy.Time(0)
#Initialize variables and empty lists
trans = []
rot = []
dist = []
id_list = []
#tag_pose = Pose()
tag_found = False
global state
###Define Subsribers and Publishers
#set callback for state
rospy.Subscriber('state', Int16, set_state_callback)
rospy.Subscriber("/cameras/right_hand_camera/image", Image, xdisplay_pub)
state_pub = rospy.Publisher('state', Int16, queue_size=10, latch=True)
tag_pub = rospy.Publisher('block_position',
Pose,
queue_size=10,
latch=True)
goal_pub = rospy.Publisher('goal', Int16, queue_size=10, latch=True)
#Sleep Before entering loop
rospy.sleep(.5)
'''
try:
(transWC,rotWC) = listener.lookupTransform('base', 'right_hand_camera', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
'''
####Main ROS loop to run node until shutdown
while not rospy.is_shutdown():
#IF statement to print current state
if state == 1:
print "Entered State :", state
print "Is tag_found?:", tag_found
###Main State Machine Loop - Only run while state_callback is 1
while state == 1: # and tag_found == False:
rospy.loginfo("Entered State 1")
rtime = rospy.Time.now()
trans = []
rot = []
dist = []
id_list = []
tag_found = False
rospy.sleep(.15)
print state
##For loop to search through tf frames of markers
for n in range(1, 7):
marker_id = 'ar_marker_%d' % n
#tag_found = listener.frameExists(marker_id)
#print "Marker found is:", 'ar_marker_%d'%n
#Check to see if the frames exists for the marker
if listener.frameExists("/right_hand_camera"
) and listener.frameExists(marker_id):
#set the time for the latest lookup
rtime1 = listener.getLatestCommonTime(
"/right_hand_camera", marker_id)
#check to see if this time is greater that time at beginning of loop
if rtime1 > rtime:
#set the tag_found to true
tag_found = True
rtime1 = rtime
print "Marker list: AR_Marker", n, "exist:", tag_found
##when a frame is found for a particular Marker "n" add to list
##only 1 addition per "n" in for
if tag_found == True:
#print "looking at tag:",n
#(transWC,rotWC) = listener.lookupTransform('base', 'right_hand_camera', rtime)
posCB, quatCB = listener.lookupTransform(
'right_hand_camera', marker_id, rospy.Time(0))
#print "posCB:", posCB
#print "quatCB", quatCB
trans.append(posCB)
rot.append(quatCB)
dist.append(np.linalg.norm(posCB))
id_list.append(n)
#print marker_id, ":", posCB
tag_found = False
##IF Tag list is not empty we have a tag
if len(dist) > 0:
#set tag_found
tag_found = True
#choose the closest tag found (if more than one)
tag_index = np.argmin(dist)
tag_selected = id_list[tag_index]
tag_transform = (trans[tag_index], rot[tag_index])
print "tag index is", tag_index
#get tag and world frames into correct variables
posCB, quatCB = tag_transform
posWC, quatWC = listener.lookupTransform(
'base', 'right_hand_camera', rospy.Time(0))
print "posCB from tf:", posCB
#create proper format for pos and quat variables for functions
posCB = np.array([posCB[0], posCB[1], posCB[2]])
posWC = np.array([posWC[0], posWC[1], posWC[2]])
quatCB = np.array([quatCB[3], quatCB[0], quatCB[1], quatCB[2]])
quatWC = np.array([quatWC[3], quatWC[0], quatWC[1], quatWC[2]])
#convert quat to Rotations
rotWC = quat_to_so3(quatWC)
rotCB = quat_to_so3(quatCB)
#Build Transform SE3 matrix
gWC = RpToTrans(rotWC, posWC)
gCB = RpToTrans(rotCB, posCB)
gWB = gWC.dot(gCB)
#Reorient Orientation so that gripper is correct
beta = pi
gpick = gWB.dot(
np.array([[cos(beta), 0, -sin(beta), 0], [0, 1, 0, 0],
[sin(beta), 0, cos(beta), 0], [0, 0, 0, 1]]))
#Recreate quatWB and rotWB (Block in world frame) from pickup Transform
rotWB, posWB = TransToRp(gpick)
quatWB = so3_to_quat(rotWB)
#Create poses and orientations for block in world
bpos = Pose()
bpos.position.x = posWB[0]
bpos.position.y = posWB[1]
bpos.position.z = -.1489 #posWB[2]
bpos.orientation.x = .99
bpos.orientation.y = -.024
bpos.orientation.z = .024
bpos.orientation.w = .0133
#print statements
print "PosWB:", posWB
print "Distance is:", dist[np.argmin(dist)]
##PUBLISH TAG if it Exists
if tag_found == True:
#Set locally and publish new state
if tag_selected < 3:
goal_pub.publish(1)
else:
goal_pub.publish(2)
rospy.loginfo(2)
state_pub.publish(2)
#publish tage Pose
tag_pub.publish(bpos)
print "tag selected", tag_selected
print state
del posCB, quatCB, posWC, quatWC, posWB, quatWB
###Part of Main While
rate.sleep()
