From 14c56865eff76fe3ee5f521eabd11790056abc63 Mon Sep 17 00:00:00 2001 From: Kevin Allen Date: Mon, 14 Jan 2019 15:13:50 -0500 Subject: [PATCH 1/5] MISSIONS: move files for mil_missions conversion --- command/sub8_missions/nodes/list_missions | 10 -- command/sub8_missions/nodes/run_mission | 95 ------------------- command/sub8_missions/setup.py | 2 +- .../{missions => sub8_missions}/__init__.py | 7 ++ .../arm_torpedos.py | 0 .../{missions => sub8_missions}/autonomous.py | 0 .../barrel_roll.py | 0 .../bin_dropper.py | 0 .../{missions => sub8_missions}/buoy.py | 0 .../data_gather.py | 0 .../{missions => sub8_missions}/dice.py | 0 .../{missions => sub8_missions}/down_2m.py | 0 .../{missions => sub8_missions}/level_off.py | 0 .../move_command => sub8_missions/move.py} | 0 .../{missions => sub8_missions}/pinger.py | 0 .../{sub8 => sub8_missions}/pose_editor.py | 0 .../pose_sequence_test.py | 0 .../qualification.py | 0 .../roulette_wheel.py | 0 .../search_test.py | 0 .../{missions => sub8_missions}/square.py | 0 .../{missions => sub8_missions}/start_gate.py | 0 .../start_gate_guess.py | 0 .../start_gate_simple.py | 0 .../{missions => sub8_missions}/strip.py | 0 .../{sub8 => sub8_missions}/sub_singleton.py | 0 .../{missions => sub8_missions}/surface.py | 0 .../test_actuators.py | 0 .../{missions => sub8_missions}/the_path.py | 0 .../{missions => sub8_missions}/tx_test.py | 0 .../vision_proxies.yaml | 0 deprecated/CATKIN_IGNORE | 0 32 files changed, 8 insertions(+), 106 deletions(-) delete mode 100755 command/sub8_missions/nodes/list_missions delete mode 100755 command/sub8_missions/nodes/run_mission rename command/sub8_missions/{missions => sub8_missions}/__init__.py (53%) rename command/sub8_missions/{missions => sub8_missions}/arm_torpedos.py (100%) rename command/sub8_missions/{missions => sub8_missions}/autonomous.py (100%) rename command/sub8_missions/{missions => sub8_missions}/barrel_roll.py (100%) rename command/sub8_missions/{missions => sub8_missions}/bin_dropper.py (100%) rename command/sub8_missions/{missions => sub8_missions}/buoy.py (100%) rename command/sub8_missions/{missions => sub8_missions}/data_gather.py (100%) rename command/sub8_missions/{missions => sub8_missions}/dice.py (100%) rename command/sub8_missions/{missions => sub8_missions}/down_2m.py (100%) rename command/sub8_missions/{missions => sub8_missions}/level_off.py (100%) rename command/sub8_missions/{nodes/move_command => sub8_missions/move.py} (100%) rename command/sub8_missions/{missions => sub8_missions}/pinger.py (100%) rename command/sub8_missions/{sub8 => sub8_missions}/pose_editor.py (100%) rename command/sub8_missions/{missions => sub8_missions}/pose_sequence_test.py (100%) rename command/sub8_missions/{missions => sub8_missions}/qualification.py (100%) rename command/sub8_missions/{missions => sub8_missions}/roulette_wheel.py (100%) rename command/sub8_missions/{missions => sub8_missions}/search_test.py (100%) rename command/sub8_missions/{missions => sub8_missions}/square.py (100%) rename command/sub8_missions/{missions => sub8_missions}/start_gate.py (100%) rename command/sub8_missions/{missions => sub8_missions}/start_gate_guess.py (100%) rename command/sub8_missions/{missions => sub8_missions}/start_gate_simple.py (100%) rename command/sub8_missions/{missions => sub8_missions}/strip.py (100%) rename command/sub8_missions/{sub8 => sub8_missions}/sub_singleton.py (100%) rename command/sub8_missions/{missions => sub8_missions}/surface.py (100%) rename command/sub8_missions/{missions => sub8_missions}/test_actuators.py (100%) rename command/sub8_missions/{missions => sub8_missions}/the_path.py (100%) rename command/sub8_missions/{missions => sub8_missions}/tx_test.py (100%) rename command/sub8_missions/{sub8 => sub8_missions}/vision_proxies.yaml (100%) create mode 100644 deprecated/CATKIN_IGNORE diff --git a/command/sub8_missions/nodes/list_missions b/command/sub8_missions/nodes/list_missions deleted file mode 100755 index 95439442..00000000 --- a/command/sub8_missions/nodes/list_missions +++ /dev/null @@ -1,10 +0,0 @@ -#!/usr/bin/env python - -from sub8 import tx_sub -import missions - -print -print "Current missions:" -mission_list = [mission_name for mission_name in dir(missions) if not mission_name.startswith('_')] -print ' %s' % '\n '.join(map(str, mission_list)) -print \ No newline at end of file diff --git a/command/sub8_missions/nodes/run_mission b/command/sub8_missions/nodes/run_mission deleted file mode 100755 index 8cc893ae..00000000 --- a/command/sub8_missions/nodes/run_mission +++ /dev/null @@ -1,95 +0,0 @@ -#!/usr/bin/env python - -import signal -import traceback -import argparse -from twisted.internet import defer, reactor -import exceptions -import txros -from mil_misc_tools import text_effects -from sub8 import sub_singleton -import missions - - -fprint = text_effects.FprintFactory(title="MISSION_RUNNER").fprint - -@txros.util.cancellableInlineCallbacks -def main(): - try: - nh_args = yield txros.NodeHandle.from_argv_with_remaining('sub8_mission') - available_missions = [mission_name for mission_name in dir(missions) if not mission_name.startswith('_')] - - usage_msg = ("Input the name of the mission you would like to run.\nExamples: " + - "\n\n\trosrun sub8_missions mission stop\n\trosrun sub8_missions mission forward_1_m") - desc_msg = "-- Mr. Mission Manager --" - parser = argparse.ArgumentParser(usage=usage_msg, description=desc_msg) - parser.add_argument(dest='mission_names', nargs='+', - help="The name of the mission you'd like to run (ex: stop)") - - parser.add_argument('--test', dest='test', action='store_true', - help="Set this flag if you'd like to ignore motions") - - nh, args = nh_args - args = parser.parse_args(args[1:]) - - # Kludge to print mission list - if args.mission_names[0] == 'list': - mission_list = [mission_name for mission_name in dir(missions) if not mission_name.startswith('_')] - print ' %s' % '\n '.join(map(str, mission_list)) - return - - mission_names = args.mission_names - mission_list = [] - for mission_name in mission_names: - assert mission_name in available_missions, "'{}' is not an available mission; we have: {}".format( - mission_name, ', '.join(available_missions) - ) - mission = getattr(missions, mission_name) - mission_list.append(mission) - - sub = yield sub_singleton.get_sub(nh) - yield txros.util.wall_sleep(1.0) - - if args.test: - sub.set_test_mode() - else: - fprint('Waiting for odom...') - yield sub.tx_pose() - - fprint('Odom found!', msg_color='green', newline=2) - for mission in mission_list: - yield mission.run(sub).addErrback(catch_error) - yield txros.util.wall_sleep(1.0) # Seems to make things behave more nicely - - except Exception: - traceback.print_exc() - - finally: - fprint('Finishing mission execution') - reactor.stop() - - -def catch_error(error): - # CancelledErrors occur from something bad happen with defers in the mission - error.trap(defer.CancelledError) - if error.check(defer.CancelledError): - return - - fprint("Error caught!!!", msg_color='red') - print 'tx traceback' - print '>------------------------------<' - print error.printTraceback() - print '>------------------------------<' - - -def _start(): - # Handle ctrl+C - signal.signal(signal.SIGINT, lambda signum, frame: reactor.callFromThread(task.cancel)) - - # Handle errors in main - task = main().addErrback(lambda fail: fail.trap(defer.CancelledError)) - - -if __name__ == '__main__': - reactor.callWhenRunning(_start) - reactor.run() diff --git a/command/sub8_missions/setup.py b/command/sub8_missions/setup.py index 161f2e58..e2e87ff2 100644 --- a/command/sub8_missions/setup.py +++ b/command/sub8_missions/setup.py @@ -5,7 +5,7 @@ # fetch values from package.xml setup_args = generate_distutils_setup( - packages=['sub8', 'missions'], + packages=['sub8_missions'], ) setup(**setup_args) diff --git a/command/sub8_missions/missions/__init__.py b/command/sub8_missions/sub8_missions/__init__.py similarity index 53% rename from command/sub8_missions/missions/__init__.py rename to command/sub8_missions/sub8_missions/__init__.py index 3fa80394..5da5efa9 100644 --- a/command/sub8_missions/missions/__init__.py +++ b/command/sub8_missions/sub8_missions/__init__.py @@ -6,3 +6,10 @@ __import__(module[:-3], locals(), globals()) del module del os +# flake8: noqa +import sub_singleton as sub_singleton +import pose_editor + +from sub_singleton import Searcher +from sub_singleton import SonarPointcloud +from sub_singleton import SonarObjects diff --git a/command/sub8_missions/missions/arm_torpedos.py b/command/sub8_missions/sub8_missions/arm_torpedos.py similarity index 100% rename from command/sub8_missions/missions/arm_torpedos.py rename to command/sub8_missions/sub8_missions/arm_torpedos.py diff --git a/command/sub8_missions/missions/autonomous.py b/command/sub8_missions/sub8_missions/autonomous.py similarity index 100% rename from command/sub8_missions/missions/autonomous.py rename to command/sub8_missions/sub8_missions/autonomous.py diff --git a/command/sub8_missions/missions/barrel_roll.py b/command/sub8_missions/sub8_missions/barrel_roll.py similarity index 100% rename from command/sub8_missions/missions/barrel_roll.py rename to command/sub8_missions/sub8_missions/barrel_roll.py diff --git a/command/sub8_missions/missions/bin_dropper.py b/command/sub8_missions/sub8_missions/bin_dropper.py similarity index 100% rename from command/sub8_missions/missions/bin_dropper.py rename to command/sub8_missions/sub8_missions/bin_dropper.py diff --git a/command/sub8_missions/missions/buoy.py b/command/sub8_missions/sub8_missions/buoy.py similarity index 100% rename from command/sub8_missions/missions/buoy.py rename to command/sub8_missions/sub8_missions/buoy.py diff --git a/command/sub8_missions/missions/data_gather.py b/command/sub8_missions/sub8_missions/data_gather.py similarity index 100% rename from command/sub8_missions/missions/data_gather.py rename to command/sub8_missions/sub8_missions/data_gather.py diff --git a/command/sub8_missions/missions/dice.py b/command/sub8_missions/sub8_missions/dice.py similarity index 100% rename from command/sub8_missions/missions/dice.py rename to command/sub8_missions/sub8_missions/dice.py diff --git a/command/sub8_missions/missions/down_2m.py b/command/sub8_missions/sub8_missions/down_2m.py similarity index 100% rename from command/sub8_missions/missions/down_2m.py rename to command/sub8_missions/sub8_missions/down_2m.py diff --git a/command/sub8_missions/missions/level_off.py b/command/sub8_missions/sub8_missions/level_off.py similarity index 100% rename from command/sub8_missions/missions/level_off.py rename to command/sub8_missions/sub8_missions/level_off.py diff --git a/command/sub8_missions/nodes/move_command b/command/sub8_missions/sub8_missions/move.py similarity index 100% rename from command/sub8_missions/nodes/move_command rename to command/sub8_missions/sub8_missions/move.py diff --git a/command/sub8_missions/missions/pinger.py b/command/sub8_missions/sub8_missions/pinger.py similarity index 100% rename from command/sub8_missions/missions/pinger.py rename to command/sub8_missions/sub8_missions/pinger.py diff --git a/command/sub8_missions/sub8/pose_editor.py b/command/sub8_missions/sub8_missions/pose_editor.py similarity index 100% rename from command/sub8_missions/sub8/pose_editor.py rename to command/sub8_missions/sub8_missions/pose_editor.py diff --git a/command/sub8_missions/missions/pose_sequence_test.py b/command/sub8_missions/sub8_missions/pose_sequence_test.py similarity index 100% rename from command/sub8_missions/missions/pose_sequence_test.py rename to command/sub8_missions/sub8_missions/pose_sequence_test.py diff --git a/command/sub8_missions/missions/qualification.py b/command/sub8_missions/sub8_missions/qualification.py similarity index 100% rename from command/sub8_missions/missions/qualification.py rename to command/sub8_missions/sub8_missions/qualification.py diff --git a/command/sub8_missions/missions/roulette_wheel.py b/command/sub8_missions/sub8_missions/roulette_wheel.py similarity index 100% rename from command/sub8_missions/missions/roulette_wheel.py rename to command/sub8_missions/sub8_missions/roulette_wheel.py diff --git a/command/sub8_missions/missions/search_test.py b/command/sub8_missions/sub8_missions/search_test.py similarity index 100% rename from command/sub8_missions/missions/search_test.py rename to command/sub8_missions/sub8_missions/search_test.py diff --git a/command/sub8_missions/missions/square.py b/command/sub8_missions/sub8_missions/square.py similarity index 100% rename from command/sub8_missions/missions/square.py rename to command/sub8_missions/sub8_missions/square.py diff --git a/command/sub8_missions/missions/start_gate.py b/command/sub8_missions/sub8_missions/start_gate.py similarity index 100% rename from command/sub8_missions/missions/start_gate.py rename to command/sub8_missions/sub8_missions/start_gate.py diff --git a/command/sub8_missions/missions/start_gate_guess.py b/command/sub8_missions/sub8_missions/start_gate_guess.py similarity index 100% rename from command/sub8_missions/missions/start_gate_guess.py rename to command/sub8_missions/sub8_missions/start_gate_guess.py diff --git a/command/sub8_missions/missions/start_gate_simple.py b/command/sub8_missions/sub8_missions/start_gate_simple.py similarity index 100% rename from command/sub8_missions/missions/start_gate_simple.py rename to command/sub8_missions/sub8_missions/start_gate_simple.py diff --git a/command/sub8_missions/missions/strip.py b/command/sub8_missions/sub8_missions/strip.py similarity index 100% rename from command/sub8_missions/missions/strip.py rename to command/sub8_missions/sub8_missions/strip.py diff --git a/command/sub8_missions/sub8/sub_singleton.py b/command/sub8_missions/sub8_missions/sub_singleton.py similarity index 100% rename from command/sub8_missions/sub8/sub_singleton.py rename to command/sub8_missions/sub8_missions/sub_singleton.py diff --git a/command/sub8_missions/missions/surface.py b/command/sub8_missions/sub8_missions/surface.py similarity index 100% rename from command/sub8_missions/missions/surface.py rename to command/sub8_missions/sub8_missions/surface.py diff --git a/command/sub8_missions/missions/test_actuators.py b/command/sub8_missions/sub8_missions/test_actuators.py similarity index 100% rename from command/sub8_missions/missions/test_actuators.py rename to command/sub8_missions/sub8_missions/test_actuators.py diff --git a/command/sub8_missions/missions/the_path.py b/command/sub8_missions/sub8_missions/the_path.py similarity index 100% rename from command/sub8_missions/missions/the_path.py rename to command/sub8_missions/sub8_missions/the_path.py diff --git a/command/sub8_missions/missions/tx_test.py b/command/sub8_missions/sub8_missions/tx_test.py similarity index 100% rename from command/sub8_missions/missions/tx_test.py rename to command/sub8_missions/sub8_missions/tx_test.py diff --git a/command/sub8_missions/sub8/vision_proxies.yaml b/command/sub8_missions/sub8_missions/vision_proxies.yaml similarity index 100% rename from command/sub8_missions/sub8/vision_proxies.yaml rename to command/sub8_missions/sub8_missions/vision_proxies.yaml diff --git a/deprecated/CATKIN_IGNORE b/deprecated/CATKIN_IGNORE new file mode 100644 index 00000000..e69de29b From ec742df5fc805c74f65ea407baf8dd5cf48bc47a Mon Sep 17 00:00:00 2001 From: Kevin Allen Date: Mon, 14 Jan 2019 15:45:54 -0500 Subject: [PATCH 2/5] MISSIONS: converted move command, sub singleton to mil_missions --- .../sub8_launch/launch/mission_server.launch | 6 + command/sub8_missions/sub8/__init__.py | 7 - .../sub8_missions/sub8_missions/__init__.py | 17 +- command/sub8_missions/sub8_missions/move.py | 278 +++++++++--------- .../sub8_missions/sub_singleton.py | 54 ++-- 5 files changed, 159 insertions(+), 203 deletions(-) create mode 100644 command/sub8_launch/launch/mission_server.launch delete mode 100644 command/sub8_missions/sub8/__init__.py diff --git a/command/sub8_launch/launch/mission_server.launch b/command/sub8_launch/launch/mission_server.launch new file mode 100644 index 00000000..c541fed4 --- /dev/null +++ b/command/sub8_launch/launch/mission_server.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/command/sub8_missions/sub8/__init__.py b/command/sub8_missions/sub8/__init__.py deleted file mode 100644 index afed528c..00000000 --- a/command/sub8_missions/sub8/__init__.py +++ /dev/null @@ -1,7 +0,0 @@ -# flake8: noqa -import sub_singleton as sub_singleton -import pose_editor - -from sub_singleton import Searcher -from sub_singleton import SonarPointcloud -from sub_singleton import SonarObjects diff --git a/command/sub8_missions/sub8_missions/__init__.py b/command/sub8_missions/sub8_missions/__init__.py index 5da5efa9..c614429a 100644 --- a/command/sub8_missions/sub8_missions/__init__.py +++ b/command/sub8_missions/sub8_missions/__init__.py @@ -1,15 +1,2 @@ -# flake8: noqa -import os -for module in os.listdir(os.path.dirname(__file__)): - if module == '__init__.py' or module[-3:] != '.py': - continue - __import__(module[:-3], locals(), globals()) -del module -del os -# flake8: noqa -import sub_singleton as sub_singleton -import pose_editor - -from sub_singleton import Searcher -from sub_singleton import SonarPointcloud -from sub_singleton import SonarObjects +from sub_singleton import SubjuGator +from move import Move diff --git a/command/sub8_missions/sub8_missions/move.py b/command/sub8_missions/sub8_missions/move.py index e16fc01b..f0990247 100755 --- a/command/sub8_missions/sub8_missions/move.py +++ b/command/sub8_missions/sub8_missions/move.py @@ -1,171 +1,157 @@ #!/usr/bin/env python +from sub_singleton import SubjuGator from txros import util, NodeHandle from twisted.internet import defer, reactor - import numpy as np from mil_misc_tools import text_effects -from sub8 import sub_singleton from geometry_msgs.msg import PoseStamped, PointStamped, Pose, Point, Quaternion, Twist from gazebo_msgs.msg import ModelState -import missions -from txros import publisher from ros_alarms import TxAlarmBroadcaster import rospkg import yaml import os - - -import argparse +from mil_misc_tools import ThrowingArgumentParser +from sub_singleton import SubjuGator UNITS = {'m': 1, 'ft': 0.3048, 'yard': 0.9144, 'rad': 1, 'deg': 0.0174533} SHORTHAND = {"f": "forward", "b": "backward", "l": "left", "r": "right", "yl": "yaw_left", "yr": "yaw_right", "d": "down", "u": "up"} -ros_t = lambda d: util.genpy.Duration(d) -fprint = text_effects.FprintFactory(title="MOVE_COMMAND").fprint - - -@util.cancellableInlineCallbacks -def main(args): - commands = args.commands - arguments = commands[1::2] #Split into commands and arguments, every other index - commands = commands[0::2] - for i in xrange(len(commands)): - command = commands[i] - argument = arguments[i] - nh, _ = yield NodeHandle.from_argv_with_remaining("move_command", anonymous=True) - available_missions = [mission_name for mission_name in dir(missions) if not mission_name.startswith('_')] - - sub = yield sub_singleton._Sub(nh)._init() - if args.test: - fprint("Running in test mode", msg_color='yellow') - sub.set_test_mode() - fprint("Waiting for odom...") - yield sub.tx_pose() - fprint("Odom found!", msg_color='green') - 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. - fprint("Moving with the command: {}".format(argument)) - res = yield eval("sub.move.{}.go()".format(argument, **action_kwargs)) - - elif command in ['tp', 'teleport']: - try: - rospack = rospkg.RosPack() - state_set_pub = yield 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. - fprint("Error, more than one argument detected." , msg_color='red') - 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: - fprint("Incorrect number of coordinates", msg_color='red') - break - except IndexError: - fprint("Incorrect number of coordinates", msg_color='red') +class Move(SubjuGator): + @classmethod + def decode_parameters(cls, parameters): + argv = parameters.split() + return cls.parser.parse_args(argv) + + @classmethod + def init(cls): + parser = ThrowingArgumentParser(description='Command Line Mission Runner', + usage='Pass any pose editor commands with an arguments. \n\t forward 1 (moves forward 1 meter) \n\t backward 2ft (moves backward 2 feet) \n\t forward 1 backward 2ft (moves forward 1 meter then from there moves backward 2 feet), ') + parser.add_argument('commands', type=str, nargs='*', + help='Pose editor commands to run each followed by an argument to pass to the command (distance or angle usally). Optionally a unit can be added if a non-standard unit is desired.') + parser.add_argument('-s', '--speed', type=float, default=0.2, + help="How fast to execute the move. (m/s)") + parser.add_argument('-z', '--zrp', action='store_true', + help="Make end goal have zero roll and pitch.") + parser.add_argument('-b', '--blind', action='store_true', + help="Do not check if waypoint is safe.") + cls.parser = parser + + + @util.cancellableInlineCallbacks + def run(self, args): + commands = args.commands + arguments = commands[1::2] #Split into commands and arguments, every other index + 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 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 - except ValueError: + else: try: - if argument in ('list', 'l'): - fprint('Available TP locations:') - for k in f: - print ' * ' + k + 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 - argz = f[argument] - x = float(argz.split(' ')[0]) - y = float(argz.split(' ')[1]) - z = float(argz.split(' ')[2]) - except LookupError as e: - #This means we did not find the saved location referenced by the argument. - fprint("TP location not found, check input.", msg_color='red') + except IndexError: + self.send_feedback("Incorrect number of coordinates") 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(nh,'kill', node_name='kill') - yield ab.raise_alarm(problem_description='TELEPORTING: KILLING SUB', - severity=5) - yield nh.sleep(1) - yield state_set_pub.publish(modelstate) - yield nh.sleep(1) - yield ab.clear_alarm() - - except KeyboardInterrupt as e: - #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']: - fprint("Zeroing roll and pitch") - res = yield sub.move.zero_roll_and_pitch().go(**action_kwargs) - - elif command == "stop": - fprint("Stopping...") - if args.zrp: - res = yield sub.move.forward(0).zero_roll_and_pitch().go(**action_kwargs) - else: - res = yield sub.move.forward(0).go(**action_kwargs) - - else: - # Get the command from shorthand if it's there - command = SHORTHAND.get(command, command) - movement = getattr(sub.move, command) + 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 as e: + #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(nh,'kill', node_name='kill') + yield ab.raise_alarm(problem_description='TELEPORTING: KILLING SUB', + severity=5) + yield nh.sleep(1) + yield state_set_pub.publish(modelstate) + yield nh.sleep(1) + yield ab.clear_alarm() + + except KeyboardInterrupt as e: + #Catches a ctrl-c situation and ends the program. Break will just bring the program to its natural conclusion. + break - trans_move = command[:3] != "yaw" and command[:5] != "pitch" and command[:4] != "roll" - unit = "m" if trans_move else "rad" + 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) - 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:] + 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) - extra = "and zeroing pitch and roll" if args.zrp else "" - fprint("{}ing {}{} {}".format(command, amount, unit, extra)) - bad_unit = UNITS.get(unit,"BAD") - if bad_unit == "BAD": - fprint("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) - fprint("Result: {}".format(res.error)) - - defer.returnValue(reactor.stop()) - -if __name__ == '__main__': - parser = argparse.ArgumentParser(description='Command Line Mission Runner', - usage='Pass any pose editor commands with an arguments. \n\t forward 1 (moves forward 1 meter) \n\t backward 2ft (moves backward 2 feet) \n\t forward 1 backward 2ft (moves forward 1 meter then from there moves backward 2 feet), ') - parser.add_argument('commands', type=str, nargs='*', - help='Pose editor commands to run each followed by an argument to pass to the command (distance or angle usally). Optionally a unit can be added if a non-standard unit is desired.') - parser.add_argument('-t', '--test', action='store_true', - help="Runs the mission in test mode and not move the sub.") - parser.add_argument('-s', '--speed', type=float, default=0.2, - help="How fast to execute the move. (m/s)") - parser.add_argument('-z', '--zrp', action='store_true', - help="Make end goal have zero roll and pitch.") - parser.add_argument('-b', '--blind', action='store_true', - help="Do not check if waypoint is safe.") - args = parser.parse_args() - - reactor.callWhenRunning(lambda: main(args)) - reactor.run() + # 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)) diff --git a/command/sub8_missions/sub8_missions/sub_singleton.py b/command/sub8_missions/sub8_missions/sub_singleton.py index 9c1829e3..883db3cd 100644 --- a/command/sub8_missions/sub8_missions/sub_singleton.py +++ b/command/sub8_missions/sub8_missions/sub_singleton.py @@ -6,7 +6,7 @@ import rospkg from mil_msgs.msg import MoveToAction, PoseTwistStamped, RangeStamped -from sub8 import pose_editor +import pose_editor import mil_ros_tools from sub8_msgs.srv import VisionRequest, VisionRequestRequest, VisionRequest2DRequest, VisionRequest2D from mil_msgs.srv import SetGeometry, SetGeometryRequest @@ -17,6 +17,7 @@ from tf.transformations import quaternion_multiply, quaternion_from_euler from sensor_msgs.msg import PointCloud2 import sensor_msgs.point_cloud2 as pc2 +from mil_missions_core import BaseMission import numpy as np from twisted.internet import defer @@ -119,7 +120,7 @@ class _VisionProxies(object): def __init__(self, nh, file_name): rospack = rospkg.RosPack() config_file = os.path.join( - rospack.get_path('sub8_missions'), 'sub8', file_name) + rospack.get_path('sub8_missions'), 'sub8_missions', file_name) f = yaml.load(open(config_file, 'r')) self.proxies = {} @@ -224,32 +225,28 @@ def drop_marker(self): return self.set('dropper', True) -class _Sub(object): - - def __init__(self, node_handle): - self.nh = node_handle - self.test_mode = False +class SubjuGator(BaseMission): + def __init__(self, **kwargs): + super(SubjuGator, self).__init__(**kwargs) + @classmethod @util.cancellableInlineCallbacks - def _init(self): - self._moveto_action_client = yield action.ActionClient( - self.nh, 'moveto', MoveToAction) - self._odom_sub = yield self.nh.subscribe('odom', Odometry) - self._trajectory_sub = yield self.nh.subscribe('trajectory', + def _init(cls, mission_server): + super(SubjuGator, cls)._init(mission_server) + cls._moveto_action_client = yield action.ActionClient( + cls.nh, 'moveto', MoveToAction) + cls._odom_sub = yield cls.nh.subscribe('odom', Odometry) + cls._trajectory_sub = yield cls.nh.subscribe('trajectory', PoseTwistStamped) - self._trajectory_pub = yield self.nh.advertise('trajectory', + cls._trajectory_pub = yield cls.nh.advertise('trajectory', PoseTwistStamped) - self._dvl_range_sub = yield self.nh.subscribe('dvl/range', + cls._dvl_range_sub = yield cls.nh.subscribe('dvl/range', RangeStamped) - self._tf_listener = yield tf.TransformListener(self.nh) - - self.vision_proxies = _VisionProxies(self.nh, 'vision_proxies.yaml') - self.actuators = _ActuatorProxy(self.nh) - - defer.returnValue(self) + cls._tf_listener = yield tf.TransformListener(cls.nh) - def set_test_mode(self): - self.test_mode = True + cls.vision_proxies = _VisionProxies(cls.nh, 'vision_proxies.yaml') + cls.actuators = _ActuatorProxy(cls.nh) + cls.test_mode = False @property def pose(self): @@ -683,16 +680,3 @@ def _run_move_pattern(self, speed): yield self._cat_newcloud() yield self.sub.nh.sleep(2) - - -_subs = {} - - -@util.cancellableInlineCallbacks -def get_sub(node_handle, need_trajectory=True): - if node_handle not in _subs: - _subs[ - node_handle] = None # placeholder to prevent this from happening reentrantly - _subs[node_handle] = yield _Sub(node_handle)._init() - # XXX remove on nodehandle shutdown - defer.returnValue(_subs[node_handle]) From fe7be02ba18c2086e88933720a28bed8e84a8e04 Mon Sep 17 00:00:00 2001 From: Kevin Allen Date: Mon, 14 Jan 2019 18:31:30 -0500 Subject: [PATCH 3/5] MISSIONS: move square, surface mission to mil_missions; deprecate others --- command/sub8_missions/sub8_missions/square.py | 31 +++++++++---------- .../sub8_missions/sub8_missions/surface.py | 14 ++++++--- .../sub8_missions/arm_torpedos.py | 0 .../sub8_missions/sub8_missions/autonomous.py | 0 .../sub8_missions/barrel_roll.py | 0 .../sub8_missions/bin_dropper.py | 0 .../sub8_missions/sub8_missions/buoy.py | 0 .../sub8_missions/data_gather.py | 0 .../sub8_missions/sub8_missions/dice.py | 0 .../sub8_missions/sub8_missions/down_2m.py | 0 .../sub8_missions/sub8_missions/level_off.py | 0 .../sub8_missions/sub8_missions/pinger.py | 0 .../sub8_missions/pose_sequence_test.py | 0 .../sub8_missions/qualification.py | 0 .../sub8_missions/roulette_wheel.py | 0 .../sub8_missions/search_test.py | 0 .../sub8_missions/sub8_missions/start_gate.py | 0 .../sub8_missions/start_gate_guess.py | 0 .../sub8_missions/start_gate_simple.py | 0 .../sub8_missions/sub8_missions/strip.py | 0 .../sub8_missions/test_actuators.py | 0 .../sub8_missions/sub8_missions/the_path.py | 0 .../sub8_missions/sub8_missions/tx_test.py | 0 23 files changed, 24 insertions(+), 21 deletions(-) rename {command => deprecated/command}/sub8_missions/sub8_missions/arm_torpedos.py (100%) rename {command => deprecated/command}/sub8_missions/sub8_missions/autonomous.py (100%) rename {command => deprecated/command}/sub8_missions/sub8_missions/barrel_roll.py (100%) rename {command => deprecated/command}/sub8_missions/sub8_missions/bin_dropper.py (100%) rename {command => deprecated/command}/sub8_missions/sub8_missions/buoy.py (100%) rename {command => deprecated/command}/sub8_missions/sub8_missions/data_gather.py (100%) rename {command => deprecated/command}/sub8_missions/sub8_missions/dice.py (100%) rename {command => deprecated/command}/sub8_missions/sub8_missions/down_2m.py (100%) rename {command => deprecated/command}/sub8_missions/sub8_missions/level_off.py (100%) rename {command => deprecated/command}/sub8_missions/sub8_missions/pinger.py (100%) rename {command => deprecated/command}/sub8_missions/sub8_missions/pose_sequence_test.py (100%) rename {command => deprecated/command}/sub8_missions/sub8_missions/qualification.py (100%) rename {command => deprecated/command}/sub8_missions/sub8_missions/roulette_wheel.py (100%) rename {command => deprecated/command}/sub8_missions/sub8_missions/search_test.py (100%) rename {command => deprecated/command}/sub8_missions/sub8_missions/start_gate.py (100%) rename {command => deprecated/command}/sub8_missions/sub8_missions/start_gate_guess.py (100%) rename {command => deprecated/command}/sub8_missions/sub8_missions/start_gate_simple.py (100%) rename {command => deprecated/command}/sub8_missions/sub8_missions/strip.py (100%) rename {command => deprecated/command}/sub8_missions/sub8_missions/test_actuators.py (100%) rename {command => deprecated/command}/sub8_missions/sub8_missions/the_path.py (100%) rename {command => deprecated/command}/sub8_missions/sub8_missions/tx_test.py (100%) diff --git a/command/sub8_missions/sub8_missions/square.py b/command/sub8_missions/sub8_missions/square.py index a6379621..8a386717 100644 --- a/command/sub8_missions/sub8_missions/square.py +++ b/command/sub8_missions/sub8_missions/square.py @@ -1,23 +1,22 @@ +from sub_singleton import SubjuGator from txros import util SIDE_LENGTH = 1 # meters SPEED_LIMIT = .2 # m/s +class Square(SubjuGator): + @util.cancellableInlineCallbacks + def run(self): + center = self.move.forward(0).zero_roll_and_pitch() + for i in range(4): + forward = self.move.forward(SIDE_LENGTH).zero_roll_and_pitch() + right = forward.right(SIDE_LENGTH).zero_roll_and_pitch() -@util.cancellableInlineCallbacks -def run(sub): - print "Square!" + yield forward.go(speed=SPEED_LIMIT) + yield right.go(speed=SPEED_LIMIT) + yield forward.go(speed=SPEED_LIMIT) + yield center.go(speed=SPEED_LIMIT) + center = center.yaw_right_deg(90) + yield center.go(speed=SPEED_LIMIT) - center = sub.move.forward(0).zero_roll_and_pitch() - for i in range(4): - forward = sub.move.forward(SIDE_LENGTH).zero_roll_and_pitch() - right = forward.right(SIDE_LENGTH).zero_roll_and_pitch() - - yield forward.go(speed=SPEED_LIMIT) - yield right.go(speed=SPEED_LIMIT) - yield forward.go(speed=SPEED_LIMIT) - yield center.go(speed=SPEED_LIMIT) - center = center.yaw_right_deg(90) - yield center.go(speed=SPEED_LIMIT) - - print "Done!" + print "Done!" diff --git a/command/sub8_missions/sub8_missions/surface.py b/command/sub8_missions/sub8_missions/surface.py index 2ec400f2..fea0b65f 100644 --- a/command/sub8_missions/sub8_missions/surface.py +++ b/command/sub8_missions/sub8_missions/surface.py @@ -1,8 +1,12 @@ +#!/usr/bin/env python +from sub_singleton import SubjuGator from txros import util +from twisted.internet import defer -@util.cancellableInlineCallbacks -def run(sub_singleton): - print "Surfacing" - yield sub_singleton.move.depth(0.6).go() - print "Done surfacing" +class Surface(SubjuGator): + @util.cancellableInlineCallbacks + def run(self): + self.send_feedback('Surfacing') + yield self.move.depth(0.6).go() + defer.returnValue('Success!') diff --git a/command/sub8_missions/sub8_missions/arm_torpedos.py b/deprecated/command/sub8_missions/sub8_missions/arm_torpedos.py similarity index 100% rename from command/sub8_missions/sub8_missions/arm_torpedos.py rename to deprecated/command/sub8_missions/sub8_missions/arm_torpedos.py diff --git a/command/sub8_missions/sub8_missions/autonomous.py b/deprecated/command/sub8_missions/sub8_missions/autonomous.py similarity index 100% rename from command/sub8_missions/sub8_missions/autonomous.py rename to deprecated/command/sub8_missions/sub8_missions/autonomous.py diff --git a/command/sub8_missions/sub8_missions/barrel_roll.py b/deprecated/command/sub8_missions/sub8_missions/barrel_roll.py similarity index 100% rename from command/sub8_missions/sub8_missions/barrel_roll.py rename to deprecated/command/sub8_missions/sub8_missions/barrel_roll.py diff --git a/command/sub8_missions/sub8_missions/bin_dropper.py b/deprecated/command/sub8_missions/sub8_missions/bin_dropper.py similarity index 100% rename from command/sub8_missions/sub8_missions/bin_dropper.py rename to deprecated/command/sub8_missions/sub8_missions/bin_dropper.py diff --git a/command/sub8_missions/sub8_missions/buoy.py b/deprecated/command/sub8_missions/sub8_missions/buoy.py similarity index 100% rename from command/sub8_missions/sub8_missions/buoy.py rename to deprecated/command/sub8_missions/sub8_missions/buoy.py diff --git a/command/sub8_missions/sub8_missions/data_gather.py b/deprecated/command/sub8_missions/sub8_missions/data_gather.py similarity index 100% rename from command/sub8_missions/sub8_missions/data_gather.py rename to deprecated/command/sub8_missions/sub8_missions/data_gather.py diff --git a/command/sub8_missions/sub8_missions/dice.py b/deprecated/command/sub8_missions/sub8_missions/dice.py similarity index 100% rename from command/sub8_missions/sub8_missions/dice.py rename to deprecated/command/sub8_missions/sub8_missions/dice.py diff --git a/command/sub8_missions/sub8_missions/down_2m.py b/deprecated/command/sub8_missions/sub8_missions/down_2m.py similarity index 100% rename from command/sub8_missions/sub8_missions/down_2m.py rename to deprecated/command/sub8_missions/sub8_missions/down_2m.py diff --git a/command/sub8_missions/sub8_missions/level_off.py b/deprecated/command/sub8_missions/sub8_missions/level_off.py similarity index 100% rename from command/sub8_missions/sub8_missions/level_off.py rename to deprecated/command/sub8_missions/sub8_missions/level_off.py diff --git a/command/sub8_missions/sub8_missions/pinger.py b/deprecated/command/sub8_missions/sub8_missions/pinger.py similarity index 100% rename from command/sub8_missions/sub8_missions/pinger.py rename to deprecated/command/sub8_missions/sub8_missions/pinger.py diff --git a/command/sub8_missions/sub8_missions/pose_sequence_test.py b/deprecated/command/sub8_missions/sub8_missions/pose_sequence_test.py similarity index 100% rename from command/sub8_missions/sub8_missions/pose_sequence_test.py rename to deprecated/command/sub8_missions/sub8_missions/pose_sequence_test.py diff --git a/command/sub8_missions/sub8_missions/qualification.py b/deprecated/command/sub8_missions/sub8_missions/qualification.py similarity index 100% rename from command/sub8_missions/sub8_missions/qualification.py rename to deprecated/command/sub8_missions/sub8_missions/qualification.py diff --git a/command/sub8_missions/sub8_missions/roulette_wheel.py b/deprecated/command/sub8_missions/sub8_missions/roulette_wheel.py similarity index 100% rename from command/sub8_missions/sub8_missions/roulette_wheel.py rename to deprecated/command/sub8_missions/sub8_missions/roulette_wheel.py diff --git a/command/sub8_missions/sub8_missions/search_test.py b/deprecated/command/sub8_missions/sub8_missions/search_test.py similarity index 100% rename from command/sub8_missions/sub8_missions/search_test.py rename to deprecated/command/sub8_missions/sub8_missions/search_test.py diff --git a/command/sub8_missions/sub8_missions/start_gate.py b/deprecated/command/sub8_missions/sub8_missions/start_gate.py similarity index 100% rename from command/sub8_missions/sub8_missions/start_gate.py rename to deprecated/command/sub8_missions/sub8_missions/start_gate.py diff --git a/command/sub8_missions/sub8_missions/start_gate_guess.py b/deprecated/command/sub8_missions/sub8_missions/start_gate_guess.py similarity index 100% rename from command/sub8_missions/sub8_missions/start_gate_guess.py rename to deprecated/command/sub8_missions/sub8_missions/start_gate_guess.py diff --git a/command/sub8_missions/sub8_missions/start_gate_simple.py b/deprecated/command/sub8_missions/sub8_missions/start_gate_simple.py similarity index 100% rename from command/sub8_missions/sub8_missions/start_gate_simple.py rename to deprecated/command/sub8_missions/sub8_missions/start_gate_simple.py diff --git a/command/sub8_missions/sub8_missions/strip.py b/deprecated/command/sub8_missions/sub8_missions/strip.py similarity index 100% rename from command/sub8_missions/sub8_missions/strip.py rename to deprecated/command/sub8_missions/sub8_missions/strip.py diff --git a/command/sub8_missions/sub8_missions/test_actuators.py b/deprecated/command/sub8_missions/sub8_missions/test_actuators.py similarity index 100% rename from command/sub8_missions/sub8_missions/test_actuators.py rename to deprecated/command/sub8_missions/sub8_missions/test_actuators.py diff --git a/command/sub8_missions/sub8_missions/the_path.py b/deprecated/command/sub8_missions/sub8_missions/the_path.py similarity index 100% rename from command/sub8_missions/sub8_missions/the_path.py rename to deprecated/command/sub8_missions/sub8_missions/the_path.py diff --git a/command/sub8_missions/sub8_missions/tx_test.py b/deprecated/command/sub8_missions/sub8_missions/tx_test.py similarity index 100% rename from command/sub8_missions/sub8_missions/tx_test.py rename to deprecated/command/sub8_missions/sub8_missions/tx_test.py From fd79c3f98d559fcbcbb44ac74c37e0a113ad6f61 Mon Sep 17 00:00:00 2001 From: Kevin Allen Date: Mon, 14 Jan 2019 19:39:33 -0500 Subject: [PATCH 4/5] FORMAT: fix formatting issues with missions --- command/sub8_missions/sub8_missions/move.py | 117 +++++++++++------- command/sub8_missions/sub8_missions/square.py | 1 + .../sub8_missions/sub_singleton.py | 6 +- 3 files changed, 75 insertions(+), 49 deletions(-) diff --git a/command/sub8_missions/sub8_missions/move.py b/command/sub8_missions/sub8_missions/move.py index f0990247..52da8ad5 100755 --- a/command/sub8_missions/sub8_missions/move.py +++ b/command/sub8_missions/sub8_missions/move.py @@ -1,17 +1,13 @@ #!/usr/bin/env python from sub_singleton import SubjuGator -from txros import util, NodeHandle -from twisted.internet import defer, reactor -import numpy as np -from mil_misc_tools import text_effects -from geometry_msgs.msg import PoseStamped, PointStamped, Pose, Point, Quaternion, Twist +from txros import util +from geometry_msgs.msg import Pose, Point, Quaternion, Twist from gazebo_msgs.msg import ModelState from ros_alarms import TxAlarmBroadcaster import rospkg import yaml import os from mil_misc_tools import ThrowingArgumentParser -from sub_singleton import SubjuGator UNITS = {'m': 1, 'ft': 0.3048, 'yard': 0.9144, 'rad': 1, 'deg': 0.0174533} SHORTHAND = {"f": "forward", "b": "backward", "l": "left", "r": "right", "yl": "yaw_left", "yr": "yaw_right", @@ -27,22 +23,28 @@ def decode_parameters(cls, parameters): @classmethod def init(cls): parser = ThrowingArgumentParser(description='Command Line Mission Runner', - usage='Pass any pose editor commands with an arguments. \n\t forward 1 (moves forward 1 meter) \n\t backward 2ft (moves backward 2 feet) \n\t forward 1 backward 2ft (moves forward 1 meter then from there moves backward 2 feet), ') + usage='Pass any pose editor commands with an arguments. \n\t\ + forward 1 (moves forward 1 meter) \n\t\ + backward 2ft (moves backward 2 feet)\ + \n\t forward 1 backward 2ft\ + (moves forward 1 meter then from there moves backward 2 feet), ') parser.add_argument('commands', type=str, nargs='*', - help='Pose editor commands to run each followed by an argument to pass to the command (distance or angle usally). Optionally a unit can be added if a non-standard unit is desired.') + help='Pose editor commands to run each followed by an argument to pass to the command\ + (distance or angle usally). \ + Optionally a unit can be added if a non-standard unit is desired.') parser.add_argument('-s', '--speed', type=float, default=0.2, - help="How fast to execute the move. (m/s)") + help="How fast to execute the move. (m/s)") parser.add_argument('-z', '--zrp', action='store_true', - help="Make end goal have zero roll and pitch.") + help="Make end goal have zero roll and pitch.") parser.add_argument('-b', '--blind', action='store_true', - help="Do not check if waypoint is safe.") + help="Do not check if waypoint is safe.") cls.parser = parser - @util.cancellableInlineCallbacks def run(self, args): commands = args.commands - arguments = commands[1::2] #Split into commands and arguments, every other index + # 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] @@ -50,41 +52,55 @@ def run(self, args): 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)} + 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)) + # 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 nh.advertise('gazebo/set_model_state', ModelState) - config_file = os.path.join(rospack.get_path('sub8_gazebo'), 'config', 'teleport_locs.yaml') + 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.") + # 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. + # 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") + self.send_feedback( + "Incorrect number of coordinates") break except IndexError: - self.send_feedback("Incorrect number of coordinates") + self.send_feedback( + "Incorrect number of coordinates") break except ValueError: try: if argument in ('list', 'l'): - self.send_feedback('Available TP locations:') + self.send_feedback( + 'Available TP locations:') for k in f: print ' * ' + k break @@ -92,29 +108,35 @@ def run(self, args): x = float(argz.split(' ')[0]) y = float(argz.split(' ')[1]) z = float(argz.split(' ')[2]) - except LookupError as e: - #This means we did not find the saved location referenced by the argument. - self.send_feedback("TP location not found, check input.") + 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(nh,'kill', node_name='kill') + 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 nh.sleep(1) + severity=5) + yield self.nh.sleep(1) yield state_set_pub.publish(modelstate) - yield nh.sleep(1) + yield self.nh.sleep(1) yield ab.clear_alarm() - except KeyboardInterrupt as e: - #Catches a ctrl-c situation and ends the program. Break will just bring the program to its natural conclusion. + 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']: @@ -133,19 +155,22 @@ def run(self, args): command = SHORTHAND.get(command, command) movement = getattr(self.move, command) - trans_move = command[:3] != "yaw" and command[:5] != "pitch" and command[:4] != "roll" + 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 + 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") + 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 diff --git a/command/sub8_missions/sub8_missions/square.py b/command/sub8_missions/sub8_missions/square.py index 8a386717..4f8fab04 100644 --- a/command/sub8_missions/sub8_missions/square.py +++ b/command/sub8_missions/sub8_missions/square.py @@ -4,6 +4,7 @@ SIDE_LENGTH = 1 # meters SPEED_LIMIT = .2 # m/s + class Square(SubjuGator): @util.cancellableInlineCallbacks def run(self): diff --git a/command/sub8_missions/sub8_missions/sub_singleton.py b/command/sub8_missions/sub8_missions/sub_singleton.py index 883db3cd..d5f55083 100644 --- a/command/sub8_missions/sub8_missions/sub_singleton.py +++ b/command/sub8_missions/sub8_missions/sub_singleton.py @@ -237,11 +237,11 @@ def _init(cls, mission_server): cls.nh, 'moveto', MoveToAction) cls._odom_sub = yield cls.nh.subscribe('odom', Odometry) cls._trajectory_sub = yield cls.nh.subscribe('trajectory', - PoseTwistStamped) + PoseTwistStamped) cls._trajectory_pub = yield cls.nh.advertise('trajectory', - PoseTwistStamped) + PoseTwistStamped) cls._dvl_range_sub = yield cls.nh.subscribe('dvl/range', - RangeStamped) + RangeStamped) cls._tf_listener = yield tf.TransformListener(cls.nh) cls.vision_proxies = _VisionProxies(cls.nh, 'vision_proxies.yaml') From d882b1e428b41db1e29b900cb889f71ef30af5c8 Mon Sep 17 00:00:00 2001 From: Kevin Allen Date: Tue, 22 Jan 2019 16:35:09 -0500 Subject: [PATCH 5/5] MISSIONS: various fixes for mil_missions integration --- command/sub8_launch/launch/sub8.launch | 1 + legacy/rise_6dof/src/rise_6dof/controller.py | 2 +- scripts/bash_aliases.sh | 3 +-- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/command/sub8_launch/launch/sub8.launch b/command/sub8_launch/launch/sub8.launch index 4c03b97e..b0be2cb7 100644 --- a/command/sub8_launch/launch/sub8.launch +++ b/command/sub8_launch/launch/sub8.launch @@ -19,4 +19,5 @@ + diff --git a/legacy/rise_6dof/src/rise_6dof/controller.py b/legacy/rise_6dof/src/rise_6dof/controller.py index 4792e2f8..9364fb91 100644 --- a/legacy/rise_6dof/src/rise_6dof/controller.py +++ b/legacy/rise_6dof/src/rise_6dof/controller.py @@ -3,7 +3,7 @@ import numpy from tf import transformations -from sub8.pose_editor import quat_to_rotvec +from mil_ros_tools.geometry_helpers import quat_to_rotvec numpy.set_printoptions(suppress=True, linewidth=130) diff --git a/scripts/bash_aliases.sh b/scripts/bash_aliases.sh index eae9a923..9c7a560e 100755 --- a/scripts/bash_aliases.sh +++ b/scripts/bash_aliases.sh @@ -15,8 +15,7 @@ alias rsub="ros_connect -n ${HOSTNAMES[0]}" alias sshsub="ssh sub8@${HOSTNAMES[0]} -Y" # Missions -alias subm="rosrun sub8_missions run_mission" -alias subc="rosrun sub8_missions move_command" +alias submove="runmission Move" # Cameras alias subfps="rostopic hz $bag_front_cams $bag_down_cam"