def run(sub):
print_info = text_effects.FprintFactory(title=MISSION)
print_bad = text_effects.FprintFactory(title=MISSION, msg_color="red")
print_good = text_effects.FprintFactory(title=MISSION, msg_color="green")
print_info.fprint("STARTING")
# Wait for vision services, enable perception
print_info.fprint("ACTIVATING PERCEPTION SERVICE")
sub.vision_proxies.path_marker.start()
pattern = []
#pattern = [sub.move.right(1), sub.move.forward(1), sub.move.left(1), sub.move.backward(1),
# sub.move.right(2), sub.move.forward(2), sub.move.left(2), sub.move.backward(2)]
s = Searcher(sub, sub.vision_proxies.path_marker.get_pose, pattern)
resp = None
print_info.fprint("RUNNING SEARCH PATTERN")
resp = yield s.start_search(loop=False, timeout=TIMEOUT_SECONDS)
if resp is None:
print_bad.fprint("MARKER NOT FOUND")
defer.returnValue(None)
print_good.fprint("PATH MARKER POSE FOUND")
print_info.fprint("TRANFORMING MARKER POSE TO /map FROM {}".format(
resp.pose.header.frame_id))
cam_to_baselink = yield sub._tf_listener.get_transform(
'/base_link', '/' + resp.pose.header.frame_id)
cam_to_map = yield sub._tf_listener.get_transform(
'/map', '/' + resp.pose.header.frame_id)
marker_position = cam_to_map.transform_point(
rosmsg_to_numpy(resp.pose.pose.position))
marker_orientation = cam_to_map.transform_quaternion(
rosmsg_to_numpy(resp.pose.pose.orientation))
move = sub.move.set_orientation(marker_orientation).zero_roll_and_pitch()
position = marker_position.copy()
position[2] = move._pose.position[2]
position[0] = position[0] + cam_to_baselink._p[0]
position[1] = position[1] + cam_to_baselink._p[1]
move = move.set_position(position)
print_info.fprint("MOVING TO MARKER POSE")
yield move.go(speed=0.2)
print_good.fprint("ALIGNED TO PATH MARKER, DONE!")
sub.vision_proxies.path_marker.stop()
defer.returnValue(True)
def run(sub):
fprint = text_effects.FprintFactory(title='START_GATE (SIMPLE)').fprint
fprint('Remembering starting orientation')
sub_start_position, sub_start_orientation = yield sub.tx_pose()
fprint('Going down')
down = sub.move.down(1)
yield down.go(speed=0.1)
fprint('Going forward')
yield down.set_orientation(sub_start_orientation).zero_roll_and_pitch(
).forward(FORWARD_METERS).go(speed=0.2)
fprint('Done!')
from txros import util
import numpy as np
from sensor_msgs.msg import CameraInfo
from image_geometry import PinholeCameraModel
import mil_ros_tools
from twisted.internet import defer
from geometry_msgs.msg import Point
from mil_misc_tools import text_effects
from std_srvs.srv import SetBool, SetBoolRequest
from sub8_msgs.srv import GuessRequest, GuessRequestRequest
import mil_ros_tools
import rospy
fprint = text_effects.FprintFactory(title="BALL_DROP", msg_color="cyan").fprint
SPEED = 0.25
FAST_SPEED = 1
SEARCH_HEIGHT = 1.5
HEIGHT_BALL_DROPER = 0.75
TRAVEL_DEPTH = 0.75 # 2
class BallDrop(SubjuGator):
@util.cancellableInlineCallbacks
def run(self, args):
try:
ree = rospy.ServiceProxy('/vision/garlic/enable', SetBool)
resp = ree(True)
from txros import util
import rospy
from std_srvs.srv import Trigger
import numpy as np
import mil_ros_tools
from mil_misc_tools import text_effects
from sub8_msgs.srv import GuessRequest, GuessRequestRequest
from .sub_singleton import SubjuGator
fprint = text_effects.FprintFactory(title="PINGER", msg_color="cyan").fprint
SPEED = 0.6
DOWN_SPEED = 0.1
DOWN = 1.5
class StartGateGuess(SubjuGator):
@util.cancellableInlineCallbacks
def run(self, args):
fprint('Getting Guess Locations')
sub_start_position, sub_start_orientation = yield self.tx_pose()
save_pois = rospy.ServiceProxy('/poi_server/save_to_param', Trigger)
_ = save_pois()
gate_1 = np.array(
rospy.get_param('/poi_server/initial_pois/start_gate1'))
gate_2 = np.array(
rospy.get_param('/poi_server/initial_pois/start_gate2'))
#!/usr/bin/env python
from __future__ import division
import txros
from twisted.internet import defer
from mil_misc_tools import text_effects
from .sub_singleton import SubjuGator, SonarObjects
from mil_ros_tools import rosmsg_to_numpy
from scipy.spatial import distance
import numpy as np
fprint = text_effects.FprintFactory(title="START_GATE",
msg_color="cyan").fprint
SPEED = 0.6
CAREFUL_SPEED = 0.3
# How many meters to pass the gate by
DIST_AFTER_GATE = 1
RIGHT_OR_LEFT = 1
class StartGate(SubjuGator):
@txros.util.cancellableInlineCallbacks
def run(self, args):
fprint('Waiting for odom')
import txros
from twisted.internet import defer
from ros_alarms import TxAlarmListener, TxAlarmBroadcaster
from mil_misc_tools import text_effects
import genpy
# Import missions here
import pinger
fprint = text_effects.FprintFactory(title="AUTO_MISSION").fprint
WAIT_SECONDS = 5.0
@txros.util.cancellableInlineCallbacks
def run_mission(sub, mission, timeout):
# timeout in seconds
m = mission.run(sub).addErrback(lambda x: None)
start_time = yield sub.nh.get_time()
while sub.nh.get_time() - start_time < genpy.Duration(timeout):
# oof what a hack
if len(m.callbacks) == 0:
m.cancel()
defer.returnValue(True)
yield sub.nh.sleep(0.5)
fprint('MISSION TIMEOUT', msg_color='red')
m.cancel()
defer.returnValue(False)
@txros.util.cancellableInlineCallbacks
def do_mission(sub):
import numpy as np
import sys
import serial
import rospy
import rospkg
import rosparam
import sub8_thruster_comm as thrust_comm
from sub8_exception import SubjuGatorException
import mil_misc_tools.text_effects as te
from mil_misc_tools.terminal_input import get_ch
__author__ = 'David Soto'
rospack = rospkg.RosPack()
fprint = te.FprintFactory(title='ThrusterSpinner').fprint
def ports_from_layout(layout):
'''Load and handle the thruster bus layout'''
port_dict = {}
thruster_definitions = layout['thrusters']
msg = te.Printer('Instantiating thruster_port:')
for port_info in layout['thruster_ports']:
try:
port = port_info['port']
thruster_names = port_info['thruster_names']
thruster_port = thrust_comm.thruster_comm_factory(
port_info, thruster_definitions, fake=False)
import mil_ros_tools
from twisted.internet import defer
import visualization_msgs.msg as visualization_msgs
from geometry_msgs.msg import Point, Vector3
from visualization_msgs.msg import Marker
from mil_misc_tools import text_effects
from std_srvs.srv import SetBool, SetBoolRequest
from visualization_msgs.msg import Marker, MarkerArray
from mil_misc_tools import FprintFactory
from mil_ros_tools import rosmsg_to_numpy
from sub8_msgs.srv import GuessRequest, GuessRequestRequest
import mil_ros_tools
fprint = text_effects.FprintFactory(title="VAMPIRES", msg_color="cyan").fprint
pub_cam_ray = None
SPEED = 0.25
X_OFFSET = 0
Y_OFFSET = 0
Z_OFFSET = 0
class VampireSlayer(SubjuGator):
@util.cancellableInlineCallbacks
def run(self, args):
self.vision_proxies.xyz_points.start()
from std_srvs.srv import SetBool, SetBoolResponse
from sub8_msgs.srv import VisionRequestResponse, VisionRequest, VisionRequest2D
from gazebo_msgs.srv import GetModelState
from std_msgs.msg import Header
import os
import rospkg
import sys
import yaml
rospack = rospkg.RosPack()
config_file = os.path.join(rospack.get_path('sub8_missions'), 'sub8',
'vision_proxies.yaml')
f = yaml.load(open(config_file, 'r'))
model_state = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
fprint = text_effects.FprintFactory(title="SIMULATOR").fprint
def handle_fake_perception(extra, target_object):
'''
Calls the GetModelState service from gazebo to get the realtime position of the model targeted.
Provides this information to the mission.
@param extra The target_name passed through some missions.
Other missions do not pass a target_name thus its lable of extra.
@param target_object Is the model name of the object targeted by the mission.
Missions that do not pass a target_name must use this.
'''
now = rospy.get_rostime()
k = np.uint32(0)
if extra != '':
#!/usr/bin/env python
from __future__ import division
import txros
from twisted.internet import defer
from mil_misc_tools import text_effects
from sub8 import SonarObjects
from mil_ros_tools import rosmsg_to_numpy
from scipy.spatial import distance
import numpy as np
fprint = text_effects.FprintFactory(title="QUALIFICATION",
msg_color="cyan").fprint
SPEED = 0.3
SPEED_CAREFUL = 0.2
# How many meters to pass the gate by
DIST_AFTER_GATE = 2
@txros.util.cancellableInlineCallbacks
def run(sub):
yield sub.move.downward(1).go()
fprint('Begin search for gates')
# Search 4 quadrants deperated by 90 degrees for the gate
start = sub.move.zero_roll_and_pitch()
# Pitch up and down to populate pointcloud
so = SonarObjects(sub, [start.pitch_down_deg(7), start] * 10)
from txros import util
import numpy as np
from mil_misc_tools import text_effects
from sub8.sub_singleton import PoseSequenceCommander
fprint = text_effects.FprintFactory(title="SEQUENCE_TEST").fprint
@util.cancellableInlineCallbacks
def run(sub_singleton):
'''Used for testing in sim. These commands may not be safe for the real sub.'''
fprint("Starting Test With Eulers", msg_color="blue")
positions = [np.array([-1, 0, 0]), np.array([0, 0, 3]),
np.array([-1, 0, 0]), np.array([0, 0, 3])]
orientations = [np.array([-1, 0, 0]), np.array([0, 0, 3]),
np.array([-1, 0, 0]), np.array([0, 0, 3])]
poses_command = PoseSequenceCommander(sub_singleton)
yield poses_command.go_to_sequence_eulers(positions, orientations)
fprint("Done With Eulers!", msg_color="blue")
fprint("Starting Test With Quaternions", msg_color="blue")
orientations = [np.array([0, 0, 0, 1]), np.array([0, 0, 0, 1]),
np.array([0, 0, 0, 1]), np.array([0, 0, 0, 1])]
yield poses_command.go_to_sequence_quaternions(positions, orientations)
fprint("Done with Quaternions", msg_color="blue")
def run(sub):
print_info = text_effects.FprintFactory(title=MISSION).fprint
print_bad = text_effects.FprintFactory(title=MISSION,
msg_color="red").fprint
print_good = text_effects.FprintFactory(title=MISSION,
msg_color="green").fprint
print_info("STARTING")
# Set geometry of marker model
print_info("SETTING GEOMETRY")
polygon = RectFinder(LENGTH, WIDTH).to_polygon()
yield sub.vision_proxies.orange_rectangle.set_geometry(polygon)
# Wait for vision services, enable perception
print_info("ACTIVATING PERCEPTION SERVICE")
yield sub.vision_proxies.orange_rectangle.start()
pattern = []
if DO_PATTERN:
start = sub.move.zero_roll_and_pitch()
r = SEARCH_RADIUS_METERS
pattern = [
start.zero_roll_and_pitch(),
start.right(r),
start.forward(r),
start.left(r),
start.backward(r),
start.right(2 * r),
start.forward(2 * r),
start.left(2 * r),
start.backward(2 * r)
]
s = Searcher(sub, sub.vision_proxies.orange_rectangle.get_pose, pattern)
resp = None
print_info("RUNNING SEARCH PATTERN")
resp = yield s.start_search(loop=False,
timeout=TIMEOUT_SECONDS,
spotings_req=1)
if resp is None or not resp.found:
print_bad("MARKER NOT FOUND")
defer.returnValue(None)
print_good("PATH MARKER POSE FOUND")
assert (resp.pose.header.frame_id == "/map")
move = sub.move
position = rosmsg_to_numpy(resp.pose.pose.position)
position[2] = move._pose.position[2] # Leave Z alone!
orientation = rosmsg_to_numpy(resp.pose.pose.orientation)
move = move.set_position(position).set_orientation(
orientation).zero_roll_and_pitch()
# Ensure SubjuGator continues to face same general direction as before (doesn't go in opposite direction)
odom_forward = tf.transformations.quaternion_matrix(
sub.move._pose.orientation).dot(forward_vec)
marker_forward = tf.transformations.quaternion_matrix(orientation).dot(
forward_vec)
if FACE_FORWARD and np.sign(odom_forward[0]) != np.sign(marker_forward[0]):
move = move.yaw_right(np.pi)
print_info("MOVING TO MARKER AT {}".format(move._pose.position))
yield move.go(speed=SPEED)
print_good("ALIGNED TO PATH MARKER. MOVE FORWARD TO NEXT CHALLENGE!")
yield sub.vision_proxies.orange_rectangle.stop()
defer.returnValue(True)
def run(sub):
fprint = text_effects.FprintFactory(title='START_GATE (SIMPLE)').fprint
fprint('Going forward')
yield sub.move.zero_roll_and_pitch().forward(FORWARD_METERS).go()
fprint('Done!')
