From 9c791bf9a462011f5f75b0555c64cd7afbb20aa7 Mon Sep 17 00:00:00 2001 From: Kevin Allen Date: Wed, 31 Oct 2018 18:56:35 -0400 Subject: [PATCH 01/19] Refactor of pauls work --- .../nodes/detect_deliver_target | 98 +++++++++++++++++++ 1 file changed, 98 insertions(+) create mode 100755 perception/navigator_vision/nodes/detect_deliver_target diff --git a/perception/navigator_vision/nodes/detect_deliver_target b/perception/navigator_vision/nodes/detect_deliver_target new file mode 100755 index 00000000..9436f210 --- /dev/null +++ b/perception/navigator_vision/nodes/detect_deliver_target @@ -0,0 +1,98 @@ +#!/usr/bin/env python +import rospy +from mil_vision_tools import VisionNode, create_object_msg +import numpy as np +import cv2 + +__author__ = "Kevin Allen" + + +class DetectDeliverTargetDetector(VisionNode): + def find_objects(self, img): + ## convert input image (img) to gray scale + cv_image = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + + ## blur resulting image + ## numerical values were chosen arbitrarily after trial and error on sample bag video files + cv_image = cv2.GaussianBlur(cv_image, (29,29), 0) + + ## apply threshold on resulting image + ## numerical values were chosen arbitrarily after tiral and error on sample bag video files + cv_image = cv2.adaptiveThreshold(cv_image, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C, cv2.THRESH_BINARY, 13, 2) + + ## apply Laplace filter on resulting image + cv_image = cv2.Laplacian(cv_image, cv2.CV_8UC1) + + ## dilate resulting image (i.e. thicken the lines) + ## numerical values were chosen arbitrarily after tiral and error on sample bag video files + kernel = np.ones((2, 2), np.uint8) + cv_image = cv2.dilate(cv_image, kernel, iterations = 3) + + ## create a list of contours stored in variable "cnts" + _, cnts, _ = cv2.findContours(cv_image.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + + """ + the following for loop cycles through all the contours in "cnts" and finds: + * centroid (cX, cY) + * area delimited by the contour (area/area_str) + * length of the contour (perimeter) + * number of sides (sides/sides_str) + If the contour has 4 straigh-edge sides and exeeds the area exceeds a certain threshold (AREA_THRESHOLD) the program prints the following data near the centroid of shape in the video: + * area (area_str) + * shape (shape) + * number of sides of the shape (sides_str) + """ + + found = [] + for num, c in enumerate(cnts, start = 1): + index = num + M = cv2.moments(c) + + ## Calculate the centroid + if M["m00"] != 0: + cX = int(M["m10"]/M["m00"]) + cY = int(M["m01"]/M["m00"]) + else: + cX, cY = 0,0 + + """If the area is greater than some threshold (AREA_THRESHOLD) then calculate the following: + * length of the contour of the shape (perimeter) + * number of sides of the shape (sides/sides_str) + * area of the shape (area_str) + """ + area = cv2.contourArea(c) + if area > 5000: + perimeter = cv2.arcLength(c, True) + sides = cv2.approxPolyDP(c, .04*perimeter, True) + sides_str = str(len(sides)) + area_str = str(cv2.contourArea(c)) + + """ If the number of sides of the shape is 4 then display the following: + * area of the shape (area_str) + * name of the shape (shape) + * number of sides + * the contour of the shape + """ + if len(sides) == 4: + shape = "rect/sqr" + cv2.putText(img, area_str, (cX, cY), cv2.FONT_HERSHEY_SIMPLEX, .5, (255, 255, 255), 2) + cv2.putText(img, shape, (cX, cY-30), cv2.FONT_HERSHEY_SIMPLEX, .5, (255, 255, 255), 2) + cv2.putText(img, sides_str, (cX, cY-60), cv2.FONT_HERSHEY_SIMPLEX, .5, (255, 255, 255), 2) + cv2.drawContours(img, cnts, num-1, (0, 255, 0), 2) + + # TODO: only add targets that persist over time + # TODO: add as "target_large" or "target_small" depending on which one it is + # TODO: base confidence on something, rather than always 1.0 + found.append(create_object_msg("target_large", contour=cnts[num-1], confidence=1.0)) + + + test = cv2.resize(img, (900, 600)) + cv2.imshow("Image window", test) + cv2.waitKey(50) + return found + + +if __name__ == "__main__": + rospy.init_node("vision_node_example") + node = DetectDeliverTargetDetector() + rospy.spin() From 995431aa215b0bcaca09c9acfc9bffd7aeee732b Mon Sep 17 00:00:00 2001 From: Kevin Allen Date: Fri, 2 Nov 2018 19:23:29 -0400 Subject: [PATCH 02/19] Messey initial work, publishing good-ish Pose --- .../nodes/detect_deliver_target | 60 +++++++++++++++---- 1 file changed, 50 insertions(+), 10 deletions(-) diff --git a/perception/navigator_vision/nodes/detect_deliver_target b/perception/navigator_vision/nodes/detect_deliver_target index 9436f210..f5997067 100755 --- a/perception/navigator_vision/nodes/detect_deliver_target +++ b/perception/navigator_vision/nodes/detect_deliver_target @@ -1,6 +1,8 @@ #!/usr/bin/env python import rospy -from mil_vision_tools import VisionNode, create_object_msg +from mil_vision_tools import VisionNode, create_object_msg, ImageMux, auto_canny, RectFinder, CentroidObjectsTracker, quaternion_from_rvec +from mil_tools import Image_Publisher, numpy_quat_pair_to_pose +from geometry_msgs.msg import PoseStamped import numpy as np import cv2 @@ -8,7 +10,18 @@ __author__ = "Kevin Allen" class DetectDeliverTargetDetector(VisionNode): + def __init__(self): + self.pose_pub = rospy.Publisher("~pose", PoseStamped, queue_size=3) + super(DetectDeliverTargetDetector, self).__init__() + self.tracker = CentroidObjectsTracker() + self.large_target_model = RectFinder(length=0.500126, width=0.500126) + self.small_target_model = RectFinder(length=0.249936, width=0.249936) + self.image_mux = ImageMux(shape=(2,2), labels=['Thresh', 'Edges', '', 'Found']) + self.debug_pub = Image_Publisher("~debug") + def find_objects(self, img): + self.tracker.clear_expired(now=self._image_sub.last_image_time) + ## convert input image (img) to gray scale cv_image = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) @@ -18,18 +31,24 @@ class DetectDeliverTargetDetector(VisionNode): ## apply threshold on resulting image ## numerical values were chosen arbitrarily after tiral and error on sample bag video files - cv_image = cv2.adaptiveThreshold(cv_image, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C, cv2.THRESH_BINARY, 13, 2) + thresh = cv2.adaptiveThreshold(cv_image, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C, cv2.THRESH_BINARY, 13, 2) + + self.image_mux[0] = thresh ## apply Laplace filter on resulting image - cv_image = cv2.Laplacian(cv_image, cv2.CV_8UC1) + edges = auto_canny(thresh) + + self.image_mux[1] = edges ## dilate resulting image (i.e. thicken the lines) ## numerical values were chosen arbitrarily after tiral and error on sample bag video files kernel = np.ones((2, 2), np.uint8) - cv_image = cv2.dilate(cv_image, kernel, iterations = 3) + dilated = cv2.dilate(edges, kernel, iterations = 3) + + self.image_mux[2] = dilated ## create a list of contours stored in variable "cnts" - _, cnts, _ = cv2.findContours(cv_image.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + _, cnts, _ = cv2.findContours(dilated, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) """ the following for loop cycles through all the contours in "cnts" and finds: @@ -54,14 +73,19 @@ class DetectDeliverTargetDetector(VisionNode): cY = int(M["m01"]/M["m00"]) else: cX, cY = 0,0 - + """If the area is greater than some threshold (AREA_THRESHOLD) then calculate the following: * length of the contour of the shape (perimeter) * number of sides of the shape (sides/sides_str) * area of the shape (area_str) - """ + """ area = cv2.contourArea(c) if area > 5000: + score = self.large_target_model.verify_contour(c) + if score > 0.1: + continue + corners = self.large_target_model.get_corners(c) + print self.tracker.add_observation(self._image_sub.last_image_time, np.array([cX, cY]), data=corners) perimeter = cv2.arcLength(c, True) sides = cv2.approxPolyDP(c, .04*perimeter, True) sides_str = str(len(sides)) @@ -84,11 +108,27 @@ class DetectDeliverTargetDetector(VisionNode): # TODO: add as "target_large" or "target_small" depending on which one it is # TODO: base confidence on something, rather than always 1.0 found.append(create_object_msg("target_large", contour=cnts[num-1], confidence=1.0)) - + + persistent = self.tracker.get_persistent_objects(min_observations=10, min_age=rospy.Duration(2.5)) + print '{} persistent objects'.format(len(persistent)) + if len(persistent) > 0: + for obj in persistent: + ps = PoseStamped() + ps.header = self._image_sub.last_image_header + tvec, rvec = self.large_target_model.get_pose_3D(obj.data, cam=self.camera_model, rectified=True) + + rvec[1] = 0. + rvec[2] = 0. + rvec = rvec / np.linalg.norm(rvec) + ps.pose = numpy_quat_pair_to_pose(tvec, quaternion_from_rvec(rvec)) + self.pose_pub.publish(ps) + print 'Translation:', tvec + print 'Rotation:', rvec, quaternion_from_rvec(rvec) + test = cv2.resize(img, (900, 600)) - cv2.imshow("Image window", test) - cv2.waitKey(50) + self.image_mux[3] = img + self.debug_pub.publish(self.image_mux()) return found From 7cc9a5116d8380f3cff109d2c2eea7863c53bf8c Mon Sep 17 00:00:00 2001 From: Kevin Allen Date: Sat, 3 Nov 2018 19:15:49 -0400 Subject: [PATCH 03/19] MISSIONS: add track target mission --- .../navigator_missions/__init__.py | 1 + .../navigator_missions/track_target.py | 105 ++++++++++++++++++ 2 files changed, 106 insertions(+) create mode 100644 mission_control/navigator_missions/navigator_missions/track_target.py diff --git a/mission_control/navigator_missions/navigator_missions/__init__.py b/mission_control/navigator_missions/navigator_missions/__init__.py index 2bbba613..a863109f 100644 --- a/mission_control/navigator_missions/navigator_missions/__init__.py +++ b/mission_control/navigator_missions/navigator_missions/__init__.py @@ -33,4 +33,5 @@ from obstacle_avoid import ObstacleAvoid from stc_jaxon import ScanTheCodeJaxon from explore_towers import ExploreTowers +from track_target import TrackTarget import pose_editor diff --git a/mission_control/navigator_missions/navigator_missions/track_target.py b/mission_control/navigator_missions/navigator_missions/track_target.py new file mode 100644 index 00000000..bf7038fa --- /dev/null +++ b/mission_control/navigator_missions/navigator_missions/track_target.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python +from navigator import Navigator +import txros +from twisted.internet import defer +from geometry_msgs.msg import PoseStamped +from mil_tools import pose_to_numpy +import numpy as np + + +class TrackTarget(Navigator): + ''' + Mission to track the detect deliver target + ''' + # Offset from the shooter to the target X, Y, Z in boat's frame conventions + OFFSET = np.array([0., 6.5, 0.], dtype=float) + # Number of shots to fire + NUMBER_SHOTS = 1 + # Distance from goal pose to fire at + DISTANCE_TOLERANCE = 0.1 + + @classmethod + @txros.util.cancellableInlineCallbacks + def init(cls): + # Store pose of shooter for later + cls.base_link_to_shooter = -(yield cls.tf_listener.get_transform("base_link", "shooter"))._p + cls.base_link_to_shooter[2] = 0. + # Subscribe to pose + cls.target_pose_sub = cls.nh.subscribe("/detect_deliver_target_detector/pose", PoseStamped) + + @txros.util.cancellableInlineCallbacks + def run(self, parameters): + goal = None + reload_wait = None + fired = 0 + + # Continuously align until all shots are fired + while fired < self.NUMBER_SHOTS: + if reload_wait is not None: + select = defer.DeferredList([self.target_pose_sub.get_next_message(), self.tx_pose, reload_wait], + fireOnOneCallback=True, fireOnOneErrback=True) + else: + select = defer.DeferredList([self.target_pose_sub.get_next_message(), self.tx_pose], + fireOnOneCallback=True, fireOnOneErrback=True) + result, index = yield select + + # New target pose + if index == 0: + # Get pose of target from computer vision + pose = result + # Convert to numpy + pos, quat = pose_to_numpy(pose.pose) + # Ensures some math bag doesn't occur, not sure why. Hacks! + quat = np.abs(quat) + + # Transform pose to ENU + transform = yield self.tf_listener.get_transform("enu", pose.header.frame_id, pose.header.stamp) + pos, quat = transform.transform_point(pos), transform.transform_quaternion(quat) + pos[2] = 0. + + # Assemble Move: + # Start directly on target but rotated so shooter faces target + move = self.move.set_position(pos).set_orientation(quat).yaw_left(3.14) + # Adjust for position of shooter + move = move.rel_position(self.base_link_to_shooter) + # Offset to optimize trajectory of launcher + move = move.rel_position(self.OFFSET) + # Force z=0 + move.position[2] = 0. + # Set new goal + goal = move.position + # Command move to this goal + yield move.go(move_type="bypass") + + # New odometry, see if we are close enough to goal to fire + elif index == 1: + # Ignore if no goal set + if goal is None: + continue + # Get distance from boat to goal + result[0][2] = 0. + distance = np.linalg.norm(goal - result[0]) + yield self.send_feedback("{} from goal".format(distance)) + + # If close enough, fire + if distance < self.DISTANCE_TOLERANCE: + # If still reloading, wait + if reload_wait is not None: + yield self.send_feedback("Aligned and waiting for reload") + continue + # Otherwise fire + yield self.send_feedback("Aligned. Firing!") + yield self.fire_launcher() + fired += 1 + reload_wait = self.reload_launcher() + + # Reload finished + elif index == 2: + yield self.send_feedback("Reloaded.") + reload_wait = None + + # Ensure mission exits after having reloaded + if reload_wait is not None: + self.send_feedback("Waiting for final reload.") + yield reload_wait + defer.returnValue("All balls fired.") From 671991fe69307d29b90797fc2bb0722e317bdc54 Mon Sep 17 00:00:00 2001 From: Kevin Allen Date: Sat, 3 Nov 2018 19:16:19 -0400 Subject: [PATCH 04/19] VISION: add tracking / PnP-based pose to detect_deliver_target --- .../nodes/detect_deliver_target | 80 ++++++++++--------- 1 file changed, 43 insertions(+), 37 deletions(-) diff --git a/perception/navigator_vision/nodes/detect_deliver_target b/perception/navigator_vision/nodes/detect_deliver_target index f5997067..26c81844 100755 --- a/perception/navigator_vision/nodes/detect_deliver_target +++ b/perception/navigator_vision/nodes/detect_deliver_target @@ -1,6 +1,7 @@ #!/usr/bin/env python import rospy -from mil_vision_tools import VisionNode, create_object_msg, ImageMux, auto_canny, RectFinder, CentroidObjectsTracker, quaternion_from_rvec +from mil_vision_tools import VisionNode, create_object_msg, ImageMux, auto_canny, RectFinder +from mil_vision_tools import CentroidObjectsTracker, quaternion_from_rvec from mil_tools import Image_Publisher, numpy_quat_pair_to_pose from geometry_msgs.msg import PoseStamped import numpy as np @@ -10,44 +11,49 @@ __author__ = "Kevin Allen" class DetectDeliverTargetDetector(VisionNode): + BORDER_THICKNESS_METERS = 0.0508 + def __init__(self): self.pose_pub = rospy.Publisher("~pose", PoseStamped, queue_size=3) - super(DetectDeliverTargetDetector, self).__init__() - self.tracker = CentroidObjectsTracker() - self.large_target_model = RectFinder(length=0.500126, width=0.500126) - self.small_target_model = RectFinder(length=0.249936, width=0.249936) - self.image_mux = ImageMux(shape=(2,2), labels=['Thresh', 'Edges', '', 'Found']) + self.tracker = CentroidObjectsTracker(max_distance=20.0) + self.large_target_model = RectFinder(length=0.500126 + self.BORDER_THICKNESS_METERS, + width=0.500126 + self.BORDER_THICKNESS_METERS) + self.small_target_model = RectFinder(length=0.249936 + self.BORDER_THICKNESS_METERS, + width=0.249936 + self.BORDER_THICKNESS_METERS) + self.image_mux = ImageMux(shape=(2, 2), labels=['Thresh', 'Edges', '', 'Found']) self.debug_pub = Image_Publisher("~debug") + self.first_object_id = -1 + super(DetectDeliverTargetDetector, self).__init__() def find_objects(self, img): self.tracker.clear_expired(now=self._image_sub.last_image_time) - ## convert input image (img) to gray scale + # convert input image (img) to gray scale cv_image = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) - ## blur resulting image - ## numerical values were chosen arbitrarily after trial and error on sample bag video files - cv_image = cv2.GaussianBlur(cv_image, (29,29), 0) + # blur resulting image + # numerical values were chosen arbitrarily after trial and error on sample bag video files + cv_image = cv2.GaussianBlur(cv_image, (29, 29), 0) - ## apply threshold on resulting image - ## numerical values were chosen arbitrarily after tiral and error on sample bag video files + # apply threshold on resulting image + # numerical values were chosen arbitrarily after tiral and error on sample bag video files thresh = cv2.adaptiveThreshold(cv_image, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C, cv2.THRESH_BINARY, 13, 2) self.image_mux[0] = thresh - ## apply Laplace filter on resulting image + # apply Laplace filter on resulting image edges = auto_canny(thresh) self.image_mux[1] = edges - ## dilate resulting image (i.e. thicken the lines) - ## numerical values were chosen arbitrarily after tiral and error on sample bag video files + # dilate resulting image (i.e. thicken the lines) + # numerical values were chosen arbitrarily after tiral and error on sample bag video files kernel = np.ones((2, 2), np.uint8) - dilated = cv2.dilate(edges, kernel, iterations = 3) + dilated = cv2.dilate(edges, kernel, iterations=3) self.image_mux[2] = dilated - ## create a list of contours stored in variable "cnts" + # create a list of contours stored in variable "cnts" _, cnts, _ = cv2.findContours(dilated, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) """ @@ -56,23 +62,23 @@ class DetectDeliverTargetDetector(VisionNode): * area delimited by the contour (area/area_str) * length of the contour (perimeter) * number of sides (sides/sides_str) - If the contour has 4 straigh-edge sides and exeeds the area exceeds a certain threshold (AREA_THRESHOLD) the program prints the following data near the centroid of shape in the video: + If the contour has 4 straigh-edge sides and exeeds the area exceeds a certain threshold (AREA_THRESHOLD) + the program prints the following data near the centroid of shape in the video: * area (area_str) * shape (shape) * number of sides of the shape (sides_str) """ found = [] - for num, c in enumerate(cnts, start = 1): - index = num + for num, c in enumerate(cnts): M = cv2.moments(c) - ## Calculate the centroid + # Calculate the centroid if M["m00"] != 0: - cX = int(M["m10"]/M["m00"]) - cY = int(M["m01"]/M["m00"]) + cX = int(M["m10"] / M["m00"]) + cY = int(M["m01"] / M["m00"]) else: - cX, cY = 0,0 + cX, cY = 0, 0 """If the area is greater than some threshold (AREA_THRESHOLD) then calculate the following: * length of the contour of the shape (perimeter) @@ -81,13 +87,13 @@ class DetectDeliverTargetDetector(VisionNode): """ area = cv2.contourArea(c) if area > 5000: - score = self.large_target_model.verify_contour(c) + score = self.large_target_model.verify_contour(c) if score > 0.1: continue corners = self.large_target_model.get_corners(c) print self.tracker.add_observation(self._image_sub.last_image_time, np.array([cX, cY]), data=corners) perimeter = cv2.arcLength(c, True) - sides = cv2.approxPolyDP(c, .04*perimeter, True) + sides = cv2.approxPolyDP(c, .04 * perimeter, True) sides_str = str(len(sides)) area_str = str(cv2.contourArea(c)) @@ -100,19 +106,23 @@ class DetectDeliverTargetDetector(VisionNode): if len(sides) == 4: shape = "rect/sqr" cv2.putText(img, area_str, (cX, cY), cv2.FONT_HERSHEY_SIMPLEX, .5, (255, 255, 255), 2) - cv2.putText(img, shape, (cX, cY-30), cv2.FONT_HERSHEY_SIMPLEX, .5, (255, 255, 255), 2) - cv2.putText(img, sides_str, (cX, cY-60), cv2.FONT_HERSHEY_SIMPLEX, .5, (255, 255, 255), 2) - cv2.drawContours(img, cnts, num-1, (0, 255, 0), 2) + cv2.putText(img, shape, (cX, cY - 30), cv2.FONT_HERSHEY_SIMPLEX, .5, (255, 255, 255), 2) + cv2.putText(img, sides_str, (cX, cY - 60), cv2.FONT_HERSHEY_SIMPLEX, .5, (255, 255, 255), 2) + cv2.drawContours(img, cnts, num - 1, (0, 255, 0), 2) # TODO: only add targets that persist over time # TODO: add as "target_large" or "target_small" depending on which one it is # TODO: base confidence on something, rather than always 1.0 - found.append(create_object_msg("target_large", contour=cnts[num-1], confidence=1.0)) + found.append(create_object_msg("target_large", contour=cnts[num - 1], confidence=1.0)) - persistent = self.tracker.get_persistent_objects(min_observations=10, min_age=rospy.Duration(2.5)) - print '{} persistent objects'.format(len(persistent)) + persistent = self.tracker.get_persistent_objects(min_observations=8, min_age=rospy.Duration(1.5)) if len(persistent) > 0: + if self.first_object_id == -1: + self.first_object_id = persistent[0].id + print 'FIRST DETECTED IS ', self.first_object_id for obj in persistent: + if obj.id != self.first_object_id: + continue ps = PoseStamped() ps.header = self._image_sub.last_image_header tvec, rvec = self.large_target_model.get_pose_3D(obj.data, cam=self.camera_model, rectified=True) @@ -122,17 +132,13 @@ class DetectDeliverTargetDetector(VisionNode): rvec = rvec / np.linalg.norm(rvec) ps.pose = numpy_quat_pair_to_pose(tvec, quaternion_from_rvec(rvec)) self.pose_pub.publish(ps) - print 'Translation:', tvec - print 'Rotation:', rvec, quaternion_from_rvec(rvec) - - test = cv2.resize(img, (900, 600)) self.image_mux[3] = img self.debug_pub.publish(self.image_mux()) return found if __name__ == "__main__": - rospy.init_node("vision_node_example") + rospy.init_node("detect_deliver_target_detector") node = DetectDeliverTargetDetector() rospy.spin() From 49f35543c75d4d61361ceb3f77a5f398b65e3d0c Mon Sep 17 00:00:00 2001 From: Kevin Allen Date: Sat, 10 Nov 2018 15:37:04 -0500 Subject: [PATCH 05/19] VISION: cleanup/paramter tuning for detect_deliver_target --- .../nodes/detect_deliver_target | 136 +++++++----------- 1 file changed, 54 insertions(+), 82 deletions(-) diff --git a/perception/navigator_vision/nodes/detect_deliver_target b/perception/navigator_vision/nodes/detect_deliver_target index 26c81844..e3a3711e 100755 --- a/perception/navigator_vision/nodes/detect_deliver_target +++ b/perception/navigator_vision/nodes/detect_deliver_target @@ -1,7 +1,7 @@ #!/usr/bin/env python import rospy -from mil_vision_tools import VisionNode, create_object_msg, ImageMux, auto_canny, RectFinder -from mil_vision_tools import CentroidObjectsTracker, quaternion_from_rvec +from mil_vision_tools import VisionNode, create_object_msg, ImageMux, auto_canny, RectFinder, putText_ul +from mil_vision_tools import CentroidObjectsTracker, quaternion_from_rvec, contour_centroid from mil_tools import Image_Publisher, numpy_quat_pair_to_pose from geometry_msgs.msg import PoseStamped import numpy as np @@ -14,108 +14,78 @@ class DetectDeliverTargetDetector(VisionNode): BORDER_THICKNESS_METERS = 0.0508 def __init__(self): + self.min_area = rospy.get_param('~min_area', default=1000) + self.max_contour_score = rospy.get_param('~max_contour_score', default=0.03) self.pose_pub = rospy.Publisher("~pose", PoseStamped, queue_size=3) self.tracker = CentroidObjectsTracker(max_distance=20.0) self.large_target_model = RectFinder(length=0.500126 + self.BORDER_THICKNESS_METERS, width=0.500126 + self.BORDER_THICKNESS_METERS) self.small_target_model = RectFinder(length=0.249936 + self.BORDER_THICKNESS_METERS, width=0.249936 + self.BORDER_THICKNESS_METERS) - self.image_mux = ImageMux(shape=(2, 2), labels=['Thresh', 'Edges', '', 'Found']) + self.image_mux = ImageMux(shape=(2, 2), labels=['Blur', 'Threshold', 'Edges', 'Found']) self.debug_pub = Image_Publisher("~debug") self.first_object_id = -1 super(DetectDeliverTargetDetector, self).__init__() def find_objects(self, img): + # Clear old objects from being tracked self.tracker.clear_expired(now=self._image_sub.last_image_time) - # convert input image (img) to gray scale - cv_image = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + # Convert image to grayscale for thresholding + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) - # blur resulting image - # numerical values were chosen arbitrarily after trial and error on sample bag video files - cv_image = cv2.GaussianBlur(cv_image, (29, 29), 0) + # Blur image for easier edge detection + blur = cv2.GaussianBlur(gray, (11, 11), 0) + self.image_mux[0] = blur - # apply threshold on resulting image - # numerical values were chosen arbitrarily after tiral and error on sample bag video files - thresh = cv2.adaptiveThreshold(cv_image, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C, cv2.THRESH_BINARY, 13, 2) + # Threshold image to keep only black regions, like the target border + _, thresh = cv2.threshold(blur, 50, 255, cv2.THRESH_BINARY_INV) + self.image_mux[1] = thresh - self.image_mux[0] = thresh + # Find edges in thresholded image using Canny + edges = auto_canny(thresh, sigma=0.66) - # apply Laplace filter on resulting image - edges = auto_canny(thresh) + # Find contours in edge mask + _, cnts, _ = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - self.image_mux[1] = edges + # Draw contours for debugging + contours_img = np.zeros(edges.shape, dtype=edges.dtype) + cv2.drawContours(contours_img, cnts, -1, 255, 3) - # dilate resulting image (i.e. thicken the lines) - # numerical values were chosen arbitrarily after tiral and error on sample bag video files - kernel = np.ones((2, 2), np.uint8) - dilated = cv2.dilate(edges, kernel, iterations=3) - - self.image_mux[2] = dilated - - # create a list of contours stored in variable "cnts" - _, cnts, _ = cv2.findContours(dilated, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - - """ - the following for loop cycles through all the contours in "cnts" and finds: - * centroid (cX, cY) - * area delimited by the contour (area/area_str) - * length of the contour (perimeter) - * number of sides (sides/sides_str) - If the contour has 4 straigh-edge sides and exeeds the area exceeds a certain threshold (AREA_THRESHOLD) - the program prints the following data near the centroid of shape in the video: - * area (area_str) - * shape (shape) - * number of sides of the shape (sides_str) - """ + self.image_mux[2] = contours_img found = [] + # Loop through the contours, filtering out those unlikely to be targets for num, c in enumerate(cnts): - M = cv2.moments(c) - - # Calculate the centroid - if M["m00"] != 0: - cX = int(M["m10"] / M["m00"]) - cY = int(M["m01"] / M["m00"]) - else: - cX, cY = 0, 0 - - """If the area is greater than some threshold (AREA_THRESHOLD) then calculate the following: - * length of the contour of the shape (perimeter) - * number of sides of the shape (sides/sides_str) - * area of the shape (area_str) - """ + # Remove very small contours (likely to just be noise) area = cv2.contourArea(c) - if area > 5000: - score = self.large_target_model.verify_contour(c) - if score > 0.1: - continue - corners = self.large_target_model.get_corners(c) - print self.tracker.add_observation(self._image_sub.last_image_time, np.array([cX, cY]), data=corners) - perimeter = cv2.arcLength(c, True) - sides = cv2.approxPolyDP(c, .04 * perimeter, True) - sides_str = str(len(sides)) - area_str = str(cv2.contourArea(c)) - - """ If the number of sides of the shape is 4 then display the following: - * area of the shape (area_str) - * name of the shape (shape) - * number of sides - * the contour of the shape - """ - if len(sides) == 4: - shape = "rect/sqr" - cv2.putText(img, area_str, (cX, cY), cv2.FONT_HERSHEY_SIMPLEX, .5, (255, 255, 255), 2) - cv2.putText(img, shape, (cX, cY - 30), cv2.FONT_HERSHEY_SIMPLEX, .5, (255, 255, 255), 2) - cv2.putText(img, sides_str, (cX, cY - 60), cv2.FONT_HERSHEY_SIMPLEX, .5, (255, 255, 255), 2) - cv2.drawContours(img, cnts, num - 1, (0, 255, 0), 2) - - # TODO: only add targets that persist over time - # TODO: add as "target_large" or "target_small" depending on which one it is - # TODO: base confidence on something, rather than always 1.0 - found.append(create_object_msg("target_large", contour=cnts[num - 1], confidence=1.0)) + if area < self.min_area: + continue + + # Score the contour based on Hough shape invariants compared + # to the known shape of the target, smaller is better + score = self.large_target_model.verify_contour(c) + if score > self.max_contour_score: + continue + + # Attempt to reduce the contour to a 4 sided polygon representing the corners + corners = self.large_target_model.get_corners(c, epsilon_range=(0.01, 0.04)) + if corners is None: + continue + # Track this contour with the previous contours + centroid = contour_centroid(c) + obj = self.tracker.add_observation(self._image_sub.last_image_time, np.array(centroid), data=corners) + + # Draw this contour for debugging + putText_ul(img, str(obj.id), centroid, color=(0, 0, 255), fontScale=2) + cv2.drawContours(img, [corners], -1, (0, 255, 0), 3) + + found.append(create_object_msg("target_large", contour=cnts[num - 1], confidence=1.0)) + + # Get objects which have persisted over many frames persistent = self.tracker.get_persistent_objects(min_observations=8, min_age=rospy.Duration(1.5)) + if len(persistent) > 0: if self.first_object_id == -1: self.first_object_id = persistent[0].id @@ -123,11 +93,13 @@ class DetectDeliverTargetDetector(VisionNode): for obj in persistent: if obj.id != self.first_object_id: continue + ps = PoseStamped() ps.header = self._image_sub.last_image_header - tvec, rvec = self.large_target_model.get_pose_3D(obj.data, cam=self.camera_model, rectified=True) - - rvec[1] = 0. + # Get 3D pose of target use PnP + tvec, rvec = self.small_target_model.get_pose_3D(obj.data, cam=self.camera_model, rectified=True) + # Throw out up/down and convert to quaternion + # rvec[1] = 0. rvec[2] = 0. rvec = rvec / np.linalg.norm(rvec) ps.pose = numpy_quat_pair_to_pose(tvec, quaternion_from_rvec(rvec)) From adca7ac6307c71b0e83dc5781a8b742a1ca81ba7 Mon Sep 17 00:00:00 2001 From: jaxon Date: Sun, 9 Dec 2018 03:59:27 -0500 Subject: [PATCH 06/19] Detect deliver mission --- .../navigator_missions/__init__.py | 3 +- .../navigator_missions/detect_deliver.py | 381 +----------------- .../navigator_missions/detect_deliver_2016.py | 373 +++++++++++++++++ .../navigator_missions/detect_deliver_find.py | 171 ++++++++ 4 files changed, 560 insertions(+), 368 deletions(-) create mode 100644 mission_control/navigator_missions/navigator_missions/detect_deliver_2016.py create mode 100644 mission_control/navigator_missions/navigator_missions/detect_deliver_find.py diff --git a/mission_control/navigator_missions/navigator_missions/__init__.py b/mission_control/navigator_missions/navigator_missions/__init__.py index a863109f..4f150d5a 100644 --- a/mission_control/navigator_missions/navigator_missions/__init__.py +++ b/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 @@ -34,4 +33,6 @@ 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 import pose_editor diff --git a/mission_control/navigator_missions/navigator_missions/detect_deliver.py b/mission_control/navigator_missions/navigator_missions/detect_deliver.py index e1c376d5..5596bbe6 100644 --- a/mission_control/navigator_missions/navigator_missions/detect_deliver.py +++ b/mission_control/navigator_missions/navigator_missions/detect_deliver.py @@ -1,373 +1,20 @@ #!/usr/bin/env python -from __future__ import division -import txros -import numpy as np -import tf.transformations as trns -from navigator_msgs.msg import ShooterDoAction -from navigator_msgs.srv import CameraToLidarTransform, CameraToLidarTransformRequest -from geometry_msgs.msg import Point, PoseStamped -from twisted.internet import defer -import mil_tools -from mil_misc_tools.text_effects import fprint -from navigator_tools import MissingPerceptionObject -import genpy -from navigator import Navigator - +from txros import util +from navigator_missions.navigator import Navigator +from detect_deliver_find import DetectDeliverFind +from track_target import TrackTarget class DetectDeliver(Navigator): - shoot_distance_meters = 2.7 - theta_offset = np.pi / 2.0 - spotings_req = 1 - circle_radius = 10 - circle_direction = "cw" - platform_radius = 0.925 - search_timeout_seconds = 300 - SHAPE_CENTER_TO_BIG_TARGET = 0.35 - SHAPE_CENTER_TO_SMALL_TARGET = -0.42 - WAIT_BETWEEN_SHOTS = 5 # Seconds to wait between shooting - NUM_BALLS = 4 - LOOK_AT_TIME = 5 - FOREST_SLEEP = 15 - BACKUP_DISTANCE = 5 - - def __init__(self): - super(DetectDeliver, self).__init__() - self.identified_shapes = {} - self.last_shape_error = "" - self.last_lidar_error = "" - self.shape_pose = None - self.align_forest_pause = False @classmethod def init(cls): - cls.shooter_pose_sub = cls.nh.subscribe("/shooter_pose", PoseStamped) - cls.cameraLidarTransformer = cls.nh.get_service_client( - "/camera_to_lidar/right_right_cam", CameraToLidarTransform) - cls.shooterLoad = txros.action.ActionClient( - cls.nh, '/shooter/load', ShooterDoAction) - cls.shooterFire = txros.action.ActionClient( - cls.nh, '/shooter/fire', ShooterDoAction) - cls.shooter_baselink_tf = yield cls.tf_listener.get_transform('/base_link', '/shooter') - - def _bounding_rect(self, points): - np_points = map(mil_tools.rosmsg_to_numpy, points) - xy_max = np.max(np_points, axis=0) - xy_min = np.min(np_points, axis=0) - return np.append(xy_max, xy_min) - - @txros.util.cancellableInlineCallbacks - def set_shape_and_color(self): - target = yield self.mission_params["detect_deliver_target"].get() - if target == "BIG": - self.target_offset_meters = self.SHAPE_CENTER_TO_BIG_TARGET - elif target == "SMALL": - self.target_offset_meters = self.SHAPE_CENTER_TO_SMALL_TARGET - self.Shape = yield self.mission_params["detect_deliver_shape"].get() - self.Color = yield self.mission_params["detect_deliver_color"].get() - fprint("Color={} Shape={} Target={}".format(self.Color, self.Shape, target), - title="DETECT DELIVER", msg_color='green') - - @txros.util.cancellableInlineCallbacks - def get_waypoint(self): - res = yield self.database_query("shooter") - if not res.found: - fprint("shooter waypoint not found", title="DETECT DELIVER", msg_color='red') - raise MissingPerceptionObject("shooter", "Detect Deliver Waypoint not found") - self.waypoint_res = res - - @txros.util.cancellableInlineCallbacks - def get_any_shape(self): - shapes = yield self.get_shape() - if shapes.found: - for shape in shapes.shapes.list: - normal_res = yield self.get_normal(shape) - if normal_res.success: - enu_cam_tf = yield self.tf_listener.get_transform( - '/enu', - '/' + shape.header.frame_id, - shape.header.stamp) - self.update_shape(shape, normal_res, enu_cam_tf) - defer.returnValue(((shape.Shape, shape.Color), self.identified_shapes[(shape.Shape, shape.Color)])) - else: - fprint("Normal not found Error={}".format(normal_res.error), - title="DETECT DELIVER", msg_color='red') - else: - fprint("shape not found Error={}".format(shapes.error), title="DETECT DELIVER", msg_color="red") - defer.returnValue(False) - - @txros.util.cancellableInlineCallbacks - def circle_search(self): - platform_np = mil_tools.rosmsg_to_numpy(self.waypoint_res.objects[0].position) - yield self.move.look_at(platform_np).set_position(platform_np).backward(self.circle_radius)\ - .yaw_left(90, unit='deg').go(move_type="drive") - - done_circle = False - - @txros.util.cancellableInlineCallbacks - def do_circle(): - yield self.move.circle_point(platform_np, direction=self.circle_direction).go() - done_circle = True # noqa flake8 cant see that it is defined above - - do_circle() - while not done_circle: - res = yield self.get_any_shape() - if res is False: - yield self.nh.sleep(0.25) - continue - fprint("Shape ({}found, using normal to look at other 3 shapes if needed".format( - res[0]), title="DETECT DELIVER", msg_color="green") - # circle_defer.cancel() - shape_color, found_shape_pose = res - if self.correct_shape(shape_color): - self.shape_pose = found_shape_pose - return - # Pick other 3 to look at - rot_right = np.array([[0, -1], [1, 0]]) - (shape_point, shape_normal) = found_shape_pose - rotated_norm = np.append(rot_right.dot(shape_normal[:2]), 0) - center_point = shape_point - shape_normal * (self.platform_radius / 2.0) - - point_opposite_side = center_point - shape_normal * self.circle_radius - move_opposite_side = self.move.set_position( - point_opposite_side).look_at(center_point).yaw_left(90, unit='deg') - - left_or_whatever_point = center_point + rotated_norm * self.circle_radius - move_left_or_whatever = self.move.set_position( - left_or_whatever_point).look_at(center_point).yaw_left(90, unit='deg') - - right_or_whatever_point = center_point - rotated_norm * self.circle_radius - move_right_or_whatever = self.move.set_position( - right_or_whatever_point).look_at(center_point).yaw_left(90, unit='deg') - - yield self.search_sides((move_right_or_whatever, move_opposite_side, move_left_or_whatever)) - return - fprint("No shape found after complete circle", title="DETECT DELIVER", msg_color='red') - raise Exception("No shape found on platform") - - def update_shape(self, shape_res, normal_res, tf): - self.identified_shapes[(shape_res.Shape, shape_res.Color)] = self.get_shape_pos(normal_res, tf) - - def correct_shape(self, (shape, color)): - return (self.Color == "ANY" or self.Color == color) and (self.Shape == "ANY" or self.Shape == shape) - - @txros.util.cancellableInlineCallbacks - def search_side(self): - fprint("Searching side", title="DETECT DELIVER", msg_color='green') - start_time = self.nh.get_time() - while self.nh.get_time() - start_time < genpy.Duration(self.LOOK_AT_TIME): - res = yield self.get_any_shape() - if res is not False: - defer.returnValue(res) - yield self.nh.sleep(0.1) - defer.returnValue(False) - - @txros.util.cancellableInlineCallbacks - def search_sides(self, moves): - for move in moves: - yield move.go(move_type="drive") - res = yield self.search_side() - if res is False: - fprint("No shape found on side", title="DETECT DELIVER", msg_color='red') - continue - shape_color, found_pose = res - if self.correct_shape(shape_color): - self.shape_pose = found_pose - return - fprint("Saw (Shape={}, Color={}) on this side".format( - shape_color[0], shape_color[1]), title="DETECT DELIVER", msg_color='green') - - @txros.util.cancellableInlineCallbacks - def search_shape(self): - shapes = yield self.get_shape() - if shapes.found: - for shape in shapes.shapes.list: - normal_res = yield self.get_normal(shape) - if normal_res.success: - enu_cam_tf = yield self.tf_listener.get_transform('/enu', '/' + shape.header.frame_id, - shape.header.stamp) - if self.correct_shape(shape): - self.shape_pose = self.get_shape_pos(normal_res, enu_cam_tf) - defer.returnValue(True) - self.update_shape(shape, normal_res, enu_cam_tf) - - else: - if not self.last_lidar_error == normal_res.error: - fprint("Normal not found Error={}".format(normal_res.error), - title="DETECT DELIVER", msg_color='red') - self.last_lidar_error = normal_res.error - else: - if not self.last_shape_error == shapes.error: - fprint("shape not found Error={}".format(shapes.error), title="DETECT DELIVER", msg_color="red") - self.last_shape_error = shapes.error - defer.returnValue(False) - - def select_backup_shape(self): - for (shape, color), point_normal in self.identified_shapes.iteritems(): - self.shape_pose = point_normal - if self.Shape == shape or self.Color == color: - fprint("Correct shape not found, resorting to shape={} color={}".format( - shape, color), title="DETECT DELIVER", msg_color='yellow') - return - if self.shape_pose is None: - raise Exception("None seen") - fprint("Correct shape not found, resorting to random shape", title="DETECT DELIVER", msg_color='yellow') - - @txros.util.cancellableInlineCallbacks - def align_to_target(self): - if self.shape_pose is None: - self.select_backup_shape() - goal_point, goal_orientation = self.get_aligned_pose(self.shape_pose[0], self.shape_pose[1]) - move = self.move.set_position(goal_point).set_orientation(goal_orientation).forward(self.target_offset_meters) - # Adjust for location of shooter - move = move.left(-self.shooter_baselink_tf._p[1]).forward(-self.shooter_baselink_tf._p[0]) - fprint("Aligning to shoot at {}".format(move), title="DETECT DELIVER", msg_color='green') - move_complete = yield move.go(move_type="skid", blind=True) - defer.returnValue(move_complete) - - def get_shape(self): - return self.vision_proxies["get_shape"].get_response(Shape="ANY", Color="ANY") - - def get_aligned_pose(self, enupoint, enunormal): - aligned_position = enupoint + self.shoot_distance_meters * enunormal # moves x meters away - angle = np.arctan2(-enunormal[0], enunormal[1]) - aligned_orientation = trns.quaternion_from_euler(0, 0, angle) # Align perpindicular - return (aligned_position, aligned_orientation) - - def get_shape_pos(self, normal_res, enu_cam_tf): - enunormal = enu_cam_tf.transform_vector(mil_tools.rosmsg_to_numpy(normal_res.normal)) - enupoint = enu_cam_tf.transform_point(mil_tools.rosmsg_to_numpy(normal_res.closest)) - return (enupoint, enunormal) - - @txros.util.cancellableInlineCallbacks - def get_normal(self, shape): - req = CameraToLidarTransformRequest() - req.header = shape.header - req.point = Point() - req.point.x = shape.CenterX - req.point.y = shape.CenterY - rect = self._bounding_rect(shape.points) - req.tolerance = int(min(rect[0] - rect[3], rect[1] - rect[4]) / 2.0) - normal_res = yield self.cameraLidarTransformer(req) - if not self.normal_is_sane(normal_res.normal): - normal_res.success = False - normal_res.error = "UNREASONABLE NORMAL" - defer.returnValue(normal_res) - - def normal_is_sane(self, vector3): - return abs(mil_tools.rosmsg_to_numpy(vector3)[1]) < 0.4 - - @txros.util.cancellableInlineCallbacks - def shoot_all_balls(self): - for i in range(self.NUM_BALLS): - goal = yield self.shooterLoad.send_goal(ShooterDoAction()) - fprint("Loading Shooter {}".format(i), title="DETECT DELIVER", msg_color='green') - yield goal.get_result() - yield self.nh.sleep(2) - goal = yield self.shooterFire.send_goal(ShooterDoAction()) - fprint("Firing Shooter {}".format(i), title="DETECT DELIVER", msg_color='green') - yield goal.get_result() - fprint("Waiting {} seconds between shots".format( - self.WAIT_BETWEEN_SHOTS), title="DETECT DELIVER", msg_color='green') - yield self.nh.sleep(self.WAIT_BETWEEN_SHOTS) - - @txros.util.cancellableInlineCallbacks - def continuously_align(self): - fprint("Starting Forest Align", title="DETECT DELIVER", msg_color='green') - while True: - shooter_pose = yield self.shooter_pose_sub.get_next_message() - if self.align_forest_pause: - yield self.nh.sleep(0.1) - continue - shooter_pose = shooter_pose.pose - - cen = np.array([shooter_pose.position.x, shooter_pose.position.y]) - yaw = trns.euler_from_quaternion([shooter_pose.orientation.x, - shooter_pose.orientation.y, - shooter_pose.orientation.z, - shooter_pose.orientation.w])[2] - q = trns.quaternion_from_euler(0, 0, yaw) - p = np.append(cen, 0) - # fprint("Forest Aligning to p=[{}] q=[{}]".format(p, q), title="DETECT DELIVER", msg_color='green') - - # Prepare move to follow shooter - move = self.move.set_position(p).set_orientation(q).yaw_right(90, 'deg') - - # Adjust move for location of target - move = move.forward(self.target_offset_meters) - - # Adjust move for location of launcher - move = move.left(-self.shooter_baselink_tf._p[1]).forward(-self.shooter_baselink_tf._p[0]) - - # Move away a fixed distance to make the shot - move = move.left(self.shoot_distance_meters) - - yield move.go(move_type='bypass') - - @txros.util.cancellableInlineCallbacks - def shoot_and_align_forest(self): - move = yield self.align_to_target() - if move.failure_reason != "": - fprint("Error Aligning with target = {}. Ending mission :(".format( - move.failure_reason), title="DETECT DELIVER", msg_color="red") - return - fprint("Aligned successs. Shooting while using forest realign", title="DETECT DELIVER", msg_color="green") - align_defer = self.continuously_align() - fprint("Sleeping for {} seconds to allow for alignment", - title="DETECT DELIVER".format(self.FOREST_SLEEP), msg_color="green") - yield self.nh.sleep(self.FOREST_SLEEP) - for i in range(self.NUM_BALLS): - goal = yield self.shooterLoad.send_goal(ShooterDoAction()) - fprint("Loading Shooter {}".format(i), title="DETECT DELIVER", msg_color='green') - yield goal.get_result() - yield self.nh.sleep(2) - self.align_forest_pause = True - goal = yield self.shooterFire.send_goal(ShooterDoAction()) - fprint("Firing Shooter {}".format(i), title="DETECT DELIVER", msg_color='green') - yield goal.get_result() - yield self.nh.sleep(1) - self.align_forest_pause = False - fprint("Waiting {} seconds between shots".format( - self.WAIT_BETWEEN_SHOTS), title="DETECT DELIVER", msg_color='green') - yield self.nh.sleep(self.WAIT_BETWEEN_SHOTS) - align_defer.cancel() - - @txros.util.cancellableInlineCallbacks - def backup_from_target(self): - yield self.move.left(self.BACKUP_DISTANCE).go() - - @txros.util.cancellableInlineCallbacks - def shoot_and_align(self): - move = yield self.align_to_target() - if move.failure_reason != "": - fprint("Error Aligning with target = {}. Ending mission :(".format( - move.failure_reason), title="DETECT DELIVER", msg_color="red") - return - fprint("Aligned successs. Shooting without realignment", title="DETECT DELIVER", msg_color="green") - yield self.shoot_all_balls() - - @txros.util.cancellableInlineCallbacks - def setup_mission(self): - stc_color = yield self.mission_params["scan_the_code_color3"].get(raise_exception=False) - if stc_color is False: - color = "ANY" - else: - color = stc_color - # color = "ANY" - shape = "ANY" - fprint("Setting search shape={} color={}".format(shape, color), title="DETECT DELIVER", msg_color='green') - yield self.mission_params["detect_deliver_shape"].set(shape) - yield self.mission_params["detect_deliver_color"].set(color) - - @txros.util.cancellableInlineCallbacks - def run(self, parameters): - yield self.setup_mission() - fprint("STARTING MISSION", title="DETECT DELIVER", msg_color='green') - yield self.vision_proxies["get_shape"].start() - yield self.set_shape_and_color() # Get correct goal shape/color from params - yield self.get_waypoint() # Get waypoint of shooter target - yield self.circle_search() # Go to waypoint and circle until target found - # yield self.shoot_and_align() # Align to target and shoot - yield self.shoot_and_align_forest() # Align to target and shoot - yield self.backup_from_target() - yield self.vision_proxies["get_shape"].stop() - fprint("ENDING MISSION", title="DETECT DELIVER", msg_color='green') + cls.detect_deiliver_find = DetectDeliverFind() + cls.track_target = TrackTarget() + pass + + @util.cancellableInlineCallbacks + def run(self, args): + self.send_feedback('Starting detect deliver') + yield self.detect_deiliver_find.run(self.detect_deiliver_find.decode_parameters(args)) + yield self.track_target.run(self.track_target.decode_parameters(args)) + self.send_feedback('Done with detect deliver') \ No newline at end of file diff --git a/mission_control/navigator_missions/navigator_missions/detect_deliver_2016.py b/mission_control/navigator_missions/navigator_missions/detect_deliver_2016.py new file mode 100644 index 00000000..e1c376d5 --- /dev/null +++ b/mission_control/navigator_missions/navigator_missions/detect_deliver_2016.py @@ -0,0 +1,373 @@ +#!/usr/bin/env python +from __future__ import division +import txros +import numpy as np +import tf.transformations as trns +from navigator_msgs.msg import ShooterDoAction +from navigator_msgs.srv import CameraToLidarTransform, CameraToLidarTransformRequest +from geometry_msgs.msg import Point, PoseStamped +from twisted.internet import defer +import mil_tools +from mil_misc_tools.text_effects import fprint +from navigator_tools import MissingPerceptionObject +import genpy +from navigator import Navigator + + +class DetectDeliver(Navigator): + shoot_distance_meters = 2.7 + theta_offset = np.pi / 2.0 + spotings_req = 1 + circle_radius = 10 + circle_direction = "cw" + platform_radius = 0.925 + search_timeout_seconds = 300 + SHAPE_CENTER_TO_BIG_TARGET = 0.35 + SHAPE_CENTER_TO_SMALL_TARGET = -0.42 + WAIT_BETWEEN_SHOTS = 5 # Seconds to wait between shooting + NUM_BALLS = 4 + LOOK_AT_TIME = 5 + FOREST_SLEEP = 15 + BACKUP_DISTANCE = 5 + + def __init__(self): + super(DetectDeliver, self).__init__() + self.identified_shapes = {} + self.last_shape_error = "" + self.last_lidar_error = "" + self.shape_pose = None + self.align_forest_pause = False + + @classmethod + def init(cls): + cls.shooter_pose_sub = cls.nh.subscribe("/shooter_pose", PoseStamped) + cls.cameraLidarTransformer = cls.nh.get_service_client( + "/camera_to_lidar/right_right_cam", CameraToLidarTransform) + cls.shooterLoad = txros.action.ActionClient( + cls.nh, '/shooter/load', ShooterDoAction) + cls.shooterFire = txros.action.ActionClient( + cls.nh, '/shooter/fire', ShooterDoAction) + cls.shooter_baselink_tf = yield cls.tf_listener.get_transform('/base_link', '/shooter') + + def _bounding_rect(self, points): + np_points = map(mil_tools.rosmsg_to_numpy, points) + xy_max = np.max(np_points, axis=0) + xy_min = np.min(np_points, axis=0) + return np.append(xy_max, xy_min) + + @txros.util.cancellableInlineCallbacks + def set_shape_and_color(self): + target = yield self.mission_params["detect_deliver_target"].get() + if target == "BIG": + self.target_offset_meters = self.SHAPE_CENTER_TO_BIG_TARGET + elif target == "SMALL": + self.target_offset_meters = self.SHAPE_CENTER_TO_SMALL_TARGET + self.Shape = yield self.mission_params["detect_deliver_shape"].get() + self.Color = yield self.mission_params["detect_deliver_color"].get() + fprint("Color={} Shape={} Target={}".format(self.Color, self.Shape, target), + title="DETECT DELIVER", msg_color='green') + + @txros.util.cancellableInlineCallbacks + def get_waypoint(self): + res = yield self.database_query("shooter") + if not res.found: + fprint("shooter waypoint not found", title="DETECT DELIVER", msg_color='red') + raise MissingPerceptionObject("shooter", "Detect Deliver Waypoint not found") + self.waypoint_res = res + + @txros.util.cancellableInlineCallbacks + def get_any_shape(self): + shapes = yield self.get_shape() + if shapes.found: + for shape in shapes.shapes.list: + normal_res = yield self.get_normal(shape) + if normal_res.success: + enu_cam_tf = yield self.tf_listener.get_transform( + '/enu', + '/' + shape.header.frame_id, + shape.header.stamp) + self.update_shape(shape, normal_res, enu_cam_tf) + defer.returnValue(((shape.Shape, shape.Color), self.identified_shapes[(shape.Shape, shape.Color)])) + else: + fprint("Normal not found Error={}".format(normal_res.error), + title="DETECT DELIVER", msg_color='red') + else: + fprint("shape not found Error={}".format(shapes.error), title="DETECT DELIVER", msg_color="red") + defer.returnValue(False) + + @txros.util.cancellableInlineCallbacks + def circle_search(self): + platform_np = mil_tools.rosmsg_to_numpy(self.waypoint_res.objects[0].position) + yield self.move.look_at(platform_np).set_position(platform_np).backward(self.circle_radius)\ + .yaw_left(90, unit='deg').go(move_type="drive") + + done_circle = False + + @txros.util.cancellableInlineCallbacks + def do_circle(): + yield self.move.circle_point(platform_np, direction=self.circle_direction).go() + done_circle = True # noqa flake8 cant see that it is defined above + + do_circle() + while not done_circle: + res = yield self.get_any_shape() + if res is False: + yield self.nh.sleep(0.25) + continue + fprint("Shape ({}found, using normal to look at other 3 shapes if needed".format( + res[0]), title="DETECT DELIVER", msg_color="green") + # circle_defer.cancel() + shape_color, found_shape_pose = res + if self.correct_shape(shape_color): + self.shape_pose = found_shape_pose + return + # Pick other 3 to look at + rot_right = np.array([[0, -1], [1, 0]]) + (shape_point, shape_normal) = found_shape_pose + rotated_norm = np.append(rot_right.dot(shape_normal[:2]), 0) + center_point = shape_point - shape_normal * (self.platform_radius / 2.0) + + point_opposite_side = center_point - shape_normal * self.circle_radius + move_opposite_side = self.move.set_position( + point_opposite_side).look_at(center_point).yaw_left(90, unit='deg') + + left_or_whatever_point = center_point + rotated_norm * self.circle_radius + move_left_or_whatever = self.move.set_position( + left_or_whatever_point).look_at(center_point).yaw_left(90, unit='deg') + + right_or_whatever_point = center_point - rotated_norm * self.circle_radius + move_right_or_whatever = self.move.set_position( + right_or_whatever_point).look_at(center_point).yaw_left(90, unit='deg') + + yield self.search_sides((move_right_or_whatever, move_opposite_side, move_left_or_whatever)) + return + fprint("No shape found after complete circle", title="DETECT DELIVER", msg_color='red') + raise Exception("No shape found on platform") + + def update_shape(self, shape_res, normal_res, tf): + self.identified_shapes[(shape_res.Shape, shape_res.Color)] = self.get_shape_pos(normal_res, tf) + + def correct_shape(self, (shape, color)): + return (self.Color == "ANY" or self.Color == color) and (self.Shape == "ANY" or self.Shape == shape) + + @txros.util.cancellableInlineCallbacks + def search_side(self): + fprint("Searching side", title="DETECT DELIVER", msg_color='green') + start_time = self.nh.get_time() + while self.nh.get_time() - start_time < genpy.Duration(self.LOOK_AT_TIME): + res = yield self.get_any_shape() + if res is not False: + defer.returnValue(res) + yield self.nh.sleep(0.1) + defer.returnValue(False) + + @txros.util.cancellableInlineCallbacks + def search_sides(self, moves): + for move in moves: + yield move.go(move_type="drive") + res = yield self.search_side() + if res is False: + fprint("No shape found on side", title="DETECT DELIVER", msg_color='red') + continue + shape_color, found_pose = res + if self.correct_shape(shape_color): + self.shape_pose = found_pose + return + fprint("Saw (Shape={}, Color={}) on this side".format( + shape_color[0], shape_color[1]), title="DETECT DELIVER", msg_color='green') + + @txros.util.cancellableInlineCallbacks + def search_shape(self): + shapes = yield self.get_shape() + if shapes.found: + for shape in shapes.shapes.list: + normal_res = yield self.get_normal(shape) + if normal_res.success: + enu_cam_tf = yield self.tf_listener.get_transform('/enu', '/' + shape.header.frame_id, + shape.header.stamp) + if self.correct_shape(shape): + self.shape_pose = self.get_shape_pos(normal_res, enu_cam_tf) + defer.returnValue(True) + self.update_shape(shape, normal_res, enu_cam_tf) + + else: + if not self.last_lidar_error == normal_res.error: + fprint("Normal not found Error={}".format(normal_res.error), + title="DETECT DELIVER", msg_color='red') + self.last_lidar_error = normal_res.error + else: + if not self.last_shape_error == shapes.error: + fprint("shape not found Error={}".format(shapes.error), title="DETECT DELIVER", msg_color="red") + self.last_shape_error = shapes.error + defer.returnValue(False) + + def select_backup_shape(self): + for (shape, color), point_normal in self.identified_shapes.iteritems(): + self.shape_pose = point_normal + if self.Shape == shape or self.Color == color: + fprint("Correct shape not found, resorting to shape={} color={}".format( + shape, color), title="DETECT DELIVER", msg_color='yellow') + return + if self.shape_pose is None: + raise Exception("None seen") + fprint("Correct shape not found, resorting to random shape", title="DETECT DELIVER", msg_color='yellow') + + @txros.util.cancellableInlineCallbacks + def align_to_target(self): + if self.shape_pose is None: + self.select_backup_shape() + goal_point, goal_orientation = self.get_aligned_pose(self.shape_pose[0], self.shape_pose[1]) + move = self.move.set_position(goal_point).set_orientation(goal_orientation).forward(self.target_offset_meters) + # Adjust for location of shooter + move = move.left(-self.shooter_baselink_tf._p[1]).forward(-self.shooter_baselink_tf._p[0]) + fprint("Aligning to shoot at {}".format(move), title="DETECT DELIVER", msg_color='green') + move_complete = yield move.go(move_type="skid", blind=True) + defer.returnValue(move_complete) + + def get_shape(self): + return self.vision_proxies["get_shape"].get_response(Shape="ANY", Color="ANY") + + def get_aligned_pose(self, enupoint, enunormal): + aligned_position = enupoint + self.shoot_distance_meters * enunormal # moves x meters away + angle = np.arctan2(-enunormal[0], enunormal[1]) + aligned_orientation = trns.quaternion_from_euler(0, 0, angle) # Align perpindicular + return (aligned_position, aligned_orientation) + + def get_shape_pos(self, normal_res, enu_cam_tf): + enunormal = enu_cam_tf.transform_vector(mil_tools.rosmsg_to_numpy(normal_res.normal)) + enupoint = enu_cam_tf.transform_point(mil_tools.rosmsg_to_numpy(normal_res.closest)) + return (enupoint, enunormal) + + @txros.util.cancellableInlineCallbacks + def get_normal(self, shape): + req = CameraToLidarTransformRequest() + req.header = shape.header + req.point = Point() + req.point.x = shape.CenterX + req.point.y = shape.CenterY + rect = self._bounding_rect(shape.points) + req.tolerance = int(min(rect[0] - rect[3], rect[1] - rect[4]) / 2.0) + normal_res = yield self.cameraLidarTransformer(req) + if not self.normal_is_sane(normal_res.normal): + normal_res.success = False + normal_res.error = "UNREASONABLE NORMAL" + defer.returnValue(normal_res) + + def normal_is_sane(self, vector3): + return abs(mil_tools.rosmsg_to_numpy(vector3)[1]) < 0.4 + + @txros.util.cancellableInlineCallbacks + def shoot_all_balls(self): + for i in range(self.NUM_BALLS): + goal = yield self.shooterLoad.send_goal(ShooterDoAction()) + fprint("Loading Shooter {}".format(i), title="DETECT DELIVER", msg_color='green') + yield goal.get_result() + yield self.nh.sleep(2) + goal = yield self.shooterFire.send_goal(ShooterDoAction()) + fprint("Firing Shooter {}".format(i), title="DETECT DELIVER", msg_color='green') + yield goal.get_result() + fprint("Waiting {} seconds between shots".format( + self.WAIT_BETWEEN_SHOTS), title="DETECT DELIVER", msg_color='green') + yield self.nh.sleep(self.WAIT_BETWEEN_SHOTS) + + @txros.util.cancellableInlineCallbacks + def continuously_align(self): + fprint("Starting Forest Align", title="DETECT DELIVER", msg_color='green') + while True: + shooter_pose = yield self.shooter_pose_sub.get_next_message() + if self.align_forest_pause: + yield self.nh.sleep(0.1) + continue + shooter_pose = shooter_pose.pose + + cen = np.array([shooter_pose.position.x, shooter_pose.position.y]) + yaw = trns.euler_from_quaternion([shooter_pose.orientation.x, + shooter_pose.orientation.y, + shooter_pose.orientation.z, + shooter_pose.orientation.w])[2] + q = trns.quaternion_from_euler(0, 0, yaw) + p = np.append(cen, 0) + # fprint("Forest Aligning to p=[{}] q=[{}]".format(p, q), title="DETECT DELIVER", msg_color='green') + + # Prepare move to follow shooter + move = self.move.set_position(p).set_orientation(q).yaw_right(90, 'deg') + + # Adjust move for location of target + move = move.forward(self.target_offset_meters) + + # Adjust move for location of launcher + move = move.left(-self.shooter_baselink_tf._p[1]).forward(-self.shooter_baselink_tf._p[0]) + + # Move away a fixed distance to make the shot + move = move.left(self.shoot_distance_meters) + + yield move.go(move_type='bypass') + + @txros.util.cancellableInlineCallbacks + def shoot_and_align_forest(self): + move = yield self.align_to_target() + if move.failure_reason != "": + fprint("Error Aligning with target = {}. Ending mission :(".format( + move.failure_reason), title="DETECT DELIVER", msg_color="red") + return + fprint("Aligned successs. Shooting while using forest realign", title="DETECT DELIVER", msg_color="green") + align_defer = self.continuously_align() + fprint("Sleeping for {} seconds to allow for alignment", + title="DETECT DELIVER".format(self.FOREST_SLEEP), msg_color="green") + yield self.nh.sleep(self.FOREST_SLEEP) + for i in range(self.NUM_BALLS): + goal = yield self.shooterLoad.send_goal(ShooterDoAction()) + fprint("Loading Shooter {}".format(i), title="DETECT DELIVER", msg_color='green') + yield goal.get_result() + yield self.nh.sleep(2) + self.align_forest_pause = True + goal = yield self.shooterFire.send_goal(ShooterDoAction()) + fprint("Firing Shooter {}".format(i), title="DETECT DELIVER", msg_color='green') + yield goal.get_result() + yield self.nh.sleep(1) + self.align_forest_pause = False + fprint("Waiting {} seconds between shots".format( + self.WAIT_BETWEEN_SHOTS), title="DETECT DELIVER", msg_color='green') + yield self.nh.sleep(self.WAIT_BETWEEN_SHOTS) + align_defer.cancel() + + @txros.util.cancellableInlineCallbacks + def backup_from_target(self): + yield self.move.left(self.BACKUP_DISTANCE).go() + + @txros.util.cancellableInlineCallbacks + def shoot_and_align(self): + move = yield self.align_to_target() + if move.failure_reason != "": + fprint("Error Aligning with target = {}. Ending mission :(".format( + move.failure_reason), title="DETECT DELIVER", msg_color="red") + return + fprint("Aligned successs. Shooting without realignment", title="DETECT DELIVER", msg_color="green") + yield self.shoot_all_balls() + + @txros.util.cancellableInlineCallbacks + def setup_mission(self): + stc_color = yield self.mission_params["scan_the_code_color3"].get(raise_exception=False) + if stc_color is False: + color = "ANY" + else: + color = stc_color + # color = "ANY" + shape = "ANY" + fprint("Setting search shape={} color={}".format(shape, color), title="DETECT DELIVER", msg_color='green') + yield self.mission_params["detect_deliver_shape"].set(shape) + yield self.mission_params["detect_deliver_color"].set(color) + + @txros.util.cancellableInlineCallbacks + def run(self, parameters): + yield self.setup_mission() + fprint("STARTING MISSION", title="DETECT DELIVER", msg_color='green') + yield self.vision_proxies["get_shape"].start() + yield self.set_shape_and_color() # Get correct goal shape/color from params + yield self.get_waypoint() # Get waypoint of shooter target + yield self.circle_search() # Go to waypoint and circle until target found + # yield self.shoot_and_align() # Align to target and shoot + yield self.shoot_and_align_forest() # Align to target and shoot + yield self.backup_from_target() + yield self.vision_proxies["get_shape"].stop() + fprint("ENDING MISSION", title="DETECT DELIVER", msg_color='green') diff --git a/mission_control/navigator_missions/navigator_missions/detect_deliver_find.py b/mission_control/navigator_missions/navigator_missions/detect_deliver_find.py new file mode 100644 index 00000000..f4abe09d --- /dev/null +++ b/mission_control/navigator_missions/navigator_missions/detect_deliver_find.py @@ -0,0 +1,171 @@ +#!/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 + + +class DetectDeliverFind(Navigator): + DOCK_SIZE_LONG = 16.0 + DOCK_SIZE_SHORT = 8.0 + + CIRCLE_DISTANCE = 5.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('-a', '--scanall', action='store_true', + help='setting scans all four sides of the dock, not just the short sides') + 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=5, + help='distance to scan the images from') + cls.parser = parser + + cls.junk_1 = 0 # TODO REMOVE THIS -------------------------- + + @util.cancellableInlineCallbacks + def run(self, args): + # Parse Arguments + scan_all = args.scanall + override_scale = args.overridescale + pre_circle = args.circle + scan_dist = args.scandist + + # 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).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 not scan_all: + # TODO: Does this always work? + # Remove 0 and 2; these are the long side ones. + self.scans = [self.scans[1], self.scans[3]] + + # 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() + 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 = 1 + 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]]) + 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() + + 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): + # TODO SCAN IMAGE --------------------------- + self.junk_1 += 1 + if self.junk_1 > 4: + self.junk_1 = 0 + + if self.junk_1 == 2: + val = yield True + defer.returnValue(val) + else: + val = yield False + defer.returnValue(val) From 50e78e1c22e70b63de356e51dcf361fdcd5255a8 Mon Sep 17 00:00:00 2001 From: jaxon Date: Sun, 9 Dec 2018 04:07:51 -0500 Subject: [PATCH 07/19] Formatting fixes --- .../navigator_missions/navigator_missions/detect_deliver.py | 3 ++- .../navigator_missions/detect_deliver_find.py | 3 +-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/mission_control/navigator_missions/navigator_missions/detect_deliver.py b/mission_control/navigator_missions/navigator_missions/detect_deliver.py index 5596bbe6..1d9dea48 100644 --- a/mission_control/navigator_missions/navigator_missions/detect_deliver.py +++ b/mission_control/navigator_missions/navigator_missions/detect_deliver.py @@ -4,6 +4,7 @@ from detect_deliver_find import DetectDeliverFind from track_target import TrackTarget + class DetectDeliver(Navigator): @classmethod @@ -17,4 +18,4 @@ def run(self, args): self.send_feedback('Starting detect deliver') yield self.detect_deiliver_find.run(self.detect_deiliver_find.decode_parameters(args)) yield self.track_target.run(self.track_target.decode_parameters(args)) - self.send_feedback('Done with detect deliver') \ No newline at end of file + self.send_feedback('Done with detect deliver') diff --git a/mission_control/navigator_missions/navigator_missions/detect_deliver_find.py b/mission_control/navigator_missions/navigator_missions/detect_deliver_find.py index f4abe09d..55ad5b56 100644 --- a/mission_control/navigator_missions/navigator_missions/detect_deliver_find.py +++ b/mission_control/navigator_missions/navigator_missions/detect_deliver_find.py @@ -37,7 +37,7 @@ def init(cls): help='distance to scan the images from') cls.parser = parser - cls.junk_1 = 0 # TODO REMOVE THIS -------------------------- + cls.junk_1 = 0 # TODO REMOVE THIS -------------------------- @util.cancellableInlineCallbacks def run(self, args): @@ -111,7 +111,6 @@ def run(self, args): self.send_feedback('Done! In position to line up for shot.') - @util.cancellableInlineCallbacks def find_dock(self, override_scale): # Get Dock From 7fe44de69414b6650a54a6e898ba984ba7b3f2b7 Mon Sep 17 00:00:00 2001 From: jaxon Date: Fri, 14 Dec 2018 02:00:35 -0500 Subject: [PATCH 08/19] Working on docking. Docking not done. --- .../perception/starboard_transformer.launch | 6 ++ .../navigator_missions/detect_deliver_find.py | 33 ++++-- .../navigator_missions/discount_docking.py | 82 ++++++++++++++ .../navigator_missions/docking.py | 101 ++++++++++++++++++ 4 files changed, 214 insertions(+), 8 deletions(-) create mode 100644 mission_control/navigator_launch/launch/perception/starboard_transformer.launch create mode 100644 mission_control/navigator_missions/navigator_missions/discount_docking.py create mode 100644 mission_control/navigator_missions/navigator_missions/docking.py diff --git a/mission_control/navigator_launch/launch/perception/starboard_transformer.launch b/mission_control/navigator_launch/launch/perception/starboard_transformer.launch new file mode 100644 index 00000000..5e84b269 --- /dev/null +++ b/mission_control/navigator_launch/launch/perception/starboard_transformer.launch @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/mission_control/navigator_missions/navigator_missions/detect_deliver_find.py b/mission_control/navigator_missions/navigator_missions/detect_deliver_find.py index 55ad5b56..1ddd4e0b 100644 --- a/mission_control/navigator_missions/navigator_missions/detect_deliver_find.py +++ b/mission_control/navigator_missions/navigator_missions/detect_deliver_find.py @@ -25,8 +25,12 @@ def init(cls): parser = ThrowingArgumentParser(description='Detect Deliver Find', usage='''Default parameters: \'runtask DetectDeliverFind \'''') - parser.add_argument('-a', '--scanall', action='store_true', - help='setting scans all four sides of the dock, not just the short sides') + #parser.add_argument('-a', '--scanall', action='store_true', + # help='setting scans all four sides of the dock, not just the short sides') + 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''') @@ -35,6 +39,10 @@ def init(cls): PCODAR gather enough information to produce scale and an accurate orientation''') parser.add_argument('-d', '--scandist', type=int, default=5, 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=1, + help='the distance to go to as the end point') cls.parser = parser cls.junk_1 = 0 # TODO REMOVE THIS -------------------------- @@ -42,10 +50,14 @@ def init(cls): @util.cancellableInlineCallbacks def run(self, args): # Parse Arguments - scan_all = args.scanall + #scan_all = args.scanall + long_scan = args.longscan + short_scan = args.shortscan override_scale = args.overridescale pre_circle = args.circle scan_dist = args.scandist + look_in = args.lookin + end_dist = args.enddist # Find the dock yield self.find_dock(override_scale) @@ -72,10 +84,12 @@ def run(self, args): self.build_scan_positions(scan_dist, boat_pose) # If we don't want to scan all, remove scans on long side - if not scan_all: - # TODO: Does this always work? - # Remove 0 and 2; these are the long side ones. + if short_scan and not long_scan: + # Remove 1 and 3; these are the long side ones. self.scans = [self.scans[1], self.scans[3]] + elif long_scan and not 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]) @@ -100,11 +114,14 @@ def run(self, args): # Calculate closer location angle = correct_scan[3] - dist = 1 + 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]]) - lpt = pt + np.array([math.sin(angle), -math.cos(angle), 0]) + 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() diff --git a/mission_control/navigator_missions/navigator_missions/discount_docking.py b/mission_control/navigator_missions/navigator_missions/discount_docking.py new file mode 100644 index 00000000..c7ec0177 --- /dev/null +++ b/mission_control/navigator_missions/navigator_missions/discount_docking.py @@ -0,0 +1,82 @@ +#!/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 + + +class DiscountDocking(Navigator): + + @classmethod + def decode_parameters(cls, parameters): + argv = parameters.split() + return cls.parser.parse_args(argv) + + @classmethod + def init(cls): + cls.detect_deiliver_find = DetectDeliverFind() + + @util.cancellableInlineCallbacks + def run(self, args): + # Parse Arguments + + #yield self.detect_deiliver_find.run(self.detect_deiliver_find.decode_parameters(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!') + + @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 calculate_docking_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) * dock_start_dist + self.dock_position[0], + math.sin(angle) * dock_start_dist + 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 \ No newline at end of file diff --git a/mission_control/navigator_missions/navigator_missions/docking.py b/mission_control/navigator_missions/navigator_missions/docking.py new file mode 100644 index 00000000..4f173b5f --- /dev/null +++ b/mission_control/navigator_missions/navigator_missions/docking.py @@ -0,0 +1,101 @@ +#!/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 +import time +from mil_msgs.srv import CameraToLidarTransform +from mil_msgs.msg import ObjectInImage +import txros +from geometry_msgs 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=15) + cls.parser = parser + + cls.bboxsub = cls.nh.subscribe("/bbox_pub", ObjectInImage) + + #cls.camera_lidar_tf = cls.nh.get_service_client('/camera_lidar_transformer', CameraToLidarTransform) + + @util.cancellableInlineCallbacks + def run(self, args): + # Parse Arguments + wait_time = args.time + + #dock = yield self.get_sorted_objects(name='dock', n=1) + #dock = self.dock[0][0] + #dock_position = rosmsg_to_numpy(self.dock.pose.position) + dock_position = np.array([58, -382, 0]) + + timeout = None + dock_timing = False + while True: + center_frame = yield self.get_center_frame() + pose_offset = yield self.get_pose_offset(center_frame) + target_pose_offset = np.array([5, 0, 0]) + diff_pose = pose_offset - target_pose_offset + + print pose_offset + print("DELTA: " + str(diff_pose)) + + #yield self.move.forward(diff_pose[0]).left(diff_pose[1]).look_at(self.dock_position).go() + + + if not dock_timing and pose_offset[0] < 2 and pose_offset[1] < 2: + dock_timing = True + timeout = time.time() + 20 + + if dock_timing and time.time() > timeout: + break + + #yield self.move.backward(10).go() + + + self.send_feedback('Done with docking!') + + @util.cancellableInlineCallbacks + def get_center_frame(self): + msg = yield self.bboxsub.get_next_message() + print msg + tmp = (rosmsg_to_numpy(msg.points[0]) + rosmsg_to_numpy(msg.points[1]) / 2.0) + print tmp + defer.returnValue(tmp) + + @util.cancellableInlineCallbacks + def get_pose_offset(self, center_frame): + msg = CameraToLidarTransform() + msg.header.stamp = self.nh.get_time() + msg.point = Point(center_frame[0] * 720, center_frame[1] * 480) + msg.tolerance = 20 + + pose_offset = yield self.camera_lidar_tf(msg) + + print pose_offset + + pose_offset = pose_offset.closest + + + #boat_pose = yield self.tx_pose + #boat_pose = boat_pose[0] + + + #pose_offset = np.array([125, 0, 0]) - boat_pose + + + defer.returnValue(pose_offset) \ No newline at end of file From 1ce783daebb733a402916296a3a9426e33c665a1 Mon Sep 17 00:00:00 2001 From: jaxon Date: Fri, 14 Dec 2018 02:12:41 -0500 Subject: [PATCH 09/19] init file --- .../navigator_missions/navigator_missions/__init__.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/mission_control/navigator_missions/navigator_missions/__init__.py b/mission_control/navigator_missions/navigator_missions/__init__.py index 4f150d5a..072ad0e1 100644 --- a/mission_control/navigator_missions/navigator_missions/__init__.py +++ b/mission_control/navigator_missions/navigator_missions/__init__.py @@ -35,4 +35,6 @@ 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 import pose_editor From c1da2f9edc97bd5c137a806b84feb336857b9edf Mon Sep 17 00:00:00 2001 From: jaxon Date: Fri, 14 Dec 2018 14:01:57 -0500 Subject: [PATCH 10/19] Perception for docking appears to work in bag --- .../navigator_missions/discount_docking.py | 1 + .../navigator_missions/docking.py | 90 ++++++++++++------- 2 files changed, 59 insertions(+), 32 deletions(-) diff --git a/mission_control/navigator_missions/navigator_missions/discount_docking.py b/mission_control/navigator_missions/navigator_missions/discount_docking.py index c7ec0177..f2f383bc 100644 --- a/mission_control/navigator_missions/navigator_missions/discount_docking.py +++ b/mission_control/navigator_missions/navigator_missions/discount_docking.py @@ -7,6 +7,7 @@ import math from mil_misc_tools import ThrowingArgumentParser import tf.transformations as tform +from detect_deliver_find import DetectDeliverFind class DiscountDocking(Navigator): diff --git a/mission_control/navigator_missions/navigator_missions/docking.py b/mission_control/navigator_missions/navigator_missions/docking.py index 4f173b5f..53ae9160 100644 --- a/mission_control/navigator_missions/navigator_missions/docking.py +++ b/mission_control/navigator_missions/navigator_missions/docking.py @@ -8,10 +8,10 @@ from mil_misc_tools import ThrowingArgumentParser import tf.transformations as tform import time -from mil_msgs.srv import CameraToLidarTransform -from mil_msgs.msg import ObjectInImage +from mil_msgs.srv import CameraToLidarTransform, CameraToLidarTransformRequest +from mil_msgs.msg import ObjectsInImage import txros -from geometry_msgs import Point +from geometry_msgs.msg import Point class Docking(Navigator): @@ -29,37 +29,33 @@ def init(cls): parser.add_argument('-t', '--time', type=int, default=15) cls.parser = parser - cls.bboxsub = cls.nh.subscribe("/bbox_pub", ObjectInImage) + cls.bboxsub = cls.nh.subscribe("/bbox_pub", ObjectsInImage) - #cls.camera_lidar_tf = cls.nh.get_service_client('/camera_lidar_transformer', CameraToLidarTransform) + 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 - #dock = yield self.get_sorted_objects(name='dock', n=1) - #dock = self.dock[0][0] - #dock_position = rosmsg_to_numpy(self.dock.pose.position) - dock_position = np.array([58, -382, 0]) + dock = yield self.get_sorted_objects(name='dock', n=1) + dock = self.dock[0][0] + dock_position = rosmsg_to_numpy(self.dock.pose.position) + #dock_position = np.array([58, -382, 0]) + + center_frame = yield self.get_center_frame() + pose_offset = yield self.get_target_pt(center_frame) + print pose_offset + yield self.move.set_position(pose_offset).look_at(dock_position).go() timeout = None dock_timing = False while True: - center_frame = yield self.get_center_frame() - pose_offset = yield self.get_pose_offset(center_frame) - target_pose_offset = np.array([5, 0, 0]) - diff_pose = pose_offset - target_pose_offset - - print pose_offset - print("DELTA: " + str(diff_pose)) - - #yield self.move.forward(diff_pose[0]).left(diff_pose[1]).look_at(self.dock_position).go() - + if not dock_timing and pose_offset[0] < 2 and pose_offset[1] < 2: dock_timing = True - timeout = time.time() + 20 + timeout = time.time() + wait_time if dock_timing and time.time() > timeout: break @@ -71,24 +67,54 @@ def run(self, args): @util.cancellableInlineCallbacks def get_center_frame(self): - msg = yield self.bboxsub.get_next_message() - print msg - tmp = (rosmsg_to_numpy(msg.points[0]) + rosmsg_to_numpy(msg.points[1]) / 2.0) - print tmp + 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]) + c1 = np.array([c1[1], c1[0], 0]) + c2 = np.array([c2[1], c2[0], 0]) + c1 *= 4 + c2 *= 4 + tmp = (((c1 + c2) / 2.0), msgf) + #print tmp defer.returnValue(tmp) @util.cancellableInlineCallbacks - def get_pose_offset(self, center_frame): - msg = CameraToLidarTransform() - msg.header.stamp = self.nh.get_time() - msg.point = Point(center_frame[0] * 720, center_frame[1] * 480) - msg.tolerance = 20 + 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 = 100 + + #print 'msg' + #print msg + #print '----' pose_offset = yield self.camera_lidar_tf(msg) - print pose_offset + #print pose_offset + + #pose_offset = rosmsg_to_numpy(pose_offset.closest) + + + 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 + + #print normal + #print found_pt + + normal *= 3 + found_pt += normal + - pose_offset = pose_offset.closest #boat_pose = yield self.tx_pose @@ -98,4 +124,4 @@ def get_pose_offset(self, center_frame): #pose_offset = np.array([125, 0, 0]) - boat_pose - defer.returnValue(pose_offset) \ No newline at end of file + defer.returnValue(found_pt) \ No newline at end of file From 6a080459da691c01b3596ffeae7d858a18897f0f Mon Sep 17 00:00:00 2001 From: jaxon Date: Fri, 14 Dec 2018 14:18:42 -0500 Subject: [PATCH 11/19] working on docking --- .../navigator_missions/navigator_missions/docking.py | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/mission_control/navigator_missions/navigator_missions/docking.py b/mission_control/navigator_missions/navigator_missions/docking.py index 53ae9160..27217f07 100644 --- a/mission_control/navigator_missions/navigator_missions/docking.py +++ b/mission_control/navigator_missions/navigator_missions/docking.py @@ -41,7 +41,6 @@ def run(self, args): dock = yield self.get_sorted_objects(name='dock', n=1) dock = self.dock[0][0] dock_position = rosmsg_to_numpy(self.dock.pose.position) - #dock_position = np.array([58, -382, 0]) center_frame = yield self.get_center_frame() pose_offset = yield self.get_target_pt(center_frame) @@ -51,8 +50,6 @@ def run(self, args): timeout = None dock_timing = False while True: - - if not dock_timing and pose_offset[0] < 2 and pose_offset[1] < 2: dock_timing = True timeout = time.time() + wait_time @@ -60,7 +57,7 @@ def run(self, args): if dock_timing and time.time() > timeout: break - #yield self.move.backward(10).go() + yield self.move.backward(10).go() self.send_feedback('Done with docking!') From 11741d0327cc92aa2096a6f8128496284a6fb967 Mon Sep 17 00:00:00 2001 From: jaxon Date: Fri, 14 Dec 2018 20:57:51 -0500 Subject: [PATCH 12/19] working --- .../navigator_missions/docking.py | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/mission_control/navigator_missions/navigator_missions/docking.py b/mission_control/navigator_missions/navigator_missions/docking.py index 27217f07..516f2608 100644 --- a/mission_control/navigator_missions/navigator_missions/docking.py +++ b/mission_control/navigator_missions/navigator_missions/docking.py @@ -26,7 +26,7 @@ def init(cls): parser = ThrowingArgumentParser(description='Dock', usage='''Default parameters: \'runtask Docking \'''') - parser.add_argument('-t', '--time', type=int, default=15) + parser.add_argument('-t', '--time', type=int, default=-1) cls.parser = parser cls.bboxsub = cls.nh.subscribe("/bbox_pub", ObjectsInImage) @@ -44,18 +44,13 @@ def run(self, args): center_frame = yield self.get_center_frame() pose_offset = yield self.get_target_pt(center_frame) + symbol = pose_offset[1]. print pose_offset - yield self.move.set_position(pose_offset).look_at(dock_position).go() + yield self.move.set_position(pose_offset).look_at(dock_position).go(blind=True) timeout = None dock_timing = False - while True: - if not dock_timing and pose_offset[0] < 2 and pose_offset[1] < 2: - dock_timing = True - timeout = time.time() + wait_time - - if dock_timing and time.time() > timeout: - break + yield self.nh.sleep(wait_time) yield self.move.backward(10).go() @@ -73,7 +68,7 @@ def get_center_frame(self): c2 = np.array([c2[1], c2[0], 0]) c1 *= 4 c2 *= 4 - tmp = (((c1 + c2) / 2.0), msgf) + tmp = (((c1 + c2) / 2.0), msgf, msg.find) #print tmp defer.returnValue(tmp) From d953d124da339eaeff5e1c13ca00264f8395a5eb Mon Sep 17 00:00:00 2001 From: jaxon Date: Sat, 15 Dec 2018 02:53:07 -0500 Subject: [PATCH 13/19] Cleaning up night before finals --- .../navigator_missions/docking.py | 114 ++++++++++++------ 1 file changed, 78 insertions(+), 36 deletions(-) diff --git a/mission_control/navigator_missions/navigator_missions/docking.py b/mission_control/navigator_missions/navigator_missions/docking.py index 516f2608..645cde2b 100644 --- a/mission_control/navigator_missions/navigator_missions/docking.py +++ b/mission_control/navigator_missions/navigator_missions/docking.py @@ -30,7 +30,6 @@ def init(cls): 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 @@ -38,21 +37,68 @@ def run(self, args): # Parse Arguments wait_time = args.time - dock = yield self.get_sorted_objects(name='dock', n=1) - dock = self.dock[0][0] - dock_position = rosmsg_to_numpy(self.dock.pose.position) + # 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='UNCLASSIFIED', n=10) + 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() - pose_offset = yield self.get_target_pt(center_frame) - symbol = pose_offset[1]. - print pose_offset - yield self.move.set_position(pose_offset).look_at(dock_position).go(blind=True) + 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 + print target_pt + + if wait_time == -1: + if 'triangle' in symbol: + wait_time = 7 + elif 'circle' in symbol: + wait_time = 17 + else: # Cruciform + wait_time = 27 - timeout = None - dock_timing = False + # 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('------------------------------------------------') - yield self.move.backward(10).go() + # 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!') @@ -64,12 +110,7 @@ def get_center_frame(self): #print msg c1 = rosmsg_to_numpy(msg.points[0]) c2 = rosmsg_to_numpy(msg.points[1]) - c1 = np.array([c1[1], c1[0], 0]) - c2 = np.array([c2[1], c2[0], 0]) - c1 *= 4 - c2 *= 4 - tmp = (((c1 + c2) / 2.0), msgf, msg.find) - #print tmp + tmp = (((c1 + c2) / 2.0), msgf, msg.name) defer.returnValue(tmp) @util.cancellableInlineCallbacks @@ -78,7 +119,7 @@ def get_target_pt(self, center_frame): 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 = 100 + msg.tolerance = 500 #print 'msg' #print msg @@ -88,9 +129,6 @@ def get_target_pt(self, center_frame): #print pose_offset - #pose_offset = rosmsg_to_numpy(pose_offset.closest) - - 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) @@ -100,20 +138,24 @@ def get_target_pt(self, center_frame): found_pt = cam_to_enu.transform_point(found_pt) found_pt[2] = 0 - #print normal - #print found_pt + print('foundpt {}'.format(found_pt)) + print('normal {}'.format(normal)) + # Extend out by normal multiplier normal *= 3 - found_pt += normal - - - - - #boat_pose = yield self.tx_pose - #boat_pose = boat_pose[0] - - - #pose_offset = np.array([125, 0, 0]) - boat_pose - - - defer.returnValue(found_pt) \ No newline at end of file + found_pt_1 = found_pt + normal + found_pt_2 = found_pt + -1 * normal + + print('foundpt1 {}'.format(found_pt_1)) + print('foundpt2 {}'.format(found_pt_2)) + + # 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 + print('selected 2') + else: + found_pt = found_pt_1 + print('selected 1') + + defer.returnValue(found_pt) From d2e271295bf24b41b11d04208295fd3cb7dc8c67 Mon Sep 17 00:00:00 2001 From: jaxon Date: Sat, 15 Dec 2018 04:02:47 -0500 Subject: [PATCH 14/19] cleaning up --- .../navigator_missions/discount_docking.py | 66 ------------------- .../navigator_missions/docking.py | 27 +------- 2 files changed, 3 insertions(+), 90 deletions(-) diff --git a/mission_control/navigator_missions/navigator_missions/discount_docking.py b/mission_control/navigator_missions/navigator_missions/discount_docking.py index f2f383bc..906be06c 100644 --- a/mission_control/navigator_missions/navigator_missions/discount_docking.py +++ b/mission_control/navigator_missions/navigator_missions/discount_docking.py @@ -1,32 +1,11 @@ #!/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 detect_deliver_find import DetectDeliverFind class DiscountDocking(Navigator): - - @classmethod - def decode_parameters(cls, parameters): - argv = parameters.split() - return cls.parser.parse_args(argv) - - @classmethod - def init(cls): - cls.detect_deiliver_find = DetectDeliverFind() - @util.cancellableInlineCallbacks def run(self, args): - # Parse Arguments - - #yield self.detect_deiliver_find.run(self.detect_deiliver_find.decode_parameters(args)) - yield self.move.forward(5).go() yield self.navigator.nh.sleep(10) @@ -35,49 +14,4 @@ def run(self, args): yield self.move.backward(5).go() yield self.move.backward(5).go() - self.send_feedback('Done!') - - @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 calculate_docking_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) * dock_start_dist + self.dock_position[0], - math.sin(angle) * dock_start_dist + 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 \ No newline at end of file diff --git a/mission_control/navigator_missions/navigator_missions/docking.py b/mission_control/navigator_missions/navigator_missions/docking.py index 645cde2b..dfd69220 100644 --- a/mission_control/navigator_missions/navigator_missions/docking.py +++ b/mission_control/navigator_missions/navigator_missions/docking.py @@ -4,13 +4,9 @@ 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 -import time from mil_msgs.srv import CameraToLidarTransform, CameraToLidarTransformRequest from mil_msgs.msg import ObjectsInImage -import txros from geometry_msgs.msg import Point @@ -42,7 +38,7 @@ def run(self, args): largest_size = 0 boat_pos = (yield self.tx_pose)[0] # Get 10 closest unclassified objects - unclass = yield self.get_sorted_objects(name='UNCLASSIFIED', n=10) + 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) @@ -75,14 +71,12 @@ def run(self, args): self.send_feedback('Identified target') # Identify the time to wait in the dock - print target_pt - if wait_time == -1: if 'triangle' in symbol: wait_time = 7 elif 'circle' in symbol: wait_time = 17 - else: # Cruciform + else: # Cruciform wait_time = 27 # Go to pose @@ -100,14 +94,13 @@ def run(self, args): 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 + # print msg c1 = rosmsg_to_numpy(msg.points[0]) c2 = rosmsg_to_numpy(msg.points[1]) tmp = (((c1 + c2) / 2.0), msgf, msg.name) @@ -121,14 +114,8 @@ def get_target_pt(self, center_frame): msg.point = Point(x=center_frame[0][0], y=center_frame[0][1], z=0.0) msg.tolerance = 500 - #print 'msg' - #print msg - #print '----' - pose_offset = yield self.camera_lidar_tf(msg) - #print pose_offset - 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) @@ -138,24 +125,16 @@ def get_target_pt(self, center_frame): found_pt = cam_to_enu.transform_point(found_pt) found_pt[2] = 0 - print('foundpt {}'.format(found_pt)) - print('normal {}'.format(normal)) - # Extend out by normal multiplier normal *= 3 found_pt_1 = found_pt + normal found_pt_2 = found_pt + -1 * normal - print('foundpt1 {}'.format(found_pt_1)) - print('foundpt2 {}'.format(found_pt_2)) - # 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 - print('selected 2') else: found_pt = found_pt_1 - print('selected 1') defer.returnValue(found_pt) From a64e90671e592760bcd8d5247e32256a42bf53e8 Mon Sep 17 00:00:00 2001 From: jaxon Date: Sat, 15 Dec 2018 04:07:32 -0500 Subject: [PATCH 15/19] Didn't need to add this --- .../launch/perception/starboard_transformer.launch | 6 ------ 1 file changed, 6 deletions(-) delete mode 100644 mission_control/navigator_launch/launch/perception/starboard_transformer.launch diff --git a/mission_control/navigator_launch/launch/perception/starboard_transformer.launch b/mission_control/navigator_launch/launch/perception/starboard_transformer.launch deleted file mode 100644 index 5e84b269..00000000 --- a/mission_control/navigator_launch/launch/perception/starboard_transformer.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - \ No newline at end of file From 949514f1ade1f30c3148af642ae92623f50241c8 Mon Sep 17 00:00:00 2001 From: jaxon Date: Sat, 15 Dec 2018 05:55:08 -0500 Subject: [PATCH 16/19] Working before finals --- .../navigator_missions/detect_deliver_find.py | 50 +++++++++---------- .../navigator_missions/navigator.py | 1 + 2 files changed, 26 insertions(+), 25 deletions(-) diff --git a/mission_control/navigator_missions/navigator_missions/detect_deliver_find.py b/mission_control/navigator_missions/navigator_missions/detect_deliver_find.py index 1ddd4e0b..d1ccbbb1 100644 --- a/mission_control/navigator_missions/navigator_missions/detect_deliver_find.py +++ b/mission_control/navigator_missions/navigator_missions/detect_deliver_find.py @@ -7,13 +7,14 @@ 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 = 5.0 + math.sqrt((DOCK_SIZE_SHORT / 2)**2 + (DOCK_SIZE_LONG / 2)**2) + CIRCLE_DISTANCE = 8.0 + math.sqrt((DOCK_SIZE_SHORT / 2)**2 + (DOCK_SIZE_LONG / 2)**2) @classmethod def decode_parameters(cls, parameters): @@ -25,8 +26,6 @@ def init(cls): parser = ThrowingArgumentParser(description='Detect Deliver Find', usage='''Default parameters: \'runtask DetectDeliverFind \'''') - #parser.add_argument('-a', '--scanall', action='store_true', - # help='setting scans all four sides of the dock, not just the short sides') parser.add_argument('-l', '--longscan', action='store_true', help='set to scan the long side') parser.add_argument('-s', '--shortscan', action='store_true', @@ -37,22 +36,21 @@ def init(cls): 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=5, + 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=1, + parser.add_argument('-e', '--enddist', type=int, default=5, help='the distance to go to as the end point') cls.parser = parser - cls.junk_1 = 0 # TODO REMOVE THIS -------------------------- + cls.bboxsub = cls.nh.subscribe("/bbox_pub", ObjectsInImage) @util.cancellableInlineCallbacks def run(self, args): # Parse Arguments - #scan_all = args.scanall - long_scan = args.longscan - short_scan = args.shortscan + self.long_scan = args.longscan + self.short_scan = args.shortscan override_scale = args.overridescale pre_circle = args.circle scan_dist = args.scandist @@ -70,7 +68,7 @@ def run(self, args): 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).go() + 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 @@ -84,10 +82,10 @@ def run(self, args): self.build_scan_positions(scan_dist, boat_pose) # If we don't want to scan all, remove scans on long side - if short_scan and not long_scan: + 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 long_scan and not short_scan: + 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]] @@ -99,7 +97,8 @@ def run(self, args): 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.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 @@ -124,7 +123,7 @@ def run(self, args): 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() + yield self.move.set_position(pt).look_at(lpt).go(blind=True) self.send_feedback('Done! In position to line up for shot.') @@ -174,14 +173,15 @@ def build_scan_positions(self, scan_dist, boat_pose): @util.cancellableInlineCallbacks def scan_image(self): - # TODO SCAN IMAGE --------------------------- - self.junk_1 += 1 - if self.junk_1 > 4: - self.junk_1 = 0 - - if self.junk_1 == 2: - val = yield True - defer.returnValue(val) - else: - val = yield False - defer.returnValue(val) + 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) diff --git a/mission_control/navigator_missions/navigator_missions/navigator.py b/mission_control/navigator_missions/navigator_missions/navigator.py index 5d03e25e..149f39e4 100644 --- a/mission_control/navigator_missions/navigator_missions/navigator.py +++ b/mission_control/navigator_missions/navigator_missions/navigator.py @@ -65,6 +65,7 @@ class Navigator(BaseTask): net_stc_results = None net_entrance_results = None max_grinch_effort = 500 + docking_scan = 'NA' def __init__(self, **kwargs): super(Navigator, self).__init__(**kwargs) From 980188110d21f29cc5b38d6eca3f7793cb363bb6 Mon Sep 17 00:00:00 2001 From: jaxon Date: Sat, 15 Dec 2018 11:49:08 -0500 Subject: [PATCH 17/19] working --- .../navigator_missions/navigator_missions/navigator.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/mission_control/navigator_missions/navigator_missions/navigator.py b/mission_control/navigator_missions/navigator_missions/navigator.py index 149f39e4..0c931722 100644 --- a/mission_control/navigator_missions/navigator_missions/navigator.py +++ b/mission_control/navigator_missions/navigator_missions/navigator.py @@ -65,7 +65,6 @@ class Navigator(BaseTask): net_stc_results = None net_entrance_results = None max_grinch_effort = 500 - docking_scan = 'NA' def __init__(self, **kwargs): super(Navigator, self).__init__(**kwargs) @@ -153,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 + self.docking_scan = 'NA' + @classmethod def _grinch_limit_switch_cb(cls, data): cls.grinch_limit_switch_pressed = data.data From 6ef49d6e56edd716bd0131b8a6e821249ca2abf7 Mon Sep 17 00:00:00 2001 From: jaxon Date: Sat, 15 Dec 2018 12:00:06 -0500 Subject: [PATCH 18/19] working... --- .../navigator_missions/__init__.py | 1 + .../navigator_missions/detect_deliver_find.py | 3 +++ .../navigator_missions/shoot_balls.py | 21 +++++++++++++++++++ 3 files changed, 25 insertions(+) create mode 100644 mission_control/navigator_missions/navigator_missions/shoot_balls.py diff --git a/mission_control/navigator_missions/navigator_missions/__init__.py b/mission_control/navigator_missions/navigator_missions/__init__.py index 072ad0e1..85406db8 100644 --- a/mission_control/navigator_missions/navigator_missions/__init__.py +++ b/mission_control/navigator_missions/navigator_missions/__init__.py @@ -37,4 +37,5 @@ from detect_deliver import DetectDeliver from discount_docking import DiscountDocking from docking import Docking +from shoot_balls import ShootBalls import pose_editor diff --git a/mission_control/navigator_missions/navigator_missions/detect_deliver_find.py b/mission_control/navigator_missions/navigator_missions/detect_deliver_find.py index d1ccbbb1..3ed95b44 100644 --- a/mission_control/navigator_missions/navigator_missions/detect_deliver_find.py +++ b/mission_control/navigator_missions/navigator_missions/detect_deliver_find.py @@ -57,6 +57,9 @@ def run(self, args): 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) diff --git a/mission_control/navigator_missions/navigator_missions/shoot_balls.py b/mission_control/navigator_missions/navigator_missions/shoot_balls.py new file mode 100644 index 00000000..c3a9ce81 --- /dev/null +++ b/mission_control/navigator_missions/navigator_missions/shoot_balls.py @@ -0,0 +1,21 @@ +#!/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 ShootBalls(Navigator): + @util.cancellableInlineCallbacks + def run(self, args): + for i in range(0, 4): + yield self.reload_launcher() + yield self.nh.sleep(2) + yield self.fire_launcher() + yield self.nh.sleep(2) + yield self.set_vision_off() \ No newline at end of file From e816fbfe1645c5c85cbbb5d6086b5d4ce801a123 Mon Sep 17 00:00:00 2001 From: jaxon Date: Sat, 15 Dec 2018 12:08:55 -0500 Subject: [PATCH 19/19] Fix bug --- .../navigator_missions/navigator_missions/navigator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission_control/navigator_missions/navigator_missions/navigator.py b/mission_control/navigator_missions/navigator_missions/navigator.py index 0c931722..6cfd73ac 100644 --- a/mission_control/navigator_missions/navigator_missions/navigator.py +++ b/mission_control/navigator_missions/navigator_missions/navigator.py @@ -152,7 +152,7 @@ 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 - self.docking_scan = 'NA' + cls.docking_scan = 'NA' @classmethod def _grinch_limit_switch_cb(cls, data):