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 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 do_mission(self, navigator, planner, module, **kwargs):
"""Perform this mission."""
yield planner.publish("Starting", self)
to_run = getattr(module, self.mission_script)
fprint(self.name, msg_color="green", title="STARTING MISSION")
res = yield to_run.main(navigator, attempts=self.attempts, **kwargs)
defer.returnValue(res)
def _err_base_mission(self, err):
self.running_mission = False
self.current_mission_name = None
if err.type == defer.CancelledError:
fprint("Base mission cancelled", msg_color="red", title="BASE MISSION ERROR:")
else:
fprint(err, msg_color="red", title="BASE MISSION ERROR:")
def _run_search_pattern(self, loop, **kwargs):
'''
Look around using the search pattern.
If `loop` is true, then keep iterating over the list until timeout is reached or we find it.
'''
if self.search_pattern is None:
return
def pattern():
for pose in self.search_pattern:
fprint("Going to next position.", title="SEARCHER")
if type(pose) == list or type(pose) == np.ndarray:
yield self.nav.move.relative(pose).go(**kwargs)
else:
yield pose.go(**kwargs)
yield self.nav.nh.sleep(2)
if not loop:
fprint("Search Pattern Over", title="SEARCHER")
self.pattern_done = True
fprint("Executing search pattern.", title="SEARCHER")
if loop:
while True:
yield util.cancellableInlineCallbacks(pattern)()
else:
yield util.cancellableInlineCallbacks(pattern)()
def shoot_and_align(self):
move = yield self.align_to_target()
if move.failure_reason != "":
fprint("Error Aligning with target = {}. Ending mission :(".format(move.failure_reason), title="DETECT DELIVER", msg_color="red")
return
fprint("Aligned successs. Shooting without realignment", title="DETECT DELIVER", msg_color="green")
yield self.shoot_all_balls()
def continuously_align(self):
fprint("Starting Forest Align", title="DETECT DELIVER", msg_color='green')
while True:
shooter_pose = yield self.shooter_pose_sub.get_next_message()
if self.align_forest_pause:
yield self.nh.sleep(0.1)
continue
shooter_pose = shooter_pose.pose
cen = np.array([shooter_pose.position.x, shooter_pose.position.y])
yaw = trns.euler_from_quaternion([shooter_pose.orientation.x,
shooter_pose.orientation.y,
shooter_pose.orientation.z,
shooter_pose.orientation.w])[2]
q = trns.quaternion_from_euler(0, 0, yaw)
p = np.append(cen, 0)
# fprint("Forest Aligning to p=[{}] q=[{}]".format(p, q), title="DETECT DELIVER", msg_color='green')
# Prepare move to follow shooter
move = self.move.set_position(p).set_orientation(q).yaw_right(90, 'deg')
# Adjust move for location of target
move = move.forward(self.target_offset_meters)
# Adjust move for location of launcher
move = move.left(-self.shooter_baselink_tf._p[1]).forward(-self.shooter_baselink_tf._p[0])
# Move away a fixed distance to make the shot
move = move.left(self.shoot_distance_meters)
yield move.go(move_type='bypass')
def search_shape(self):
shapes = yield self.get_shape()
if shapes.found:
for shape in shapes.shapes.list:
normal_res = yield self.get_normal(shape)
if normal_res.success:
enu_cam_tf = yield self.tf_listener.get_transform(
'/enu', '/' + shape.header.frame_id,
shape.header.stamp)
if self.correct_shape(shape):
self.shape_pose = self.get_shape_pos(
normal_res, enu_cam_tf)
defer.returnValue(True)
self.update_shape(shape, normal_res, enu_cam_tf)
else:
if not self.last_lidar_error == normal_res.error:
fprint("Normal not found Error={}".format(
normal_res.error),
title="DETECT DELIVER",
msg_color='red')
self.last_lidar_error = normal_res.error
else:
if not self.last_shape_error == shapes.error:
fprint("shape not found Error={}".format(shapes.error),
title="DETECT DELIVER",
msg_color="red")
self.last_shape_error = shapes.error
defer.returnValue(False)
def _cb_mission(self, result):
if not self.failed:
yield self.publish("Ending")
fprint(str(result) + " TIME: " + str((self.nh.get_time() - self.current_mission.start_time).to_sec()),
msg_color="green", title="{} MISSION COMPLETE: ".format(self.current_mission.name))
self.points += self.current_mission.points
self._mission_complete()
def _err_mission(self, err):
if hasattr(
err, "type"
) and err.type == defer.CancelledError: # This means there was a timeout
fprint(self.current_mission.name,
msg_color="red",
title="MISSION TIMEOUT")
yield self.publish("TimingOut")
if (TimeoutManager.can_repeat(
self.missions_left, self._get_time_left(),
self.current_mission)) or len(self.missions_left) == 1:
yield self.publish("Retrying")
self.failed = False
self.current_mission.timeout = self.current_mission.min_time
yield self._run_mission(self.current_mission, redo=True)
defer.returnValue(False)
else:
fprint(err,
msg_color="red",
title="{} MISSION ERROR: ".format(
self.current_mission.name))
yield self.publish("Failing")
yield self.current_mission.safe_exit(self.navigator, err, self,
self.module)
self._mission_complete()
def _run_search_pattern(self, loop, **kwargs):
'''
Look around using the search pattern.
If `loop` is true, then keep iterating over the list until timeout is reached or we find it.
'''
if self.search_pattern is None:
return
def pattern():
for pose in self.search_pattern:
fprint("Going to next position.", title="SEARCHER")
if type(pose) == list or type(pose) == np.ndarray:
yield self.nav.move.relative(pose).go(**kwargs)
else:
yield pose.go(**kwargs)
yield self.nav.nh.sleep(2)
if not loop:
fprint("Search Pattern Over", title="SEARCHER")
self.pattern_done = True
fprint("Executing search pattern.", title="SEARCHER")
if loop:
while True:
yield util.cancellableInlineCallbacks(pattern)()
else:
yield util.cancellableInlineCallbacks(pattern)()
def count_pipes(self):
"""Count the number of pipes in between the breaks."""
second_hpipe_found = False
while not second_hpipe_found:
try:
frame = yield self.curr_image
except util.TimeoutError:
fprint("Image isn't being received, fail mission", msg_color="red")
raise Exception("Image isn't being received")
# get all the pipes in the current frame
hpipes, vpipes = self._get_all_pipes(frame)
# Look for NEW pipes
new_hpipes, new_vpipes = self._update_pipes(
hpipes, self.old_hpipe_pos), self._update_pipes(vpipes, self.old_vpipe_pos)
# the second hpipe is found
if len(new_hpipes) > 0 and self.hpipe_found:
defer.returnValue(self.count)
# the first hpipe is found
elif len(new_hpipes) > 0 and not self.hpipe_found:
self.hpipe_found = True
for pipe in vpipes:
if pipe[1] > new_hpipes[0][1]:
self.count += 1
# if its above first h, add to count
elif self.hpipe_found:
for pipe in new_vpipes:
if pipe[1] > new_hpipes[0][1]:
self.count += 1
self.old_hpipe_pos.extend(new_hpipes)
self.old_vspipe_pos.extend(new_vpipes)
def do_mission(self, navigator, planner, module, **kwargs):
"""Perform this mission."""
yield planner.publish("Starting", self)
to_run = getattr(module, self.mission_script)
fprint(self.name, msg_color="green", title="STARTING MISSION")
res = yield to_run.main(navigator, attempts=self.attempts, **kwargs)
defer.returnValue(res)
def continuously_align(self):
fprint("Starting Forest Align", title="DETECT DELIVER", msg_color='green')
try:
while True:
shooter_pose = yield self.shooter_pose_sub.get_next_message()
if self.align_forest_pause:
yield self.navigator.nh.sleep(0.1)
continue
shooter_pose = shooter_pose.pose
cen = np.array([shooter_pose.position.x, shooter_pose.position.y])
yaw = trns.euler_from_quaternion([shooter_pose.orientation.x,
shooter_pose.orientation.y,
shooter_pose.orientation.z,
shooter_pose.orientation.w])[2]
q = trns.quaternion_from_euler(0, 0, yaw)
p = np.append(cen,0)
#fprint("Forest Aligning to p=[{}] q=[{}]".format(p, q), title="DETECT DELIVER", msg_color='green')
#Prepare move to follow shooter
move = self.navigator.move.set_position(p).set_orientation(q).yaw_right(90, 'deg')
#Adjust move for location of target
move = move.forward(self.target_offset_meters)
#Adjust move for location of launcher
move = move.left(-self.shooter_baselink_tf._p[1]).forward(-self.shooter_baselink_tf._p[0])
#Move away a fixed distance to make the shot
move = move.left(self.shoot_distance_meters)
yield move.go(move_type='bypass')
except Exception:
traceback.print_exc()
raise
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 _object_gone_missing(self, missing_objects):
fprint("This object {} is no longer in the list".format(missing_objects), msg_color="red")
self.helper.stop_ensuring_object_permanence()
for o in missing_objects:
if o in self.found:
self.helper.remove_found(o)
self.found.remove(o)
self.mission_defer.cancel()
def _init(cls, mission_runner):
super(Navigator, cls)._init(mission_runner)
cls.vision_proxies = {}
cls._load_vision_services()
cls.launcher_state = "inactive"
cls._actuator_timing = yield cls.nh.get_param("~actuator_timing")
cls.mission_params = {}
cls._load_mission_params()
# If you don't want to use txros
cls.pose = None
cls.ecef_pose = None
cls.killed = '?'
cls.odom_loss = '?'
if (yield cls.nh.has_param('/is_simulation')):
cls.sim = yield cls.nh.get_param('/is_simulation')
else:
cls.sim = False
# For missions to access clicked points / poses
cls.rviz_goal = cls.nh.subscribe("/move_base_simple/goal", PoseStamped)
cls.rviz_point = cls.nh.subscribe("/clicked_point", PointStamped)
cls._moveto_client = action.ActionClient(cls.nh, 'move_to', MoveAction)
def odom_set(odom):
return setattr(cls, 'pose', mil_tools.odometry_to_numpy(odom)[0])
cls._odom_sub = cls.nh.subscribe('odom', Odometry, odom_set)
def enu_odom_set(odom):
return setattr(cls, 'ecef_pose', mil_tools.odometry_to_numpy(odom)[0])
cls._ecef_odom_sub = cls.nh.subscribe('absodom', Odometry, enu_odom_set)
cls.hydrophones = TxHydrophonesClient(cls.nh)
cls.poi = TxPOIClient(cls.nh)
cls._grinch_lower_time = yield cls.nh.get_param("~grinch_lower_time")
cls._grinch_raise_time = yield cls.nh.get_param("~grinch_raise_time")
cls.grinch_limit_switch_pressed = False
cls._grinch_limit_switch_sub = yield cls.nh.subscribe('/limit_switch', Bool, cls._grinch_limit_switch_cb)
cls._winch_motor_pub = cls.nh.advertise("/grinch_winch/cmd", Command)
cls._grind_motor_pub = cls.nh.advertise("/grinch_spin/cmd", Command)
try:
cls._actuator_client = cls.nh.get_service_client('/actuator_driver/actuate', SetValve)
cls._database_query = cls.nh.get_service_client('/database/requests', ObjectDBQuery)
cls._camera_database_query = cls.nh.get_service_client(
'/camera_database/requests', navigator_srvs.CameraDBQuery)
cls._change_wrench = cls.nh.get_service_client('/wrench/select', MuxSelect)
cls._change_trajectory = cls.nh.get_service_client('/trajectory/select', MuxSelect)
except AttributeError, err:
fprint("Error getting service clients in nav singleton init: {}".format(
err), title="NAVIGATOR", msg_color='red')
def search_side(self):
fprint("Searching side",title="DETECT DELIVER", msg_color='green')
start_time = self.navigator.nh.get_time()
while self.navigator.nh.get_time() - start_time < genpy.Duration(self.LOOK_AT_TIME):
res = yield self.get_any_shape()
if not res == False:
defer.returnValue(res)
yield self.navigator.nh.sleep(0.1)
defer.returnValue(False)
def run(self):
fprint("PINGER EXIT: Starting", msg_color='green')
self.gate_index = yield self.navigator.mission_params["acoustic_pinger_active_index"].get()
self.gate_index = self.gate_index - 1
yield self.get_objects()
yield self.set_side()
self.get_gate_thru_points()
yield self.go_thru_gate()
fprint("PINGER EXIT: Done", msg_color='green')
def set_shape_and_color(self):
target = yield self.navigator.mission_params["detect_deliver_target"].get()
if target == "BIG":
self.target_offset_meters = self.SHAPE_CENTER_TO_BIG_TARGET
elif target == "SMALL":
self.target_offset_meters = self.SHAPE_CENTER_TO_SMALL_TARGET
self.Shape = yield self.navigator.mission_params["detect_deliver_shape"].get()
self.Color = yield self.navigator.mission_params["detect_deliver_color"].get()
fprint("Color={} Shape={} Target={}".format(self.Color, self.Shape, target), title="DETECT DELIVER", msg_color='green')
def align_to_target(self):
if self.shape_pose == None:
self.select_backup_shape()
goal_point, goal_orientation = self.get_aligned_pose(self.shape_pose[0], self.shape_pose[1])
move = self.navigator.move.set_position(goal_point).set_orientation(goal_orientation).forward(self.target_offset_meters)
move = move.left(-self.shooter_baselink_tf._p[1]).forward(-self.shooter_baselink_tf._p[0]) #Adjust for location of shooter
fprint("Aligning to shoot at {}".format(move), title="DETECT DELIVER", msg_color='green')
move_complete = yield move.go(move_type="skid", blind=True)
defer.returnValue(move_complete)
def search_side(self):
fprint("Searching side", title="DETECT DELIVER", msg_color='green')
start_time = self.nh.get_time()
while self.nh.get_time() - start_time < genpy.Duration(self.LOOK_AT_TIME):
res = yield self.get_any_shape()
if res is not False:
defer.returnValue(res)
yield self.nh.sleep(0.1)
defer.returnValue(False)
def select_backup_shape(self):
for (shape, color), point_normal in self.identified_shapes.iteritems():
self.shape_pose = point_normal
if self.Shape == shape or self.Color == color:
fprint("Correct shape not found, resorting to shape={} color={}".format(shape, color), title="DETECT DELIVER", msg_color='yellow')
return
if self.shape_pose == None:
raise Exception("None seen")
fprint("Correct shape not found, resorting to random shape", title="DETECT DELIVER", msg_color='yellow')
def get_waypoint(self):
res = yield self.database_query("shooter")
if not res.found:
fprint("shooter waypoint not found",
title="DETECT DELIVER",
msg_color='red')
raise MissingPerceptionObject("shooter",
"Detect Deliver Waypoint not found")
self.waypoint_res = res
def _err_base_mission(self, err):
self.running_mission = False
self.current_mission_name = None
if err.type == defer.CancelledError:
fprint("Base mission cancelled",
msg_color="red",
title="BASE MISSION ERROR:")
else:
fprint(err, msg_color="red", title="BASE MISSION ERROR:")
def refresh(self):
"""
Called when the state of the DAG needs to be updated due to a mission completing or an object being found.
CALLED ASYNCHRONOUS
"""
for mission in self.tree:
if self.can_complete(mission) and not self._is_in_queue(mission) and mission.name != self.current_mission_name:
fprint("mission: {}".format(mission.name), msg_color="blue", title="ADDING")
self.queue.put(mission)
def search_samples(self):
"""Move to each observation point and listen to the pinger while sitting still"""
yield self.get_observation_poses()
for i, p in enumerate(self.observation_points):
yield self.stop_listen()
yield self.move.set_position(p).look_at(self.look_at_points[i]).go()
yield self.start_listen()
fprint("PINGER: Listening To Pinger at point {}".format(p), msg_color='green')
yield self.nh.sleep(self.LISTEN_TIME)
yield self.stop_listen()
def _mission_complete(self, mission):
self.tree.remove(mission)
self.current_mission_name = None
for m in mission.children:
self.tree.append(m)
self.refresh()
self.completed_mission += 1
if self.completed_mission == self.total_mission_count:
fprint("ALL MISSIONS COMPLETE", msg_color="green")
self.keep_running = False
def _object_gone_missing(self, missing_objects):
fprint(
"This object {} is no longer in the list".format(missing_objects),
msg_color="red")
self.helper.stop_ensuring_object_permanence()
for o in missing_objects:
if o in self.found:
self.helper.remove_found(o)
self.found.remove(o)
self.mission_defer.cancel()
def run(self):
fprint("PINGER EXIT: Starting", msg_color='green')
self.gate_index = yield self.navigator.mission_params[
"acoustic_pinger_active_index"].get()
self.gate_index = self.gate_index - 1
yield self.get_objects()
yield self.set_side()
self.get_gate_thru_points()
yield self.go_thru_gate()
fprint("PINGER EXIT: Done", msg_color='green')
def _end_mission(self, result):
self.pub_msn_info.publish(String("Ending Mission {}".format(self.current_mission_name)))
fprint(str(result) + " TIME: " + str((self.nh.get_time() - self.current_mission_start_time).to_sec()),
msg_color="green", title="{} MISSION COMPLETE: ".format(self.current_mission_name))
self.running_mission = False
self.current_mission_name = None
self.helper.stop_ensuring_object_permanence()
self._mission_complete(self.current_mission)
self.current_mission = None
self.current_mission_timeout = None
def main(navigator, **kwargs):
nh = navigator.nh
attempts = kwargs["attempts"]
fprint("{} running".format(__name__), msg_color='red')
if attempts > 1:
yield nh.sleep(1)
else:
yield nh.sleep(2)
fprint("{} stopped running".format(__name__), msg_color='red')
def _mission_complete(self, mission):
self.tree.remove(mission)
self.current_mission_name = None
for m in mission.children:
self.tree.append(m)
self.refresh()
self.completed_mission += 1
if self.completed_mission == self.total_mission_count:
fprint("ALL MISSIONS COMPLETE", msg_color="green")
self.keep_running = False
def setup_mission(self):
stc_color = yield self.mission_params["scan_the_code_color3"].get(raise_exception=False)
if stc_color is False:
color = "ANY"
else:
color = stc_color
# color = "ANY"
shape = "ANY"
fprint("Setting search shape={} color={}".format(shape, color), title="DETECT DELIVER", msg_color='green')
yield self.mission_params["detect_deliver_shape"].set(shape)
yield self.mission_params["detect_deliver_color"].set(color)
def setup_mission(navigator):
stc_color = yield navigator.mission_params["scan_the_code_color3"].get(raise_exception=False)
if stc_color == False:
color = "ANY"
else:
color = stc_color
#color = "ANY"
shape = "ANY"
fprint("Setting search shape={} color={}".format(shape, color), title="DETECT DELIVER", msg_color='green')
yield navigator.mission_params["detect_deliver_shape"].set(shape)
yield navigator.mission_params["detect_deliver_color"].set(color)
def new_item(self, obj):
"""
Callback for a new object being found.
ASYNCHRONOUS
"""
fprint("NEW ITEM: {}".format(obj.name), msg_color="blue")
if self.base_mission is not None and self.current_mission_name is self.base_mission.name:
self.mission_defer.cancel()
self.found.add(obj.name)
self.refresh()
def _cb_mission(self, result):
if not self.failed:
yield self.publish("Ending")
fprint(str(result) + " TIME: " + str(
(self.nh.get_time() -
self.current_mission.start_time).to_sec()),
msg_color="green",
title="{} MISSION COMPLETE: ".format(
self.current_mission.name))
self.points += self.current_mission.points
self._mission_complete()
def new_item(self, obj):
"""
Callback for a new object being found.
ASYNCHRONOUS
"""
fprint("NEW ITEM: {}".format(obj.name), msg_color="blue")
if self.base_mission is not None and self.current_mission_name is self.base_mission.name:
self.mission_defer.cancel()
self.found.add(obj.name)
self.refresh()
def set_active_pinger(self):
"""Set the paramter for the active pinger identified for use in other mission"""
fprint("PINGER: setting active pinger to Gate_{}".format(int(self.gate_index) + 1), msg_color='green')
yield self.get_colored_buoys()
if self.color_wrong and self.gate_index == 2:
yield self.mission_params["acoustic_pinger_active_index_correct"].set(1)
elif self.color_wrong and self.gate_index == 0:
yield self.mission_params["acoustic_pinger_active_index_correct"].set(3)
else:
yield self.mission_params["acoustic_pinger_active_index_correct"].set(int(self.gate_index) + 1)
yield self.mission_params["acoustic_pinger_active_index"].set(int(self.gate_index) + 1)
def pattern():
for pose in self.search_pattern:
fprint("Going to next position.", title="SEARCHER")
if type(pose) == list or type(pose) == np.ndarray:
yield self.nav.move.relative(pose).go(**kwargs)
else:
yield pose.go(**kwargs)
yield self.nav.nh.sleep(2)
if not loop:
fprint("Search Pattern Over", title="SEARCHER")
self.pattern_done = True
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 run(self, parameters):
yield self.setup_mission()
fprint("STARTING MISSION", title="DETECT DELIVER", msg_color='green')
yield self.vision_proxies["get_shape"].start()
yield self.set_shape_and_color() # Get correct goal shape/color from params
yield self.get_waypoint() # Get waypoint of shooter target
yield self.circle_search() # Go to waypoint and circle until target found
# yield self.shoot_and_align() # Align to target and shoot
yield self.shoot_and_align_forest() # Align to target and shoot
yield self.backup_from_target()
yield self.vision_proxies["get_shape"].stop()
fprint("ENDING MISSION", title="DETECT DELIVER", msg_color='green')
def safe_exit(self, navigator, err, planner, module):
"""Run a safe exit of a mission."""
try:
to_run = getattr(module, self.name)
if hasattr(to_run, 'safe_exit'):
yield to_run.safe_exit(navigator, err)
else:
fprint("Hmmmm. This isn't good. Your mission failed, and there was no safe exit. "
"I hope this mission doesn't have any children.", msg_color="red")
except Exception as exp:
print exp
fprint("Oh man this is pretty bad, your mission's safe exit failed. SHAME!", msg_color="red")
def pattern():
for pose in self.search_pattern:
fprint("Going to next position.", title="SEARCHER")
if type(pose) == list or type(pose) == np.ndarray:
yield self.nav.move.relative(pose).go(**kwargs)
else:
yield pose.go(**kwargs)
yield self.nav.nh.sleep(2)
if not loop:
fprint("Search Pattern Over", title="SEARCHER")
self.pattern_done = True
def search_sides(self, moves):
for move in moves:
yield move.go(move_type="drive")
res = yield self.search_side()
if res == False:
fprint("No shape found on side",title="DETECT DELIVER", msg_color='red')
continue
shape_color, found_pose = res
if self.correct_shape(shape_color):
self.shape_pose = found_pose
return
fprint("Saw (Shape={}, Color={}) on this side".format(shape_color[0], shape_color[1]),title="DETECT DELIVER", msg_color='green')
def _init(cls, task_runner):
super(Navigator, cls)._init(task_runner)
cls.vision_proxies = {}
cls._load_vision_services()
cls.mission_params = {}
cls._load_mission_params()
# If you don't want to use txros
cls.pose = None
cls.ecef_pose = None
cls.enu_bounds = None
cls.killed = '?'
cls.odom_loss = '?'
if (yield cls.nh.has_param('/is_simulation')):
cls.sim = yield cls.nh.get_param('/is_simulation')
else:
cls.sim = False
# Just some pre-created publishers for missions to use for debugging
cls._point_cloud_pub = cls.nh.advertise("navigator_points", PointCloud)
cls._pose_pub = cls.nh.advertise("navigator_pose", PoseStamped)
cls._moveto_client = action.ActionClient(cls.nh, 'move_to', MoveAction)
def odom_set(odom):
return setattr(cls, 'pose', mil_tools.odometry_to_numpy(odom)[0])
cls._odom_sub = cls.nh.subscribe('odom', Odometry, odom_set)
def enu_odom_set(odom):
return setattr(cls, 'ecef_pose',
mil_tools.odometry_to_numpy(odom)[0])
cls._ecef_odom_sub = cls.nh.subscribe('absodom', Odometry,
enu_odom_set)
try:
cls._database_query = cls.nh.get_service_client(
'/database/requests', navigator_srvs.ObjectDBQuery)
cls._camera_database_query = cls.nh.get_service_client(
'/camera_database/requests', navigator_srvs.CameraDBQuery)
cls._change_wrench = cls.nh.get_service_client(
'/wrench/select', MuxSelect)
except AttributeError, err:
fprint("Error getting service clients in nav singleton init: {}".
format(err),
title="NAVIGATOR",
msg_color='red')
def _end_mission(self, result):
self.pub_msn_info.publish(
String("Ending Mission {}".format(self.current_mission_name)))
fprint(str(result) + " TIME: " + str(
(self.nh.get_time() - self.current_mission_start_time).to_sec()),
msg_color="green",
title="{} MISSION COMPLETE: ".format(self.current_mission_name))
self.running_mission = False
self.current_mission_name = None
self.helper.stop_ensuring_object_permanence()
self._mission_complete(self.current_mission)
self.current_mission = None
self.current_mission_timeout = None
def refresh(self):
"""
Called when the state of the DAG needs to be updated due to a mission completing or an object being found.
CALLED ASYNCHRONOUS
"""
for mission in self.tree:
if self.can_complete(mission) and not self._is_in_queue(
mission) and mission.name != self.current_mission_name:
fprint("mission: {}".format(mission.name),
msg_color="blue",
title="ADDING")
self.queue.put(mission)
def get_any_shape(self):
shapes = yield self.get_shape()
if shapes.found:
for shape in shapes.shapes.list:
normal_res = yield self.get_normal(shape)
if normal_res.success:
enu_cam_tf = yield self.navigator.tf_listener.get_transform('/enu', '/'+shape.header.frame_id, shape.header.stamp)
self.update_shape(shape, normal_res, enu_cam_tf)
defer.returnValue( ((shape.Shape, shape.Color), self.identified_shapes[(shape.Shape, shape.Color)]) )
else:
fprint("Normal not found Error={}".format(normal_res.error), title="DETECT DELIVER", msg_color='red')
else:
fprint("shape not found Error={}".format(shapes.error), title="DETECT DELIVER", msg_color="red")
defer.returnValue(False)
def _make_alarms(cls):
cls.kill_listener = yield TxAlarmListener.init(cls.nh, 'kill', cls.kill_alarm_cb)
cls.odom_loss_listener = yield TxAlarmListener.init(
cls.nh, 'odom-kill',
lambda _, alarm: setattr(cls, 'odom_loss', alarm.raised))
fprint("Alarm listener created, listening to alarms: ", title="NAVIGATOR")
cls.kill_alarm = yield cls.kill_listener.get_alarm()
cls.killed = cls.kill_alarm.raised
cls.odom_loss = yield cls.odom_loss_listener.is_raised()
fprint("\tkill :", newline=False)
fprint(cls.killed)
fprint("\todom-kill :", newline=False)
fprint(cls.odom_loss)
def got_range(msg):
'''TODO:
- Make parallel to surface
'''
translation = (0.0, 0.0, -msg.range)
if rospy.Time.now() < rospy.Time(0.5):
listener.clear()
t = rospy.Time(0)
try:
listener.waitForTransform('/base_link', '/map', t, rospy.Duration(1))
trans, rot = listener.lookupTransform("/base_link", "/map", t)
bc.sendTransform(translation, rot, rospy.Time.now(), "/ground", "/dvl")
except (tf.Exception, tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
te.fprint(e, title='TF REPUB')
def start_search(self, timeout=120, loop=True, spotings_req=2, **kwargs):
fprint("Starting.", title="SEARCHER")
looker = self._run_look(spotings_req).addErrback(self.catch_error)
finder = self._run_search_pattern(loop, **kwargs).addErrback(self.catch_error)
start_pose = self.nav.move.forward(0)
start_time = self.nav.nh.get_time()
try:
while self.nav.nh.get_time() - start_time < genpy.Duration(timeout):
if self.object_found:
finder.cancel()
fprint("Object found.", title="SEARCHER")
defer.returnValue(True)
if self.pattern_done and not loop:
finder.cancel()
defer.returnValue(False)
yield self.nav.nh.sleep(0.1)
except KeyboardInterrupt:
# This doesn't work...
fprint("Control C detected!", title="SEARCHER")
fprint("Object NOT found. Returning to start position.", title="SEARCHER")
finder.cancel()
looker.cancel()
yield start_pose.go()
def _load_mission_params(cls, fname="mission_params.yaml"):
rospack = rospkg.RosPack()
config_file = os.path.join(rospack.get_path('navigator_missions'), 'launch', fname)
f = yaml.load(open(config_file, 'r'))
for name in f:
try:
param = f[name]["param"]
options = f[name]["options"]
desc = f[name]["description"]
default = f[name].get("default")
cls.mission_params[name] = MissionParam(cls.nh, param, options, desc, default)
except Exception, e:
err = "Error loading mission params: {}".format(e)
fprint("" + err, title="NAVIGATOR", msg_color='red')
