From bf8a94940dbf21726bc3448a0c7090a5c30d8680 Mon Sep 17 00:00:00 2001 From: jaxon Date: Mon, 29 Oct 2018 18:40:29 -0400 Subject: [PATCH 1/4] Improved multilateration script --- .../mil_passive_sonar/scripts/multilateration | 96 +++++++++++-------- 1 file changed, 58 insertions(+), 38 deletions(-) diff --git a/drivers/mil_passive_sonar/scripts/multilateration b/drivers/mil_passive_sonar/scripts/multilateration index 437c5a72..30fe27ec 100755 --- a/drivers/mil_passive_sonar/scripts/multilateration +++ b/drivers/mil_passive_sonar/scripts/multilateration @@ -13,7 +13,7 @@ from tf2_geometry_msgs import Vector3Stamped, PointStamped from std_msgs.msg import Header from geometry_msgs.msg import PointStamped, Vector3 from mil_passive_sonar.srv import FindPinger, FindPingerResponse -from std_srvs.srv import SetBool, SetBoolResponse +from std_srvs.srv import SetBool, SetBoolResponse, Trigger, TriggerResponse class MultilaterationNode(object): @@ -24,30 +24,39 @@ class MultilaterationNode(object): def __init__(self): self.tfBuffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tfBuffer) - buffer_len = rospy.get_param('~buffer_size', default=10) + buffer_len = rospy.get_param('~buffer_size', default=30) + self.distance_filter_dist = rospy.get_param('~distance_filter', default=5) + self.buffer_min = 10 self.vec_samples = deque(maxlen=buffer_len) self.enabled = False - self.global_fame = 'enu' + self.last_pos = None + self.global_frame = 'enu' self.heading_sub = rospy.Subscriber('/hydrophones/direction', Vector3Stamped, self.heading_cb, queue_size=10) self.position_pub = rospy.Publisher('/hydrophones/position', PointStamped, queue_size=1) self.enable_srv = rospy.Service('~enable', SetBool, self.enable_cb) + self.reset_srv = rospy.Service('~reset', Trigger, + self.reset_cb) def enable_cb(self, req): - if req.data is not self.enabled: - self.vec_samples.clear() + #if req.data is not self.enabled: + # self.vec_samples.clear() self.enabled = req.data return SetBoolResponse(success=True) + def reset_cb(self, req): + self.vec_samples.clear() + return {'success': True} + def heading_cb(self, p_message): if not self.enabled: return try: transformed_vec = self.tfBuffer.transform(p_message, - self.global_fame, rospy.Duration(2)) - transformed_origin = self.tfBuffer.lookup_transform(self.global_fame, + self.global_frame, rospy.Duration(2)) + transformed_origin = self.tfBuffer.lookup_transform(self.global_frame, p_message.header.frame_id, p_message.header.stamp, rospy.Duration(2)) vec = rosmsg_to_numpy(transformed_vec.vector) @@ -57,50 +66,61 @@ class MultilaterationNode(object): except tf2_ros.TransformException, e: rospy.logwarn('TF Exception: {}'.format(e)) return + if self.last_pos is not None and np.linalg.norm(self.last_pos - origin) < 0.2: + rospy.loginfo("Observation too close to previous, ignoring") + return + self.last_pos = origin self.vec_samples.append((origin, origin + vec)) - if len(self.vec_samples) == self.vec_samples.maxlen: + if len(self.vec_samples) >= self.buffer_min: self.publish_position() def publish_position(self): - line_array = np.array([(p[0][0], p[0][1], p[1][0], p[1][1]) for p in self.vec_samples]) + #line_array = np.array([(p[0][0], p[0][1], p[1][0], p[1][1]) for p in self.vec_samples]) + line_array = np.array([(np.array(p[0][0:2]), np.array(p[1][0:2])) for p in self.vec_samples]) where = self.ls_line_intersection2d(line_array) ps = PointStamped() + ps.header.frame_id = self.global_frame ps.header.stamp = rospy.Time.now() ps.point = numpy_to_point(where) self.position_pub.publish(ps) - def ls_line_intersection2d(self, line_array): - """ - Find the intersection of lines in the least-squares sense. - start - Nx3 numpy array of start points - end - Nx3 numpy array of end points - https://en.wikipedia.org/wiki/Line-line_intersection#In_two_dimensions_2 - """ - def line_segment_norm(line_end_pts): - assert len(line_end_pts) == 4 - return np.linalg.norm(line_end_pts[2:] - line_end_pts[:2]) - - begin_pts = line_array[:, :2] - diffs = line_array[:, 2:4] - begin_pts - norms = np.apply_along_axis(line_segment_norm, 1, line_array).reshape(diffs.shape[0], 1) - rot_left_90 = np.array([[0, -1], [1, 0]]) - perp_unit_vecs = np.apply_along_axis(lambda unit_diffs: rot_left_90.dot(unit_diffs), 1, diffs / norms) - A_sum = np.zeros((2, 2)) - Ap_sum = np.zeros((2, 1)) - - for x, y in zip(begin_pts, perp_unit_vecs): - begin = x.reshape(2, 1) - perp_vec = y.reshape(2, 1) - A = perp_vec.dot(perp_vec.T) - Ap = A.dot(begin) - A_sum += A - Ap_sum += Ap - - return np.linalg.inv(A_sum).dot(Ap_sum) + def ls_line_intersection2d(self, lines_array): + xvals = np.array([]) + yvals = np.array([]) + + for line1 in lines_array: + for line2 in lines_array: + if np.array_equal(line1, line2): + continue + + boat_pos1 = line1[0] + boat_pos2 = line2[0] + + #print(boat_pos1) + + # https://stackoverflow.com/a/47823499 + t, s = np.linalg.solve(np.array([line1[1]-line1[0], line2[0]-line2[1]]).T, line2[0]-line1[0]) + intersection = (1-t)*line1[0] + t*line1[1] + + distance1 = np.linalg.norm(intersection - boat_pos1) + distance2 = np.linalg.norm(intersection - boat_pos2) + + #print(str(distance1) + ', ' + str(distance2)) + + if distance1 < self.distance_filter_dist or distance2 < self.distance_filter_dist: + #print('throw out') + continue # Throw out intersections that are too close to the boat + + #print('using ' + str(intersection[0])) + xvals = np.append(xvals, intersection[0]) + yvals = np.append(yvals, intersection[1]) + + #print(xvals) + average_intersection = np.array([np.mean(xvals), np.mean(yvals)]) + return average_intersection if __name__ == '__main__': rospy.init_node('multilateration') MultilaterationNode() rospy.spin() - From 069b494604526c6a8d2b81491d3b64bfe2a761cc Mon Sep 17 00:00:00 2001 From: jaxon Date: Fri, 9 Nov 2018 15:58:23 -0500 Subject: [PATCH 2/4] Improving multilateration script. Ready for testing; lots of mess at the moment --- .../mil_passive_sonar/scripts/multilateration | 111 +++++++++++++++++- 1 file changed, 108 insertions(+), 3 deletions(-) diff --git a/drivers/mil_passive_sonar/scripts/multilateration b/drivers/mil_passive_sonar/scripts/multilateration index 30fe27ec..547862f0 100755 --- a/drivers/mil_passive_sonar/scripts/multilateration +++ b/drivers/mil_passive_sonar/scripts/multilateration @@ -24,7 +24,7 @@ class MultilaterationNode(object): def __init__(self): self.tfBuffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tfBuffer) - buffer_len = rospy.get_param('~buffer_size', default=30) + buffer_len = rospy.get_param('~buffer_size', default=15) self.distance_filter_dist = rospy.get_param('~distance_filter', default=5) self.buffer_min = 10 self.vec_samples = deque(maxlen=buffer_len) @@ -75,16 +75,20 @@ class MultilaterationNode(object): self.publish_position() def publish_position(self): - #line_array = np.array([(p[0][0], p[0][1], p[1][0], p[1][1]) for p in self.vec_samples]) + #line_array2 = np.array([(p[0][0], p[0][1], p[1][0], p[1][1]) for p in self.vec_samples]) + #print(line_array2) + #where2 = self.lsn_line_intersection2d(line_array2) line_array = np.array([(np.array(p[0][0:2]), np.array(p[1][0:2])) for p in self.vec_samples]) where = self.ls_line_intersection2d(line_array) + if where is None: + return ps = PointStamped() ps.header.frame_id = self.global_frame ps.header.stamp = rospy.Time.now() ps.point = numpy_to_point(where) self.position_pub.publish(ps) - def ls_line_intersection2d(self, lines_array): + def avg_line_intersection2d(self, lines_array): xvals = np.array([]) yvals = np.array([]) @@ -119,6 +123,107 @@ class MultilaterationNode(object): average_intersection = np.array([np.mean(xvals), np.mean(yvals)]) return average_intersection + def lsn_line_intersection2d(self, line_array): + """ + Find the intersection of lines in the least-squares sense. + start - Nx3 numpy array of start points + end - Nx3 numpy array of end points + https://en.wikipedia.org/wiki/Line-line_intersection#In_two_dimensions_2 + """ + def line_segment_norm(line_end_pts): + assert len(line_end_pts) == 4 + return np.linalg.norm(line_end_pts[2:] - line_end_pts[:2]) + + begin_pts = line_array[:, :2] + diffs = line_array[:, 2:4] - begin_pts + norms = np.apply_along_axis(line_segment_norm, 1, line_array).reshape(diffs.shape[0], 1) + rot_left_90 = np.array([[0, -1], [1, 0]]) + perp_unit_vecs = np.apply_along_axis(lambda unit_diffs: rot_left_90.dot(unit_diffs), 1, diffs / norms) + A_sum = np.zeros((2, 2)) + Ap_sum = np.zeros((2, 1)) + + for x, y in zip(begin_pts, perp_unit_vecs): + begin = x.reshape(2, 1) + perp_vec = y.reshape(2, 1) + A = perp_vec.dot(perp_vec.T) + Ap = A.dot(begin) + A_sum += A + Ap_sum += Ap + + return np.linalg.inv(A_sum).dot(Ap_sum) + + def ls_line_intersection2d(self, lines_array): + line_array = np.array([[0, 0, 0, 0]]) + line_intersecting_array = np.array([]) + + for line1 in lines_array: + line_intersections = 0 + for line2 in lines_array: + if np.array_equal(line1, line2): + continue + + boat_pos1 = line1[0] + boat_pos2 = line2[0] + + #print(boat_pos1 + + # https://stackoverflow.com/a/47823499 + t, s = np.linalg.solve(np.array([line1[1]-line1[0], line2[0]-line2[1]]).T, line2[0]-line1[0]) + intersection = (1-t)*line1[0] + t*line1[1] + + distance1 = np.linalg.norm(intersection - boat_pos1) + distance2 = np.linalg.norm(intersection - boat_pos2) + + #print(str(distance1) + ', ' + str(distance2)) + + if distance1 < self.distance_filter_dist or distance2 < self.distance_filter_dist: + line_intersections = line_intersections + 1 + #print('found an intersection') + #print('throwing out ' + str(line1) + ', ' + str(line2)) + #already_tossed_combinations = np.append(already_tossed_combinations, np.array([line1, line2])) + #break + + if line_intersections < 4: + line_array = np.vstack([line_array, np.array([line1[0][0], line1[0][1], line1[1][0], line1[1][1]])]) + #print('used a line') + #else: + #print('threw out line') + + line_array = line_array[1:] + + + if len(line_array) < 3: + return None + + + """ + Find the intersection of lines in the least-squares sense. + start - Nx3 numpy array of start points + end - Nx3 numpy array of end points + https://en.wikipedia.org/wiki/Line-line_intersection#In_two_dimensions_2 + """ + def line_segment_norm(line_end_pts): + assert len(line_end_pts) == 4 + return np.linalg.norm(line_end_pts[2:] - line_end_pts[:2]) + + begin_pts = line_array[:, :2] + diffs = line_array[:, 2:4] - begin_pts + norms = np.apply_along_axis(line_segment_norm, 1, line_array).reshape(diffs.shape[0], 1) + rot_left_90 = np.array([[0, -1], [1, 0]]) + perp_unit_vecs = np.apply_along_axis(lambda unit_diffs: rot_left_90.dot(unit_diffs), 1, diffs / norms) + A_sum = np.zeros((2, 2)) + Ap_sum = np.zeros((2, 1)) + + for x, y in zip(begin_pts, perp_unit_vecs): + begin = x.reshape(2, 1) + perp_vec = y.reshape(2, 1) + A = perp_vec.dot(perp_vec.T) + Ap = A.dot(begin) + A_sum += A + Ap_sum += Ap + + return np.linalg.inv(A_sum).dot(Ap_sum) + if __name__ == '__main__': rospy.init_node('multilateration') From 0ecce88a91a1856c00bb5f04d58dda6363be79ac Mon Sep 17 00:00:00 2001 From: jaxon Date: Wed, 28 Nov 2018 17:44:45 -0500 Subject: [PATCH 3/4] Multilateration filtering working. Documented --- .../mil_passive_sonar/scripts/multilateration | 121 +++++------------- 1 file changed, 31 insertions(+), 90 deletions(-) diff --git a/drivers/mil_passive_sonar/scripts/multilateration b/drivers/mil_passive_sonar/scripts/multilateration index 547862f0..9d909c80 100755 --- a/drivers/mil_passive_sonar/scripts/multilateration +++ b/drivers/mil_passive_sonar/scripts/multilateration @@ -25,8 +25,10 @@ class MultilaterationNode(object): self.tfBuffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tfBuffer) buffer_len = rospy.get_param('~buffer_size', default=15) - self.distance_filter_dist = rospy.get_param('~distance_filter', default=5) - self.buffer_min = 10 + self.minimum_delta_dist = rospy.get_param('~min_delta', default=0.2) + self.filter_dist = rospy.get_param('~filter_dist', default=5) + self.filter_count = rospy.get_param('~filter_count', default=4) + self.buffer_min = rospy.get_param('~buffer_min', default=10) self.vec_samples = deque(maxlen=buffer_len) self.enabled = False self.last_pos = None @@ -41,8 +43,6 @@ class MultilaterationNode(object): self.reset_cb) def enable_cb(self, req): - #if req.data is not self.enabled: - # self.vec_samples.clear() self.enabled = req.data return SetBoolResponse(success=True) @@ -54,6 +54,7 @@ class MultilaterationNode(object): if not self.enabled: return try: + # Transform ping into gloabl frame transformed_vec = self.tfBuffer.transform(p_message, self.global_frame, rospy.Duration(2)) transformed_origin = self.tfBuffer.lookup_transform(self.global_frame, @@ -66,93 +67,44 @@ class MultilaterationNode(object): except tf2_ros.TransformException, e: rospy.logwarn('TF Exception: {}'.format(e)) return - if self.last_pos is not None and np.linalg.norm(self.last_pos - origin) < 0.2: + + # Check if two samples were taken too close to each other + if self.last_pos is not None and np.linalg.norm(self.last_pos - origin) < self.minimum_delta_dist: rospy.loginfo("Observation too close to previous, ignoring") return self.last_pos = origin + + # Add ping to samples list self.vec_samples.append((origin, origin + vec)) + + # If there are enough samples, calculate position if len(self.vec_samples) >= self.buffer_min: self.publish_position() def publish_position(self): - #line_array2 = np.array([(p[0][0], p[0][1], p[1][0], p[1][1]) for p in self.vec_samples]) - #print(line_array2) - #where2 = self.lsn_line_intersection2d(line_array2) + # Setup lines from samples line_array = np.array([(np.array(p[0][0:2]), np.array(p[1][0:2])) for p in self.vec_samples]) + + # Calculate least squares intersection where = self.ls_line_intersection2d(line_array) + + # If no point could be determined, don't publish if where is None: return + + # Publish point ps = PointStamped() ps.header.frame_id = self.global_frame ps.header.stamp = rospy.Time.now() ps.point = numpy_to_point(where) self.position_pub.publish(ps) - def avg_line_intersection2d(self, lines_array): - xvals = np.array([]) - yvals = np.array([]) - - for line1 in lines_array: - for line2 in lines_array: - if np.array_equal(line1, line2): - continue - - boat_pos1 = line1[0] - boat_pos2 = line2[0] - - #print(boat_pos1) - - # https://stackoverflow.com/a/47823499 - t, s = np.linalg.solve(np.array([line1[1]-line1[0], line2[0]-line2[1]]).T, line2[0]-line1[0]) - intersection = (1-t)*line1[0] + t*line1[1] - - distance1 = np.linalg.norm(intersection - boat_pos1) - distance2 = np.linalg.norm(intersection - boat_pos2) - - #print(str(distance1) + ', ' + str(distance2)) - - if distance1 < self.distance_filter_dist or distance2 < self.distance_filter_dist: - #print('throw out') - continue # Throw out intersections that are too close to the boat - - #print('using ' + str(intersection[0])) - xvals = np.append(xvals, intersection[0]) - yvals = np.append(yvals, intersection[1]) - - #print(xvals) - average_intersection = np.array([np.mean(xvals), np.mean(yvals)]) - return average_intersection - - def lsn_line_intersection2d(self, line_array): + def ls_line_intersection2d(self, lines_array): """ Find the intersection of lines in the least-squares sense. - start - Nx3 numpy array of start points - end - Nx3 numpy array of end points - https://en.wikipedia.org/wiki/Line-line_intersection#In_two_dimensions_2 + Filter lobs such that no lob can have more than a number of intersections with other lines within a radius of the boat """ - def line_segment_norm(line_end_pts): - assert len(line_end_pts) == 4 - return np.linalg.norm(line_end_pts[2:] - line_end_pts[:2]) - begin_pts = line_array[:, :2] - diffs = line_array[:, 2:4] - begin_pts - norms = np.apply_along_axis(line_segment_norm, 1, line_array).reshape(diffs.shape[0], 1) - rot_left_90 = np.array([[0, -1], [1, 0]]) - perp_unit_vecs = np.apply_along_axis(lambda unit_diffs: rot_left_90.dot(unit_diffs), 1, diffs / norms) - A_sum = np.zeros((2, 2)) - Ap_sum = np.zeros((2, 1)) - - for x, y in zip(begin_pts, perp_unit_vecs): - begin = x.reshape(2, 1) - perp_vec = y.reshape(2, 1) - A = perp_vec.dot(perp_vec.T) - Ap = A.dot(begin) - A_sum += A - Ap_sum += Ap - - return np.linalg.inv(A_sum).dot(Ap_sum) - - def ls_line_intersection2d(self, lines_array): line_array = np.array([[0, 0, 0, 0]]) line_intersecting_array = np.array([]) @@ -165,43 +117,32 @@ class MultilaterationNode(object): boat_pos1 = line1[0] boat_pos2 = line2[0] - #print(boat_pos1 - + # Calculate the intersection of the two lines # https://stackoverflow.com/a/47823499 t, s = np.linalg.solve(np.array([line1[1]-line1[0], line2[0]-line2[1]]).T, line2[0]-line1[0]) intersection = (1-t)*line1[0] + t*line1[1] + # Calculate the distance to the boat's position from the intersection distance1 = np.linalg.norm(intersection - boat_pos1) distance2 = np.linalg.norm(intersection - boat_pos2) - #print(str(distance1) + ', ' + str(distance2)) + # If the distances are within the filter radius, increment the intersections count + if distance1 < self.filter_dist or distance2 < self.filter_dist: + line_intersections += 1 - if distance1 < self.distance_filter_dist or distance2 < self.distance_filter_dist: - line_intersections = line_intersections + 1 - #print('found an intersection') - #print('throwing out ' + str(line1) + ', ' + str(line2)) - #already_tossed_combinations = np.append(already_tossed_combinations, np.array([line1, line2])) - #break - - if line_intersections < 4: + if line_intersections < self.filter_dist: line_array = np.vstack([line_array, np.array([line1[0][0], line1[0][1], line1[1][0], line1[1][1]])]) - #print('used a line') - #else: - #print('threw out line') - line_array = line_array[1:] + line_array = line_array[1:] + # If we don't have at least 3 lines left, return no results if len(line_array) < 3: return None - """ - Find the intersection of lines in the least-squares sense. - start - Nx3 numpy array of start points - end - Nx3 numpy array of end points - https://en.wikipedia.org/wiki/Line-line_intersection#In_two_dimensions_2 - """ + # https://en.wikipedia.org/wiki/Line-line_intersection#In_two_dimensions_2 + def line_segment_norm(line_end_pts): assert len(line_end_pts) == 4 return np.linalg.norm(line_end_pts[2:] - line_end_pts[:2]) From 82ebc6cad68952fe6e1e4e7ae93274c893550d0a Mon Sep 17 00:00:00 2001 From: jaxon Date: Fri, 30 Nov 2018 00:05:52 -0500 Subject: [PATCH 4/4] Fix Kevin's comments - Generalized global frame - Bug with filter - Additional documentation - Additional feedback --- drivers/mil_passive_sonar/scripts/multilateration | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/drivers/mil_passive_sonar/scripts/multilateration b/drivers/mil_passive_sonar/scripts/multilateration index 9d909c80..5e68561b 100755 --- a/drivers/mil_passive_sonar/scripts/multilateration +++ b/drivers/mil_passive_sonar/scripts/multilateration @@ -32,7 +32,7 @@ class MultilaterationNode(object): self.vec_samples = deque(maxlen=buffer_len) self.enabled = False self.last_pos = None - self.global_frame = 'enu' + self.global_frame = rospy.get_param('~global_frame', default='enu') self.heading_sub = rospy.Subscriber('/hydrophones/direction', Vector3Stamped, self.heading_cb, queue_size=10) self.position_pub = rospy.Publisher('/hydrophones/position', @@ -90,6 +90,7 @@ class MultilaterationNode(object): # If no point could be determined, don't publish if where is None: + rospy.logwarn_throttle(10.0, 'Could not determine point; likely because too many lobs were filtered out due to intersections close to their origins') return # Publish point @@ -105,6 +106,8 @@ class MultilaterationNode(object): Filter lobs such that no lob can have more than a number of intersections with other lines within a radius of the boat """ + # Filter out lobs that intersect with many other lobs close to their origin + line_array = np.array([[0, 0, 0, 0]]) line_intersecting_array = np.array([]) @@ -130,7 +133,7 @@ class MultilaterationNode(object): if distance1 < self.filter_dist or distance2 < self.filter_dist: line_intersections += 1 - if line_intersections < self.filter_dist: + if line_intersections < self.filter_count: line_array = np.vstack([line_array, np.array([line1[0][0], line1[0][1], line1[1][0], line1[1][1]])])