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_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/command/sub8_missions/missions/__init__.py b/command/sub8_missions/missions/__init__.py
deleted file mode 100644
index 3fa80394..00000000
--- a/command/sub8_missions/missions/__init__.py
+++ /dev/null
@@ -1,8 +0,0 @@
-# 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
diff --git a/command/sub8_missions/missions/square.py b/command/sub8_missions/missions/square.py
deleted file mode 100644
index a6379621..00000000
--- a/command/sub8_missions/missions/square.py
+++ /dev/null
@@ -1,23 +0,0 @@
-from txros import util
-
-SIDE_LENGTH = 1 # meters
-SPEED_LIMIT = .2 # m/s
-
-
-@util.cancellableInlineCallbacks
-def run(sub):
- print "Square!"
-
- 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!"
diff --git a/command/sub8_missions/missions/surface.py b/command/sub8_missions/missions/surface.py
deleted file mode 100644
index 2ec400f2..00000000
--- a/command/sub8_missions/missions/surface.py
+++ /dev/null
@@ -1,8 +0,0 @@
-from txros import util
-
-
-@util.cancellableInlineCallbacks
-def run(sub_singleton):
- print "Surfacing"
- yield sub_singleton.move.depth(0.6).go()
- print "Done surfacing"
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/move_command b/command/sub8_missions/nodes/move_command
deleted file mode 100755
index e16fc01b..00000000
--- a/command/sub8_missions/nodes/move_command
+++ /dev/null
@@ -1,171 +0,0 @@
-#!/usr/bin/env python
-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
-
-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')
- break
- except ValueError:
- try:
- if argument in ('list', 'l'):
- fprint('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.
- fprint("TP location not found, check input.", msg_color='red')
- 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)
-
- 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 ""
- 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()
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/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
new file mode 100644
index 00000000..c614429a
--- /dev/null
+++ b/command/sub8_missions/sub8_missions/__init__.py
@@ -0,0 +1,2 @@
+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
new file mode 100755
index 00000000..52da8ad5
--- /dev/null
+++ b/command/sub8_missions/sub8_missions/move.py
@@ -0,0 +1,182 @@
+#!/usr/bin/env python
+from sub_singleton import SubjuGator
+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
+
+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"}
+
+
+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
+ # 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))
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/sub8_missions/square.py b/command/sub8_missions/sub8_missions/square.py
new file mode 100644
index 00000000..4f8fab04
--- /dev/null
+++ b/command/sub8_missions/sub8_missions/square.py
@@ -0,0 +1,23 @@
+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()
+
+ 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!"
diff --git a/command/sub8_missions/sub8/sub_singleton.py b/command/sub8_missions/sub8_missions/sub_singleton.py
similarity index 94%
rename from command/sub8_missions/sub8/sub_singleton.py
rename to command/sub8_missions/sub8_missions/sub_singleton.py
index 9c1829e3..d5f55083 100644
--- a/command/sub8_missions/sub8/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',
- PoseTwistStamped)
- self._trajectory_pub = yield self.nh.advertise('trajectory',
- PoseTwistStamped)
- self._dvl_range_sub = yield self.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)
-
- def set_test_mode(self):
- self.test_mode = True
+ 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)
+ cls._trajectory_pub = yield cls.nh.advertise('trajectory',
+ PoseTwistStamped)
+ cls._dvl_range_sub = yield cls.nh.subscribe('dvl/range',
+ RangeStamped)
+ cls._tf_listener = yield tf.TransformListener(cls.nh)
+
+ 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])
diff --git a/command/sub8_missions/sub8_missions/surface.py b/command/sub8_missions/sub8_missions/surface.py
new file mode 100644
index 00000000..fea0b65f
--- /dev/null
+++ b/command/sub8_missions/sub8_missions/surface.py
@@ -0,0 +1,12 @@
+#!/usr/bin/env python
+from sub_singleton import SubjuGator
+from txros import util
+from twisted.internet import defer
+
+
+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/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
diff --git a/command/sub8_missions/missions/arm_torpedos.py b/deprecated/command/sub8_missions/sub8_missions/arm_torpedos.py
similarity index 100%
rename from command/sub8_missions/missions/arm_torpedos.py
rename to deprecated/command/sub8_missions/sub8_missions/arm_torpedos.py
diff --git a/command/sub8_missions/missions/autonomous.py b/deprecated/command/sub8_missions/sub8_missions/autonomous.py
similarity index 100%
rename from command/sub8_missions/missions/autonomous.py
rename to deprecated/command/sub8_missions/sub8_missions/autonomous.py
diff --git a/command/sub8_missions/missions/barrel_roll.py b/deprecated/command/sub8_missions/sub8_missions/barrel_roll.py
similarity index 100%
rename from command/sub8_missions/missions/barrel_roll.py
rename to deprecated/command/sub8_missions/sub8_missions/barrel_roll.py
diff --git a/command/sub8_missions/missions/bin_dropper.py b/deprecated/command/sub8_missions/sub8_missions/bin_dropper.py
similarity index 100%
rename from command/sub8_missions/missions/bin_dropper.py
rename to deprecated/command/sub8_missions/sub8_missions/bin_dropper.py
diff --git a/command/sub8_missions/missions/buoy.py b/deprecated/command/sub8_missions/sub8_missions/buoy.py
similarity index 100%
rename from command/sub8_missions/missions/buoy.py
rename to deprecated/command/sub8_missions/sub8_missions/buoy.py
diff --git a/command/sub8_missions/missions/data_gather.py b/deprecated/command/sub8_missions/sub8_missions/data_gather.py
similarity index 100%
rename from command/sub8_missions/missions/data_gather.py
rename to deprecated/command/sub8_missions/sub8_missions/data_gather.py
diff --git a/command/sub8_missions/missions/dice.py b/deprecated/command/sub8_missions/sub8_missions/dice.py
similarity index 100%
rename from command/sub8_missions/missions/dice.py
rename to deprecated/command/sub8_missions/sub8_missions/dice.py
diff --git a/command/sub8_missions/missions/down_2m.py b/deprecated/command/sub8_missions/sub8_missions/down_2m.py
similarity index 100%
rename from command/sub8_missions/missions/down_2m.py
rename to deprecated/command/sub8_missions/sub8_missions/down_2m.py
diff --git a/command/sub8_missions/missions/level_off.py b/deprecated/command/sub8_missions/sub8_missions/level_off.py
similarity index 100%
rename from command/sub8_missions/missions/level_off.py
rename to deprecated/command/sub8_missions/sub8_missions/level_off.py
diff --git a/command/sub8_missions/missions/pinger.py b/deprecated/command/sub8_missions/sub8_missions/pinger.py
similarity index 100%
rename from command/sub8_missions/missions/pinger.py
rename to deprecated/command/sub8_missions/sub8_missions/pinger.py
diff --git a/command/sub8_missions/missions/pose_sequence_test.py b/deprecated/command/sub8_missions/sub8_missions/pose_sequence_test.py
similarity index 100%
rename from command/sub8_missions/missions/pose_sequence_test.py
rename to deprecated/command/sub8_missions/sub8_missions/pose_sequence_test.py
diff --git a/command/sub8_missions/missions/qualification.py b/deprecated/command/sub8_missions/sub8_missions/qualification.py
similarity index 100%
rename from command/sub8_missions/missions/qualification.py
rename to deprecated/command/sub8_missions/sub8_missions/qualification.py
diff --git a/command/sub8_missions/missions/roulette_wheel.py b/deprecated/command/sub8_missions/sub8_missions/roulette_wheel.py
similarity index 100%
rename from command/sub8_missions/missions/roulette_wheel.py
rename to deprecated/command/sub8_missions/sub8_missions/roulette_wheel.py
diff --git a/command/sub8_missions/missions/search_test.py b/deprecated/command/sub8_missions/sub8_missions/search_test.py
similarity index 100%
rename from command/sub8_missions/missions/search_test.py
rename to deprecated/command/sub8_missions/sub8_missions/search_test.py
diff --git a/command/sub8_missions/missions/start_gate.py b/deprecated/command/sub8_missions/sub8_missions/start_gate.py
similarity index 100%
rename from command/sub8_missions/missions/start_gate.py
rename to deprecated/command/sub8_missions/sub8_missions/start_gate.py
diff --git a/command/sub8_missions/missions/start_gate_guess.py b/deprecated/command/sub8_missions/sub8_missions/start_gate_guess.py
similarity index 100%
rename from command/sub8_missions/missions/start_gate_guess.py
rename to deprecated/command/sub8_missions/sub8_missions/start_gate_guess.py
diff --git a/command/sub8_missions/missions/start_gate_simple.py b/deprecated/command/sub8_missions/sub8_missions/start_gate_simple.py
similarity index 100%
rename from command/sub8_missions/missions/start_gate_simple.py
rename to deprecated/command/sub8_missions/sub8_missions/start_gate_simple.py
diff --git a/command/sub8_missions/missions/strip.py b/deprecated/command/sub8_missions/sub8_missions/strip.py
similarity index 100%
rename from command/sub8_missions/missions/strip.py
rename to deprecated/command/sub8_missions/sub8_missions/strip.py
diff --git a/command/sub8_missions/missions/test_actuators.py b/deprecated/command/sub8_missions/sub8_missions/test_actuators.py
similarity index 100%
rename from command/sub8_missions/missions/test_actuators.py
rename to deprecated/command/sub8_missions/sub8_missions/test_actuators.py
diff --git a/command/sub8_missions/missions/the_path.py b/deprecated/command/sub8_missions/sub8_missions/the_path.py
similarity index 100%
rename from command/sub8_missions/missions/the_path.py
rename to deprecated/command/sub8_missions/sub8_missions/the_path.py
diff --git a/command/sub8_missions/missions/tx_test.py b/deprecated/command/sub8_missions/sub8_missions/tx_test.py
similarity index 100%
rename from command/sub8_missions/missions/tx_test.py
rename to deprecated/command/sub8_missions/sub8_missions/tx_test.py
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"