def ar_tag_sub_callback(self, msg):
for marker in msg.markers:
if marker.id == BOX_TAG_ID:
if self.box_tag_id == None:
self.box_tag_id = marker.id
util.signal(quantity=1, onColor=Led.RED)
rospy.sleep(0.5)
self.box_tag_saw = True
else:
self.box_tag_saw = False
if marker.id == GOAL_TAG_ID:
if self.goal_tag_id == None:
self.goal_tag_id = marker.id
util.signal(quantity=1, onColor=Led.GREEN)
if self.goal_stall_id == None:
try:
(trans, _) = self.listener.lookupTransform(
'/map', '/ar_marker_' + str(self.goal_tag_id),
rospy.Time(0))
goal_tag_point = Point(trans[0], trans[1], trans[2])
self.goal_stall_id = get_cloest_stall(goal_tag_point)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print "TF look up exception when look up goal tag"
def execute(self, userdata):
if rospy.is_shutdown():
return 'end'
else:
contour = userdata.SearchContour_in_contour
print "Looking for contour ", contour
cd = detectshapes.ContourDetector()
image_sub = rospy.Subscriber("camera/rgb/image_raw", Image, self.shape_cam_callback)
print "Waiting for camera/rgb/image_raw message..."
rospy.wait_for_message("camera/rgb/image_raw", Image)
for stall_id in ['6', '7', '8']:
goal_pose = util.goal_pose('map',
point=PARK_SPOT_WAYPOINTS[stall_id][0],
quaternion=PARK_SPOT_WAYPOINTS[stall_id][1])
self.client.send_goal(goal_pose)
print "waiting for result ", goal_pose.target_pose.header.frame_id
self.client.wait_for_result()
while self.hsv == None:
pass
_, red_contours = cd.getContours(self.hsv)
if len(red_contours) > 0:
print red_contours, contour['shape_at_loc2']
if contour['shape_at_loc2'] in red_contours:
util.signal(1, onColor1=Led.ORANGE)
rospy.sleep(1)
util.signal(2, onColor1=Led.ORANGE, onColor2=Led.GREEN)
return 'completed'
return 'completed'
def execute(self, userdata):
if rospy.is_shutdown():
return 'end'
else:
contour = userdata.SearchContour_in_contour
assert isinstance(contour, detectshapes.Contour)
cd = detectshapes.ContourDetector()
image_sub = rospy.Subscriber("camera/rgb/image_raw", Image,
self.shape_cam_callback)
print "Waiting for camera/rgb/image_raw message..."
rospy.wait_for_message("camera/rgb/image_raw", Image)
for stall_id in ['5', '6', '7']:
goal_pose = util.goal_pose(
'map',
point=PARK_SPOT_WAYPOINTS[stall_id][0],
quaternion=PARK_SPOT_WAYPOINTS[stall_id][1])
self.client.send_goal(goal_pose)
self.client.wait_for_result()
while self.hsv == None:
pass
_, red_contours = cd.getContours(self.hsv)
if len(red_contours) > 0:
if red_contours[0] == contour:
util.signal(1, onColor=Led.ORANGE)
util.signal(2, onColor=Led.ORANGE)
return 'completed'
def execute(self):
begin = time.time()
outcome = self.sm.execute()
if outcome == 'end':
util.signal(2)
print 'time used:', time.time() - begin
rospy.spin()
self.sis.stop()
def ar_tag_sub_callback(self, msg):
for marker in msg.markers:
if marker.id == 6 and self.box_tag_id == None:
self.box_tag_id = marker.id
util.signal(quantity=1, onColor=Led.RED)
if marker.id == 30 and self.goal_tag_id == None:
self.goal_tag_id = marker.id
util.signal(quantity=1, onColor=Led.GREEN)
def observe(self):
global twist_pub
cd = ContourDetector()
image_sub = rospy.Subscriber("camera/rgb/image_raw", Image,
self.shape_cam_callback)
time.sleep(2)
tmp = time.time()
while True and (time.time() - tmp) < 5:
_, red_contours1 = cd.getContours(self.hsv, 1)
if len(red_contours1) > 0:
print "numer of objects:", len(red_contours1)
signal(len(red_contours1), onColor=Led.ORANGE)
break
def execute(self, userdata):
global g_start
if rospy.is_shutdown():
return 'end'
else:
waypoints = self.get_waypoints()
for pose in waypoints:
goal = self.goal_pose(pose)
self.client.send_goal(goal)
self.client.wait_for_result()
rospy.sleep(1.5)
util.signal()
g_start = False
return 'arrived'
def observe(self):
global current_work, shape_at_loc2, redline_count_loc3, task3_finished
cd = ContourDetector()
image_sub = rospy.Subscriber("camera/rgb/image_raw", Image,
self.shape_cam_callback)
time.sleep(1)
tmp = time.time()
while True and (time.time() - tmp) < 5:
_, red_contours1 = cd.getContours(self.hsv, 3, redline_count_loc3)
if len(red_contours1) > 0:
if red_contours1[0] == shape_at_loc2:
print "Matched: ", red_contours1[0]
signal(1)
task3_finished = True
break
time.sleep(0.5)
def observe(self):
global shape_at_loc2
cd = ContourDetector()
image_sub = rospy.Subscriber("camera/rgb/image_raw", Image,
self.shape_cam_callback)
time.sleep(1)
tmp = time.time()
while True and (time.time() - tmp) < 5:
green_contours1, red_contours1 = cd.getContours(self.hsv, 2)
if len(green_contours1) > 0:
shape_at_loc2 = green_contours1[0]
print "shape at loc2: ", shape_at_loc2
signal(len(green_contours1) + len(red_contours1))
break
time.sleep(0.5)
def execute(self, userdata):
global g_twist_pub, g_full_red_line_count, current_twist, g_work4_returned
if self.subscribered == False:
self.subscribe()
self.subscribered = True
if self.loop_start_time == None:
self.loop_start_time = time.time()
if g_full_red_line_count == 2 and not g_work4_returned:
twist = Twist()
g_twist_pub.publish(twist)
rospy.sleep(0.5)
tmp_time = time.time()
while time.time() - tmp_time < 1.85:
g_twist_pub.publish(current_twist)
g_twist_pub.publish(Twist())
rotate(-35)
tmp_time = time.time()
while time.time() - tmp_time < 1.7:
g_twist_pub.publish(current_twist)
g_twist_pub.publish(Twist())
self.usb_cam_subscriber.unregister()
self.subscribered = False
g_work4_returned = True
return 'work4'
else:
if g_full_red_line_count == 3:
g_work4_returned = False
# start line
if g_full_red_line_count == 4:
g_full_red_line_count = 0
g_twist_pub.publish(Twist())
util.signal(2)
self.loop_start_time = None
g_twist_pub.publish(current_twist)
return 'running'
def search(self, process, search_orientation=[0]):
if process['ARtag_found'] == False:
ar_sub = rospy.Subscriber("ar_pose_marker", AlvarMarkers,
self.ar_callback)
rospy.wait_for_message("ar_pose_marker", AlvarMarkers)
if process['contour_found'][1] == False:
image_sub = rospy.Subscriber("camera/rgb/image_raw", Image,
self.shape_cam_callback)
rospy.wait_for_message("camera/rgb/image_raw", Image)
for angle in search_orientation:
if angle != 0:
util.rotate(angle)
if process['ARtag_found'] == False and self.search_ARtag():
util.signal(1, onColor=Led.GREEN)
process['ARtag_found'] = True
if process['contour_found'][1] == False and self.search_contour(
process):
util.signal(1, onColor=Led.ORANGE)
process['contour_found'][1] = True
if process['ARtag_found'] and process['contour_found'][1]:
break
#unregister
if process['spot_id'] == process['unmarked_spot_id'][0]:
util.signal(1, onColor=Led.RED)
process['unmarked_spot_id'][1] = True
return process
def execute(self, userdata):
# set to None for each loop
self.box_tag_id = None
self.goal_tag_id = None
self.goal_stall_id = None
self.box_tag_saw = None
if self.is_initial_pose_set == False or True:
self.set_init_map_pose()
self.is_initial_pose_set = True
ar_tag_sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.ar_tag_sub_callback)
print "Waiting for /ar_pose_marker message..."
rospy.wait_for_message('/ar_pose_marker', AlvarMarkers)
self.twist_pub = rospy.Publisher("/cmd_vel_mux/input/teleop", Twist, queue_size=1)
current_box_stall_id = None
tmp_time = time.time()
Is_in_stall_5 = False
while True:
if rospy.is_shutdown():
ar_tag_sub.unregister()
return 'end'
if self.box_tag_id != None and self.goal_tag_id != None:
try:
(trans, _) = self.listener.lookupTransform('/map', '/ar_marker_' + str(self.box_tag_id), rospy.Time(0))
box_tag_point = Point(trans[0], trans[1], trans[2])
current_box_stall_id= get_cloest_stall(box_tag_point)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print "TF look up exception when look up goal tag"
if current_box_stall_id != None and self.goal_stall_id != None:
print "current_box_stall_id", current_box_stall_id, 'self.goal_stall_id', self.goal_stall_id
if current_box_stall_id == 1 or current_box_stall_id == 5:
return 'completed'
if current_box_stall_id == self.goal_stall_id:
return 'completed'
break
twist = Twist()
twist.angular.z = - 0.4
self.twist_pub.publish(twist)
if (time.time() - tmp_time) > 5 and not Is_in_stall_5:
goal_pose = util.goal_pose('map', PARK_SPOT_WAYPOINTS['6'][0], Quaternion(0,0,0,1))
self.client.send_goal(goal_pose)
print "waiting for result ", goal_pose.target_pose.header.frame_id
self.client.wait_for_result()
Is_in_stall_5 = True
box_is_left = True
if int(current_box_stall_id) > int(self.goal_stall_id):
box_is_left = False
if box_is_left:
# if current_box_stall_id == 2:
# point = PARK_SPOT_WAYPOINTS['6'][0]
# quaternion = PARK_SPOT_WAYPOINTS['6'][1]
# goal_pose = util.goal_pose('map', point, quaternion)
# self.client.send_goal(goal_pose)
# print "waiting for result ", goal_pose.target_pose.header.frame_id
# self.client.wait_for_result()
point = PARK_SPOT_WAYPOINTS[str(current_box_stall_id-1)][0]
quaternion = PARK_SPOT_WAYPOINTS[str(current_box_stall_id-1)][1]
goal_pose = util.goal_pose('map', point, quaternion)
self.client.send_goal(goal_pose)
print "waiting for result ", goal_pose.target_pose.header.frame_id
self.client.wait_for_result()
util.rotate(-92)
for i in range(abs(current_box_stall_id - self.goal_stall_id)):
if i == 0 and current_box_stall_id == 2:
push_dist = SQUARE_DIST * 2 - BOX_EDGE_LENGTH/2 - ROBOT_LENGTH/2 - 0.1
util.move(push_dist, linear_scale=0.1)
else:
util.move(-0.7, linear_scale=0.3)
if not self.go_to_side('box_front'):
return 'completed'
push_dist = SQUARE_DIST + AMCL_APPROACH_BOX - 0.05
util.move(push_dist, linear_scale= 0.1)
else:
point = PARK_SPOT_WAYPOINTS[str(int(current_box_stall_id)+1)][0]
quaternion = PARK_SPOT_WAYPOINTS[str(int(current_box_stall_id)+1)][1]
goal_pose = util.goal_pose('map', point, quaternion)
self.client.send_goal(goal_pose)
print "waiting for result ", goal_pose.target_pose.header.frame_id
self.client.wait_for_result()
if current_box_stall_id == 3:
util.rotate(81)
else:
util.rotate(89)
for i in range(abs(current_box_stall_id - self.goal_stall_id)):
if i == 0 and current_box_stall_id == 4:
push_dist = SQUARE_DIST * 2 - BOX_EDGE_LENGTH/2 - ROBOT_LENGTH/2 - 0.1
util.move(push_dist, linear_scale=0.1)
else:
util.move(-0.7, linear_scale=0.3)
if not self.go_to_side('box_front'):
return 'completed'
# if i == 0:
# util.rotate(6, max_error=2, anglular_scale=0.5)
push_dist = SQUARE_DIST + AMCL_APPROACH_BOX - 0.07
util.move(push_dist, linear_scale= 0.1)
util.signal(2, onColor1 = Led.GREEN, onColor2=Led.RED)
util.move(-0.2, linear_scale = 0.3)
ar_tag_sub.unregister()
return 'completed'
def execute(self, userdata):
if self.trial == 0:
self.set_init_map_pose()
self.trial += 1
print "trial: ", self.trial
if self.trial > 2:
goal_pose = util.goal_pose('map', ON_RAMP_WAYPOINT[0],
ON_RAMP_WAYPOINT[1])
self.client.send_goal(goal_pose)
self.client.wait_for_result()
return 'completed'
ar_tag_sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers,
self.ar_tag_sub_callback)
print "Waiting for /ar_pose_marker message..."
rospy.wait_for_message('/ar_pose_marker', AlvarMarkers)
self.twist_pub = rospy.Publisher("/cmd_vel_mux/input/teleop",
Twist,
queue_size=1)
current_box_stall_id = None
tmp_time = time.time()
while True:
if rospy.is_shutdown():
ar_tag_sub.unregister()
return 'end'
if self.box_tag_id != None and self.goal_tag_id != None:
try:
(trans, _) = self.listener.lookupTransform(
'/map', '/ar_marker_' + str(self.box_tag_id),
rospy.Time(0))
box_tag_point = Point(trans[0], trans[1], trans[2])
current_box_stall_id = get_cloest_stall(box_tag_point)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print "TF look up exception when look up goal tag"
if current_box_stall_id != None and self.goal_stall_id != None:
assert current_box_stall_id > 1 and current_box_stall_id < 5, \
"current_box_stall_id = {0}".format(str(current_box_stall_id))
assert current_box_stall_id != self.goal_stall_id, \
"current_box_stall_id = {0}, self.goal_stall_id = {1}".format(str(current_box_stall_id), \
str(self.goal_stall_id))
break
twist = Twist()
twist.angular.z = -0.4
self.twist_pub.publish(twist)
if time.time() - tmp_time > 5:
goal_pose = util.goal_pose('map', PARK_SPOT_WAYPOINTS['6'][0],
Quaternion(0, 0, 0, 1))
self.client.send_goal(goal_pose)
print "waiting for result ", goal_pose.target_pose.header.frame_id
self.client.wait_for_result()
if time.time() - tmp_time > 20:
goal_pose = util.goal_pose('map', PARK_SPOT_WAYPOINTS['7'][0],
Quaternion(0, 0, 0, 1))
self.client.send_goal(goal_pose)
print "waiting for result ", goal_pose.target_pose.header.frame_id
self.client.wait_for_result()
box_is_left = True
if int(current_box_stall_id) > int(self.goal_stall_id):
box_is_left = False
if box_is_left:
if current_box_stall_id == 2:
point = PARK_SPOT_WAYPOINTS['6'][0]
quaternion = PARK_SPOT_WAYPOINTS['6'][1]
goal_pose = util.goal_pose('map', point, quaternion)
self.client.send_goal(goal_pose)
print "waiting for result ", goal_pose.target_pose.header.frame_id
self.client.wait_for_result()
point = PARK_SPOT_WAYPOINTS[str(current_box_stall_id - 1)][0]
quaternion = PARK_SPOT_WAYPOINTS[str(current_box_stall_id - 1)][1]
goal_pose = util.goal_pose('map', point, quaternion)
self.client.send_goal(goal_pose)
print "waiting for result ", goal_pose.target_pose.header.frame_id
self.client.wait_for_result()
#util.signal(1, onColor=Led.BLACK) #debug
util.rotate(-92)
for i in range(abs(current_box_stall_id - self.goal_stall_id)):
if i == 0 and current_box_stall_id == 2:
push_dist = SQUARE_DIST * 2 - BOX_EDGE_LENGTH / 2 - ROBOT_LENGTH / 2 - 0.1
util.move(push_dist, linear_scale=0.1)
else:
util.move(-0.6, linear_scale=0.3)
self.go_to_side('box_front')
# util.move(AMCL_APPROACH_BOX)
# rospy.sleep(1)
push_dist = SQUARE_DIST + AMCL_APPROACH_BOX - 0.05
util.move(push_dist, linear_scale=0.2)
else:
point = PARK_SPOT_WAYPOINTS[str(int(current_box_stall_id) + 1)][0]
quaternion = PARK_SPOT_WAYPOINTS[str(
int(current_box_stall_id) + 1)][1]
goal_pose = util.goal_pose('map', point, quaternion)
self.client.send_goal(goal_pose)
print "waiting for result ", goal_pose.target_pose.header.frame_id
self.client.wait_for_result()
#util.signal(1, onColor=Led.BLACK) #debug
util.rotate(90)
for i in range(abs(current_box_stall_id - self.goal_stall_id)):
if i == 0:
util.move(-0.3, linear_scale=0.3)
else:
util.move(-0.6, linear_scale=0.3)
self.go_to_side('box_front')
if i == 0:
util.rotate(6, max_error=2, anglular_scale=0.5)
push_dist = SQUARE_DIST + AMCL_APPROACH_BOX - 0.07
util.move(push_dist, linear_scale=0.2)
# if self.fine_tune(box_is_left, push_dist) == 'lost_box':
# goal_pose = util.goal_pose('map', OFF_RAMP_WAYPOINT[0], OFF_RAMP_WAYPOINT[1])
# self.client.send_goal(goal_pose)
# self.client.wait_for_result()
# return 'restart'
util.signal(2, onColor=Led.GREEN, onColor2=Led.RED)
util.move(-0.2)
ar_tag_sub.unregister()
return 'completed'
def execute(self, userdata):
self.set_init_map_pose()
ar_tag_sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers,
self.ar_tag_sub_callback)
print "Waiting for /ar_pose_marker message..."
rospy.wait_for_message('/ar_pose_marker', AlvarMarkers)
twist_pub = rospy.Publisher("/cmd_vel_mux/input/teleop",
Twist,
queue_size=1)
tmp_time = time.time()
while True:
if rospy.is_shutdown():
return 'end'
if self.box_tag_id != None and self.goal_tag_id != None:
break
twist = Twist()
twist.angular.z = -0.2
twist_pub.publish(twist)
if time.time() - tmp_time > 5:
goal_pose = util.goal_pose('map', PARK_SPOT_WAYPOINTS['6'][0],
PARK_SPOT_WAYPOINTS['6'][1])
self.client.send_goal(goal_pose)
self.client.wait_for_result()
util.rotate(87)
while True:
if rospy.is_shutdown():
ar_tag_sub.unregister()
return 'end'
try:
(trans, rots) = self.listener.lookupTransform(
'map', '/ar_marker_' + str(self.box_tag_id), rospy.Time(0))
point = Point(trans[0], trans[1], trans[2])
#quaternion = Quaternion(rots[0],rots[1],rots[2],rots[3])
quaternion = Quaternion(0, 0, 0.118, 0.993)
self.box_waypoint = util.goal_pose('map', point, quaternion)
print "self.box_waypoint", self.box_waypoint
(trans, rots) = self.listener.lookupTransform(
'/map', '/ar_marker_' + str(self.goal_tag_id),
rospy.Time(0))
point = Point(trans[0], trans[1], 0.010)
# quaternion = Quaternion(rots[0],rots[1],rots[2],rots[3])
quaternion = Quaternion(0, 0, 0.118, 0.993)
self.goal_waypoint = util.goal_pose('map', point, quaternion)
print "self.goal_waypoint", self.goal_waypoint
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
finally:
if self.box_waypoint != None and self.goal_waypoint != None:
break
print "Try to get waypoints:", self.box_waypoint, self.goal_waypoint
print get_cloest_stall(self.goal_waypoint.target_pose.pose.position)
assert self.box_waypoint != None and self.goal_waypoint != None
self.box_stall_id = get_cloest_stall(
self.box_waypoint.target_pose.pose.position)
self.goal_stall_id = get_cloest_stall(
self.goal_waypoint.target_pose.pose.position)
print self.box_stall_id, self.goal_stall_id
assert self.box_stall_id != self.goal_stall_id
box_is_left = True
if int(self.box_stall_id) > int(self.goal_stall_id):
box_is_left = False
square_dist = 0.825
if box_is_left:
point = PARK_SPOT_WAYPOINTS[str(int(self.box_stall_id) - 1)][0]
quaternion = PARK_SPOT_WAYPOINTS[str(int(self.box_stall_id) -
1)][1]
goal_pose = util.goal_pose('map', point, quaternion)
self.client.send_goal(goal_pose)
self.client.wait_for_result()
util.signal(1, onColor=Led.BLACK) #debug
util.rotate(-87)
push_dist = (abs(int(self.box_stall_id) - int(self.goal_stall_id))
+ 1) * square_dist - 0.4
util.move(push_dist)
util.signal(2, onColor=Led.GREEN)
util.rotate(87)
else:
point = PARK_SPOT_WAYPOINTS[str(int(self.box_stall_id) + 1)][0]
quaternion = PARK_SPOT_WAYPOINTS[str(int(self.box_stall_id) +
1)][1]
goal_pose = util.goal_pose('map', point, quaternion)
self.client.send_goal(goal_pose)
self.client.wait_for_result()
util.signal(1, onColor=Led.BLACK) #debug
util.rotate(87)
push_dist = abs(
int(self.box_stall_id) - int(self.goal_stall_id) +
1) * square_dist
util.move(push_dist)
util.signal(2, onColor=Led.GREEN)
util.rotate(-87)
ar_tag_sub.unregister()
return 'completed'
from array import array
from multiprocessing import shared_memory
from util import wait, signal
mem = shared_memory.SharedMemory(name='prod_con_buffer')
buff = mem.buf.cast('i')
print(mem.name)
try:
while True:
wait(buff, 2)
wait(buff, 1)
print("{0:-^50}".format("CONSUMER"))
print("shared variable read: %d" % buff[0])
signal(buff, 1)
except KeyboardInterrupt:
pass
del buff
mem.close()
def go_to_box_sides(self, position):
br = tf.TransformBroadcaster()
listener = tf.TransformListener()
side_offset_from_middle = 0.5
middle_offset_from_relative= (0, 0, -0.167)
relative_rotation = (0, 0, 0, 1)
# point (x+=>x-, y+=>z-, z+=>y-)
sides = {"box_front":[(0, 0, side_offset_from_middle ), (0, 0.707, 0, 0.707)],
"box_left": [(-side_offset_from_middle, 0, 0), (0, 0, 0, 1)],
"box_right":[(side_offset_from_middle, 0, 0), (0, 0, 1, 0)]}
# sides = {"box_front":[(0, 0, side_offset_from_middle), (0, 0, 0, 1)],
# "box_left": [(0, -side_offset_from_middle, 0), (0, 0, 0.707, 0.707)],
# "box_right":[(0, side_offset_from_middle, 0), (0, 0, -0.707, 0.707)]}
assert position in sides
tmp_time = time.time()
while True or (time.time() - tmp_time) > 100:
br.sendTransform(
middle_offset_from_relative,
relative_rotation,
rospy.Time.now(),
"box_middle",
"ar_marker_6",
)
#position = "box_left"
br.sendTransform(
sides[position][0],
sides[position][1],
rospy.Time.now(),
position,
"box_middle",
)
# position = "box_right"
# br.sendTransform(
# # front
# sides[position][0],
# sides[position][1],
# rospy.Time.now(),
# position,
# "box_middle",
# )
# position = "box_front"
# br.sendTransform(
# # front
# sides[position][0],
# sides[position][1],
# rospy.Time.now(),
# position,
# "box_middle",
# )
rospy.sleep(0.2)
try:
(trans,rot) = listener.lookupTransform('/map', position, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print("look up exceptions")
continue
else:
# point = Point(trans[0], trans[1], trans[2])
# quaternion = Quaternion(rot[0],rot[1],rot[2],rot[3])
# quaternion = Quaternion(0, 0, 0.707, 0.707)
point = Point(0, 0, 0)
quaternion = Quaternion(0, 0, 0, 1)
goal_pose = util.goal_pose(position, point, quaternion)
self.client.send_goal(goal_pose)
self.client.wait_for_result()
util.signal(1)
break
