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 go_to_side(self, side):
tmp_time = time.time()
while True:
self.br.sendTransform(
BOX_MIDDLE_OFFSET_FROM_TAG,
BOX_MIDDLE_ROTATION_FROM_TAG,
rospy.Time.now(),
"box_middle",
"ar_marker_" + str(BOX_TAG_ID),
)
self.br.sendTransform(
BOX_SIDES[side][0],
BOX_SIDES[side][1],
rospy.Time.now(),
side,
"box_middle",
)
rospy.sleep(0.2)
try:
_, _ = self.listener.lookupTransform('/map', side, rospy.Time(0))
goal_pose = util.goal_pose(side)
self.client.send_goal(goal_pose)
print "waiting for result ", goal_pose.target_pose.header.frame_id
self.client.wait_for_result()
return True
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print "TF look up exception when look up goal tag"
if time.time() - tmp_time > 15:
return False
else:
return True
def execute(self, userdata):
if rospy.is_shutdown():
return 'end'
else:
process = userdata.Park_in_process
if process['spot_id'] == 1:
self.set_init_map_pose()
if process['spot_id'] > 8:
return 'return'
waypoint = PARK_SPOT_WAYPOINTS[str(process['spot_id'])]
goal = util.goal_pose(waypoint, 'map', 'list')
self.client.send_goal(goal)
self.client.wait_for_result()
#util.signal(process['spot_id'], onColor=Led.BLACK) #debug
search_orientation = [0]
if process['spot_id'] == 1:
search_orientation.append(90)
elif process['spot_id'] == 5:
search_orientation.append(-90)
process = self.search(process, search_orientation)
process['spot_id'] += 1
if process['ARtag_found'] and process['contour_found'][
1] and process['unmarked_spot_id'][1]:
return 'return'
return 'next'
def execute(self, userdata):
if rospy.is_shutdown():
return 'end'
else:
goal_pose = util.goal_pose('map', ON_RAMP_WAYPOINT[0], ON_RAMP_WAYPOINT[1])
self.client.send_goal(goal_pose)
print "waiting for result ", goal_pose.target_pose.header.frame_id
self.client.wait_for_result()
return 'returned'
def execute(self, userdata):
if rospy.is_shutdown():
return 'end'
else:
goal = util.goal_pose(ON_RAMP_WAYPOINT, frame_id='map')
self.client.send_goal(goal)
self.client.wait_for_result()
self.move_forward(0.2)
return 'returned'
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'
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