def circle_dock_bays(self):
'''
Circle the dock until the bays are identified by the perception service
'''
done_circle = False
@txros.util.cancellableInlineCallbacks
def circle_bay():
yield self.navigator.move.look_at(self.dock_pose).set_position(self.dock_pose)\
.backward(self.CIRCLE_RADIUS).look_at(self.dock_pose).go()
yield self.navigator.move.circle_point(self.dock_pose, direction='ccw').go()
done_circle = True # noqa flake8 doesn't see that it is defined above
print_good("Circling dock to get bays")
circle_bay()
start_time = self.navigator.nh.get_time()
while self.navigator.nh.get_time() - start_time < genpy.Duration(self.BAY_SEARCH_TIMEOUT) and not done_circle:
res = yield self.call_get_bays()
if not res.success:
yield self.navigator.nh.sleep(0.1)
continue
self.bay_poses = (mil_tools.rosmsg_to_numpy(res.bays[0]),
mil_tools.rosmsg_to_numpy(res.bays[1]),
mil_tools.rosmsg_to_numpy(res.bays[2]))
self.bay_normal = mil_tools.rosmsg_to_numpy(res.normal)
print_good("Got GetDockShapes response")
return
raise Exception("Bays not found after circling or timed out")
def get_colored_buoy(self, color):
"""
Returns the closest colored buoy with the specified color
"""
buoy_field = yield self.database_query("BuoyField")
buoy_field_point = mil_tools.rosmsg_to_numpy(
buoy_field.objects[0].position)
def _dist_from_bf(pt):
return np.linalg.norm(buoy_field_point - pt)
totems = yield self.database_query("totem")
correct_colored = [
totem for totem in totems.objects if np.all(
np.round(
mil_tools.rosmsg_to_numpy(totem.color,
keys=['r', 'g', 'b'])) == color)
]
if len(correct_colored) == 0:
closest = None
else:
closest = sorted(correct_colored,
key=lambda totem: _dist_from_bf(
mil_tools.rosmsg_to_numpy(totem.position)))[0]
defer.returnValue(closest)
def get_dock_search_markers(self):
left_res = yield self.navigator.database_query("DockEdgeLeft")
left_pose = mil_tools.rosmsg_to_numpy(left_res.objects[0].position)
right_res = yield self.navigator.database_query("DockEdgeRight")
right_pose = mil_tools.rosmsg_to_numpy(right_res.objects[0].position)
search_line_vector = right_pose - left_pose
search_line_vector_normal = search_line_vector / np.linalg.norm(search_line_vector)
if np.isnan(search_line_vector_normal[0]):
raise Exception("Gate Edge Markers are not in a line. Perhaps they were not placed")
rot_right = np.array([[0, 1, 0], [-1, 0, 0], [0, 0, 0]])
search_line_rot = rot_right.dot(search_line_vector_normal)
left_search = left_pose + search_line_rot * self.MARKER_DISTANCE
right_search = right_pose + search_line_rot * self.MARKER_DISTANCE
move_left = self.navigator.move.set_position(left_search).look_at(left_pose)
move_right = self.navigator.move.set_position(right_search).look_at(right_pose)
pose = self.navigator.pose[0][:2]
distance_test = np.array([np.linalg.norm(pose - move_left.position[:2]),
np.linalg.norm(pose - move_right.position[:2])])
if np.argmin(distance_test) == 0:
self.search_moves = (move_left, move_right)
else:
self.search_moves = (move_right, move_left)
def get_closest_object(navigator):
pose = yield navigator.tx_pose
buoy_field = yield navigator.database_query("BuoyField")
buoy_field_np = mil_tools.rosmsg_to_numpy(buoy_field.objects[0].position)
# Find which totems we haven't explored yet
totems = yield navigator.database_query("all", raise_exception=False)
if not totems.found:
# Need to search for more totems
defer.returnValue([None, explored_ids])
u_totems = [totem for totem in totems.objects if totem.id not in explored_ids]
u_totems_np = map(lambda totem: mil_tools.rosmsg_to_numpy(totem.position), u_totems)
if len(u_totems_np) == 0:
defer.returnValue([None, explored_ids])
# Find the closest buoys, favoring the ones closest to the boat.
# J = wa * ca ** 2 + wb * c2 ** 2
# a := boat
# b := buoy field ROI
wa = .6
wb = .4
ca = np.linalg.norm(u_totems_np - pose[0], axis=1)
cb = np.linalg.norm(u_totems_np - buoy_field_np, axis=1)
J = wa * ca + wb * cb
target_totem = u_totems[np.argmin(J)]
explored_ids.append(target_totem.id)
defer.returnValue([target_totem, explored_ids])
def from_vrx_urdf(cls, urdf_string):
urdf = URDF.from_xml_string(urdf_string)
buff = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(buff) # noqa
names = []
positions = []
angles = []
for link in urdf.links:
find = link.name.find('_propeller_link')
if find == -1:
continue
name = link.name[:find]
try:
trans = buff.lookup_transform('base_link', link.name,
rospy.Time(), rospy.Duration(10))
except tf2_ros.TransformException as e:
raise Exception(e)
translation = rosmsg_to_numpy(trans.transform.translation)
rot = rosmsg_to_numpy(trans.transform.rotation)
yaw = euler_from_quaternion(rot)[2]
names.append(name)
positions.append(translation[0:2])
angles.append(yaw)
return cls(names, positions, angles, vrx_force_to_command,
(250., -100.))
def run(self, parameters):
# middle_point = np.array([-10, -70, 0])
est_coral_survey = yield self.database_query("CoralSurvey")
# Going to get all the objects and using the closest one as the totem
totem = yield self.database_query("all")
# Get the closest totem object to the boat
totems_np = map(lambda obj: mil_tools.rosmsg_to_numpy(obj.position), totem.objects)
dist = map(lambda totem_np: np.linalg.norm(
totem_np - mil_tools.rosmsg_to_numpy(est_coral_survey.objects[0].position)), totems_np)
middle_point = totems_np[np.argmin(dist)]
print "Totem sorted:", totems_np
print "Totem selected: ", totems_np[0]
quad = yield self.mission_params["acoustic_pinger_active_index_correct"].get()
waypoint_from_center = np.array([10 * np.sqrt(2)])
# Publish ogrid with boundaries to stay inside
ogrid = OgridFactory(middle_point, draw_borders=True)
msg = ogrid.get_message()
latched = self.latching_publisher("/mission_ogrid", OccupancyGrid, msg)
# Construct waypoint list along NSEW directions then rotate 45 degrees to get a good spot to go to.
directions = [EAST, NORTH, WEST, SOUTH]
# for quad in quads_to_search:
mid = self.move.set_position(middle_point).set_orientation(directions[quad - 1])
search_center = mid.yaw_left(45, "deg").forward(waypoint_from_center).set_orientation(NORTH)
yield search_center.left(6).go()
yield self.move.circle_point(search_center.position, direction='cw').go()
yield self.mission_params["coral_survey_shape1"].set(shapes[0])
latched.cancel()
defer.returnValue(None)
def get_dock_search_markers(self):
left_res = yield self.navigator.database_query("DockEdgeLeft")
left_pose = mil_tools.rosmsg_to_numpy(left_res.objects[0].position)
right_res = yield self.navigator.database_query("DockEdgeRight")
right_pose = mil_tools.rosmsg_to_numpy(right_res.objects[0].position)
search_line_vector = right_pose - left_pose
search_line_vector_normal = search_line_vector / np.linalg.norm(search_line_vector)
if np.isnan(search_line_vector_normal[0]):
raise Exception("Gate Edge Markers are not in a line. Perhaps they were not placed")
rot_right = np.array([[0, 1, 0], [-1, 0, 0], [0,0,0]])
search_line_rot = rot_right.dot(search_line_vector_normal)
left_search = left_pose + search_line_rot * self.MARKER_DISTANCE
right_search = right_pose + search_line_rot * self.MARKER_DISTANCE
move_left = self.navigator.move.set_position(left_search).look_at(left_pose)
move_right = self.navigator.move.set_position(right_search).look_at(right_pose)
pose = self.navigator.pose[0][:2]
distance_test = np.array([np.linalg.norm(pose - move_left.position[:2]),
np.linalg.norm(pose - move_right.position[:2])])
if np.argmin(distance_test) == 0:
self.search_moves = (move_left, move_right)
else:
self.search_moves = (move_right, move_left)
def circle_dock_bays(self):
'''
Circle the dock until the bays are identified by the perception service
'''
done_circle = False
@txros.util.cancellableInlineCallbacks
def circle_bay():
yield self.navigator.move.look_at(self.dock_pose).set_position(self.dock_pose).backward(self.CIRCLE_RADIUS).look_at(self.dock_pose).go()
yield self.navigator.move.circle_point(self.dock_pose, direction='ccw').go()
done_circle = True
print_good("Circling dock to get bays")
circle_bay()
start_time = self.navigator.nh.get_time()
while self.navigator.nh.get_time() - start_time < genpy.Duration(self.BAY_SEARCH_TIMEOUT) and not done_circle:
res = yield self.call_get_bays()
if not res.success:
yield self.navigator.nh.sleep(0.1)
continue
self.bay_poses = (mil_tools.rosmsg_to_numpy(res.bays[0]),
mil_tools.rosmsg_to_numpy(res.bays[1]),
mil_tools.rosmsg_to_numpy(res.bays[2]))
self.bay_normal = mil_tools.rosmsg_to_numpy(res.normal)
print_good("Got GetDockShapes response")
return
raise Exception("Bays not found after circling or timed out")
def get_target_pt(self, center_frame):
msg = CameraToLidarTransformRequest()
msg.header.stamp = center_frame[1].header.stamp
msg.header.frame_id = center_frame[1].header.frame_id
msg.point = Point(x=center_frame[0][0], y=center_frame[0][1], z=0.0)
msg.tolerance = 500
pose_offset = yield self.camera_lidar_tf(msg)
cam_to_enu = yield self.tf_listener.get_transform(
'enu', center_frame[1].header.frame_id)
normal = rosmsg_to_numpy(pose_offset.normal)
normal = cam_to_enu.transform_vector(normal)
normal = normal[0:2] / np.linalg.norm(normal[0:2])
normal = np.append(normal, [0])
found_pt = rosmsg_to_numpy(pose_offset.closest)
found_pt = cam_to_enu.transform_point(found_pt)
found_pt[2] = 0
# Extend out by normal multiplier
normal *= 3
found_pt_1 = found_pt + normal
found_pt_2 = found_pt + -1 * normal
# Which is closer
boat_pos = (yield self.tx_pose)[0]
if np.linalg.norm(found_pt_1 - boat_pos) > np.linalg.norm(found_pt_2 -
boat_pos):
found_pt = found_pt_2
else:
found_pt = found_pt_1
defer.returnValue(found_pt)
def get_closest_totem(self, explored_ids):
pose = yield self.tx_pose
buoy_field = yield self.database_query("BuoyField")
buoy_field_np = mil_tools.rosmsg_to_numpy(
buoy_field.objects[0].position)
# Find which totems we haven't explored yet
totems = yield self.database_query("totem", raise_exception=False)
if not totems.found:
# Need to search for more totems
defer.returnValue([None, explored_ids])
u_totems = [
totem for totem in totems.objects if totem.id not in explored_ids
]
u_totems_np = map(
lambda totem: mil_tools.rosmsg_to_numpy(totem.position), u_totems)
if len(u_totems_np) == 0:
defer.returnValue([None, explored_ids])
# Find the closest buoys, favoring the ones closest to the boat.
# J = wa * ca ** 2 + wb * c2 ** 2
# a := boat
# b := buoy field ROI
wa = .7
wb = .3
ca = np.linalg.norm(u_totems_np - pose[0], axis=1)
cb = np.linalg.norm(u_totems_np - buoy_field_np, axis=1)
J = wa * ca + wb * cb
target_totem = u_totems[np.argmin(J)]
explored_ids.append(target_totem.id)
defer.returnValue([target_totem, explored_ids])
def get_bouy_go_round_target(self):
return_array = []
#gets the xy and state of the scan the code from the database
scan_the_code = np.array([])
res = yield self.database_query('stc_platform')
#makes sure that only 1 scan the code exists
assert len(res.objects) == 1
#raises error if the scan the code platform is nto
if not res.found:
raise TaskException(query + ' not found in object database')
point = rosmsg_to_numpy(res.objects[0].pose.position)[:2]
#runs the function that retrives/runs the scan the code state True for circle scan
#the code, False for circle the black totem
scan_the_code = point
return_array.append(scan_the_code)
#print scan_the_code
#this portion of the method gets the location of the nearest black totem
#gets all of the black totems from the database
num_of_black_totems = 1
black_totems = yield self.database_query('totem_black')
black_totems_poses = []
for i in black_totems.objects:
point = rosmsg_to_numpy(i.pose.position)[:2]
black_totems_poses.append(point)
#the follwing determins which is the closest
#i wish python had a do while loop
closest = black_totems_poses[0]
dist = ((black_totems_poses[0][0] - self.pose[0][0])**2) + (
(black_totems_poses[0][1] - self.pose[0][1])**2)
j = 0 #an index for populating the dist_temp array
while j < len(black_totems_poses):
dist_temp = ((black_totems_poses[j][0] - self.pose[0][0])**2) + (
(black_totems_poses[j][1] - self.pose[0][1])**2)
if dist_temp < dist:
dist = dist_temp
closest = black_totems[j]
j += 1
#closest now has the position of the closest black totem
#closest is a np array
return_array.append(closest)
#returnValue has the scan the code and closest black totem location
defer.returnValue(return_array)
def get_center_frame(self):
msgf = yield self.bboxsub.get_next_message()
msg = msgf.objects[0]
# print msg
c1 = rosmsg_to_numpy(msg.points[0])
c2 = rosmsg_to_numpy(msg.points[1])
tmp = (((c1 + c2) / 2.0), msgf, msg.name)
defer.returnValue(tmp)
def get_objects(self):
"""Get position of 3 gates from database"""
gate_1 = yield self.navigator.database_query("Gate_1")
assert gate_1.found, "Gate 1 Not found"
gate_1_pos = mil_tools.rosmsg_to_numpy(gate_1.objects[0].position)[:2]
gate_2 = yield self.navigator.database_query("Gate_2")
assert gate_2.found, "Gate 2 Not found"
gate_2_pos = mil_tools.rosmsg_to_numpy(gate_2.objects[0].position)[:2]
gate_3 = yield self.navigator.database_query("Gate_3")
assert gate_3.found, "Gate 3 Not found"
gate_3_pos = mil_tools.rosmsg_to_numpy(gate_3.objects[0].position)[:2]
self.gate_poses = np.array([gate_1_pos, gate_2_pos, gate_3_pos])
def go_through_next_two_buoys(self):
buoys = yield self.get_two_closest_cones(TwoClosestConesRequest())
self.task_done = buoys.no_more_buoys
print(buoys)
if self.task_done:
return
pos1 = rosmsg_to_numpy(buoys.object1)
pos2 = rosmsg_to_numpy(buoys.object2)
gate = self.get_gate(pos1, pos2, (yield self.tx_pose)[0])
yield self.go_thru_gate(gate)
def get_objects(self):
"""Get position of 3 gates from database"""
gate_1 = yield self.database_query("Gate_1")
assert gate_1.found, "Gate 1 Not found"
gate_1_pos = mil_tools.rosmsg_to_numpy(gate_1.objects[0].position)[:2]
gate_2 = yield self.database_query("Gate_2")
assert gate_2.found, "Gate 2 Not found"
gate_2_pos = mil_tools.rosmsg_to_numpy(gate_2.objects[0].position)[:2]
gate_3 = yield self.database_query("Gate_3")
assert gate_3.found, "Gate 3 Not found"
gate_3_pos = mil_tools.rosmsg_to_numpy(gate_3.objects[0].position)[:2]
self.gate_poses = np.array([gate_1_pos, gate_2_pos, gate_3_pos])
def get_closest_objects(position, objects, max_len=3, max_dist=30):
num = len(objects)
idx = max_len
if num < max_len:
idx = num
objects = sorted(objects, key=lambda x: np.linalg.norm(position - rosmsg_to_numpy(x.position)))
objects = objects[:idx]
dists = map(lambda x: np.linalg.norm(position - rosmsg_to_numpy(x.position)), objects)
final_objs = []
for i, d in enumerate(dists):
if d < max_dist:
final_objs.append(objects[i])
else:
break
return final_objs
def move_cb(self, msg):
fprint("Move request received!", msg_color="blue")
if msg.move_type not in ['hold', 'drive', 'skid', 'circle']:
fprint("Move type '{}' not found".format(msg.move_type),
msg_color='red')
self.move_server.set_aborted(MoveResult('move_type'))
return
self.blind = msg.blind
p = PoseStamped()
p.header = make_header(frame="enu")
p.pose = msg.goal
self.goal_pose_pub.publish(p)
# Sleep before you continue
rospy.sleep(1)
yaw = trns.euler_from_quaternion(rosmsg_to_numpy(
msg.goal.orientation))[2]
if not self.is_feasible(
np.array([msg.goal.position.x, msg.goal.position.y, yaw]),
np.zeros(3)):
fprint("Not feasible", msg_color='red')
self.move_server.set_aborted(MoveResult('occupied'))
return
fprint("Finished move!", newline=2)
self.move_server.set_succeeded(MoveResult(''))
def circle_search(self):
platform_np = mil_tools.rosmsg_to_numpy(
self.waypoint_res.objects[0].position)
yield self.move.look_at(platform_np).set_position(platform_np).backward(self.circle_radius)\
.yaw_left(90, unit='deg').go(move_type="drive")
done_circle = False
@txros.util.cancellableInlineCallbacks
def do_circle():
yield self.move.circle_point(platform_np,
direction=self.circle_direction).go()
done_circle = True # noqa flake8 cant see that it is defined above
do_circle()
while not done_circle:
res = yield self.get_any_shape()
if res is False:
yield self.nh.sleep(0.25)
continue
fprint(
"Shape ({}found, using normal to look at other 3 shapes if needed"
.format(res[0]),
title="DETECT DELIVER",
msg_color="green")
# circle_defer.cancel()
shape_color, found_shape_pose = res
if self.correct_shape(shape_color):
self.shape_pose = found_shape_pose
return
# Pick other 3 to look at
rot_right = np.array([[0, -1], [1, 0]])
(shape_point, shape_normal) = found_shape_pose
rotated_norm = np.append(rot_right.dot(shape_normal[:2]), 0)
center_point = shape_point - shape_normal * (self.platform_radius /
2.0)
point_opposite_side = center_point - shape_normal * self.circle_radius
move_opposite_side = self.move.set_position(
point_opposite_side).look_at(center_point).yaw_left(90,
unit='deg')
left_or_whatever_point = center_point + rotated_norm * self.circle_radius
move_left_or_whatever = self.move.set_position(
left_or_whatever_point).look_at(center_point).yaw_left(
90, unit='deg')
right_or_whatever_point = center_point - rotated_norm * self.circle_radius
move_right_or_whatever = self.move.set_position(
right_or_whatever_point).look_at(center_point).yaw_left(
90, unit='deg')
yield self.search_sides(
(move_right_or_whatever, move_opposite_side,
move_left_or_whatever))
return
fprint("No shape found after complete circle",
title="DETECT DELIVER",
msg_color='red')
raise Exception("No shape found on platform")
def move_cb(self, msg):
fprint("Move request received!", msg_color="blue")
if msg.move_type not in ['hold', 'drive', 'skid', 'circle']:
fprint("Move type '{}' not found".format(msg.move_type), msg_color='red')
self.move_server.set_aborted(MoveResult('move_type'))
return
self.blind = msg.blind
p = PoseStamped()
p.header = make_header(frame="enu")
p.pose = msg.goal
self.goal_pose_pub.publish(p)
# Sleep before you continue
rospy.sleep(1)
yaw = trns.euler_from_quaternion(rosmsg_to_numpy(msg.goal.orientation))[2]
if not self.is_feasible(np.array([msg.goal.position.x, msg.goal.position.y, yaw]), np.zeros(3)):
fprint("Not feasible", msg_color='red')
self.move_server.set_aborted(MoveResult('occupied'))
return
fprint("Finished move!", newline=2)
self.move_server.set_succeeded(MoveResult(''))
def run(self, parameters):
# Go to autonomous mode
yield self.change_wrench('autonomous')
if not parameters.pcodar:
self.send_feedback('Please click between the end tower of the navigation pass.')
target_point = yield self.rviz_point.get_next_message()
target_point = rosmsg_to_numpy(target_point.point)
us = (yield self.tx_pose)[0]
distance = np.linalg.norm(target_point - us) + self.END_MARGIN_METERS
distance_per_move = distance / parameters.num_moves
for i in range(parameters.num_moves):
self.send_feedback("Doing move {}/{}".format(i + 1, parameters.num_moves))
yield self.move.look_at(target_point).forward(distance_per_move).go(blind=True)
defer.returnValue(True)
else:
_, closest_reds = yield self.get_sorted_objects("totem_red", 2)
_, closest_greens = yield self.get_sorted_objects("totem_green", 2)
# Rename the totems for their symantic name
green_close = closest_greens[0]
green_far = closest_greens[1]
red_close = closest_reds[0]
red_far = closest_reds[1]
# Get the two center points between gata markers
begin_midpoint = (green_close + red_close) / 2.0
end_midpoint = (green_far + red_far) / 2.0
# Start a little behind the entrance
yield self.move.set_position(begin_midpoint).look_at(end_midpoint).backward(self.START_MARGIN_METERS).go()
# Then move a little passed the exit
yield self.move.look_at(end_midpoint).set_position(end_midpoint)\
.forward(self.END_MARGIN_METERS).go(blind=True)
defer.returnValue(True)
def get_sorted_objects(self, name, n=-1, throw=True, **kwargs):
'''
Get the closest N objects with a particular name from the PCODAR database
@param name: the name of the object
@param n: the number of objects to get, if -1, get all of them
@param throw: If true, raise exception if not enough objects present
@param **kwargs: other kwargs to give to database_query
@return tuple([sorted_object_messages, [object_positions]) of sorted object messages
and their positions as a Nx3 numpy array.
'''
objects = (yield self.database_query(object_name=name,
**kwargs)).objects
if n != -1 and len(objects) < n:
if throw:
raise Exception('Could not get {} {} objects'.format(n, name))
else:
n = len(objects)
if n == 0:
defer.returnValue(None)
positions = np.empty((len(objects), 3))
for i, obj in enumerate(objects):
positions[i, :] = mil_tools.rosmsg_to_numpy(obj.pose.position)
nav_pose = (yield self.tx_pose)[0]
distances = np.linalg.norm(positions - nav_pose, axis=1)
distances_argsort = np.argsort(distances)
if n != -1:
distances_argsort = distances_argsort[:n]
objects_sorted = [objects[i] for i in distances_argsort]
defer.returnValue((objects_sorted, positions[distances_argsort, :]))
def go_to_objects(navigator, position, objs):
objects = get_closest_objects(position, objs)
for o in objects:
fprint("MOVING TO OBJECT WITH ID {}".format(o.id), msg_color="green")
yield navigator.nh.sleep(5)
pos = nt.rosmsg_to_numpy(o.position)
yield navigator.move.look_at(pos).set_position(pos).backward(7).go()
def go_to_objects(navigator, position, objs):
objects = get_closest_objects(position, objs)
for o in objects:
fprint("MOVING TO OBJECT WITH ID {}".format(o.id), msg_color="green")
yield navigator.nh.sleep(5)
pos = nt.rosmsg_to_numpy(o.position)
yield navigator.move.look_at(pos).set_position(pos).backward(7).go()
def find_stc(self):
pose = None
# see if we already got scan the code tower
try:
_, poses = yield self.get_sorted_objects(name='stc_platform', n=1)
pose = poses[0]
# incase stc platform not already identified
except Exception as e:
# get all pcodar objects
try:
_, poses = yield self.get_sorted_objects(name='UNKNOWN', n=-1)
# if no pcodar objects, drive forward
except Exception as e:
yield self.move.forward(50).go()
# get all pcodar objects
_, poses = yield self.get_sorted_objects(name='UNKNOWN', n=-1)
# if still no pcodar objects, guess RGB and exit mission
# go to nearest obj to get better data on that obj
print 'going to nearest object'
yield self.move.set_position(poses[0]).go()
# get data on closest obj
msgs, poses = yield self.get_sorted_objects(name='UNKNOWN', n=1)
if np.linalg.norm(rosmsg_to_numpy(msgs[0].scale)) > 6.64:
# much bigger than scale of stc
# then we found the dock
yield self.pcodar_label(msgs[0].id, 'dock')
# get other things
msgs, poses = yield self.get_sorted_objects(name='UNKNOWN', n=1)
# if no other things, throw error and exit mission
yield self.pcodar_label(msgs[0].id, 'stc_platform')
pose = poses[0]
else: # if about same size as stc, lable it stc
yield self.pcodar_label(msgs[0].id, 'stc_platform')
pose = poses[0]
defer.returnValue(pose)
def check_color(totem, color_map):
for name, color in color_map:
if mil_tools.rosmsg_to_numpy(totem.color, keys=['r', 'g',
'b']) == color:
return name
return DEFAULT_COLOR
def run(self, args):
# Get position of 3 gates based on position of totems
gates = yield self.get_gates()
# Get heading towards pinger from Andy hydrophone system
self.send_feedback(
'All gates clicked on! Waiting for pinger heading...')
heading = yield self.pinger_heading.get_next_message()
self.send_feedback('Recieved pinger heading')
# Convert heading and hydophones from to enu
hydrophones_to_enu = yield self.tf_listener.get_transform(
'enu', heading.header.frame_id)
hydrophones_origin = hydrophones_to_enu._p[0:2]
heading = rosmsg_to_numpy(heading.vector)
heading_enu = hydrophones_to_enu.transform_vector(heading)
heading_enu = heading_enu[0:2] / np.linalg.norm(heading_enu[0:2])
pinger_line = self.line(hydrophones_origin,
hydrophones_origin + heading_enu)
gates_line = self.line(gates[0], gates[-1])
# Find intersection of these two lines. This is the approximate position of the pinger
intersection = self.intersection(pinger_line, gates_line)
if intersection is None:
raise Exception('No intersection')
self.send_feedback('Pinger is roughly at {}'.format(intersection))
distances = []
for gate in gates:
distances.append(np.linalg.norm(gate[0:2] - intersection))
argmin = np.argmin(np.array(distances))
self.send_feedback('Pinger is likely at gate {}'.format(argmin + 1))
gate = gates[argmin][:2]
between_vector = (gates[0] - gates[-1])[:2]
# Rotate that vector to point through the buoys
c = np.cos(np.radians(90))
s = np.sin(np.radians(90))
R = np.array([[c, -s], [s, c]])
direction_vector = R.dot(between_vector)
direction_vector /= np.linalg.norm(direction_vector)
position = self.pose[0][:2]
if np.linalg.norm(position -
(gate + direction_vector)) > np.linalg.norm(
position - (gate - direction_vector)):
direction_vector = -direction_vector
before_distance = 3.0
after_distance = 5.0
before = np.append(gate + direction_vector * before_distance, 0)
after = np.append(gate - direction_vector * after_distance, 0)
self.send_feedback('Moving in front of gate')
yield self.move.set_position(before).look_at(after).go()
self.send_feedback('Going through')
yield self.move.set_position(after).go()
defer.returnValue('My god it actually worked!')
def from_urdf(cls, urdf_string, transmission_suffix='_thruster_transmission'):
'''
Load from an URDF string. Expects each thruster to be connected a transmission ending in the specified suffix.
A transform between the propeller joint and base_link must be available
'''
urdf = URDF.from_xml_string(urdf_string)
buff = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(buff) # noqa
names = []
joints = []
positions = []
angles = []
limit = -1
ratio = -1
for transmission in urdf.transmissions:
find = transmission.name.find(transmission_suffix)
if find != -1 and find + len(transmission_suffix) == len(transmission.name):
if len(transmission.joints) != 1:
raise Exception('Transmission {} does not have 1 joint'.format(transmission.name))
if len(transmission.actuators) != 1:
raise Exception('Transmission {} does not have 1 actuator'.format(transmission.name))
t_ratio = transmission.actuators[0].mechanicalReduction
if ratio != -1 and ratio != t_ratio:
raise Exception('Transmission {} has a different reduction ratio (not supported)'.format(t_ratio))
ratio = t_ratio
joint = None
for t_joint in urdf.joints:
if t_joint.name == transmission.joints[0].name:
joint = t_joint
if joint is None:
rospy.logerr('Transmission joint {} not found'.format(transmission.joints[0].name))
try:
trans = buff.lookup_transform('base_link', joint.child, rospy.Time(), rospy.Duration(10))
except tf2_ros.TransformException as e:
raise Exception(e)
translation = rosmsg_to_numpy(trans.transform.translation)
rot = rosmsg_to_numpy(trans.transform.rotation)
yaw = euler_from_quaternion(rot)[2]
names.append(transmission.name[0:find])
positions.append(translation[0:2])
angles.append(yaw)
joints.append(joint.name)
if limit != -1 and joint.limit.effort != limit:
raise Exception('Thruster {} had a different limit, cannot proceed'.format(joint.name))
limit = joint.limit.effort
return cls(names, positions, angles, ratio, limit, joints=joints)
def get_colored_buoy(navigator, color):
"""
Returns the closest colored buoy with the specified color
"""
buoy_field = yield navigator.database_query("BuoyField")
buoy_field_point = mil_tools.rosmsg_to_numpy(buoy_field.objects[0].position)
_dist_from_bf = lambda pt: np.linalg.norm(buoy_field_point - pt)
totems = yield navigator.database_query("totem")
correct_colored = [totem for totem in totems.objects if np.all(np.round(mil_tools.rosmsg_to_numpy(totem.color, keys=['r', 'g', 'b'])) == color)]
if len(correct_colored) == 0:
closest = None
else:
closest = sorted(correct_colored, key=lambda totem: _dist_from_bf(mil_tools.rosmsg_to_numpy(totem.position)))[0]
defer.returnValue(closest)
def myfunc(navigator, looking_for, center_marker):
# Updated
high_prob_objs = ["shooter", "dock"]
pos = yield navigator.tx_pose
pos = pos[0]
if center_marker is None or center_marker == "None":
defer.returnValue(True)
try:
center_marker = yield navigator.database_query(
object_name=center_marker.name)
center_marker = center_marker.objects[0]
print center_marker.name
except:
fprint("A marker has not been set", msg_color="red")
defer.returnValue(False)
mark_pos = nt.rosmsg_to_numpy(center_marker.position)
dist = np.linalg.norm(pos - mark_pos)
if dist > 10:
yield navigator.move.look_at(mark_pos).set_position(mark_pos).go()
defer.returnValue(True)
if looking_for is None or looking_for == "None":
defer.returnValue(True)
pos = yield navigator.tx_pose
pos = pos[0]
if looking_for in high_prob_objs:
try:
obj = yield navigator.database_query(object_name=looking_for)
fprint("EXPLORER FOUND OBJECT", msg_color="blue")
yield navigator.database_query(
cmd="lock {} {}".format(obj.id, looking_for))
defer.returnValue(True)
except MissingPerceptionObject:
fprint("The object {} is not in the database".format(looking_for),
msg_color="red")
try:
objs = yield navigator.database_query(object_name="all")
objs = get_closest_objects(pos, objs.objects)
except Exception as e:
print e
defer.returnValue(False)
# for o in objs:
# obj_pos = nt.rosmsg_to_numpy(o.position)
# yield navigator.move.look_at(obj_pos).set_position(mark_pos).backward(7).go()
# cam_obj = yield navigator.camera_database_query(object_name=looking_for, id=o.id)
# if cam_obj.found:
# yield navigator.database_query(cmd="lock {} {}".format(o.id, looking_for))
# fprint("EXPLORER FOUND OBJECT", msg_color="blue")
# defer.returnValue(True)
fprint("NO OBJECT FOUND", msg_color="blue")
defer.returnValue(False)
def find_closest_object_given_ray(self, objects, ray, ray_base, tol = 3):
# Compare a given bounding box with objects found by LIDAR. Find the closest object if any.
for o in objects:
distance = np.linalg.norm(np.cross(ray, mil_tools.rosmsg_to_numpy(o.pose.position) - ray_base))
if distance > tol:
continue
else:
return o
return None
def main(navigator, **kwargs):
#middle_point = np.array([-10, -70, 0])
est_coral_survey = yield navigator.database_query("CoralSurvey")
# Going to get all the objects and using the closest one as the totem
totem = yield navigator.database_query("all")
# Get the closest totem object to the boat
totems_np = map(lambda obj: mil_tools.rosmsg_to_numpy(obj.position),
totem.objects)
dist = map(
lambda totem_np: np.linalg.norm(totem_np - mil_tools.rosmsg_to_numpy(
est_coral_survey.objects[0].position)), totems_np)
middle_point = totems_np[np.argmin(dist)]
print "Totem sorted:", totems_np
print "Totem selected: ", totems_np[0]
quads_to_search = [1, 2, 3, 4]
quad = yield navigator.mission_params[
"acoustic_pinger_active_index_correct"].get()
waypoint_from_center = np.array([10 * np.sqrt(2)])
# Publish ogrid with boundaries to stay inside
ogrid = OgridFactory(middle_point, draw_borders=True)
msg = ogrid.get_message()
latched = navigator.latching_publisher("/mission_ogrid", OccupancyGrid,
msg)
# Construct waypoint list along NSEW directions then rotate 45 degrees to get a good spot to go to.
directions = [EAST, NORTH, WEST, SOUTH]
waypoints = []
#for quad in quads_to_search:
mid = navigator.move.set_position(middle_point).set_orientation(
directions[quad - 1])
search_center = mid.yaw_left(
45, "deg").forward(waypoint_from_center).set_orientation(NORTH)
yield search_center.left(6).go()
yield navigator.move.circle_point(search_center.position,
direction='cw').go()
yield navigator.mission_params["coral_survey_shape1"].set(shapes[0])
latched.cancel()
defer.returnValue(None)
def _dist(self, x):
if self.position is None:
success = yield threads.deferToThread(self._wait_for_position)
if not success:
raise Exception("There is a problem with odom.")
if self.navigator is not None:
position = yield self.navigator.tx_pose
position = position[0]
self.position = position
defer.returnValue(np.linalg.norm(nt.rosmsg_to_numpy(x.position) - self.position))
def get_pinger_pose(self):
"""Query pinger perception for the location of the pinger after observing"""
res = yield self.pinger_client(FindPingerRequest())
if res.pinger_position.x == 0:
self.pinger_pose = self.gate_poses[1]
pinger_pose = res.pinger_position
self.pinger_pose = mil_tools.rosmsg_to_numpy(pinger_pose)[:2]
self.distances = np.array([np.linalg.norm(self.pinger_pose - self.gate_poses[0]),
np.linalg.norm(self.pinger_pose - self.gate_poses[1]),
np.linalg.norm(self.pinger_pose - self.gate_poses[2])])
def get_closest_objects(position, objects, max_len=3, max_dist=30):
num = len(objects)
idx = max_len
if num < max_len:
idx = num
objects = sorted(
objects,
key=lambda x: np.linalg.norm(position - rosmsg_to_numpy(x.position)))
objects = objects[:idx]
dists = map(
lambda x: np.linalg.norm(position - rosmsg_to_numpy(x.position)),
objects)
final_objs = []
for i, d in enumerate(dists):
if d < max_dist:
final_objs.append(objects[i])
else:
break
return final_objs
def myfunc(navigator, looking_for, center_marker):
# Updated
high_prob_objs = ["shooter", "dock"]
pos = yield navigator.tx_pose
pos = pos[0]
if center_marker is None or center_marker == "None":
defer.returnValue(True)
try:
center_marker = yield navigator.database_query(object_name=center_marker.name)
center_marker = center_marker.objects[0]
print center_marker.name
except:
fprint("A marker has not been set", msg_color="red")
defer.returnValue(False)
mark_pos = nt.rosmsg_to_numpy(center_marker.position)
dist = np.linalg.norm(pos - mark_pos)
if dist > 10:
yield navigator.move.look_at(mark_pos).set_position(mark_pos).go()
defer.returnValue(True)
if looking_for is None or looking_for == "None":
defer.returnValue(True)
pos = yield navigator.tx_pose
pos = pos[0]
if looking_for in high_prob_objs:
try:
obj = yield navigator.database_query(object_name=looking_for)
fprint("EXPLORER FOUND OBJECT", msg_color="blue")
yield navigator.database_query(cmd="lock {} {}".format(obj.id, looking_for))
defer.returnValue(True)
except MissingPerceptionObject:
fprint("The object {} is not in the database".format(looking_for), msg_color="red")
try:
objs = yield navigator.database_query(object_name="all")
objs = get_closest_objects(pos, objs.objects)
except Exception as e:
print e
defer.returnValue(False)
# for o in objs:
# obj_pos = nt.rosmsg_to_numpy(o.position)
# yield navigator.move.look_at(obj_pos).set_position(mark_pos).backward(7).go()
# cam_obj = yield navigator.camera_database_query(object_name=looking_for, id=o.id)
# if cam_obj.found:
# yield navigator.database_query(cmd="lock {} {}".format(o.id, looking_for))
# fprint("EXPLORER FOUND OBJECT", msg_color="blue")
# defer.returnValue(True)
fprint("NO OBJECT FOUND", msg_color="blue")
defer.returnValue(False)
def _dist(self, x):
if self.position is None:
success = yield threads.deferToThread(self._wait_for_position)
if not success:
raise Exception("There is a problem with odom.")
if self.navigator is not None:
position = yield self.navigator.tx_pose
position = position[0]
self.position = position
defer.returnValue(
np.linalg.norm(nt.rosmsg_to_numpy(x.position) - self.position))
def find_dock(self, override_scale):
# Get Dock
self.dock = yield self.get_sorted_objects(name='dock', n=1)
self.dock = self.dock[0][0]
# Get dock parameters
self.dock_position = rosmsg_to_numpy(self.dock.pose.position)
self.dock_orientation = rosmsg_to_numpy(self.dock.pose.orientation)
self.dock_scale = rosmsg_to_numpy(self.dock.scale)
self.dock_orientation = tform.euler_from_quaternion(
self.dock_orientation)
# If scale should be overwritten
if override_scale:
# Write long/short appropriately
if self.dock_scale[0] > self.dock_scale[1]:
self.dock_scale[0] = self.DOCK_SIZE_LONG
self.dock_scale[1] = self.DOCK_SIZE_SHORT
else:
self.dock_scale[0] = self.DOCK_SIZE_SHORT
self.dock_scale[1] = self.DOCK_SIZE_LONG
def _make_bounds(cls):
fprint("Constructing bounds.", title="NAVIGATOR")
if (yield cls.nh.has_param("/bounds/enforce")):
_bounds = cls.nh.get_service_client('/get_bounds', navigator_srvs.Bounds)
yield _bounds.wait_for_service()
resp = yield _bounds(navigator_srvs.BoundsRequest())
if resp.enforce:
cls.enu_bounds = [mil_tools.rosmsg_to_numpy(bound) for bound in resp.bounds]
else:
fprint("No bounds param found, defaulting to none.", title="NAVIGATOR")
cls.enu_bounds = None
def find_stc2(self):
pose = None
print("entering find_stc")
# see if we already got scan the code tower
try:
_, poses = yield self.get_sorted_objects(name='stc_platform', n=1)
pose = poses[0]
# incase stc platform not already identified
except Exception as e:
print("could not find stc_platform")
# get all pcodar objects
try:
print("check for any objects")
msgs, poses = yield self.get_sorted_objects(name='UNKNOWN',
n=-1)
# if no pcodar objects, drive forward
except Exception as e:
print("literally no objects?")
yield self.move.forward(25).go()
# get first pcodar objects
msgs, poses = yield self.get_sorted_objects(name='UNKNOWN',
n=-1)
# if still no pcodar objects, guess RGB and exit mission
# go to nearest obj to get better data on that obj
print('going to nearest small object')
# determine the dock and stc_buoy based on cluster size
dock_pose = None
for i in range(len(msgs)):
#Sometimes the dock is perceived as multiple objects
#Ignore any objects that are the dock
#I haven't found an overlap for a cluster tolerance that
#keeps the entire dock together 100% of the time
#while not including the stc buoy if it is too close
#if dock_pose is not None and \
# np.linalg.norm(dock_pose[0] - poses[i][0]) < 10:
# continue
if np.linalg.norm(rosmsg_to_numpy(msgs[i].scale)) > 4.0:
# much bigger than scale of stc
# then we found the dock
yield self.pcodar_label(msgs[i].id, 'dock')
dock_pose = poses[i]
else: # if about same size as stc, lable it stc
yield self.pcodar_label(msgs[i].id, 'stc_platform')
pose = poses[i]
break
print("leaving find_stc")
defer.returnValue(pose)
def ping_recv(self, p_message):
try:
# Transform the ping into enu
hydrophones_to_enu = yield self.tf_listener.get_transform('enu', p_message.header.frame_id)
hydrophones_origin = hydrophones_to_enu._p[0:2]
heading = rosmsg_to_numpy(p_message.vector)
heading_enu = hydrophones_to_enu.transform_vector(heading)
heading_enu = heading_enu[0:2] / np.linalg.norm(heading_enu[0:2])
# Track the ping
self.intersect_vectors.append((hydrophones_origin[0:2], hydrophones_origin[0:2] + heading_enu[0:2]))
except tf2_ros.TransformException, e:
self.send_feedback('TF Exception: {}'.format(e))
def get_objects_in_radius(self, pos, radius, objects="all"):
req = ObjectDBQueryRequest()
req.name = 'all'
resp = yield self._database(req)
ans = []
if not resp.found:
defer.returnValue(ans)
for o in resp.objects:
if objects == "all" or o.name in objects:
dist = np.linalg.norm(pos - nt.rosmsg_to_numpy(o.position))
if dist < radius:
ans.append(o)
defer.returnValue(ans)
def convert_cb(self, req):
'''
Callback for conversion service. Converts all points
from the in_frame to to_frame.
'''
if req.frame not in self.FRAMES:
return CoordinateConversionResponse(message='in_frame not valid')
if req.to_frame not in self.FRAMES:
return CoordinateConversionResponse(message='to_frame not valid')
idx = self.FRAMES.index(req.to_frame)
converted = np.zeros((len(req.points), 3))
func = getattr(self, req.frame)
for i in range(len(req.points)):
converted[i] = func(rosmsg_to_numpy(req.points[i]))[idx]
return CoordinateConversionResponse(converted=numpy_to_points(converted))
def get_object_map(self):
'''
Gets the latest objects
returns tuple (object_dict, position_dict)
object_dict: maps object id to classification
position_dict: maps object_id to position in enu as numpy array
'''
current_objects_msg = (yield self.database_query(name='all')).objects
ret = {}
positions = {}
for obj in current_objects_msg:
classification = obj.labeled_classification
ret[obj.id] = classification
positions[obj.id] = rosmsg_to_numpy(obj.pose.position)
defer.returnValue((ret, positions))
def circle_search(self):
platform_np = mil_tools.rosmsg_to_numpy(self.waypoint_res.objects[0].position)
yield self.move.look_at(platform_np).set_position(platform_np).backward(self.circle_radius)\
.yaw_left(90, unit='deg').go(move_type="drive")
done_circle = False
@txros.util.cancellableInlineCallbacks
def do_circle():
yield self.move.circle_point(platform_np, direction=self.circle_direction).go()
done_circle = True # noqa flake8 cant see that it is defined above
do_circle()
while not done_circle:
res = yield self.get_any_shape()
if res is False:
yield self.nh.sleep(0.25)
continue
fprint("Shape ({}found, using normal to look at other 3 shapes if needed".format(
res[0]), title="DETECT DELIVER", msg_color="green")
# circle_defer.cancel()
shape_color, found_shape_pose = res
if self.correct_shape(shape_color):
self.shape_pose = found_shape_pose
return
# Pick other 3 to look at
rot_right = np.array([[0, -1], [1, 0]])
(shape_point, shape_normal) = found_shape_pose
rotated_norm = np.append(rot_right.dot(shape_normal[:2]), 0)
center_point = shape_point - shape_normal * (self.platform_radius / 2.0)
point_opposite_side = center_point - shape_normal * self.circle_radius
move_opposite_side = self.move.set_position(
point_opposite_side).look_at(center_point).yaw_left(90, unit='deg')
left_or_whatever_point = center_point + rotated_norm * self.circle_radius
move_left_or_whatever = self.move.set_position(
left_or_whatever_point).look_at(center_point).yaw_left(90, unit='deg')
right_or_whatever_point = center_point - rotated_norm * self.circle_radius
move_right_or_whatever = self.move.set_position(
right_or_whatever_point).look_at(center_point).yaw_left(90, unit='deg')
yield self.search_sides((move_right_or_whatever, move_opposite_side, move_left_or_whatever))
return
fprint("No shape found after complete circle", title="DETECT DELIVER", msg_color='red')
raise Exception("No shape found on platform")
def myfunc(navigator, **kwargs):
pos = yield navigator.tx_pose
pos = pos[0]
exploring = ["Exploring1", "Exploring2", "Exploring3", "Exploring4"]
for e in exploring:
try:
objects = yield navigator.database_query(e)
o = objects.objects[0]
print o.name
pos = nt.rosmsg_to_numpy(o.position)
yield navigator.move.set_position(pos).go()
nt.fprint(o.name, msg_color='green')
except:
nt.fprint("Missing Marker", msg_color="red")
def get_stc_points(self, msg, stc_pos):
trans = yield self.my_tf.get_transform("/front_right_cam", "/velodyne", msg.header.stamp)
trans1 = yield self.my_tf.get_transform("/front_right_cam", "/enu", msg.header.stamp)
stc_pos = rosmsg_to_numpy(stc_pos)
stc_pos = np.append(stc_pos, 1)
position = trans1.as_matrix().dot(stc_pos)
if position[3] < 1E-15:
raise ZeroDivisionError
position[0] /= position[3]
position[1] /= position[3]
position[2] /= position[3]
position = position[:3]
stereo_points = []
for point in pc2.read_points(msg, skip_nans=True):
stereo_points.append(np.array([point[0], point[1], point[2], 1]))
stereo_points = map(lambda x: trans.as_matrix().dot(x), stereo_points)
points = []
for p in stereo_points:
if p[3] < 1E-15:
raise ZeroDivisionError
p[0] /= p[3]
p[1] /= p[3]
p[2] /= p[3]
points.append(p[:3])
points_keep = []
for p in points:
# print npl.norm(p - poition)
if npl.norm(p - position) < 20:
points_keep.append(p)
points_keep = sorted(points_keep, key=lambda x: x[1])
keep_num = int(.1 * len(points_keep))
points_keep = points_keep[:keep_num]
# self.pers_points.extend(points_keep)
# max_num = 200
# if len(self.pers_points) > max_num:
# self.pers_points = self.pers_points[len(self.pers_points) - max_num:len(self.pers_points)]
defer.returnValue(points_keep)
def get_shape_pos(self, normal_res, enu_cam_tf):
enunormal = enu_cam_tf.transform_vector(mil_tools.rosmsg_to_numpy(normal_res.normal))
enupoint = enu_cam_tf.transform_point(mil_tools.rosmsg_to_numpy(normal_res.closest))
return (enupoint, enunormal)
def from_srv(cls, srv):
return cls(mil_tools.rosmsg_to_numpy(srv.position), srv.color)
def get_waypoint(self):
res = yield self.navigator.database_query(self.WAYPOINT_NAME)
self.dock_pose = mil_tools.rosmsg_to_numpy(res.objects[0].position)
def normal_is_sane(self, vector3):
return abs(mil_tools.rosmsg_to_numpy(vector3)[1]) < 0.4
def run(self, args):
if not self.pose:
raise Exception('Cant move: No odom')
commands = args.commands
arguments = commands[1::2]
commands = commands[0::2]
self.send_feedback('Switching wrench to autonomous')
yield self.change_wrench('autonomous')
for i in xrange(len(commands)):
command = commands[i]
argument = arguments[i]
action_kwargs = {'move_type': args.movetype, 'speed_factor': args.speedfactor}
action_kwargs['blind'] = args.blind
if args.speedfactor is not None:
if ',' in args.speedfactor:
sf = np.array(map(float, args.speedfactor[1:-1].split(',')))
else:
sf = [float(args.speedfactor)] * 3
action_kwargs['speed_factor'] = sf
if args.plantime is not None:
action_kwargs['initial_plan_time'] = float(args.plantime)
if args.focus is not None:
focus = np.array(map(float, args.focus[1:-1].split(',')))
focus[2] = 1 # Tell lqrrt we want to look at the point
point = numpy_to_point(focus)
action_kwargs['focus'] = point
if command == 'custom':
# Let the user input custom commands, the eval may be dangerous so do away with that at some point.
self.send_feedback("Moving with the command: {}".format(argument))
res = yield eval("self.move.{}.go(move_type='{move_type}')".format(argument, **action_kwargs))
elif command == 'rviz':
self.send_feedback('Select a 2D Nav Goal in RVIZ')
target_pose = yield util.wrap_time_notice(self.rviz_goal.get_next_message(), 2, "Rviz goal")
self.send_feedback('RVIZ pose recieved!')
res = yield self.move.to_pose(target_pose).go(**action_kwargs)
elif command == 'circle':
self.send_feedback('Select a Publish Point in RVIZ')
target_point = yield util.wrap_time_notice(self.rviz_point.get_next_message(), 2, "Rviz point")
self.send_feedback('RVIZ point recieved!')
target_point = rosmsg_to_numpy(target_point.point)
distance = np.linalg.norm(target_point - self.pose[0])
target_point = rosmsg_to_numpy(target_point.point)
direction = 'cw' if argument == '-1' else 'ccw'
res = yield self.move.circle_point(target_point, direction=direction).go(radius=distance)
else:
shorthand = {"f": "forward", "b": "backward", "l": "left",
"r": "right", "yl": "yaw_left", "yr": "yaw_right"}
command = command if command not in shorthand.keys() else shorthand[command]
movement = getattr(self.move, command)
trans_move = command[:3] != "yaw"
unit = "m" if trans_move else "rad"
amount = argument
# See if there's a non standard unit at the end of the argument
if not argument[-1].isdigit():
last_digit_index = [i for i, c in enumerate(argument) if c.isdigit()][-1] + 1
amount = float(argument[:last_digit_index])
unit = argument[last_digit_index:]
# Get the kwargs to pass to the action server
station_hold = amount == '0'
if station_hold:
action_kwargs['move_type'] = 'hold'
msg = "Moving {} ".format(command) if trans_move else "Yawing {} ".format(command[4:])
self.send_feedback(msg + "{}{}".format(amount, unit))
res = yield movement(float(amount), unit).go(**action_kwargs)
if res.failure_reason is not '':
raise Exception('Move failed. Reason: {}'.format(res.failure_reason))
defer.returnValue('Move completed successfully!')
def check_color(totem, color_map):
for name, color in color_map:
if mil_tools.rosmsg_to_numpy(totem.color, keys=['r', 'g', 'b']) == color:
return name
return DEFAULT_COLOR
def main(navigator, **kwargs):
# rgb color map to param vlues
color_map = {'BLUE': [0, 0, 1], 'RED': [1, 0, 0], 'YELLOW': [1, 1, 0], 'GREEN': [0, 1, 0]}
directions = {'RED': 'cw', 'GREEN': 'ccw', 'BLUE': 'cw', 'YELLOW': 'ccw'}
ogrid = OccupancyGridFactory(navigator)
explored_ids = []
all_found = False
# Get colors of intrest and directions
c1 = navigator.mission_params['scan_the_code_color1'].get()
c2 = navigator.mission_params['scan_the_code_color2'].get()
c3 = navigator.mission_params['scan_the_code_color3'].get()
colors = [c1, c2, c3]
buoy_field = yield navigator.database_query("BuoyField")
buoy_field_point = mil_tools.rosmsg_to_numpy(buoy_field.objects[0].position)
_dist_from_bf = lambda pt: np.linalg.norm(buoy_field_point - pt)
# We want to go to an observation point based on solar position
center = navigator.move.set_position(buoy_field_point).set_orientation(get_solar_q())
searched = []
for color in colors:
target = None
need_recolor = False
color = yield color
direction = directions[color]
fprint("Going to totem colored {} in direction {}".format(color, direction), title="CIRCLE_TOTEM")
target = yield get_colored_buoy(navigator, color_map[color])
if target is None or _dist_from_bf(mil_tools.rosmsg_to_numpy(target.position)) > (BF_WIDTH / 2 + BF_EST_COFIDENCE):
# Need to do something
fprint("No suitable totems found, going to circle any nearby totems.", msg_color='red', title="CIRCLE_TOTEM")
target, searched = yield get_closest_totem(navigator, searched)
need_recolor = True
target_np = mil_tools.rosmsg_to_numpy(target.position)
circler = navigator.move.d_circle_point(point=target_np, radius=TOTEM_SAFE_DIST, direction=direction)
# Give the totem a look at
for p in circler:
res = yield p.go(speed_factor=SPEED_FACTOR)
if res.failure_reason is '':
break
if need_recolor:
fprint("Recoloring...")
# Check for color?
yield navigator.nh.sleep(5)
target = yield navigator.database_query(str(target.id), raise_exception=False)
if len(target.objects) == 0:
direction = directions[DEFAULT_COLOR]
else:
direction = directions[check_color(target.objects[0], color_map)]
mult = 1 if direction == 'cw' else -1
left_offset = mult * CIRCLE_OFFSET
tangent_circler = navigator.move.d_circle_point(point=target_np, radius=ROT_SAFE_DIST, theta_offset=mult * 1.57, direction=direction)
# Point tangent
for p in tangent_circler:
res = yield p.go(speed_factor=SPEED_FACTOR)
if res.failure_reason is '':
break
msg, goal = ogrid.circle_around(target_np, direction=direction)
latched_pub = ogrid.latching_publisher(msg)
yield navigator.nh.sleep(2)
goal = navigator.move.set_position(np.append(goal, 1)).look_at(navigator.pose[0]).left(left_offset).backward(2)
yield goal.go(move_type='drive!', initial_plan_time=10)
latched_pub.cancel()
fprint("Canceling ogrid")
yield navigator.nh.sleep(3)
print "Mission result:", res
defer.returnValue(None)
