Skip to content

Commit

Permalink
FORMAT: fix formatting issues with missions
Browse files Browse the repository at this point in the history
  • Loading branch information
@kev-the-dev
kev-the-dev committed Jan 15, 2019
1 parent 8791273 commit c28ee83
Show file tree
Hide file tree
Showing 3 changed files with 75 additions and 49 deletions.
117 changes: 71 additions & 46 deletions command/sub8_missions/sub8_missions/move.py
View file
Original file line number Diff line number Diff line change
@@ -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",
Expand All @@ -27,94 +23,120 @@ 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]
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)}
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
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.")
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']:
Expand All @@ -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
Expand Down
1 change: 1 addition & 0 deletions command/sub8_missions/sub8_missions/square.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
SIDE_LENGTH = 1 # meters
SPEED_LIMIT = .2 # m/s


class Square(SubjuGator):
@util.cancellableInlineCallbacks
def run(self):
Expand Down
6 changes: 3 additions & 3 deletions command/sub8_missions/sub8_missions/sub_singleton.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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')
Expand Down

0 comments on commit c28ee83

Please sign in to comment.