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!')