def do_mission(sub):
fprint("RUNNING MISSION", msg_color="blue")
# Chain 1 missions
try:
completed = yield run_mission(sub, pinger, 400)
if not completed: # if we timeout
pass
else:
if (yield sub.nh.has_param('pinger_where')):
if (yield sub.nh.get_param('pinger_where')) == 0:
fprint('Running roulette')
# do roulette
pass
if (yield sub.nh.get_param('pinger_where')) == 1:
fprint('Running cash in')
# do cash in challenge
pass
except Exception as e:
fprint("Error in Chain 1 missions!", msg_color="red")
print e
# Create a mission kill alarm and kill in the final area
ab = yield TxAlarmBroadcaster.init(sub.nh, "mission-kill")
yield ab.raise_alarm()
fprint("MISSION COMPLETE", msg_color="green")
def do_mission(sub):
fprint("RUNNING MISSION", msg_color="blue")
# Chain 1 missions
try:
yield square.run(sub)
except Exception as e:
fprint("Error in Chain 1 missions!", msg_color="red")
print e
# Create a mission kill alarm and kill in the final area
ab = yield TxAlarmBroadcaster.init(sub.nh, "mission-kill")
yield ab.raise_alarm()
fprint("MISSION COMPLETE", msg_color="green")
def do_mission(sub):
fprint("RUNNING MISSION", msg_color="blue")
# Chain 1 missions
try:
yield square.run(sub)
except Exception as e:
fprint("Error in Chain 1 missions!", msg_color="red")
print e
# Create a mission kill alarm and kill in the final area
ab = yield TxAlarmBroadcaster.init(sub.nh, "mission-kill")
yield ab.raise_alarm()
fprint("MISSION COMPLETE", msg_color="green")
def do_mission(self):
fprint("RUNNING MISSION", msg_color="blue")
try:
# Run start gate mission
yield self.run_mission(StartGate(), 400)
# Go to pinger and do corresponding mission
completed = yield self.run_mission(Pinger(), 400)
if not completed: # if we timeout
pass
else:
if (yield self.nh.has_param('pinger_where')):
if (yield self.nh.get_param('pinger_where')) == 0:
fprint('Surface Mission')
yield self.run_mission(Surface(), 30)
elif (yield self.nh.get_param('pinger_where')) == 1:
fprint('Shooting Mission')
yield self.run_mission(FireTorpedos(), 400)
# Go to the other pinger mission and do respective mission
completed = yield self.run_mission(Pinger(), 400)
if not completed: # if we timeout
pass
else:
if (yield self.nh.has_param('pinger_where')):
if (yield self.nh.get_param('pinger_where')) == 0:
fprint('Surface Mission')
yield self.run_mission(Surface(), 30)
elif (yield self.nh.get_param('pinger_where')) == 1:
fprint('Shooting Mission')
yield self.run_mission(FireTorpedos(), 400)
fprint("Vampire Slayer")
yield self.run_mission(VampireSlayer(), 400)
fprint("Garlic drop?")
yield self.rub_mission(BallDrop(), 400)
except Exception as e:
fprint("Error in Chain 1 missions!", msg_color="red")
print e
# Create a mission kill alarm and kill in the final area
ab = yield TxAlarmBroadcaster.init(self.nh, "mission-kill")
yield ab.raise_alarm()
fprint("MISSION COMPLETE", msg_color="green")
def main():
nh = yield txros.NodeHandle.from_argv('tx_alarm_test')
alarm_name = "tx"
ab = yield TxAlarmBroadcaster.init(nh, alarm_name)
al = yield TxAlarmListener.init(nh, alarm_name, nowarn=True)
assert (yield al.is_cleared())
yield ab.raise_alarm()
assert (yield al.is_raised())
yield ab.raise_alarm()
assert (yield al.is_raised())
yield ab.clear_alarm()
assert (yield al.is_cleared())
var = False
@txros.util.cancellableInlineCallbacks
def cb(nh, alarm):
global var
var = True
yield nh.sleep(1)
print "DONE SLEEPING"
yield al.add_callback(cb, call_when_raised=False)
yield nh.sleep(.1)
assert not var
yield ab.raise_alarm()
assert not var
yield ab.clear_alarm()
assert not var
print "\nPassed"
yield nh.sleep(2)
def run(self, args):
commands = args.commands
# Split into commands and arguments, every other index
arguments = commands[1::2]
commands = commands[0::2]
for i in xrange(len(commands)):
command = commands[i]
argument = arguments[i]
self.send_feedback("Waiting for odom...")
yield self.tx_pose()
self.send_feedback("Odom found!")
action_kwargs = {
'speed': float(args.speed),
'blind': bool(args.blind)
}
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()".format(
argument, **action_kwargs))
elif command in ['tp', 'teleport']:
try:
rospack = rospkg.RosPack()
state_set_pub = yield self.nh.advertise(
'gazebo/set_model_state', ModelState)
config_file = os.path.join(rospack.get_path('sub8_gazebo'),
'config', 'teleport_locs.yaml')
f = yaml.load(open(config_file, 'r'))
if len(arguments) > 1:
# Command only takes in one string so to prevent this
# command from flowing over into movement we break
# before it proceeds.
self.send_feedback(
"Error, more than one argument detected.")
break
else:
try:
x = float(argument.split(' ')[0])
y = float(argument.split(' ')[1])
z = float(argument.split(' ')[2])
# Assumption is if we make it this far, we have successfully
# bound the previous three coordinates.
# The below would fail if we entered a location name instead of coords
# but we should have caught by this point.
# This is to catch anything over 3 coordinates. If
# only two were given then we would also error out
# above.
if len(argument.split(' ')) != 3:
self.send_feedback(
"Incorrect number of coordinates")
break
except IndexError:
self.send_feedback(
"Incorrect number of coordinates")
break
except ValueError:
try:
if argument in ('list', 'l'):
self.send_feedback(
'Available TP locations:')
for k in f:
print ' * ' + k
break
argz = f[argument]
x = float(argz.split(' ')[0])
y = float(argz.split(' ')[1])
z = float(argz.split(' ')[2])
except LookupError:
# This means we did not find the saved location
# referenced by the argument.
self.send_feedback(
"TP location not found, check input.")
break
modelstate = ModelState(
model_name='sub8',
pose=Pose(position=Point(x, y, z),
orientation=Quaternion(0, 0, 0, 0)),
twist=Twist(linear=Point(0, 0, 0),
angular=Point(0, 0, 0)),
reference_frame='world')
# Sometimes you need to sleep in order to get the thing to publish
# Apparently there is a latency when you set a publisher and it needs to actually hook into it.
# As an additional note, given the way we do trajectory in the sim, we must kill sub to prevent
# the trajectory from overriding our teleport and
# bringing it back to its expected position.
ab = TxAlarmBroadcaster(self.nh,
'kill',
node_name='kill')
yield ab.raise_alarm(
problem_description='TELEPORTING: KILLING SUB',
severity=5)
yield self.nh.sleep(1)
yield state_set_pub.publish(modelstate)
yield self.nh.sleep(1)
yield ab.clear_alarm()
except KeyboardInterrupt:
# Catches a ctrl-c situation and ends the program. Break
# will just bring the program to its natural conclusion.
break
elif command in ['zrp', 'level_off', 'zpr']:
self.send_feedback("Zeroing roll and pitch")
res = yield self.move.zero_roll_and_pitch().go(**action_kwargs)
elif command == "stop":
self.send_feedback("Stopping...")
if args.zrp:
res = yield self.move.forward(0).zero_roll_and_pitch().go(
**action_kwargs)
else:
res = yield self.move.forward(0).go(**action_kwargs)
else:
# Get the command from shorthand if it's there
command = SHORTHAND.get(command, command)
movement = getattr(self.move, command)
trans_move = command[:
3] != "yaw" and command[:
5] != "pitch" and command[:
4] != "roll"
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 = argument[:last_digit_index]
unit = argument[last_digit_index:]
extra = "and zeroing pitch and roll" if args.zrp else ""
self.send_feedback("{}ing {}{} {}".format(
command, amount, unit, extra))
bad_unit = UNITS.get(unit, "BAD")
if bad_unit == "BAD":
self.send_feedback("BAD UNIT")
break
goal = movement(float(amount) * UNITS.get(unit, 1))
if args.zrp:
res = yield goal.zero_roll_and_pitch().go(**action_kwargs)
else:
res = yield goal.go(**action_kwargs)
self.send_feedback("Result: {}".format(res.error))
def run(self, args):
commands = args.commands
# Split into commands and arguments, every other index
arguments = commands[1::2]
commands = commands[0::2]
for i in xrange(len(commands)):
command = commands[i]
argument = arguments[i]
self.send_feedback("Waiting for odom...")
yield self.tx_pose()
self.send_feedback("Odom found!")
action_kwargs = {'speed': float(
args.speed), 'blind': bool(args.blind)}
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()".format(argument, **action_kwargs))
elif command in ['tp', 'teleport']:
try:
rospack = rospkg.RosPack()
state_set_pub = yield self.nh.advertise('gazebo/set_model_state', ModelState)
config_file = os.path.join(rospack.get_path(
'sub8_gazebo'), 'config', 'teleport_locs.yaml')
f = yaml.load(open(config_file, 'r'))
if len(arguments) > 1:
# Command only takes in one string so to prevent this
# command from flowing over into movement we break
# before it proceeds.
self.send_feedback(
"Error, more than one argument detected.")
break
else:
try:
x = float(argument.split(' ')[0])
y = float(argument.split(' ')[1])
z = float(argument.split(' ')[2])
# Assumption is if we make it this far, we have successfully
# bound the previous three coordinates.
# The below would fail if we entered a location name instead of coords
# but we should have caught by this point.
# This is to catch anything over 3 coordinates. If
# only two were given then we would also error out
# above.
if len(argument.split(' ')) != 3:
self.send_feedback(
"Incorrect number of coordinates")
break
except IndexError:
self.send_feedback(
"Incorrect number of coordinates")
break
except ValueError:
try:
if argument in ('list', 'l'):
self.send_feedback(
'Available TP locations:')
for k in f:
print ' * ' + k
break
argz = f[argument]
x = float(argz.split(' ')[0])
y = float(argz.split(' ')[1])
z = float(argz.split(' ')[2])
except LookupError:
# This means we did not find the saved location
# referenced by the argument.
self.send_feedback(
"TP location not found, check input.")
break
modelstate = ModelState(
model_name='sub8',
pose=Pose(position=Point(x, y, z),
orientation=Quaternion(0, 0, 0, 0)),
twist=Twist(linear=Point(0, 0, 0),
angular=Point(0, 0, 0)),
reference_frame='world')
# Sometimes you need to sleep in order to get the thing to publish
# Apparently there is a latency when you set a publisher and it needs to actually hook into it.
# As an additional note, given the way we do trajectory in the sim, we must kill sub to prevent
# the trajectory from overriding our teleport and
# bringing it back to its expected position.
ab = TxAlarmBroadcaster(self.nh, 'kill', node_name='kill')
yield ab.raise_alarm(problem_description='TELEPORTING: KILLING SUB',
severity=5)
yield self.nh.sleep(1)
yield state_set_pub.publish(modelstate)
yield self.nh.sleep(1)
yield ab.clear_alarm()
except KeyboardInterrupt:
# Catches a ctrl-c situation and ends the program. Break
# will just bring the program to its natural conclusion.
break
elif command in ['zrp', 'level_off', 'zpr']:
self.send_feedback("Zeroing roll and pitch")
res = yield self.move.zero_roll_and_pitch().go(**action_kwargs)
elif command == "stop":
self.send_feedback("Stopping...")
if args.zrp:
res = yield self.move.forward(0).zero_roll_and_pitch().go(**action_kwargs)
else:
res = yield self.move.forward(0).go(**action_kwargs)
else:
# Get the command from shorthand if it's there
command = SHORTHAND.get(command, command)
movement = getattr(self.move, command)
trans_move = command[:3] != "yaw" and command[:
5] != "pitch" and command[:4] != "roll"
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 = argument[:last_digit_index]
unit = argument[last_digit_index:]
extra = "and zeroing pitch and roll" if args.zrp else ""
self.send_feedback("{}ing {}{} {}".format(
command, amount, unit, extra))
bad_unit = UNITS.get(unit, "BAD")
if bad_unit == "BAD":
self.send_feedback("BAD UNIT")
break
goal = movement(float(amount) * UNITS.get(unit, 1))
if args.zrp:
res = yield goal.zero_roll_and_pitch().go(**action_kwargs)
else:
res = yield goal.go(**action_kwargs)
self.send_feedback("Result: {}".format(res.error))