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(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))