From b2c96754d6d203f8acdc36bc194d8de11188e319 Mon Sep 17 00:00:00 2001 From: RustyBamboo Date: Tue, 23 Jul 2019 15:24:13 -0400 Subject: [PATCH] MISSIONS: balldrop, pinger, startgate --- .../launch/subsystems/perception.launch | 8 ++-- .../sub8_missions/sub8_missions/__init__.py | 3 ++ .../sub8_missions/sub8_missions/autonomous.py | 9 +++-- .../sub8_missions/sub8_missions/ball_drop.py | 14 ++++++- command/sub8_missions/sub8_missions/pinger.py | 4 +- .../sub8_missions/start_gate_guess.py | 39 +++++++++---------- .../sub8_missions/sub_singleton.py | 4 +- .../sub8_missions/vision_proxies.yaml | 4 +- .../ml_classifiers/localizer.py | 15 +++---- .../ml_classifiers/object_detection.py | 6 +-- .../sub8_perception/nodes/guess_server.py | 4 +- 11 files changed, 63 insertions(+), 47 deletions(-) diff --git a/command/sub8_launch/launch/subsystems/perception.launch b/command/sub8_launch/launch/subsystems/perception.launch index 7ac4f854..b526bc7f 100644 --- a/command/sub8_launch/launch/subsystems/perception.launch +++ b/command/sub8_launch/launch/subsystems/perception.launch @@ -1,7 +1,7 @@ - - - + + + @@ -12,6 +12,8 @@ + + diff --git a/command/sub8_missions/sub8_missions/__init__.py b/command/sub8_missions/sub8_missions/__init__.py index 9f652712..20a2aba0 100644 --- a/command/sub8_missions/sub8_missions/__init__.py +++ b/command/sub8_missions/sub8_missions/__init__.py @@ -12,3 +12,6 @@ from .gripper_test import GripperTest from .strip import Strip from .ball_drop import BallDrop +from .arm_torpedos import FireTorpedos +from .dracula_grab import DraculaGrabber +from .start_gate_guess import StartGateGuess diff --git a/command/sub8_missions/sub8_missions/autonomous.py b/command/sub8_missions/sub8_missions/autonomous.py index 6608269a..bc7c74a3 100644 --- a/command/sub8_missions/sub8_missions/autonomous.py +++ b/command/sub8_missions/sub8_missions/autonomous.py @@ -10,6 +10,8 @@ from .pinger import Pinger from .surface import Surface from .vampire_slayer import VampireSlayer +from .arm_torpedos import FireTorpedos +from .ball_drop import BallDrop fprint = text_effects.FprintFactory(title="AUTO_MISSION").fprint @@ -52,8 +54,7 @@ def do_mission(self): yield self.run_mission(Surface(), 30) elif (yield self.nh.get_param('pinger_where')) == 1: fprint('Shooting Mission') - yield self.move.yaw_right_deg(179).go(speed=0.4) - yield self.move.yaw_right_deg(179).go(speed=0.4) + yield self.run_mission(FireTorpedos(), 400) # Go to the other pinger mission and do respective mission completed = yield self.run_mission(Pinger(), 400) @@ -67,12 +68,12 @@ def do_mission(self): elif (yield self.nh.get_param('pinger_where')) == 1: fprint('Shooting Mission') - yield self.move.yaw_right_deg(179).go(speed=0.4) - yield self.move.yaw_right_deg(179).go(speed=0.4) + yield self.run_mission(FireTorpedos(), 400) fprint("Vampire Slayer") yield self.run_mission(VampireSlayer(), 400) fprint("Garlic drop?") + yield self.rub_mission(BallDrop(), 400) except Exception as e: fprint("Error in Chain 1 missions!", msg_color="red") diff --git a/command/sub8_missions/sub8_missions/ball_drop.py b/command/sub8_missions/sub8_missions/ball_drop.py index db91869f..fd1ca0a1 100644 --- a/command/sub8_missions/sub8_missions/ball_drop.py +++ b/command/sub8_missions/sub8_missions/ball_drop.py @@ -12,6 +12,7 @@ from sub8_msgs.srv import GuessRequest, GuessRequestRequest import mil_ros_tools +import rospy fprint = text_effects.FprintFactory(title="BALL_DROP", msg_color="cyan").fprint @@ -19,14 +20,23 @@ FAST_SPEED = 1 SEARCH_HEIGHT = 1.5 -HEIGHT_BALL_DROPER = 0.5 -TRAVEL_DEPTH = 0.5 # 2 +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) + if not resp: + print("Error, failed to init neural net.") + return + except rospy.ServiceException, e: + print("Service Call Failed") + fprint('Enabling cam_ray publisher') yield self.nh.sleep(1) diff --git a/command/sub8_missions/sub8_missions/pinger.py b/command/sub8_missions/sub8_missions/pinger.py index 54c43a68..772fa1ff 100644 --- a/command/sub8_missions/sub8_missions/pinger.py +++ b/command/sub8_missions/sub8_missions/pinger.py @@ -20,7 +20,7 @@ PINGER_HEIGHT = 0.75 # how high to go above pinger after found MOVE_AT_DEPTH = 0.5 # how low to swim and move -POSITION_TOL = 0.075 # how close to pinger before quiting +POSITION_TOL = 0.09 # how close to pinger before quiting Z_POSITION_TOL = -0.53 @@ -132,7 +132,7 @@ def run(self, args): pinger_2_req) fprint('Transformed guess: {}'.format(pinger_guess)) # Check if the pinger aligns with guess - check, vec = self.check_with_guess(vec, pinger_guess) + # check, vec = self.check_with_guess(vec, pinger_guess) fprint('move to {}'.format(vec)) yield self.fancy_move(self, vec) diff --git a/command/sub8_missions/sub8_missions/start_gate_guess.py b/command/sub8_missions/sub8_missions/start_gate_guess.py index 2a71182d..742b245e 100644 --- a/command/sub8_missions/sub8_missions/start_gate_guess.py +++ b/command/sub8_missions/sub8_missions/start_gate_guess.py @@ -7,38 +7,37 @@ fprint = text_effects.FprintFactory(title="PINGER", msg_color="cyan").fprint -SPEED = 0.2 +SPEED = 0.6 DOWN_SPEED = 0.1 DOWN = 1.5 class StartGateGuess(SubjuGator): - @util.cancellableInlineCallbacks def run(self, args): - fprint('Getting Guess Locations') + fprint('Getting Guess Locations') - sub_start_position, sub_start_orientation = yield self.tx_pose() + sub_start_position, sub_start_orientation = yield self.tx_pose() - gate_txros = yield self.nh.get_service_client('/guess_location', - GuessRequest) - gate_1_req = yield gate_txros(GuessRequestRequest(item='start_gate1')) - gate_2_req = yield gate_txros(GuessRequestRequest(item='start_gate2')) + gate_txros = yield self.nh.get_service_client('/guess_location', + GuessRequest) + gate_1_req = yield gate_txros(GuessRequestRequest(item='start_gate1')) + gate_2_req = yield gate_txros(GuessRequestRequest(item='start_gate2')) - gate_1 = mil_ros_tools.rosmsg_to_numpy( - gate_1_req.location.pose.position) - gate_2 = mil_ros_tools.rosmsg_to_numpy( - gate_2_req.location.pose.position) + gate_1 = mil_ros_tools.rosmsg_to_numpy(gate_1_req.location.pose.position) + gate_2 = mil_ros_tools.rosmsg_to_numpy(gate_2_req.location.pose.position) - mid = (gate_1 + gate_2) / 2 - fprint('Found mid {}'.format(mid)) + #mid = (gate_1 + gate_2) / 2 + #mid = gate_1 + fprint('Found mid {}'.format(gate_1)) - fprint('Looking at gate') - yield self.move.down(DOWN).set_orientation(sub_start_orientation).go( - speed=DOWN_SPEED) - yield self.move.look_at_without_pitching(mid).go(speed=DOWN_SPEED) + fprint('Looking at gate') +# yield self.move.down(DOWN).set_orientation(sub_start_orientation).go( + #########speed=DOWN_SPEED) +# yield self.move.look_at_without_pitching(mid).go(speed=DOWN_SPEED) - fprint('Going!') - yield self.move.set_position(mid).depth(DOWN).go(speed=SPEED) + fprint('Going!') + yield self.move.set_position(gate_1).depth(DOWN).go(speed=SPEED) + yield self.move.forward(1).go(speed=SPEED) diff --git a/command/sub8_missions/sub8_missions/sub_singleton.py b/command/sub8_missions/sub8_missions/sub_singleton.py index 9cb8f4b4..91a998d8 100644 --- a/command/sub8_missions/sub8_missions/sub_singleton.py +++ b/command/sub8_missions/sub8_missions/sub_singleton.py @@ -236,8 +236,8 @@ def shoot_torpedo2(self): @util.cancellableInlineCallbacks def drop_marker(self): - yield self.pulse(2) - yield self.pulse(3) + yield self.pulse(2, time=0.75) + yield self.pulse(3, time=0.25) return diff --git a/command/sub8_missions/sub8_missions/vision_proxies.yaml b/command/sub8_missions/sub8_missions/vision_proxies.yaml index e1472b6b..55909313 100644 --- a/command/sub8_missions/sub8_missions/vision_proxies.yaml +++ b/command/sub8_missions/sub8_missions/vision_proxies.yaml @@ -6,8 +6,8 @@ orange_rectangle: root: "/vision/orange_rectangle" buoy: root: "/vision/buoys" -arm_torpedos: - root: "/vision/arm_torpedos" +xyz_points: + root: "/vision/xyz_points" the_path: root: "/vision/the_path" start_gate: diff --git a/perception/sub8_perception/ml_classifiers/localizer.py b/perception/sub8_perception/ml_classifiers/localizer.py index b45d805a..0fd31b9e 100755 --- a/perception/sub8_perception/ml_classifiers/localizer.py +++ b/perception/sub8_perception/ml_classifiers/localizer.py @@ -55,12 +55,12 @@ def __init__(self): self.target = 'vamp' self.classes = 4 self.see_sub = mil_tools.Image_Subscriber( - topic="/camera/front/right/image_raw", callback=self.img_callback) + topic="/camera/front/left/image_raw", callback=self.img_callback) else: self.target = 'stake' self.classes = 1 self.see_sub = mil_tools.Image_Subscriber( - topic="/camera/front/right/image_raw", callback=self.img_callback) + topic="/camera/front/left/image_raw", callback=self.img_callback) # Number of frames self.num_frames = rospy.get_param('~num_frames', 0) # Number of objects we detect @@ -146,12 +146,13 @@ def img_callback(self, data): if len(bbox) > 0: pointx = (bbox[0][0] + bbox[1][0]) /2 pointy = (bbox[0][1] + bbox[1][1]) /2 - pointxdist = bbox[0][0] - bbox[1][0] - pointydist = bbox[0][1] - bbox[1][1] - msg = Point(x=pointx, y=pointy, z=-1) - print("Point: ", pointx, "Point: ", pointy) + pointxdist = abs(bbox[0][0] - bbox[1][0]) + pointydist = abs(bbox[0][1] - bbox[1][1]) + print(pointxdist) + msg = Point(x=pointx, y=pointy, z=self.see_sub.last_image_time.to_sec()) + print("X: ", pointx, "Y: ", pointy, "TIMESTAMP: ", msg.z) self.bbox_pub.publish(msg) - roi = RegionOfInterest(x_offset=bbox[0][0], y_offset=bbox[0][1], height = pointydist, width=pointxdist) + roi = RegionOfInterest(x_offset=int(bbox[0][0]), y_offset=int(bbox[0][1]), height = int(pointydist), width=int(pointxdist)) self.roi_pub.publish(roi) diff --git a/perception/sub8_perception/ml_classifiers/object_detection.py b/perception/sub8_perception/ml_classifiers/object_detection.py index f7d0c127..274e9c51 100755 --- a/perception/sub8_perception/ml_classifiers/object_detection.py +++ b/perception/sub8_perception/ml_classifiers/object_detection.py @@ -12,8 +12,8 @@ on enable, and close it on disable. ''' -# Exec names without the .py -exec_names = ['dice_detection', 'path_localizer'] +# Exec instructions +exec_names = ['vamp', 'stake', 'garlic'] # Store subprocesses in global alive_processes = {} @@ -37,7 +37,7 @@ def enable_callback(self, srv): rospy.loginfo('Enabling {}'.format(self.name)) # Open up a subprocess under PID alive_processes[self.name] = subprocess.Popen( - 'rosrun sub8_perception {}.py'.format(self.name), + 'rosrun sub8_perception localizer.py --{}'.format(self.name), shell=True, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, diff --git a/perception/sub8_perception/nodes/guess_server.py b/perception/sub8_perception/nodes/guess_server.py index 23187a60..7cddcd95 100755 --- a/perception/sub8_perception/nodes/guess_server.py +++ b/perception/sub8_perception/nodes/guess_server.py @@ -11,7 +11,7 @@ class Guess: def __init__(self): rospy.sleep(1.0) self.items = [ - 'pinger_surface', 'pinger_shooter', 'vampire_slayer' + 'pinger_surface', 'pinger_shooter', 'vampire_slayer', 'start_gate1', 'start_gate2' ] self.guess_service = rospy.Service('guess_location', GuessRequest, self.request_location) @@ -41,7 +41,7 @@ def __init__(self): spacer = 0 for i in self.items: self.markers.append(InteractiveMarker()) - self.markers[spacer].header.frame_id = "map" + self.markers[spacer].header.frame_id = "/map" self.markers[spacer].name = i self.markers[spacer].description = i self.markers[spacer].controls.append(box_control)