Merge pull request #462 from jaxnb/detect-deliver-mission

Detect deliver mission

mission_control/navigator_missions/navigator_missions/__init__.py

@@ -5,7 +5,6 @@
 Wait = mil_tasks_core.MakeWait(Navigator)
 del mil_tasks_core
 
-from detect_deliver import DetectDeliver
 from teleop import Teleop
 from circle import Circle
 from circle_tower import CircleTower
@@ -33,4 +32,10 @@
 from obstacle_avoid import ObstacleAvoid
 from stc_jaxon import ScanTheCodeJaxon
 from explore_towers import ExploreTowers
+from track_target import TrackTarget
+from detect_deliver_find import DetectDeliverFind
+from detect_deliver import DetectDeliver
+from discount_docking import DiscountDocking
+from docking import Docking
+from shoot_balls import ShootBalls
 import pose_editor

mission_control/navigator_missions/navigator_missions/detect_deliver_find.py

@@ -0,0 +1,190 @@
+#!/usr/bin/env python
+from txros import util
+from navigator_missions.navigator import Navigator
+import numpy as np
+from mil_tools import rosmsg_to_numpy
+from twisted.internet import defer
+import math
+from mil_misc_tools import ThrowingArgumentParser
+import tf.transformations as tform
+from mil_msgs.msg import ObjectsInImage
+
+
+class DetectDeliverFind(Navigator):
+    DOCK_SIZE_LONG = 16.0
+    DOCK_SIZE_SHORT = 8.0
+
+    CIRCLE_DISTANCE = 8.0 + math.sqrt((DOCK_SIZE_SHORT / 2)**2 + (DOCK_SIZE_LONG / 2)**2)
+
+    @classmethod
+    def decode_parameters(cls, parameters):
+        argv = parameters.split()
+        return cls.parser.parse_args(argv)
+
+    @classmethod
+    def init(cls):
+        parser = ThrowingArgumentParser(description='Detect Deliver Find',
+                                        usage='''Default parameters: \'runtask DetectDeliverFind
+                                         \'''')
+        parser.add_argument('-l', '--longscan', action='store_true',
+                            help='set to scan the long side')
+        parser.add_argument('-s', '--shortscan', action='store_true',
+                            help = 'set to scan the short side')
+        parser.add_argument('-o', '--overridescale', action='store_true',
+                            help='''setting causes manual dock size to replace scale, where scale is only
+                                    used to determine which side is longer''')
+        parser.add_argument('-c', '--circle', action='store_true',
+                            help='''setting causes navigator to circle the dock once first in order to help
+                                    PCODAR gather enough information to produce scale and an accurate orientation''')
+        parser.add_argument('-d', '--scandist', type=int, default=6,
+                            help='distance to scan the images from')
+        parser.add_argument('-i', '--lookin', action='store_true',
+                            help='look into the dock at the end instead of being side ways')
+        parser.add_argument('-e', '--enddist', type=int, default=5,
+                            help='the distance to go to as the end point')
+        cls.parser = parser
+
+        cls.bboxsub = cls.nh.subscribe("/bbox_pub", ObjectsInImage)
+
+    @util.cancellableInlineCallbacks
+    def run(self, args):
+        # Parse Arguments
+        self.long_scan = args.longscan
+        self.short_scan = args.shortscan
+        override_scale = args.overridescale
+        pre_circle = args.circle
+        scan_dist = args.scandist
+        look_in = args.lookin
+        end_dist = args.enddist
+
+        # Turn on ML classifier for docks
+        yield self.set_vision_dock()
+
+        # Find the dock
+        yield self.find_dock(override_scale)
+
+        # Get the boat pose
+        boat_pose = yield self.tx_pose
+        boat_pose = boat_pose[0]
+
+        # If extra scanning circle is enabled, circle
+        if pre_circle:
+            start_vect = (boat_pose - self.dock_position) / np.linalg.norm(boat_pose - self.dock_position)
+            start_pt = self.dock_position + start_vect * self.CIRCLE_DISTANCE
+            yield self.move.set_position(start_pt).look_at(self.dock_position).yaw_right(1.57).go()
+            yield self.move.circle_point(self.dock_position).go()
+
+        # Find the dock
+        yield self.find_dock(override_scale)
+
+        # Get the boat pose
+        boat_pose = yield self.tx_pose
+        boat_pose = boat_pose[0]
+
+        # Determine scan points
+        self.build_scan_positions(scan_dist, boat_pose)
+
+        # If we don't want to scan all, remove scans on long side
+        if self.short_scan and not self.long_scan:
+            # Remove 1 and 3; these are the long side ones.
+            self.scans = [self.scans[1], self.scans[3]]
+        elif self.long_scan and not self.short_scan:
+            # Remove 0 and 2; these are the short side ones.
+            self.scans = [self.scans[0], self.scans[2]]
+
+        # Determine the closest scan point
+        scanbtdists = np.array([sc[4] for sc in self.scans])
+        closest_scan = np.argmin(scanbtdists)
+
+        # Scan each point, stop and set correct, correct_scan, when correct found.
+        correct = False
+        correct_scan_idx = -1
+        for i in range(closest_scan, len(self.scans)) + range(0, closest_scan):
+            #yield self.move.set_position(self.scans[i][0]).look_at(self.scans[i][1]).go()
+            yield self.move.set_position(self.scans[i][0]).look_at(self.dock_position).go()
+            correct = yield self.scan_image()
+            if correct:
+                correct_scan_idx = i
+                break
+
+        # No correct scan image found. Exiting.
+        if not correct:
+            self.send_feedback('Image not found')
+            return
+
+        correct_scan = self.scans[correct_scan_idx]
+
+        # Calculate closer location
+        angle = correct_scan[3]
+        dist = end_dist
+        pt = np.array([math.cos(angle) * (dist + self.dock_scale[0] / 2) + self.dock_position[0],
+                       math.sin(angle) * (dist + self.dock_scale[1] / 2) + self.dock_position[1],
+                       self.dock_position[2]])
+        if look_in:
+            lpt = self.dock_position
+        else:
+            lpt = pt + np.array([math.sin(angle), -math.cos(angle), 0])
+
+        # Go to closer location
+        yield self.move.set_position(pt).look_at(lpt).go(blind=True)
+
+        self.send_feedback('Done! In position to line up for shot.')
+
+    @util.cancellableInlineCallbacks
+    def find_dock(self, override_scale):
+        # Get Dock
+        self.dock = yield self.get_sorted_objects(name='dock', n=1)
+        self.dock = self.dock[0][0]
+
+        # Get dock parameters
+        self.dock_position = rosmsg_to_numpy(self.dock.pose.position)
+        self.dock_orientation = rosmsg_to_numpy(self.dock.pose.orientation)
+        self.dock_scale = rosmsg_to_numpy(self.dock.scale)
+        self.dock_orientation = tform.euler_from_quaternion(self.dock_orientation)
+
+        # If scale should be overwritten
+        if override_scale:
+            # Write long/short appropriately
+            if self.dock_scale[0] > self.dock_scale[1]:
+                self.dock_scale[0] = self.DOCK_SIZE_LONG
+                self.dock_scale[1] = self.DOCK_SIZE_SHORT
+            else:
+                self.dock_scale[0] = self.DOCK_SIZE_SHORT
+                self.dock_scale[1] = self.DOCK_SIZE_LONG
+
+    def build_scan_positions(self, scan_dist, boat_pose):
+        angle = self.dock_orientation[2]
+        self.scans = []
+
+        for i in range(0, 4):
+            # Calculate scan point and point to look at
+            pt = np.array([math.cos(angle) * (scan_dist + self.dock_scale[0] / 2) + self.dock_position[0],
+                           math.sin(angle) * (scan_dist + self.dock_scale[1] / 2) + self.dock_position[1],
+                           self.dock_position[2]])
+            lpt = pt + np.array([math.sin(angle), -math.cos(angle), 0])
+
+            # Calculate distance from scan point to dock and boat
+            ptdist = np.linalg.norm(pt - self.dock_position)
+            btdist = np.linalg.norm(pt - boat_pose)
+
+            # Store scan
+            scan = (pt, lpt, ptdist, angle, btdist)
+            self.scans.append(scan)
+
+            # 4 sides to dock
+            angle -= math.pi / 2
+
+    @util.cancellableInlineCallbacks
+    def scan_image(self):
+        msgf = yield self.bboxsub.get_next_message()
+        for msg in msgf.objects:
+            if not (('circle' in msg.name) or ('triangle' in msg.name) or ('cruciform' in msg.name)):
+                continue
+
+            if self.long_scan:
+                self.docking_scan = msg.name
+                defer.returnValue(True)
+            elif self.short_scan:
+                if self.docking_scan is msg.name:
+                    defer.returnValue(True)
+        defer.returnValue(False)

mission_control/navigator_missions/navigator_missions/discount_docking.py

@@ -0,0 +1,17 @@
+#!/usr/bin/env python
+from txros import util
+from navigator_missions.navigator import Navigator
+
+
+class DiscountDocking(Navigator):
+    @util.cancellableInlineCallbacks
+    def run(self, args):
+        yield self.move.forward(5).go()
+
+        yield self.navigator.nh.sleep(10)
+
+        yield self.move.backward(5).go()
+        yield self.move.backward(5).go()
+        yield self.move.backward(5).go()
+
+        self.send_feedback('Done!')

mission_control/navigator_missions/navigator_missions/docking.py

@@ -0,0 +1,140 @@
+#!/usr/bin/env python
+from txros import util
+from navigator_missions.navigator import Navigator
+import numpy as np
+from mil_tools import rosmsg_to_numpy
+from twisted.internet import defer
+from mil_misc_tools import ThrowingArgumentParser
+from mil_msgs.srv import CameraToLidarTransform, CameraToLidarTransformRequest
+from mil_msgs.msg import ObjectsInImage
+from geometry_msgs.msg import Point
+
+
+class Docking(Navigator):
+
+    @classmethod
+    def decode_parameters(cls, parameters):
+        argv = parameters.split()
+        return cls.parser.parse_args(argv)
+
+    @classmethod
+    def init(cls):
+        parser = ThrowingArgumentParser(description='Dock',
+                                        usage='''Default parameters: \'runtask Docking
+                                         \'''')
+        parser.add_argument('-t', '--time', type=int, default=-1)
+        cls.parser = parser
+
+        cls.bboxsub = cls.nh.subscribe("/bbox_pub", ObjectsInImage)
+        cls.camera_lidar_tf = cls.nh.get_service_client('/camera_to_lidar/front_right_cam', CameraToLidarTransform)
+
+    @util.cancellableInlineCallbacks
+    def run(self, args):
+        # Parse Arguments
+        wait_time = args.time
+
+        # Find Dock
+        dock_position = None
+        largest_size = 0
+        boat_pos = (yield self.tx_pose)[0]
+        # Get 10 closest unclassified objects
+        unclass = yield self.get_sorted_objects(name='UNKNOWN', n=10, throw=False)
+        for obj in unclass[0]:
+            point = rosmsg_to_numpy(obj.pose.position)
+            scale = rosmsg_to_numpy(obj.scale)
+
+            # Filter such that we know the dock is closer than 20 meters
+            if np.linalg.norm(point - boat_pos) > 20:
+                break
+
+            size = scale[0] * scale[1]
+
+            if size > largest_size:
+                largest_size = size
+                dock_position = point
+
+        if dock_position is None:
+            self.send_feedback('Cancelling, failed to find dock position')
+            return
+
+        self.send_feedback('Found dock, looking for image')
+
+        # Find the camera input
+        center_frame = yield self.get_center_frame()
+        symbol = center_frame[2].lower()
+
+        self.send_feedback('Identified {}'.format(symbol))
+
+        # Find the target point
+        target_pt = yield self.get_target_pt(center_frame)
+
+        self.send_feedback('Identified target')
+
+        # Identify the time to wait in the dock
+        if wait_time == -1:
+            if 'triangle' in symbol:
+                wait_time = 7
+            elif 'circle' in symbol:
+                wait_time = 17
+            else:  # Cruciform
+                wait_time = 27
+
+        # Go to pose
+        self.send_feedback('Moving into dock')
+        yield self.move.set_position(target_pt).look_at(dock_position).go(blind=True)
+
+        # Sleep the appropriate amount of time
+        self.send_feedback('------------------------------------------------')
+        self.send_feedback('!!!!!!!!!!!!! STATION KEEPING !!!!!!!!!!!!!!!!!!')
+        yield self.nh.sleep(wait_time)
+        self.send_feedback('!!!!!!!!!!!!!!! EXITING DOCK !!!!!!!!!!!!!!!!!!!')
+        self.send_feedback('------------------------------------------------')
+
+        # Back out of the dock
+        yield self.move.backward(5).go(blind=True)
+        yield self.move.backward(5).go(blind=True)
+
+        self.send_feedback('Done with docking!')
+
+    @util.cancellableInlineCallbacks
+    def get_center_frame(self):
+        msgf = yield self.bboxsub.get_next_message()
+        msg = msgf.objects[0]
+        # print msg
+        c1 = rosmsg_to_numpy(msg.points[0])
+        c2 = rosmsg_to_numpy(msg.points[1])
+        tmp = (((c1 + c2) / 2.0), msgf, msg.name)
+        defer.returnValue(tmp)
+
+    @util.cancellableInlineCallbacks
+    def get_target_pt(self, center_frame):
+        msg = CameraToLidarTransformRequest()
+        msg.header.stamp = center_frame[1].header.stamp
+        msg.header.frame_id = center_frame[1].header.frame_id
+        msg.point = Point(x=center_frame[0][0], y=center_frame[0][1], z=0.0)
+        msg.tolerance = 500
+
+        pose_offset = yield self.camera_lidar_tf(msg)
+
+        cam_to_enu = yield self.tf_listener.get_transform('enu', center_frame[1].header.frame_id)
+        normal = rosmsg_to_numpy(pose_offset.normal)
+        normal = cam_to_enu.transform_vector(normal)
+        normal = normal[0:2] / np.linalg.norm(normal[0:2])
+        normal = np.append(normal, [0])
+        found_pt = rosmsg_to_numpy(pose_offset.closest)
+        found_pt = cam_to_enu.transform_point(found_pt)
+        found_pt[2] = 0
+
+        # Extend out by normal multiplier
+        normal *= 3
+        found_pt_1 = found_pt + normal
+        found_pt_2 = found_pt + -1 * normal
+
+        # Which is closer
+        boat_pos = (yield self.tx_pose)[0]
+        if np.linalg.norm(found_pt_1 - boat_pos) > np.linalg.norm(found_pt_2 - boat_pos):
+            found_pt = found_pt_2
+        else:
+            found_pt = found_pt_1
+
+        defer.returnValue(found_pt)

mission_control/navigator_missions/navigator_missions/navigator.py

@@ -152,6 +152,8 @@ def enu_odom_set(odom):
             enu_odom = util.wrap_time_notice(cls._ecef_odom_sub.get_next_message(), 2, "ENU Odom listener")
             yield defer.gatherResults([odom, enu_odom])  # Wait for all those to finish
 
+        cls.docking_scan = 'NA'
+
     @classmethod
     def _grinch_limit_switch_cb(cls, data):
         cls.grinch_limit_switch_pressed = data.data