From e005d1f2ec8577610b5d726771cea69e654baea8 Mon Sep 17 00:00:00 2001 From: Matthew Reynolds Date: Fri, 23 Nov 2018 22:01:19 -0500 Subject: [PATCH 01/17] Added utility module for common math calculations --- boat_utilities/CMakeLists.txt | 11 ++ boat_utilities/package.xml | 12 ++ boat_utilities/setup.py | 10 ++ boat_utilities/src/boat_utilities/__init__.py | 6 + boat_utilities/src/boat_utilities/angles.py | 105 ++++++++++++++++++ boat_utilities/src/boat_utilities/points.py | 20 ++++ 6 files changed, 164 insertions(+) create mode 100644 boat_utilities/CMakeLists.txt create mode 100644 boat_utilities/package.xml create mode 100644 boat_utilities/setup.py create mode 100644 boat_utilities/src/boat_utilities/__init__.py create mode 100644 boat_utilities/src/boat_utilities/angles.py create mode 100644 boat_utilities/src/boat_utilities/points.py diff --git a/boat_utilities/CMakeLists.txt b/boat_utilities/CMakeLists.txt new file mode 100644 index 0000000..dd5e8c7 --- /dev/null +++ b/boat_utilities/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 2.8.3) +project(boat_utilities) + +# Load catkin and package dependencies +find_package(catkin REQUIRED) + +# Enable Python module support +catkin_python_setup() + +# Declare a catkin package +catkin_package() diff --git a/boat_utilities/package.xml b/boat_utilities/package.xml new file mode 100644 index 0000000..adffa31 --- /dev/null +++ b/boat_utilities/package.xml @@ -0,0 +1,12 @@ + + + boat_utilities + 0.0.0 + The boat_utilities package + + Matt Reynolds + + TODO + + catkin + diff --git a/boat_utilities/setup.py b/boat_utilities/setup.py new file mode 100644 index 0000000..f731299 --- /dev/null +++ b/boat_utilities/setup.py @@ -0,0 +1,10 @@ +#!/usr/bin/env python +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +package_info = generate_distutils_setup( + packages=['boat_utilities'], + package_dir={'': 'src'}, +) + +setup(**package_info) diff --git a/boat_utilities/src/boat_utilities/__init__.py b/boat_utilities/src/boat_utilities/__init__.py new file mode 100644 index 0000000..3f007b7 --- /dev/null +++ b/boat_utilities/src/boat_utilities/__init__.py @@ -0,0 +1,6 @@ +#!/usr/bin/env python +import sys +sys.dont_write_bytecode = True + +import points +import angles diff --git a/boat_utilities/src/boat_utilities/angles.py b/boat_utilities/src/boat_utilities/angles.py new file mode 100644 index 0000000..8e4740c --- /dev/null +++ b/boat_utilities/src/boat_utilities/angles.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python + +# NOTE: All operations in this module use degrees CCW from East, as in a unit circle +# TODO: Switch to rads +__UNITS = 360.0 + + +def cosd(angle): + """Calculate the cosine of the following angle, in degrees.""" + from math import cos, radians + return cos(radians(angle)) + + +def sind(angle): + """Calculate the sine of the following angle, in degrees.""" + from math import sin, radians + return sin(radians(angle)) + + +def tand(angle): + """Calculate the tangent of the following angle, in degrees.""" + from math import tan, radians + return tan(radians(angle)) + + +def atan2d(y, x): + """Calculate the arctan of the following angle, in degrees.""" + from math import atan2, degrees + return degrees(atan2(y, x)) + + +def normalize(angle): + """Normalize the angle to be 0 to 360.""" + from math import fmod + return fmod(fmod(angle, __UNITS) + __UNITS, __UNITS) + + +def normalize_signed(angle): + """Normalize the angle to be -180 to 180.""" + angle = normalize(angle) + if angle > __UNITS/2: + angle -= __UNITS + return angle + + +def is_within_bounds(val, lower, upper): + """Determine whether lower < val < upper. + + Args: + val The value to check + lower The lower bound + upper The upper bound + Returns: + `True` if the value is between the specified bounds + """ + val = normalize(val) + lower = normalize(lower) + upper = normalize(upper) + + if val > upper: + val -= __UNITS + if lower > upper: + lower -= __UNITS + + return lower <= val and val <= upper + + +def is_on_right(angle, ref): + """Determine whether the specified angle is within the 180 degrees to the right of the reference. + + Args: + angle The angle to check + ref The reference angle to compare against + Returns: + `True` if `angle` is within 180 degrees to the right of `ref` + """ + angle = normalize(angle) + ref = normalize(ref) + ref_opp = normalize(ref + __UNITS/2) + + # If the range (ref, ref_opp) doesn't cross 0, we check if ref_opp < angle < ref + if ref > __UNITS/2: + return is_within_bounds(angle, ref, ref_opp) + else: + return not is_within_bounds(angle, ref, ref_opp) + + +def is_on_left(angle, ref): + """Determine whether the specified angle is within the 180 degrees to the left of the reference. + + Args: + angle The angle to check + ref The reference angle to compare against + Returns: + `True` if `angle` is within 180 degrees to the left of `ref` + """ + angle = normalize(angle) + ref = normalize(ref) + ref_opp = normalize(ref + __UNITS/2) + + # If the range (ref, ref_opp) doesn't cross 0, we check if !(ref_opp < angle < ref) + if ref > __UNITS/2: + return not is_within_bounds(angle, ref, ref_opp) + else: + return is_within_bounds(angle, ref, ref_opp) diff --git a/boat_utilities/src/boat_utilities/points.py b/boat_utilities/src/boat_utilities/points.py new file mode 100644 index 0000000..30dc5fe --- /dev/null +++ b/boat_utilities/src/boat_utilities/points.py @@ -0,0 +1,20 @@ +#!/usr/bin/env python + + +def dist(a, b): + """Determine the distance between between two points.""" + from math import sqrt + return sqrt((a.y - b.y)**2 + (a.x - b.x)**2) + + +def is_within_dist(a, b, tol): + """Determine if the dist between two points is within the specified tolerance. + + Args: + p1: The first point to compare + p2: The second point to compare + dist: The tolerance + Returns: + `True` if the points are within the tolerance + """ + return dist(a,b) < tol From 98fc5b3621f02140d2239d5b2281d4a8f556860a Mon Sep 17 00:00:00 2001 From: Matthew Reynolds Date: Sat, 24 Nov 2018 00:23:53 -0500 Subject: [PATCH 02/17] Updated boat_interfaces to use boat_utilities --- boat_interfaces/CMakeLists.txt | 3 ++- boat_interfaces/package.xml | 1 + boat_interfaces/src/pid.py | 17 +++---------- boat_interfaces/src/rudder_node.py | 9 +++---- boat_interfaces/src/tacking_action.py | 36 +++++---------------------- 5 files changed, 16 insertions(+), 50 deletions(-) diff --git a/boat_interfaces/CMakeLists.txt b/boat_interfaces/CMakeLists.txt index b72be41..5cc5ac8 100644 --- a/boat_interfaces/CMakeLists.txt +++ b/boat_interfaces/CMakeLists.txt @@ -8,11 +8,12 @@ find_package(catkin REQUIRED COMPONENTS actionlib std_msgs boat_msgs + boat_utilities ) # Declare a catkin package catkin_package( - CATKIN_DEPENDS rospy actionlib std_msgs boat_msgs + CATKIN_DEPENDS rospy actionlib std_msgs boat_msgs boat_utilities ) # Mark executable scripts (ie. Those with a __main__) for installation diff --git a/boat_interfaces/package.xml b/boat_interfaces/package.xml index be5e501..6e7a086 100644 --- a/boat_interfaces/package.xml +++ b/boat_interfaces/package.xml @@ -15,6 +15,7 @@ actionlib std_msgs boat_msgs + boat_utilities rosserial_python diff --git a/boat_interfaces/src/pid.py b/boat_interfaces/src/pid.py index 06640c0..5f38552 100755 --- a/boat_interfaces/src/pid.py +++ b/boat_interfaces/src/pid.py @@ -1,6 +1,7 @@ #!/usr/bin/env python import rospy from std_msgs.msg import Bool, Float32 +from boat_utilities import angles pid_is_enabled = False kp = 1.0 @@ -8,26 +9,16 @@ ki = 0.0 output_pub = rospy.Publisher('output', Float32, queue_size=10) setpoint = 0 -pid_input = 0 # TODO: Modify this to make the PID node generalizable and usable for multiple controllers max_range = (rospy.get_param('/boat/interfaces/rudder_max') - rospy.get_param('/boat/interfaces/rudder_min')) / 2.0 # If the boat state topic changes, update local boat state -def input_callback(pid): - global pid_input - global setpoint - global kp - global max_range - global pid_is_enabled +def input_callback(input): + if pid_is_enabled: - pid_input = pid.data - e = setpoint - pid_input - if e > 180: - e -= 360 - elif e < -180: - e += 360 + e = angles.normalize_signed(setpoint - input.data) prop= kp * e # TODO: Add integral and derivative integral = 0 diff --git a/boat_interfaces/src/rudder_node.py b/boat_interfaces/src/rudder_node.py index 2b3f12c..a50c1ab 100755 --- a/boat_interfaces/src/rudder_node.py +++ b/boat_interfaces/src/rudder_node.py @@ -2,6 +2,7 @@ import rospy from boat_msgs.msg import BoatState, GPS, Joy from std_msgs.msg import Bool, Float32 +from boat_utilities import angles # Declare global variables needed for the node @@ -59,7 +60,7 @@ def compass_callback(compass): wind_heading = (ane_reading + compass.data) % 360 cur_boat_heading = compass.data - heading_msg = Float32(conform_angle(cur_boat_heading)) + heading_msg = Float32(angles.normalize_signed(cur_boat_heading)) pid_input_pub.publish(heading_msg) # Don't loginfo here, this would be somewhat redundant as this callback just parses and republishes the same information @@ -93,7 +94,7 @@ def target_heading_callback(target_heading): # We have a new valid setpoint, therefore output it #rospy.loginfo(rospy.get_caller_id() + " New rudder setpoint: %f", target_heading.data) - pid_setpoint = Float32(conform_angle(target_heading.data)) + pid_setpoint = Float32(angles.normalize_signed(target_heading.data)) pid_setpoint_pub.publish(pid_setpoint) @@ -138,10 +139,6 @@ def joy_callback(controller): rospy.loginfo(rospy.get_caller_id() + " Read value: %f", controller.right_stick_x) rudder_pos = position_msg.data -# Conform an input angle to range (-180, 180) -def conform_angle(val): - return (val + 180) % 360 - 180 - def listener(): # Setup subscribers rospy.init_node('rudder_node') diff --git a/boat_interfaces/src/tacking_action.py b/boat_interfaces/src/tacking_action.py index d3f4979..24d689e 100755 --- a/boat_interfaces/src/tacking_action.py +++ b/boat_interfaces/src/tacking_action.py @@ -3,6 +3,7 @@ from actionlib import SimpleActionServer from boat_msgs.msg import TackingAction as TackingActionMsg, TackingFeedback, TackingResult, BoatState from std_msgs.msg import Bool, Float32 +from boat_utilities import angles class TackingAction(object): # create messages that are used to publish feedback/result @@ -46,34 +47,6 @@ def compass_callback(self, heading): def boat_state_callback(self, state): self.state = state - - def gtAngle(self, angle1, angle2): - comp_angle = (angle2 + 180) % 360 - if angle2 >= 180: - return not self.is_within_bounds(angle1, angle2, comp_angle) - else: - return self.is_within_bounds(angle1, angle2, comp_angle) - - def ltAngle(self, angle1, angle2): - comp_angle = (angle2 + 180) % 360 - if angle2 >= 180: - return self.is_within_bounds(angle1, angle2, comp_angle) - else: - return not self.is_within_bounds(angle1, angle2, comp_angle) - - def is_within_bounds(self, val, boundA, boundB): - if boundA < boundB: - val -= boundA - boundB -= boundA - boundA = 0 - else: - val -= boundB - boundA -= boundB - boundB = 0 - if val < 0: - val += 360 - val = val % 360 - return (boundA <= val and val <= boundB) or (boundB <= val and val <= boundA) def tacking_callback(self, goal): # helper variables @@ -95,9 +68,11 @@ def tacking_callback(self, goal): while not success and not preempted: # start executing the action + + # From left to right of wind: if goal.direction == 1: if self.ane_reading < (180 + self.layline): - if self.ltAngle(self.cur_boat_heading, self.init_target): + if angles.is_on_right(self.cur_boat_heading, self.init_target): # Nothing relevant to publish it for other than debugging and the simulator self.target_pub.publish(Float32(self.cur_boat_heading)) if not self.rudder_pos == self.rudder_min: @@ -112,9 +87,10 @@ def tacking_callback(self, goal): print "Tacking success: ", goal.direction, self.ane_reading success = True + # From right to left of wind: elif goal.direction == -1: if self.ane_reading > (180 - self.layline): - if self.gtAngle(self.cur_boat_heading,self.init_target): + if angles.is_on_left(self.cur_boat_heading, self.init_target): # Nothing relevant to publish it for other than debugging and the simulator self.target_pub.publish(Float32(self.cur_boat_heading)) if not self.rudder_pos == self.rudder_max: From af914f3ec9839f1c5c43a8c2b46d9aaaa368a6a9 Mon Sep 17 00:00:00 2001 From: Matthew Reynolds Date: Sat, 24 Nov 2018 01:56:11 -0500 Subject: [PATCH 03/17] Fixed bug in utility module --- boat_utilities/src/boat_utilities/angles.py | 14 ++++---------- 1 file changed, 4 insertions(+), 10 deletions(-) diff --git a/boat_utilities/src/boat_utilities/angles.py b/boat_utilities/src/boat_utilities/angles.py index 8e4740c..e9b51c6 100644 --- a/boat_utilities/src/boat_utilities/angles.py +++ b/boat_utilities/src/boat_utilities/angles.py @@ -78,11 +78,8 @@ def is_on_right(angle, ref): ref = normalize(ref) ref_opp = normalize(ref + __UNITS/2) - # If the range (ref, ref_opp) doesn't cross 0, we check if ref_opp < angle < ref - if ref > __UNITS/2: - return is_within_bounds(angle, ref, ref_opp) - else: - return not is_within_bounds(angle, ref, ref_opp) + # We check if ref_opp < angle < ref + return is_within_bounds(angle, ref_opp, ref) def is_on_left(angle, ref): @@ -98,8 +95,5 @@ def is_on_left(angle, ref): ref = normalize(ref) ref_opp = normalize(ref + __UNITS/2) - # If the range (ref, ref_opp) doesn't cross 0, we check if !(ref_opp < angle < ref) - if ref > __UNITS/2: - return not is_within_bounds(angle, ref, ref_opp) - else: - return is_within_bounds(angle, ref, ref_opp) + # We check if ref < angle < ref_opp + return is_within_bounds(angle, ref, ref_opp) From 979d8d5fed90971f9da9be0d887235f83a6bf526 Mon Sep 17 00:00:00 2001 From: Matthew Reynolds Date: Sat, 24 Nov 2018 02:00:49 -0500 Subject: [PATCH 04/17] Updated boat_nav to use boat_utilities --- boat_nav/CMakeLists.txt | 3 +- boat_nav/package.xml | 1 + boat_nav/src/layline_action.py | 75 +++------ boat_nav/src/max_vmg_action.py | 6 +- boat_nav/src/navigator_node.py | 182 ++++++++------------- boat_nav/src/path_planners/planner_base.py | 21 +-- boat_nav/src/path_planners/station.py | 3 +- boat_nav/src/sensor_parser_node.py | 13 +- 8 files changed, 107 insertions(+), 197 deletions(-) diff --git a/boat_nav/CMakeLists.txt b/boat_nav/CMakeLists.txt index a473cc0..c91f813 100644 --- a/boat_nav/CMakeLists.txt +++ b/boat_nav/CMakeLists.txt @@ -8,12 +8,13 @@ find_package(catkin REQUIRED COMPONENTS actionlib std_msgs boat_msgs + boat_utilities tf ) # Declare a catkin package catkin_package( - CATKIN_DEPENDS rospy actionlib std_msgs boat_msgs tf + CATKIN_DEPENDS rospy actionlib std_msgs boat_msgs boat_utilities tf ) # Mark executable scripts (ie. Those with a __main__) for installation diff --git a/boat_nav/package.xml b/boat_nav/package.xml index 2499004..1ec3ec1 100644 --- a/boat_nav/package.xml +++ b/boat_nav/package.xml @@ -15,6 +15,7 @@ actionlib std_msgs boat_msgs + boat_utilities tf diff --git a/boat_nav/src/layline_action.py b/boat_nav/src/layline_action.py index 21db4ea..4c651c4 100755 --- a/boat_nav/src/layline_action.py +++ b/boat_nav/src/layline_action.py @@ -5,6 +5,7 @@ from boat_msgs.msg import LaylineAction as LaylineActionMsg, LaylineFeedback, LaylineResult, BoatState, Point, Waypoint, TackingAction, TackingGoal, GPS from std_msgs.msg import Float32 from actionlib_msgs.msg import GoalStatus +from boat_utilities import angles class LaylineAction(object): # create messages that are used to publish feedback/result @@ -49,43 +50,19 @@ def target_callback(self, new_target): def gps_callback(self, gps): self.boat_speed = gps.speed * 0.514444 # Knots to m/s - def is_within_bounds(self, val, boundA, boundB): - if boundA < boundB: - val -= boundA - boundB -= boundA - boundA = 0 - else: - val -= boundB - boundA -= boundB - boundB = 0 - if val < 0: - val += 360 - val = val % 360 - return (boundA <= val and val <= boundB) or (boundB <= val and val <= boundA) - - def gtAngle(self, angle1, angle2): - comp_angle = (angle2 + 180) % 360 - if angle2 >= 180: - return not self.is_within_bounds(angle1, angle2, comp_angle) - else: - return self.is_within_bounds(angle1, angle2, comp_angle) - - def ltAngle(self, angle1, angle2): - comp_angle = (angle2 + 180) % 360 - if angle2 >= 180: - return self.is_within_bounds(angle1, angle2, comp_angle) - else: - return not self.is_within_bounds(angle1, angle2, comp_angle) - def anemometer_callback(self, new_heading): self.ane_reading = new_heading.data - self.apparent_wind_heading = (self.ane_reading + self.compass) % 360 - self.wind_coming = (self.apparent_wind_heading + 180) % 360 + self.update_apparent_wind() + def compass_callback(self, compass): self.compass = compass.data - self.apparent_wind_heading = (self.ane_reading + self.compass) % 360 - self.wind_coming = (self.apparent_wind_heading + 180) % 360 + self.update_apparent_wind() + + def update_apparent_wind(self): + self.apparent_wind_heading = angles.normalize(self.ane_reading + self.compass) + self.wind_coming = angles.normalize(self.apparent_wind_heading + 180) + def layline_callback(self, goal): # helper variables @@ -102,7 +79,7 @@ def layline_callback(self, goal): pos = self.cur_pos tar = goal.target.pt - a = math.tan(math.radians(self.compass)) + a = angles.tand(self.compass) b = -1 c = tar.y - a*tar.x dx = pos.x-tar.x @@ -111,28 +88,25 @@ def layline_callback(self, goal): d_perp = abs(a*pos.x + b*pos.y + c)/math.sqrt(a*a+b*b) d_par = math.sqrt(dx*dx+dy*dy-d_perp*d_perp) - if self.gtAngle(goal.alt_tack_angle, self.wind_coming): + if angles.is_on_left(goal.alt_tack_angle, self.wind_coming): tacking_direction = 1 else: tacking_direction = -1 self._feedback.status = " Tacking away from mark to hit layline. " rospy.loginfo(rospy.get_caller_id() + self._feedback.status) - new_target = self.wind_coming - tacking_direction * self.layline - print new_target, self.wind_coming, tacking_direction - if new_target < 0: - new_target += 360 - new_target = new_target % 360 + new_target = angles.normalize(self.wind_coming - tacking_direction * self.layline) self.target_heading = new_target self.target_pub.publish(Float32(self.target_heading)) tacking_goal = TackingGoal(direction = tacking_direction) self.tacking_client.send_goal(tacking_goal) - + endtime = rospy.Time.now() + rospy.Duration(10) + # TODO: Sometimes this loop abruptly exits for no reason while (self.tacking_client.get_state() is GoalStatus.ACTIVE or\ self.tacking_client.get_state() is GoalStatus.PENDING) and rospy.Time.now() < endtime and not did_hit_midpoint: - + pos = self.cur_pos cur_d_perp = abs(a*pos.x + b*pos.y + c)/math.sqrt(a*a+b*b) if cur_d_perp < d_perp*0.48: @@ -184,15 +158,14 @@ def layline_callback(self, goal): # Wait until we hit the layline heading while (not hit_layline or self.boat_speed < self.min_speed) and not preempted: - direct_heading = math.atan2(goal.target.pt.y - self.cur_pos.y, goal.target.pt.x - self.cur_pos.x) * 180 / math.pi - direct_heading = (direct_heading + 360) % 360 - if goal.alt_tack_angle - goal.overshoot_angle < 0: - lower_bound = goal.alt_tack_angle - goal.overshoot_angle + 360 - else: - lower_bound = goal.alt_tack_angle - goal.overshoot_angle - upper_bound = (goal.alt_tack_angle + goal.overshoot_angle) % 360 - if (tacking_direction is 1 and self.gtAngle(direct_heading, upper_bound)) or\ - (tacking_direction is -1 and self.ltAngle(direct_heading, lower_bound)): + direct_heading = angles.atan2d(goal.target.pt.y - self.cur_pos.y, goal.target.pt.x - self.cur_pos.x) + direct_heading = angles.normalize(direct_heading) + + lower_bound = angles.normalize(goal.alt_tack_angle - goal.overshoot_angle) + upper_bound = angles.normalize(goal.alt_tack_angle + goal.overshoot_angle) + + if (tacking_direction is 1 and angles.is_on_left(direct_heading, upper_bound)) or\ + (tacking_direction is -1 and angles.is_on_right(direct_heading, lower_bound)): hit_layline = True if self._as.is_preempt_requested() or self.new_target: @@ -208,7 +181,7 @@ def layline_callback(self, goal): if preempted: return - self._feedback.status = " Tacking towards mark after hitting layline. " + self._feedback.status = " Hit layline. Tacking towards mark" rospy.loginfo(rospy.get_caller_id() + self._feedback.status) # Reverse tacking direction tacking_goal.direction = tacking_direction * -1 diff --git a/boat_nav/src/max_vmg_action.py b/boat_nav/src/max_vmg_action.py index 9cc38ee..28707e7 100755 --- a/boat_nav/src/max_vmg_action.py +++ b/boat_nav/src/max_vmg_action.py @@ -1,9 +1,9 @@ #!/usr/bin/env python import rospy -import math from actionlib import SimpleActionServer from boat_msgs.msg import MaxVMGAction as MaxVMGActionMsg, MaxVMGFeedback, MaxVMGResult, GPS, Point from std_msgs.msg import Bool, Float32 +from boat_utilities import angles class MaxVMGAction(object): @@ -50,12 +50,12 @@ def anemometer_callback(self, anemometer): self.ane_reading = anemometer.data def vmg(self): - return math.cos(self.heading - self.direct_heading()) * self.speed + return angles.cosd(self.heading - self.direct_heading()) * self.speed def direct_heading(self): orig = self.pos dest = self.goal.target - return math.degrees(math.atan2(dest.y-orig.y, dest.x-orig.x)) + return angles.atan2d(dest.y-orig.y, dest.x-orig.x) def execute(self, goal): self.goal = goal diff --git a/boat_nav/src/navigator_node.py b/boat_nav/src/navigator_node.py index cd448ef..5afee93 100755 --- a/boat_nav/src/navigator_node.py +++ b/boat_nav/src/navigator_node.py @@ -5,6 +5,7 @@ from boat_msgs.msg import BoatState, GPS, MaxVMGAction, MaxVMGGoal, Point, Waypoint, TackingAction, TackingGoal, LaylineAction, LaylineGoal from boat_msgs.srv import ConvertPoint from std_msgs.msg import Float32 +from boat_utilities import points, angles ## Trigger for when the wind shifts significantly new_wind = False @@ -89,7 +90,7 @@ def anemometer_callback(new_heading): global apparent_wind_heading global new_wind ane_reading = new_heading.data - new_wind_heading = (ane_reading + cur_boat_heading) % 360 + new_wind_heading = angles.normalize(ane_reading + cur_boat_heading) # Tolerance on a wind shift to be determined # Only update wind heading if a significant shift is detected, because it will then replan our upwind path @@ -118,7 +119,7 @@ def compass_callback(compass): global target_heading cur_boat_heading = compass.data - new_wind_heading = (ane_reading + cur_boat_heading) % 360 + new_wind_heading = angles.normalize(ane_reading + cur_boat_heading) # Tolerance on a wind shift to be determined # Only update wind heading if a significant shift is detected, because it will then replan our upwind path @@ -188,27 +189,24 @@ def vmg(direct_heading): def calc_global_max_vmg(wind_coming): # TODO: Make a service and make more general for object avoidance - upper_bound = wind_coming + layline - lower_bound = wind_coming - layline + upper_bound = angles.normalize(wind_coming + layline) + lower_bound = angles.normalize(wind_coming - layline) + # If direct heading is in irons, then max_vmg will be on the edge of the no go zone - if is_within_bounds(direct_heading, upper_bound, lower_bound): - theoretic_boat_speed = 0 + if angles.is_within_bounds(direct_heading, lower_bound, upper_bound): - if gtAngle(direct_heading, wind_coming): - vmg_heading = (wind_coming + layline) % 360 - theoretic_boat_speed = 2.5 * 0.514444 + # Snap to whichever edge of the no go zone is closer + if angles.is_on_left(direct_heading, wind_coming): + vmg_heading = upper_bound else: - vmg_heading = wind_coming - layline - if vmg_heading < 0: - vmg_heading += 360 - theoretic_boat_speed = 2.5 * 0.514444 + vmg_heading = lower_bound # Otherwise max vmg is just the direct heading else: - theoretic_boat_speed = 2.5 * 0.514444 # 2.5 Knots to m/s (measured boat speed) vmg_heading = direct_heading - max_vmg = theoretic_boat_speed * math.cos(math.radians(vmg_heading - direct_heading)) + theoretic_boat_speed = 2.5 * 0.514444 # 2.5 Knots to m/s (measured boat speed) + max_vmg = theoretic_boat_speed * angles.cosd(vmg_heading - direct_heading) return max_vmg, vmg_heading ## Calculate the maximum velocity made good on the current tack @@ -219,33 +217,32 @@ def calc_global_max_vmg(wind_coming): def calc_cur_tack_max_vmg(wind_coming): # TODO: Make a service and make more general for object avoidance - upper_bound = wind_coming + layline - lower_bound = wind_coming - layline - # Direct heading in irons on left of wind - if is_within_bounds(direct_heading, upper_bound, lower_bound): - theoretic_boat_speed = 0 + upper_bound = angles.normalize(wind_coming + layline) + lower_bound = angles.normalize(wind_coming - layline) - if gtAngle(target_heading, wind_coming): - vmg_heading = (wind_coming + layline) % 360 - theoretic_boat_speed = 2.5 * 0.514444 - else : - vmg_heading = wind_coming - layline - if vmg_heading < 0: - vmg_heading += 360 - theoretic_boat_speed = 2.5 * 0.514444 - - # Direct heading lies on navigatable path elsewhere in our current tack - elif (ltAngle(direct_heading, wind_coming) and ltAngle(target_heading, wind_coming)) or\ - (gtAngle(direct_heading, wind_coming) and gtAngle(target_heading, wind_coming)): - theoretic_boat_speed = 2.5 * 0.514444 # 2.5 Knots to m/s (measured boat speed) + # If direct heading is in irons, then max_vmg will be on the edge of the no go zone + if angles.is_within_bounds(direct_heading, lower_bound, upper_bound): + theoretic_boat_speed = 2.5 * 0.514444 # 2.5 Knots to m/s + + # Snap to whichever edge of the no go zone is closer + if angles.is_on_left(target_heading, wind_coming): + vmg_heading = upper_bound + else: + vmg_heading = lower_bound + + # If the direct heading is somewhere in our current tack (direct_heading and target_heading on same side of wind) + elif (angles.is_on_left(direct_heading, wind_coming) and angles.is_on_left(target_heading, wind_coming)) or\ + (angles.is_on_right(direct_heading, wind_coming) and angles.is_on_right(target_heading, wind_coming)): + theoretic_boat_speed = 2.5 * 0.514444 # 2.5 Knots to m/s vmg_heading = direct_heading - # If none of the above, the best heading will be the one closest to the other tack, however it is extremely unfavourable because clearly no best heading lies on this tack + # If none of the above, the best heading will be on the opposite tack + # however it is extremely unfavorable because clearly no best heading lies on this tack else: theoretic_boat_speed = 0 vmg_heading = apparent_wind_heading - max_vmg = theoretic_boat_speed * math.cos(math.radians(vmg_heading - direct_heading)) + max_vmg = theoretic_boat_speed * angles.cosd(vmg_heading - direct_heading) return max_vmg, vmg_heading ## Calculate the current velocity made good along the current target heading @@ -254,16 +251,17 @@ def calc_cur_tack_max_vmg(wind_coming): # @return The velocity made good # def calc_vmg(wind_coming): - upper_bound = wind_coming + layline - lower_bound = wind_coming - layline + tolerance = 1.0 + upper_bound = angles.normalize(wind_coming + layline - tolerance) + lower_bound = angles.normalize(wind_coming - layline + tolerance) - # Add a buffer for boat sailing on the layline - if is_within_bounds(target_heading, upper_bound - 1.0, lower_bound + 1.0): + # If we are in irons, our theoretical speed is 0 + if angles.is_within_bounds(target_heading, lower_bound, upper_bound): theoretic_boat_speed = 0 else: theoretic_boat_speed = 2.5 * 0.514444 # 2.5 Knots to m/s (measured boat speed) - cur_vmg = theoretic_boat_speed * math.cos(math.radians(target_heading - direct_heading)) + cur_vmg = theoretic_boat_speed * angles.cosd(target_heading - direct_heading) return cur_vmg ## Calculate the distance from the boat to the current target @@ -272,68 +270,20 @@ def calc_vmg(wind_coming): # @return The distance, in meters # def dist_to_target(position): - return math.sqrt(math.pow((target.pt.y - position.y), 2) + math.pow((target.pt.x - position.x), 2)) - -## Determine if the dist between two points is within the specified tolerance -# -# @param p1 The first `boat_msgs.msg.Point` -# @param p2 The second `boat_msgs.msg.Point` -# @param dist The tolerance distance, in meters -# @return `True` if the points are within the tolerance -# -def is_within_dist(p1, p2, dist): - a = math.pow(p1.x-p2.x, 2) + math.pow(p1.y - p2.y, 2) - return math.sqrt(a) < dist - -def gtAngle(angle1, angle2): - comp_angle = (angle2 + 180) % 360 - if angle2 >= 180: - return not is_within_bounds(angle1, angle2, comp_angle) - else: - return is_within_bounds(angle1, angle2, comp_angle) - -def ltAngle(angle1, angle2): - comp_angle = (angle2 + 180) % 360 - if angle2 >= 180: - return is_within_bounds(angle1, angle2, comp_angle) - else: - return not is_within_bounds(angle1, angle2, comp_angle) - -## Determine whether the specified value is between `boundA` and `boundB`. -# -# Note that the order of `boundA` and `boundB` do not matter, either can be the upper or lower bound -# -# @param val The value to check -# @param boundA The first of the two bounds (Either lower or upper) -# @param boundB The second of the two bounds (Either lower or upper) -# @return `True` if the value is between the specified bounds -# -def is_within_bounds(val, boundA, boundB): - if boundA < boundB: - val -= boundA - boundB -= boundA - boundA = 0 - else: - val -= boundB - boundA -= boundB - boundB = 0 - if val < 0: - val += 360 - val = val % 360 - return (boundA <= val and val <= boundB) or (boundB <= val and val <= boundA) + return points.dist(target.pt, position) ## Determine the percentage of the course to the target is remaining # # @return Percentage of course remaining # def remaining_course(): - cur_angle = math.atan2(cur_pos.y - start_pos.y, cur_pos.x - start_pos.x) * 180 / math.pi - start_angle = math.atan2(target.pt.y - start_pos.y, target.pt.x - start_pos.x) * 180 / math.pi - cur_angle = (cur_angle + 360) % 360 - start_angle = (start_angle + 360) % 360 + cur_angle = angles.atan2d(cur_pos.y - start_pos.y, cur_pos.x - start_pos.x) + start_angle = angles.atan2d(target.pt.y - start_pos.y, target.pt.x - start_pos.x) + cur_angle = angles.normalize(cur_angle) + start_angle = angles.normalize(start_angle) tot_dist = math.hypot(target.pt.y - start_pos.y, target.pt.x - start_pos.x) cur_dist = math.hypot(cur_pos.y - start_pos.y, cur_pos.x - start_pos.x) - proj_dist = cur_dist * math.cos(math.radians(cur_angle-start_angle)) + proj_dist = cur_dist * angles.cosd(cur_angle-start_angle) return 100 - (proj_dist/tot_dist) * 100.0 ## Determine if the boat is on the layline and can make the mark on the current heading @@ -343,16 +293,16 @@ def remaining_course(): # @return True for can make it, and false for not # def on_layline(wind_coming, tolerance): - # On left side of the wind - if gtAngle(direct_heading, wind_coming): - val = ltAngle(target_heading, (direct_heading + tolerance) % 360) + + # If waypoint is on left side of the wind, we are on the layline iff the target heading + # is to the right of the direct heading + if angles.is_on_left(direct_heading, wind_coming): + val = angles.is_on_right(target_heading, direct_heading + tolerance) - # On right side of the wind + # If waypoint is on right side of the wind, we are on the layline iff the target heading + # is to the left of the direct heading else: - if direct_heading - tolerance < 0: - val = gtAngle(target_heading, direct_heading - tolerance + 360) - else: - val = gtAngle(target_heading, direct_heading - tolerance) + val = angles.is_on_left(target_heading, direct_heading - tolerance) return val @@ -369,9 +319,9 @@ def awa_algorithm(): # Calculate the direct heading to the next waypoint old_direct_heading = direct_heading - direct_heading = math.atan2(target.pt.y - cur_pos.y, target.pt.x - cur_pos.x) * 180 / math.pi - direct_heading = (direct_heading + 360) % 360 # Get rid of negative angles - wind_coming = (apparent_wind_heading + 180) % 360 # Determine the direction the wind is coming from + direct_heading = angles.atan2d(target.pt.y - cur_pos.y, target.pt.x - cur_pos.x) + direct_heading = angles.normalize(direct_heading) + wind_coming = angles.normalize(apparent_wind_heading + 180) # Determine the direction the wind is coming from # TODO: Make n a function of boat speed to negate the effects of apparent wind? n = 1 + p*1.3/dist_to_target(start_pos) # Tacking weight, can add app_wind_offset here to make even less desirable @@ -393,14 +343,14 @@ def awa_algorithm(): # If not currently at our optimal vmg, during our regular upwind routine (not layline setup) if (global_max_vmg > cur_vmg or cur_tack_max_vmg > cur_vmg): # Is tack required to get to vmg_heading - if (gtAngle(global_vmg_heading, target_heading) and gtAngle(wind_coming, target_heading) and ltAngle(wind_coming, global_vmg_heading)) or\ - (ltAngle(global_vmg_heading, target_heading) and gtAngle(wind_coming, global_vmg_heading) and ltAngle(wind_coming, target_heading)): + if (angles.is_on_left(global_vmg_heading, target_heading) and angles.is_on_left(wind_coming, target_heading) and angles.is_on_right(wind_coming, global_vmg_heading)) or\ + (angles.is_on_right(global_vmg_heading, target_heading) and angles.is_on_left(wind_coming, global_vmg_heading) and angles.is_on_right(wind_coming, target_heading)): # If this loop is entered, then getting to vmg_heading requires a tack # Now we need to calculate if the tack is worth it if global_max_vmg > cur_tack_max_vmg * n and boat_speed >= min_tacking_speed: # Worth the tack, therefore determine the tacking direction and execute the action target_heading = global_vmg_heading - if gtAngle(target_heading, wind_coming): + if angles.is_on_left(target_heading, wind_coming): tacking_direction = -1 else: tacking_direction = 1 @@ -460,9 +410,9 @@ def taras_algorithm(): # Calculate the direct heading to the next waypoint # This should never be undefined, as the atan2(0,0) case would already be caught by the proximity check above - best_heading = math.atan2(target.pt.y - cur_pos.y, target.pt.x - cur_pos.x) * 180 / math.pi - best_heading = (best_heading + 360) % 360 # Get rid of negative angles - wind_coming = (apparent_wind_heading + 180) % 360 # Determine the direction the wind is coming from + best_heading = angles.atan2d(target.pt.y - cur_pos.y, target.pt.x - cur_pos.x) + best_heading = angles.normalize(best_heading) + wind_coming = angles.normalize(apparent_wind_heading + 180) # Determine the direction the wind is coming from # If the direct path isn't possible... if best_heading > wind_coming-layline and best_heading < wind_coming+layline: @@ -497,12 +447,14 @@ def taras_algorithm(): else: boat_dir = -1 - wind_coming = (apparent_wind_heading + 180) % 360 # Which direction the wind is coming from + wind_coming = angles.normalize(apparent_wind_heading + 180) # Which direction the wind is coming from - if (boat_dir is 1 and not is_within_bounds(wind_coming, cur_boat_heading, target_heading)) or\ - (boat_dir is -1 and is_within_bounds(wind_coming, cur_boat_heading, target_heading)): + # TODO: This may be broken now that is_within_bounds is not bi-directional + if (boat_dir is 1 and not angles.is_within_bounds(wind_coming, cur_boat_heading, target_heading)) or\ + (boat_dir is -1 and angles.is_within_bounds(wind_coming, cur_boat_heading, target_heading)): + # Determine which direction to tack based on the side that our goal is on - if is_within_bounds(target_heading, 90, 270): + if angles.is_within_bounds(target_heading, 90, 270): tacking_direction = -1 else: tacking_direction = 1 diff --git a/boat_nav/src/path_planners/planner_base.py b/boat_nav/src/path_planners/planner_base.py index 29cafb4..280c956 100644 --- a/boat_nav/src/path_planners/planner_base.py +++ b/boat_nav/src/path_planners/planner_base.py @@ -7,6 +7,8 @@ from boat_msgs.srv import ConvertPoint from std_msgs.msg import Float32 +from boat_utilities import angles, points + BUOY_TOL = rospy.get_param('/boat/planner/buoy_tol') ROUND_DIST = rospy.get_param('/boat/planner/round_dist') ROUND_FACTOR = rospy.get_param('/boat/planner/round_factor') @@ -181,8 +183,8 @@ def publish_target(*argv): @staticmethod def _calc_wind_coming(): """Calculate the angle of the wind.""" - Planner.new_wind_heading = (Planner.ane_reading + Planner.cur_boat_heading) % 360 - Planner.wind_coming = (Planner.new_wind_heading + 180) % 360 + Planner.new_wind_heading = angles.normalize(Planner.ane_reading + Planner.cur_boat_heading) + Planner.wind_coming = angles.normalize(Planner.new_wind_heading + 180) def boat_reached_target(self): @@ -190,20 +192,7 @@ def boat_reached_target(self): @return True if the boat is within the tolerance """ - return self.is_within_dist(self.cur_pos, Services.to_lps(self.target_waypoint), BUOY_TOL) - - - @staticmethod - def is_within_dist(p1, p2, dist): - """Determine if the dist between two points is within the specified tolerance - - @param p1: The first point to compare - @param p2: The second point to compare - @param dist: The tolerance, in that same units as the points (meters, etc) - @return True if the points are within the tolerance - """ - a = math.pow(p1.x - p2.x, 2) + math.pow(p1.y - p2.y, 2) - return math.sqrt(a) < dist + return points.is_within_dist(self.cur_pos, Services.to_lps(self.target_waypoint), BUOY_TOL) # =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= Services =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= diff --git a/boat_nav/src/path_planners/station.py b/boat_nav/src/path_planners/station.py index c96239c..2215d19 100644 --- a/boat_nav/src/path_planners/station.py +++ b/boat_nav/src/path_planners/station.py @@ -4,6 +4,7 @@ from overrides import overrides from boat_msgs.msg import BoatState, Point, PointArray, Waypoint from path_planners import Planner, Services +from boat_utilities import points HEIGHT_TO_TRAVEL = rospy.get_param('/boat/planner/station/height') MAX_WIDTH = rospy.get_param('/boat/planner/station/width') @@ -134,7 +135,7 @@ def planner(self): # If we've reached the set waypoint, flip around if self.boat_reached_target(): - if self.is_within_dist(self.target_waypoint.pt, self.start_station, 0.0001): + if points.is_within_dist(self.target_waypoint.pt, self.start_station, 0.0001): self.publish_target(Waypoint(self.end_station, Waypoint.TYPE_INTERSECT)) else: self.publish_target(Waypoint(self.start_station, Waypoint.TYPE_INTERSECT)) diff --git a/boat_nav/src/sensor_parser_node.py b/boat_nav/src/sensor_parser_node.py index 3b857b3..5b57f7e 100755 --- a/boat_nav/src/sensor_parser_node.py +++ b/boat_nav/src/sensor_parser_node.py @@ -6,6 +6,7 @@ from std_msgs.msg import Float32 from sensor_msgs.msg import Imu, NavSatFix, NavSatStatus from tf.transformations import euler_from_quaternion +from boat_utilities.angles import cosd, sind # Declare global variables needed for the node origin_lps = Point() @@ -105,7 +106,7 @@ def to_lps(gps): # Convert from gps to lps def to_gps(local): gps = Point() - + gps.y = math.degrees((local.y + origin_lps.y)/RADIUS) gps.x = math.degrees((local.x + origin_lps.x)/(RADIUS * cosd(gps.y))) return gps @@ -118,14 +119,6 @@ def get_coords(gps): coords.y = gps.latitude return coords - -def cosd(angle): - return math.cos(math.radians(angle)) - -def sind(angle): - return math.sin(math.radians(angle)) - - # Initialize the node def listener(): global origin_lps @@ -135,7 +128,7 @@ def listener(): # Setup first so that simulator can send the origin point srv1 = rospy.Service('lps_to_gps', ConvertPoint, lps_to_gps_srv) - + # setup the origin origin_coords = rospy.wait_for_message('gps_raw', GPS) origin_lps = to_lps(get_coords(origin_coords)) From 618af35863b0892fac6c5c9a622d2082e2d606a1 Mon Sep 17 00:00:00 2001 From: Matthew Reynolds Date: Sat, 24 Nov 2018 02:01:21 -0500 Subject: [PATCH 05/17] Improve documentation in tacking action --- boat_interfaces/src/tacking_action.py | 14 ++++++++------ boat_msgs/action/Tacking.action | 3 ++- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/boat_interfaces/src/tacking_action.py b/boat_interfaces/src/tacking_action.py index 24d689e..3340510 100755 --- a/boat_interfaces/src/tacking_action.py +++ b/boat_interfaces/src/tacking_action.py @@ -56,7 +56,10 @@ def tacking_callback(self, goal): self._feedback.status = "Entered Action Callback" # publish info to the console for the user - rospy.loginfo('Tacking Action: Tacking Action Startup') + if goal.direction == 1: + rospy.loginfo(rospy.get_caller_id() + ' Startup - Tacking from left to right') + else: + rospy.loginfo(rospy.get_caller_id() + ' Startup - Tacking from right to left') self.state.minor = BoatState.MIN_TACKING self.state_pub.publish(self.state) @@ -84,7 +87,6 @@ def tacking_callback(self, goal): else: self.state.minor = BoatState.MIN_PLANNING self.state_pub.publish(self.state) - print "Tacking success: ", goal.direction, self.ane_reading success = True # From right to left of wind: @@ -102,11 +104,11 @@ def tacking_callback(self, goal): else: self.state.minor = BoatState.MIN_PLANNING self.state_pub.publish(self.state) - print "Tacking success: ", goal.direction, self.ane_reading success = True if self._as.is_preempt_requested(): - rospy.loginfo('Tacking Action: Preempted') + self._feedback.status = ' Preempted' + rospy.loginfo(rospy.get_caller_id() + self._feedback.status) if self.state.major is not BoatState.MAJ_AUTONOMOUS: self.state.minor = BoatState.MIN_COMPLETE else: @@ -114,7 +116,6 @@ def tacking_callback(self, goal): self.state_pub.publish(self.state) self._result.success = False self._result.target_heading = self.target_heading - self._feedback.status = "Preempted" self._as.set_preempted() preempted = True self.rate.sleep() @@ -127,7 +128,8 @@ def tacking_callback(self, goal): self.target_pub.publish(Float32(self.cur_boat_heading)) self.target_heading = self.cur_boat_heading self._result.target_heading = self.target_heading - rospy.loginfo('Tacking Action: Success') + self._feedback.status = ' Success' + rospy.loginfo(rospy.get_caller_id() + self._feedback.status) self._as.set_succeeded(self._result) if __name__ == '__main__': diff --git a/boat_msgs/action/Tacking.action b/boat_msgs/action/Tacking.action index 40f8f33..4159280 100644 --- a/boat_msgs/action/Tacking.action +++ b/boat_msgs/action/Tacking.action @@ -1,4 +1,5 @@ -#goal definition -1, 1 +#goal definition +# 1 = From left to right of wind, -1 = From right to left of wind int32 direction --- #result definition From 681116b733617b106c85d61ef2e4b2ecc5611da4 Mon Sep 17 00:00:00 2001 From: Matthew Reynolds Date: Sat, 24 Nov 2018 02:05:37 -0500 Subject: [PATCH 06/17] Add boat_utilites to metapackage and to README --- README.md | 3 +++ boat_2018/package.xml | 1 + 2 files changed, 4 insertions(+) diff --git a/README.md b/README.md index 5f35dc9..84f1a81 100644 --- a/README.md +++ b/README.md @@ -32,6 +32,9 @@ Custom messages for path planning, gps coordinates and boat states in order to f ## boat_nav Package for planning boat navigation given the specified task. This package is "the brains" of the boat, and decides how the boat should sail autonomously. +## boat_utilites +Package for various helper functions and utilities used throughout the codebase. + ## boat_vision Package for stereoscopic machine vision. This package is responsible for identifying objects in the water including obstacles (eg. Other boats, the shore, etc) and buoys used for navigation. diff --git a/boat_2018/package.xml b/boat_2018/package.xml index 5109a05..ddc8456 100644 --- a/boat_2018/package.xml +++ b/boat_2018/package.xml @@ -19,6 +19,7 @@ boat_msgs boat_nav boat_vision + boat_utilities From a8c7828f165e7422a9c16d0aa454af5bbe91f089 Mon Sep 17 00:00:00 2001 From: Seamus Johnston Date: Mon, 26 Nov 2018 18:08:14 -0500 Subject: [PATCH 07/17] Fix typo in README.md Co-Authored-By: Matthew-Reynolds --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 84f1a81..f3c95cd 100644 --- a/README.md +++ b/README.md @@ -32,7 +32,7 @@ Custom messages for path planning, gps coordinates and boat states in order to f ## boat_nav Package for planning boat navigation given the specified task. This package is "the brains" of the boat, and decides how the boat should sail autonomously. -## boat_utilites +## boat_utilities Package for various helper functions and utilities used throughout the codebase. ## boat_vision @@ -55,4 +55,4 @@ Also be sure to download the phidgets compass calibration program [See here](htt The compass calibration script can be found in the tools section of the `boat_interfaces` package, along with a readme on how to use it. -If you want to download the python phidgets API [See here](https://www.phidgets.com/docs/Language_-_Python#Install_Phidget_Python_module_for_Linux) \ No newline at end of file +If you want to download the python phidgets API [See here](https://www.phidgets.com/docs/Language_-_Python#Install_Phidget_Python_module_for_Linux) From e415302c73ade2b191825abe0a2baccafe07c4cf Mon Sep 17 00:00:00 2001 From: Matthew Reynolds Date: Mon, 26 Nov 2018 19:11:19 -0500 Subject: [PATCH 08/17] Ensure normalize utility is used instead of modulo --- boat_interfaces/src/rudder_node.py | 30 +++--- boat_nav/src/navigator_node.py | 147 ++++++++++++++--------------- boat_nav/src/sensor_parser_node.py | 24 ++--- 3 files changed, 100 insertions(+), 101 deletions(-) diff --git a/boat_interfaces/src/rudder_node.py b/boat_interfaces/src/rudder_node.py index a50c1ab..6a481fb 100755 --- a/boat_interfaces/src/rudder_node.py +++ b/boat_interfaces/src/rudder_node.py @@ -30,7 +30,7 @@ def boat_state_callback(new_state): global rudder_pos global pid_is_enabled state = new_state - + # If disabled or auto has completed its path, then stop the boat if state.major is BoatState.MAJ_DISABLED or (state.major is BoatState.MAJ_AUTONOMOUS and state.minor is BoatState.MIN_COMPLETE): rudder_pos = 90 @@ -56,9 +56,9 @@ def compass_callback(compass): global wind_heading global pid_input_pub global ane_reading - - wind_heading = (ane_reading + compass.data) % 360 - + + wind_heading = angles.normalize(ane_reading + compass.data) + cur_boat_heading = compass.data heading_msg = Float32(angles.normalize_signed(cur_boat_heading)) pid_input_pub.publish(heading_msg) @@ -68,8 +68,8 @@ def compass_callback(compass): # Whenever the PID controller calculates a new effort, apply it to the rudder position def pid_callback(output): global rudder_pos - - rudder_pos = 90.0 + output.data + + rudder_pos = 90.0 + output.data rudder_pos_pub.publish(Float32(rudder_pos)) #rospy.loginfo(rospy.get_caller_id() + " Rudder PID output pos: %f", rudder_pos) @@ -80,18 +80,18 @@ def target_heading_callback(target_heading): global state global pid_is_enabled global rudder_pos - + # Perhaps a worthwhile check, but not really super important because this callback will never be called if these conditions are not met in path_planning_node if state.major is not BoatState.MAJ_AUTONOMOUS or state.minor is not BoatState.MIN_PLANNING: return - + # If the PID isn't enabled, activate it if not pid_is_enabled: pid_is_enabled = True pid_enable_pub.publish(Bool(True)) rospy.loginfo(rospy.get_caller_id() + " Enabling rudder PID") - - # We have a new valid setpoint, therefore output it + + # We have a new valid setpoint, therefore output it #rospy.loginfo(rospy.get_caller_id() + " New rudder setpoint: %f", target_heading.data) pid_setpoint = Float32(angles.normalize_signed(target_heading.data)) @@ -104,25 +104,25 @@ def joy_callback(controller): global pid_is_enabled global rudder_max global rudder_min - + # Make sure we're in RC control mode if state.major is not BoatState.MAJ_RC or state.minor is BoatState.MIN_TACKING: return - + # Make sure the PID is off if pid_is_enabled: pid_is_enabled = False pid_enable_pub.publish(Bool(False)) rospy.loginfo(rospy.get_caller_id() + " Disabling rudder PID") - + # If the boat is not currently tacking, then setup a message to send to the /rudder topic rudder_pos_old = rudder_pos position_msg = Float32() - + # Set the rudder position with the right joystick, if close to 90 set dead straight if abs(controller.right_stick_x - (Joy.JOY_RANGE/2.0)) <= 15: position_msg.data = 90.0 - + else: position_msg.data = 90 - (rudder_max-rudder_min)/2.0 * ((controller.right_stick_x-(Joy.JOY_RANGE/2.0))/(Joy.JOY_RANGE/2.0)) diff --git a/boat_nav/src/navigator_node.py b/boat_nav/src/navigator_node.py index 5afee93..195211e 100755 --- a/boat_nav/src/navigator_node.py +++ b/boat_nav/src/navigator_node.py @@ -73,25 +73,25 @@ ## Callback for setting the boat state when the `/boat_state` topic is updated -# +# # @param new_state The new `boat_msgs.msg.BoatState` to set -# +# def boat_state_callback(new_state): global state state = new_state ## Callback for setting the apparent wind heading when the `/anemometer` topic is updated -# +# # @param new_heading The new anemometer reading, 180 is directly infront of boat increasing CCW -# +# def anemometer_callback(new_heading): global ane_reading global apparent_wind_heading global new_wind ane_reading = new_heading.data new_wind_heading = angles.normalize(ane_reading + cur_boat_heading) - + # Tolerance on a wind shift to be determined # Only update wind heading if a significant shift is detected, because it will then replan our upwind path if abs(new_wind_heading - apparent_wind_heading) > 0.1: @@ -100,44 +100,43 @@ def anemometer_callback(new_heading): ## Callback for setting the boat speed when the `/gps_raw` topic is updated -# +# # @param gps The `boat_msgs.msg.GPS` message containing the current boat speed -# +# def gps_callback(gps): global boat_speed boat_speed = gps.speed * 0.514444 # Knots to m/s ## Callback for setting the boat's heading when the `/compass` topic is updated. -# +# # @param compass The new heading to set, in degrees CCW from East -# +# def compass_callback(compass): global apparent_wind_heading global new_wind global cur_boat_heading global target_heading - + cur_boat_heading = compass.data new_wind_heading = angles.normalize(ane_reading + cur_boat_heading) - + # Tolerance on a wind shift to be determined # Only update wind heading if a significant shift is detected, because it will then replan our upwind path if abs(new_wind_heading - apparent_wind_heading) > 0.1 : new_wind = True apparent_wind_heading = new_wind_heading - #print "Wind headings: ", ane_reading, (apparent_wind_heading + 180) % 360 - - # If we are in RC, update our target heading to be the same direction as we are pointing, so the path planner + + # If we are in RC, update our target heading to be the same direction as we are pointing, so the path planner # will work when we switch to auto if state.major is not BoatState.MAJ_AUTONOMOUS: target_heading = cur_boat_heading ## Callback for setting the target point when the `/target_point` topic is updated. -# +# # @param new_target The `boat_msgs.msg.Waypoint` to set -# +# def target_callback(new_target): global target global is_new_target @@ -152,42 +151,42 @@ def target_callback(new_target): ## Callback for setting the boat's location when the `/lps` topic is updated. -# +# # @param position The `boat_msgs.msg.Point` to set # def position_callback(position): global cur_pos - cur_pos = position - + cur_pos = position + # If the boat isn't in the autonomous planning state, exit if state.major is not BoatState.MAJ_AUTONOMOUS or state.minor is not BoatState.MIN_PLANNING: return - + # Temporary jank solution if state.challenge is BoatState.CHA_AVOID: return - + awa_algorithm() # =*=*=*=*=*=*=*=*=*=*=*=*= Calculations =*=*=*=*=*=*=*=*=*=*=*=*= ''' ## Calculate the current velocity made good along the specified heading -# +# # @param direct_heading The direct heading to the target, in degrees CCW from East # @return The current velocity made good -# +# def vmg(direct_heading): return math.cos(cur_boat_heading - direct_heading) * boat_speed ''' ## Calculate the global maximum velocity made good of the entire system -# +# # @param wind_coming The heading of the apparent wind, in degrees CCW from East # @return The theoretical max velocity made good -# +# def calc_global_max_vmg(wind_coming): - + # TODO: Make a service and make more general for object avoidance upper_bound = angles.normalize(wind_coming + layline) lower_bound = angles.normalize(wind_coming - layline) @@ -200,20 +199,20 @@ def calc_global_max_vmg(wind_coming): vmg_heading = upper_bound else: vmg_heading = lower_bound - + # Otherwise max vmg is just the direct heading else: vmg_heading = direct_heading - + theoretic_boat_speed = 2.5 * 0.514444 # 2.5 Knots to m/s (measured boat speed) max_vmg = theoretic_boat_speed * angles.cosd(vmg_heading - direct_heading) - return max_vmg, vmg_heading - + return max_vmg, vmg_heading + ## Calculate the maximum velocity made good on the current tack -# +# # @param wind_coming The heading of the apparent wind, in degrees CCW from East # @return The theoretical max velocity made good -# +# def calc_cur_tack_max_vmg(wind_coming): # TODO: Make a service and make more general for object avoidance @@ -241,23 +240,23 @@ def calc_cur_tack_max_vmg(wind_coming): else: theoretic_boat_speed = 0 vmg_heading = apparent_wind_heading - + max_vmg = theoretic_boat_speed * angles.cosd(vmg_heading - direct_heading) return max_vmg, vmg_heading ## Calculate the current velocity made good along the current target heading -# +# # @param wind_coming The heading of the apparent wind, in degrees CCW from East # @return The velocity made good -# +# def calc_vmg(wind_coming): tolerance = 1.0 upper_bound = angles.normalize(wind_coming + layline - tolerance) lower_bound = angles.normalize(wind_coming - layline + tolerance) - + # If we are in irons, our theoretical speed is 0 if angles.is_within_bounds(target_heading, lower_bound, upper_bound): - theoretic_boat_speed = 0 + theoretic_boat_speed = 0 else: theoretic_boat_speed = 2.5 * 0.514444 # 2.5 Knots to m/s (measured boat speed) @@ -265,10 +264,10 @@ def calc_vmg(wind_coming): return cur_vmg ## Calculate the distance from the boat to the current target -# +# # @param Boat's current position # @return The distance, in meters -# +# def dist_to_target(position): return points.dist(target.pt, position) @@ -293,8 +292,8 @@ def remaining_course(): # @return True for can make it, and false for not # def on_layline(wind_coming, tolerance): - - # If waypoint is on left side of the wind, we are on the layline iff the target heading + + # If waypoint is on left side of the wind, we are on the layline iff the target heading # is to the right of the direct heading if angles.is_on_left(direct_heading, wind_coming): val = angles.is_on_right(target_heading, direct_heading + tolerance) @@ -303,9 +302,9 @@ def on_layline(wind_coming, tolerance): # is to the left of the direct heading else: val = angles.is_on_left(target_heading, direct_heading - tolerance) - + return val - + # =*=*=*=*=*=*=*=*=*=*=*=*= Algorithms =*=*=*=*=*=*=*=*=*=*=*=*= @@ -316,7 +315,7 @@ def awa_algorithm(): global new_wind global is_new_target global direct_heading - + # Calculate the direct heading to the next waypoint old_direct_heading = direct_heading direct_heading = angles.atan2d(target.pt.y - cur_pos.y, target.pt.x - cur_pos.x) @@ -332,13 +331,13 @@ def awa_algorithm(): is_new_target = False found_max_vmg = False max_vmg = 0 - + cur_vmg = calc_vmg(wind_coming) # Calculate vmg on current path - global_max_vmg, global_vmg_heading = calc_global_max_vmg(wind_coming) # Calculate max global vmg + global_max_vmg, global_vmg_heading = calc_global_max_vmg(wind_coming) # Calculate max global vmg cur_tack_max_vmg, cur_tack_vmg_heading = calc_cur_tack_max_vmg(wind_coming) # Calculate the max vmg on the current tack #rospy.loginfo(rospy.get_caller_id() +" Cur VMG: %f Max VMG: %f with Heading: %f Cur Tack VMG: %f with Heading: %f Direct Heading: %f", cur_vmg, global_max_vmg, global_vmg_heading, cur_tack_max_vmg, cur_tack_vmg_heading, direct_heading) per_course_left = remaining_course() - + # TODO: Tolerance these properly # If not currently at our optimal vmg, during our regular upwind routine (not layline setup) if (global_max_vmg > cur_vmg or cur_tack_max_vmg > cur_vmg): @@ -354,11 +353,11 @@ def awa_algorithm(): tacking_direction = -1 else: tacking_direction = 1 - + heading_pub.publish(Float32(target_heading)) goal = TackingGoal(direction = tacking_direction) tacking_client.send_goal(goal) - + # Adjust time delay until the tack is considered failed, and we return to planning if not tacking_client.wait_for_result(rospy.Duration(10)): tacking_client.cancel_goal() @@ -376,7 +375,7 @@ def awa_algorithm(): target_heading = cur_tack_vmg_heading #print target_heading heading_pub.publish(Float32(target_heading)) - + # Tack is not required to get to vmg_heading, therefore set it else: target_heading = global_vmg_heading @@ -389,14 +388,14 @@ def awa_algorithm(): rospy.loginfo(rospy.get_caller_id() + " Entering the navigate to layline routine. Saved current tack angle as: %f ", target_heading) goal = LaylineGoal(alt_tack_angle = target_heading, overshoot_angle = 3.0, target = target) layline_client.send_goal(goal) - + # Adjust time delay until the layline setup action is considered failed, and we return to planning if not layline_client.wait_for_result(rospy.Duration(40)): layline_client.cancel_goal() if layline_client.get_result() is not None: target_heading = layline_client.get_result().target_heading - + rospy.Rate(100).sleep() # DEPRECATED @@ -407,21 +406,21 @@ def taras_algorithm(): global target global is_new_target global target_heading - + # Calculate the direct heading to the next waypoint # This should never be undefined, as the atan2(0,0) case would already be caught by the proximity check above best_heading = angles.atan2d(target.pt.y - cur_pos.y, target.pt.x - cur_pos.x) best_heading = angles.normalize(best_heading) wind_coming = angles.normalize(apparent_wind_heading + 180) # Determine the direction the wind is coming from - + # If the direct path isn't possible... if best_heading > wind_coming-layline and best_heading < wind_coming+layline: - + # ... and there's new wind data or a new target, update the upwind path if new_wind or is_new_target: new_wind = False is_new_target = False - + # If the current heading is still acceptable, carry on #if target_heading <= wind_coming-layline+3 or target_heading >= wind_coming+layline-3 and not is_new_target: # best_heading = target_heading @@ -431,24 +430,24 @@ def taras_algorithm(): best_heading = wind_coming + layline else: best_heading = wind_coming - layline - + # If there isn't new wind data, DON'T update the heading else: best_heading = target_heading - - + + # If the target heading has updated, decide if we need to tack, then publish the new heading if best_heading is not target_heading: target_heading = best_heading - + # If the our headings are more that 180 degrees apart, reverse travel direction if (abs(target_heading - cur_boat_heading)) > 180: - boat_dir = 1 + boat_dir = 1 else: - boat_dir = -1 - + boat_dir = -1 + wind_coming = angles.normalize(apparent_wind_heading + 180) # Which direction the wind is coming from - + # TODO: This may be broken now that is_within_bounds is not bi-directional if (boat_dir is 1 and not angles.is_within_bounds(wind_coming, cur_boat_heading, target_heading)) or\ (boat_dir is -1 and angles.is_within_bounds(wind_coming, cur_boat_heading, target_heading)): @@ -457,31 +456,31 @@ def taras_algorithm(): if angles.is_within_bounds(target_heading, 90, 270): tacking_direction = -1 else: - tacking_direction = 1 - + tacking_direction = 1 + heading_pub.publish(target_heading) - + goal = TackingGoal(direction = tacking_direction) tacking_client.send_goal(goal) - + # Adjust time delay until the tack is considered failed, and we return to planning if not tacking_client.wait_for_result(rospy.Duration(10)): tacking_client.cancel_goal() - + rospy.loginfo(rospy.get_caller_id() + " Boat State = 'Autonomous - Planning'") else: - # Publish new heading for the rudder + # Publish new heading for the rudder heading_pub.publish(target_heading) rospy.loginfo(rospy.get_caller_id() + " New target heading: %f", target_heading) - + rospy.Rate(100).sleep() ## Initialize the node -# +# # Sets up all of the subscribers, initializes the node, and blocks until # the action servers are ready -# +# def init(): rospy.init_node('navigator') rospy.Subscriber('boat_state', BoatState, boat_state_callback) @@ -489,7 +488,7 @@ def init(): rospy.Subscriber('anemometer', Float32, anemometer_callback, queue_size=1) rospy.Subscriber('target_point', Waypoint, target_callback) rospy.Subscriber('compass', Float32, compass_callback, queue_size=1) # Only want it to receive the most recent orientation - + # If the filters work, change lps to use /odometry/filtered rospy.Subscriber('lps', Point, position_callback, queue_size=1) # Only want it to receive the most recent position max_vmg_client.wait_for_server() diff --git a/boat_nav/src/sensor_parser_node.py b/boat_nav/src/sensor_parser_node.py index 5b57f7e..b8c072f 100755 --- a/boat_nav/src/sensor_parser_node.py +++ b/boat_nav/src/sensor_parser_node.py @@ -6,7 +6,7 @@ from std_msgs.msg import Float32 from sensor_msgs.msg import Imu, NavSatFix, NavSatStatus from tf.transformations import euler_from_quaternion -from boat_utilities.angles import cosd, sind +from boat_utilities.angles import cosd, sind, normalize # Declare global variables needed for the node origin_lps = Point() @@ -29,7 +29,7 @@ def gps_callback(gps): global gps_pub global lps_pub - + gps_parsed = NavSatFix() gps_parsed.header = gps.header gps_parsed.status.status = gps.status @@ -37,7 +37,7 @@ def gps_callback(gps): gps_parsed.latitude = gps.latitude gps_parsed.longitude = gps.longitude gps_parsed.altitude = gps.altitude - + # TODO: Add covariance? gps_pub.publish(gps_parsed) local = to_lps(get_coords(gps)) @@ -57,9 +57,9 @@ def orientation_callback(imu): explicit_quat = [imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w] roll, pitch, yaw = euler_from_quaternion(explicit_quat) - - yaw = math.degrees(yaw) % 360 - + + yaw = normalize(math.degrees(yaw)) + heading = yaw compass_pub.publish(Float32(heading)) @@ -77,8 +77,8 @@ def origin_override_callback(gps_coords): def gps_to_lps_srv(req): res = ConvertPointResponse() res.pt = to_lps(req.pt) - - #rospy.loginfo(rospy.get_caller_id() + " Request: GPS (long: %.1f, lat: %.1f)", req.pt.x, req.pt.y) + + #rospy.loginfo(rospy.get_caller_id() + " Request: GPS (long: %.1f, lat: %.1f)", req.pt.x, req.pt.y) #rospy.loginfo(rospy.get_caller_id() + " Response: LPS (x: %.f, y: %.f)", res.pt.x, res.pt.y) return res @@ -87,7 +87,7 @@ def gps_to_lps_srv(req): def lps_to_gps_srv(req): res = ConvertPointResponse() res.pt = to_gps(req.pt) - + #rospy.loginfo(rospy.get_caller_id() + " Request: LPS (x: %.f, y: %.f)", req.pt.x, req.pt.y) #rospy.loginfo(rospy.get_caller_id() + " Response: GPS (long: %.1f, lat: %.1f)", res.pt.x, res.pt.y) return res @@ -122,9 +122,9 @@ def get_coords(gps): # Initialize the node def listener(): global origin_lps - + rospy.init_node('sensor_parser') - + # Setup first so that simulator can send the origin point srv1 = rospy.Service('lps_to_gps', ConvertPoint, lps_to_gps_srv) @@ -133,7 +133,7 @@ def listener(): origin_coords = rospy.wait_for_message('gps_raw', GPS) origin_lps = to_lps(get_coords(origin_coords)) rospy.loginfo("Got origin: x:%f y:%f", origin_lps.x, origin_lps.y) - + rospy.Subscriber('imu/data', Imu, orientation_callback) rospy.Subscriber('gps_raw', GPS, gps_callback) rospy.Subscriber('origin_override', Point, origin_override_callback) From 334491c72c0c8506f97391800cb01b37c335020e Mon Sep 17 00:00:00 2001 From: Matthew Reynolds Date: Mon, 26 Nov 2018 19:27:15 -0500 Subject: [PATCH 09/17] Create utility to find the opposite angle (angle+180) --- boat_nav/src/layline_action.py | 36 +++---- boat_nav/src/navigator_node.py | 6 +- boat_nav/src/path_planners/planner_base.py | 104 ++++++++++---------- boat_utilities/src/boat_utilities/angles.py | 14 ++- 4 files changed, 85 insertions(+), 75 deletions(-) diff --git a/boat_nav/src/layline_action.py b/boat_nav/src/layline_action.py index 4c651c4..30d0bd0 100755 --- a/boat_nav/src/layline_action.py +++ b/boat_nav/src/layline_action.py @@ -61,7 +61,7 @@ def compass_callback(self, compass): def update_apparent_wind(self): self.apparent_wind_heading = angles.normalize(self.ane_reading + self.compass) - self.wind_coming = angles.normalize(self.apparent_wind_heading + 180) + self.wind_coming = angles.opposite(self.apparent_wind_heading) def layline_callback(self, goal): @@ -70,24 +70,24 @@ def layline_callback(self, goal): preempted = False did_hit_midpoint = False self.new_target = False - + # publish info to the console for the user self._feedback.status = " Entered Layline Action Callback. " rospy.loginfo(rospy.get_caller_id() + self._feedback.status) - - + + pos = self.cur_pos tar = goal.target.pt - + a = angles.tand(self.compass) b = -1 c = tar.y - a*tar.x dx = pos.x-tar.x dy = pos.y-tar.y - + d_perp = abs(a*pos.x + b*pos.y + c)/math.sqrt(a*a+b*b) d_par = math.sqrt(dx*dx+dy*dy-d_perp*d_perp) - + if angles.is_on_left(goal.alt_tack_angle, self.wind_coming): tacking_direction = 1 else: @@ -97,12 +97,12 @@ def layline_callback(self, goal): new_target = angles.normalize(self.wind_coming - tacking_direction * self.layline) self.target_heading = new_target self.target_pub.publish(Float32(self.target_heading)) - + tacking_goal = TackingGoal(direction = tacking_direction) self.tacking_client.send_goal(tacking_goal) endtime = rospy.Time.now() + rospy.Duration(10) - + # TODO: Sometimes this loop abruptly exits for no reason while (self.tacking_client.get_state() is GoalStatus.ACTIVE or\ self.tacking_client.get_state() is GoalStatus.PENDING) and rospy.Time.now() < endtime and not did_hit_midpoint: @@ -111,14 +111,14 @@ def layline_callback(self, goal): cur_d_perp = abs(a*pos.x + b*pos.y + c)/math.sqrt(a*a+b*b) if cur_d_perp < d_perp*0.48: did_hit_midpoint = True - + # If we hit the midpoint, cancel the current tack and return to the original heading if did_hit_midpoint: self.tacking_client.cancel_goal() - + self._feedback.status = " Reached 48% perpendicular distance, returning to original heading." rospy.loginfo(rospy.get_caller_id() + self._feedback.status) - + tacking_goal.direction = tacking_goal.direction * -1 self.tacking_client.send_goal(tacking_goal) if not self.tacking_client.wait_for_result(rospy.Duration(10)): @@ -129,14 +129,14 @@ def layline_callback(self, goal): rospy.loginfo(rospy.get_caller_id() + self._feedback.status) self._as.set_succeeded(self._result) return - + self._result.success = True self._result.target_heading = self.target_heading self._feedback.status = " Completed shortened layline action" rospy.loginfo(rospy.get_caller_id() + self._feedback.status) self._as.set_succeeded(self._result) return - + # If we hit this state, the tack failed. Attempt to return to the original heading if not self.tacking_client.get_result() or not self.tacking_client.get_result().success: self.tacking_client.cancel_goal() @@ -150,7 +150,7 @@ def layline_callback(self, goal): rospy.loginfo(rospy.get_caller_id() + self._feedback.status) self._as.set_succeeded(self._result) return - + self.target_heading = self.tacking_client.get_result().target_heading hit_layline = False self._feedback.status = " Waiting to hit layline. " @@ -176,7 +176,7 @@ def layline_callback(self, goal): self._as.set_preempted() preempted = True self.rate.sleep() - + # If preempted in the loop, exit the action if preempted: return @@ -188,7 +188,7 @@ def layline_callback(self, goal): self.target_heading = goal.alt_tack_angle self.target_pub.publish(Float32(self.target_heading)) self.tacking_client.send_goal(tacking_goal) - + # Adjust time delay until the tack is considered failed, and we return to planning if not self.tacking_client.wait_for_result(rospy.Duration(10)): self.tacking_client.cancel_goal() @@ -209,7 +209,7 @@ def layline_callback(self, goal): self._feedback.status = " Completed Layline Action. " rospy.loginfo(rospy.get_caller_id() + self._feedback.status) self._as.set_succeeded(self._result) - + if __name__ == '__main__': rospy.init_node('layline_action') server = LaylineAction(rospy.get_name()) diff --git a/boat_nav/src/navigator_node.py b/boat_nav/src/navigator_node.py index 195211e..2639bea 100755 --- a/boat_nav/src/navigator_node.py +++ b/boat_nav/src/navigator_node.py @@ -320,7 +320,7 @@ def awa_algorithm(): old_direct_heading = direct_heading direct_heading = angles.atan2d(target.pt.y - cur_pos.y, target.pt.x - cur_pos.x) direct_heading = angles.normalize(direct_heading) - wind_coming = angles.normalize(apparent_wind_heading + 180) # Determine the direction the wind is coming from + wind_coming = angles.opposite(apparent_wind_heading) # Determine the direction the wind is coming from # TODO: Make n a function of boat speed to negate the effects of apparent wind? n = 1 + p*1.3/dist_to_target(start_pos) # Tacking weight, can add app_wind_offset here to make even less desirable @@ -411,7 +411,7 @@ def taras_algorithm(): # This should never be undefined, as the atan2(0,0) case would already be caught by the proximity check above best_heading = angles.atan2d(target.pt.y - cur_pos.y, target.pt.x - cur_pos.x) best_heading = angles.normalize(best_heading) - wind_coming = angles.normalize(apparent_wind_heading + 180) # Determine the direction the wind is coming from + wind_coming = angles.opposite(apparent_wind_heading) # Determine the direction the wind is coming from # If the direct path isn't possible... if best_heading > wind_coming-layline and best_heading < wind_coming+layline: @@ -446,7 +446,7 @@ def taras_algorithm(): else: boat_dir = -1 - wind_coming = angles.normalize(apparent_wind_heading + 180) # Which direction the wind is coming from + wind_coming = angles.opposite(apparent_wind_heading) # Which direction the wind is coming from # TODO: This may be broken now that is_within_bounds is not bi-directional if (boat_dir is 1 and not angles.is_within_bounds(wind_coming, cur_boat_heading, target_heading)) or\ diff --git a/boat_nav/src/path_planners/planner_base.py b/boat_nav/src/path_planners/planner_base.py index 280c956..c7c3c01 100644 --- a/boat_nav/src/path_planners/planner_base.py +++ b/boat_nav/src/path_planners/planner_base.py @@ -20,7 +20,7 @@ class Planner: """Abstract base class for all Planners. - + All implementations should be organized in the same order as this class That is: - Class Variables @@ -29,14 +29,14 @@ class Planner: - Callbacks - Setters - Utilities - + Implementations should also use the \@overrides (\@overrides.overrides) decorator to ensure proper inheritence on the abstract setup() and planner() methods - + Note that implementations must implement the setup() and planner() methods """ __metaclass__ = ABCMeta - + # Static variables shared in in all implementations state = BoatState() waypoints = [] @@ -45,119 +45,119 @@ class Planner: wind_coming = 0 cur_boat_heading = 0 ane_reading = 0 - - + + # =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= Main Behaviour =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= - + @abstractmethod def setup(self): """Setup the planner for the specific challenge. - + There are three times when this is called: 1. When the BoatState.major enters MAJ_AUTONOMOUS 2. When the BoatState.challenge enters the specified implementation's challenge 3. When the BoatState.minor is MIN_COMPLETE or MIN_INITIALIZE and a new waypoint is added to the waypoints_raw topic """ NotImplementedError("Class %s doesn't implement setup()" % (self.__class__.__name__)) - - + + @abstractmethod def planner(self): """Compute the next point for the boat to navigate to. - + This is called whenever the boat's position is updated, and should compute the next target_waypoint for the boat to navigate towards """ NotImplementedError("Class %s doesn't implement planner()" % (self.__class__.__name__)) - - + + def traverse_waypoints_planner(self): """Navigate through the waypoints on the waypoints_raw topic sequentially.""" waypoints = self.waypoints - - # If the list of waypoints is not empty + + # If the list of waypoints is not empty if len(waypoints) > 0: - + # If the boat is close enough to the waypoint, start navigating towards the next waypoint in the path if self.boat_reached_target(): rospy.loginfo(rospy.get_caller_id() + " Reached intermediate waypoint (lat: %.2f, long: %.2f)", waypoints[0].pt.y, waypoints[0].pt.x) - + del waypoints[0] self.update_waypoints(waypoints) if len(waypoints) > 0: self.publish_target(waypoints[0]) - + # If there are no waypoints left to navigate to, exit else: self.set_minor_state(BoatState.MIN_COMPLETE) rospy.loginfo(rospy.get_caller_id() + " No waypoints left. Boat State = 'Autonomous - Complete'") - - + + # =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= Callbacks =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= - + @staticmethod def _anemometer_callback(anemometer): """Callback for anemometer reading.""" Planner.ane_reading = anemometer.data Planner._calc_wind_coming() - - + + @staticmethod def _compass_callback(compass): """Callback for compass reading.""" Planner.cur_boat_heading = compass.data Planner._calc_wind_coming() - - + + # =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= Setters =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= - + @staticmethod def set_minor_state(minor): """Set the minor state of the BoatState. - + @param minor: The state to set """ Planner.state.minor = minor _boat_state_pub.publish(Planner.state) - + @staticmethod def clear_waypoints(): """Clear all the waypoints. Equivalent to update_waypoints([])""" Planner.update_waypoints([]) - + @staticmethod def update_waypoints(new_pts): """Set and publish the specified waypoints. - + @param new_pts: The waypoints to set """ Planner.waypoints = new_pts _waypoints_pub.publish(Planner.waypoints) - + @staticmethod def publish_target(*argv): """Publish the target waypoint. - + Publish the target waypoint, performing additional computation if the waypoint is of type Waypoint.TYPE_ROUND to ensure the boat will properly round the waypoint. - + @param argv: Optionally specify the waypoint to publish. If unspecified, the Planner.target_waypoint will be used. """ assert len(argv) is 0 or len(argv) is 1, "Invalid number of arguments, expected 1" - + if len(argv) is 1: Planner.target_waypoint = argv[0] - + waypoints = Planner.waypoints target_waypoint = Planner.target_waypoint cur_pos = Planner.cur_pos - + # Perform the neccessary computation to allow rounding of buoys if target_waypoint.type is Waypoint.TYPE_ROUND and target_waypoint in waypoints and waypoints.index(target_waypoint) < len(waypoints)-1: - + r = ROUND_DIST/111319.492188 # meters to coords k = ROUND_FACTOR - + # Use the heading from the boat to the buoy and from the buoy to the next to calculate where around the target to place the waypoint. next = waypoints[waypoints.index(target_waypoint)+1] theta_boat = math.atan2(Services.to_gps(cur_pos).y - target_waypoint.pt.y, Services.to_gps(cur_pos).x - target_waypoint.pt.x) @@ -168,28 +168,28 @@ def publish_target(*argv): angle -= math.pi / 2 else: angle += math.pi / 2 - + roundPt = Point(target_waypoint.pt.x + math.cos(angle)*r, target_waypoint.pt.y + math.sin(angle)*r) Planner.target_waypoint = Waypoint(roundPt, Waypoint.TYPE_ROUND) - + # Publish the target _target_pub.publish(Planner.target_waypoint) - + ##rospy.loginfo(rospy.get_caller_id() + " New target waypoint: (long: %.2f, lat: %.2f) or (x: %.f, y: %.f)", point.x, point.y, local.x, local.y) - - + + # =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= Utility Functions =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= - + @staticmethod def _calc_wind_coming(): """Calculate the angle of the wind.""" Planner.new_wind_heading = angles.normalize(Planner.ane_reading + Planner.cur_boat_heading) - Planner.wind_coming = angles.normalize(Planner.new_wind_heading + 180) - - + Planner.wind_coming = angles.opposite(Planner.new_wind_heading) + + def boat_reached_target(self): """Determine if the boat is within BUOY_TOL meters of the target_waypoint. - + @return True if the boat is within the tolerance """ return points.is_within_dist(self.cur_pos, Services.to_lps(self.target_waypoint), BUOY_TOL) @@ -202,11 +202,11 @@ class Services: _to_lps_srv = () _to_gps_lock = threading.Lock() _to_lps_lock = threading.Lock() - + @staticmethod def to_gps(p): """Convert a point from local (meters) position system to global (coords) position system. - + @param p: The Point or Waypoint to convert @return The converted Point """ @@ -217,11 +217,11 @@ def to_gps(p): return Services._to_gps_srv(p.pt).pt else: raise ValueError("p is of invalid type " + str(type(p)) +", must be either Point or Waypoint") - + @staticmethod def to_lps(p): """Convert a point from global (coords) position system to local (meters) position system. - + @param p: The Point or Waypoint to convert @return The converted Point """ diff --git a/boat_utilities/src/boat_utilities/angles.py b/boat_utilities/src/boat_utilities/angles.py index e9b51c6..30f509f 100644 --- a/boat_utilities/src/boat_utilities/angles.py +++ b/boat_utilities/src/boat_utilities/angles.py @@ -43,6 +43,16 @@ def normalize_signed(angle): return angle +def opposite(angle): + """Calculate the opposite angle, normalized to be 0 to 360.""" + return normalize(angle + __UNITS/2) + + +def opposite_signed(angle): + """Calculate the opposite angle, normalized to be -180 to 180.""" + return normalize_signed(angle + __UNITS/2) + + def is_within_bounds(val, lower, upper): """Determine whether lower < val < upper. @@ -67,7 +77,7 @@ def is_within_bounds(val, lower, upper): def is_on_right(angle, ref): """Determine whether the specified angle is within the 180 degrees to the right of the reference. - + Args: angle The angle to check ref The reference angle to compare against @@ -84,7 +94,7 @@ def is_on_right(angle, ref): def is_on_left(angle, ref): """Determine whether the specified angle is within the 180 degrees to the left of the reference. - + Args: angle The angle to check ref The reference angle to compare against From e47b5d5037e25fecd175186bff3a4066d53a50ac Mon Sep 17 00:00:00 2001 From: Matthew Reynolds Date: Mon, 26 Nov 2018 19:31:43 -0500 Subject: [PATCH 10/17] Document threshold value in layline_action --- boat_nav/src/layline_action.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/boat_nav/src/layline_action.py b/boat_nav/src/layline_action.py index 30d0bd0..8a4f3ba 100755 --- a/boat_nav/src/layline_action.py +++ b/boat_nav/src/layline_action.py @@ -107,6 +107,10 @@ def layline_callback(self, goal): while (self.tacking_client.get_state() is GoalStatus.ACTIVE or\ self.tacking_client.get_state() is GoalStatus.PENDING) and rospy.Time.now() < endtime and not did_hit_midpoint: + # If we reach approximately half of the perpendicular distance, we know we must cancel the layline action + # and return to our original tack. We use 48% rather than 50% here since the boat cannot respond instantly + # due to intertia, etc. So this provides us a bit of buffer to get back on track + # TODO: Tune this constant pos = self.cur_pos cur_d_perp = abs(a*pos.x + b*pos.y + c)/math.sqrt(a*a+b*b) if cur_d_perp < d_perp*0.48: From 14c31c4561d0c809ee644b8b80d073cf6b8aa520 Mon Sep 17 00:00:00 2001 From: Matthew Reynolds Date: Mon, 26 Nov 2018 20:21:55 -0500 Subject: [PATCH 11/17] Add unit conversion utility module --- boat_nav/src/layline_action.py | 4 +- boat_nav/src/navigator_node.py | 12 +- boat_nav/src/path_planners/planner_base.py | 5 +- boat_nav/src/path_planners/search.py | 99 ++++++++-------- boat_utilities/src/boat_utilities/__init__.py | 1 + boat_utilities/src/boat_utilities/units.py | 111 ++++++++++++++++++ 6 files changed, 172 insertions(+), 60 deletions(-) create mode 100644 boat_utilities/src/boat_utilities/units.py diff --git a/boat_nav/src/layline_action.py b/boat_nav/src/layline_action.py index 8a4f3ba..51eecee 100755 --- a/boat_nav/src/layline_action.py +++ b/boat_nav/src/layline_action.py @@ -5,7 +5,7 @@ from boat_msgs.msg import LaylineAction as LaylineActionMsg, LaylineFeedback, LaylineResult, BoatState, Point, Waypoint, TackingAction, TackingGoal, GPS from std_msgs.msg import Float32 from actionlib_msgs.msg import GoalStatus -from boat_utilities import angles +from boat_utilities import angles, units class LaylineAction(object): # create messages that are used to publish feedback/result @@ -48,7 +48,7 @@ def target_callback(self, new_target): #print "new target" def gps_callback(self, gps): - self.boat_speed = gps.speed * 0.514444 # Knots to m/s + self.boat_speed = units.from_knots(gps.speed) def anemometer_callback(self, new_heading): self.ane_reading = new_heading.data diff --git a/boat_nav/src/navigator_node.py b/boat_nav/src/navigator_node.py index 2639bea..91e7855 100755 --- a/boat_nav/src/navigator_node.py +++ b/boat_nav/src/navigator_node.py @@ -5,7 +5,7 @@ from boat_msgs.msg import BoatState, GPS, MaxVMGAction, MaxVMGGoal, Point, Waypoint, TackingAction, TackingGoal, LaylineAction, LaylineGoal from boat_msgs.srv import ConvertPoint from std_msgs.msg import Float32 -from boat_utilities import points, angles +from boat_utilities import points, angles, units ## Trigger for when the wind shifts significantly new_wind = False @@ -105,7 +105,7 @@ def anemometer_callback(new_heading): # def gps_callback(gps): global boat_speed - boat_speed = gps.speed * 0.514444 # Knots to m/s + boat_speed = units.from_knots(gps.speed) ## Callback for setting the boat's heading when the `/compass` topic is updated. @@ -204,7 +204,7 @@ def calc_global_max_vmg(wind_coming): else: vmg_heading = direct_heading - theoretic_boat_speed = 2.5 * 0.514444 # 2.5 Knots to m/s (measured boat speed) + theoretic_boat_speed = units.from_knots(2.5) # 2.5 Knots to m/s (measured boat speed) max_vmg = theoretic_boat_speed * angles.cosd(vmg_heading - direct_heading) return max_vmg, vmg_heading @@ -221,7 +221,7 @@ def calc_cur_tack_max_vmg(wind_coming): # If direct heading is in irons, then max_vmg will be on the edge of the no go zone if angles.is_within_bounds(direct_heading, lower_bound, upper_bound): - theoretic_boat_speed = 2.5 * 0.514444 # 2.5 Knots to m/s + theoretic_boat_speed = units.from_knots(2.5) # 2.5 Knots to m/s (measured boat speed) # Snap to whichever edge of the no go zone is closer if angles.is_on_left(target_heading, wind_coming): @@ -232,7 +232,7 @@ def calc_cur_tack_max_vmg(wind_coming): # If the direct heading is somewhere in our current tack (direct_heading and target_heading on same side of wind) elif (angles.is_on_left(direct_heading, wind_coming) and angles.is_on_left(target_heading, wind_coming)) or\ (angles.is_on_right(direct_heading, wind_coming) and angles.is_on_right(target_heading, wind_coming)): - theoretic_boat_speed = 2.5 * 0.514444 # 2.5 Knots to m/s + theoretic_boat_speed = units.from_knots(2.5) # 2.5 Knots to m/s (measured boat speed) vmg_heading = direct_heading # If none of the above, the best heading will be on the opposite tack @@ -258,7 +258,7 @@ def calc_vmg(wind_coming): if angles.is_within_bounds(target_heading, lower_bound, upper_bound): theoretic_boat_speed = 0 else: - theoretic_boat_speed = 2.5 * 0.514444 # 2.5 Knots to m/s (measured boat speed) + theoretic_boat_speed = units.from_knots(2.5) # 2.5 Knots to m/s (measured boat speed) cur_vmg = theoretic_boat_speed * angles.cosd(target_heading - direct_heading) return cur_vmg diff --git a/boat_nav/src/path_planners/planner_base.py b/boat_nav/src/path_planners/planner_base.py index c7c3c01..83fadb5 100644 --- a/boat_nav/src/path_planners/planner_base.py +++ b/boat_nav/src/path_planners/planner_base.py @@ -6,8 +6,7 @@ from boat_msgs.msg import BoatState, Point, Waypoint, WaypointArray from boat_msgs.srv import ConvertPoint from std_msgs.msg import Float32 - -from boat_utilities import angles, points +from boat_utilities import angles, points, units BUOY_TOL = rospy.get_param('/boat/planner/buoy_tol') ROUND_DIST = rospy.get_param('/boat/planner/round_dist') @@ -155,7 +154,7 @@ def publish_target(*argv): # Perform the neccessary computation to allow rounding of buoys if target_waypoint.type is Waypoint.TYPE_ROUND and target_waypoint in waypoints and waypoints.index(target_waypoint) < len(waypoints)-1: - r = ROUND_DIST/111319.492188 # meters to coords + r = units.to_degrees(ROUND_DIST) k = ROUND_FACTOR # Use the heading from the boat to the buoy and from the buoy to the next to calculate where around the target to place the waypoint. diff --git a/boat_nav/src/path_planners/search.py b/boat_nav/src/path_planners/search.py index 25765f0..24edc20 100755 --- a/boat_nav/src/path_planners/search.py +++ b/boat_nav/src/path_planners/search.py @@ -6,154 +6,155 @@ from boat_msgs.msg import BoatState, Point, PointArray, Waypoint, IronsAction, IronsGoal from std_msgs.msg import Bool from path_planners import Planner +from boat_utilities import units VISION_WIDTH = rospy.get_param('/boat/vision/width') class SearchPlanner(Planner): """Planner implementation to perform a search algorithm to find a buoy.""" - + def __init__(self): self.area_center = Point() # center of search circle self.area_radius = 0 # radius of search circle self.vision_target = () self.started_pattern = False - + rospy.Subscriber('search_area', PointArray, self._search_area_callback) rospy.Subscriber('vision', PointArray, self._vision_callback) - + self.irons_client = SimpleActionClient('irons_action', IronsAction) self.found_pub = rospy.Publisher('found_target', Bool, queue_size = 10) - + self.irons_client.wait_for_server() - - + + # =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= Main Behaviour =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= - + @overrides def setup(self): self.started_pattern = False - + if self.area_radius is 0: return - + rospy.loginfo(rospy.get_caller_id() + " Setting waypoints for search challenge routine") - + self.vision_target = () self.found_pub.publish(False) - + wind_heading = self.wind_coming sweep_width = VISION_WIDTH - + # Calculate an expanding square pattern and begin traversing the pattern self.update_waypoints(self._get_expaning_square_pts(wind_heading, sweep_width)) self.publish_target(self.waypoints[0]) self.set_minor_state(BoatState.MIN_PLANNING) self.started_pattern = True - + @overrides def planner(self): - + if not self.started_pattern: return - + # If we still haven't found the target, continue traversing the pattern if self.vision_target is (): - + if len(self.waypoints) > 0: self.traverse_waypoints_planner() - + # If we ran through all the waypoints set without finding anything, restart the planner else: self.setup() - + # If we've seen the target at least once, navigate towards its last seen position else: self.publish_target(Waypoint(self.vision_target, Waypoint.TYPE_INTERSECT)) - + #Once we intercept the vision target, publish and complete if self.boat_reached_target(): #TODO: Add additional 'complete' criteria to ensure contact self.found_pub.publish(True) self.set_minor_state(BoatState.MIN_COMPLETE) #TODO: Add MIN_SHUTDOWN?? - - + + self.irons_client.send_goal(IronsGoal()) - + # Adjust time delay until the layline setup action is considered failed if not self.irons_client.wait_for_result(rospy.Duration(20)): self.irons_client.cancel_goal() - - + + # =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= Callbacks =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= - + # receives point array from topic, first point is center, second point is on the edge of the circle def _search_area_callback(self, search_area): """Callback for search area. - + The first Point is the center of the circular area, the second is a point on the edge of the area. That is, the distance between the two points describes the radius of the circlular area, centered on Point 1. - + @param search_area: The PointArray describing the search area. """ if len(search_area.points) < 2: return - + # take first point as center # take second point as point on edge of circle self.area_center = search_area.points[0] dx = (search_area.points[1].x - self.area_center.x) dy = (search_area.points[1].y - self.area_center.y) self.area_radius = math.sqrt(dx*dx+dy*dy) - + # Start seach routine if self.state.challenge is BoatState.CHA_SEARCH: self.setup() - - + + def _vision_callback(self, targets): """Callback for vision targets. - + Currently, this topic should only contain a single Point, so we do not need to determine which Point in the PointArray to sail towards - + @param targets: The PointArray of objects seen by the boat's vision system, including buoys and obstacles """ - + # Whenever the vision sees a new/updated target, save it if len(targets.points) > 0: - + # The first time we find a target, print it if self.vision_target is (): rospy.loginfo("Found target! (%.4f, %.4f). Navigating to target.", targets.points[0].x, targets.points[0].y) - + self.vision_target = targets.points[0] - - + + # =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= Utility Functions =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= - + def _get_expaning_square_pts(self, angle, width): """Calculate the points required to search the given area, using an expanding square algorithm. - + The square will be oriented such that the first point will be directly downwind. This ensures no sides of the square will ever be upwind (All sides should be ~45 degrees to wind) - + @param angle: The heading of the incoming wind, in degrees. @param width: The width of the search path, in meters """ - width = width / 111319.492188 # meters to coords + width = units.to_degrees(width) angle += 90 out = [] radius = width *1.5 increment = width * math.sqrt(2) / 4 - - # --- past stuff + + # --- past stuff # Add an extra point for better startup central coverage # out.append(self._make_waypoint(angle-90, width/2)) - + #x = math.cos(math.radians(angle-90))*radius #y = math.sin(math.radians(angle-90))*radius #out.append(Waypoint(Point(x,y), Waypoint.TYPE_INTERSECT)) - + # --- # Set 4 points in the beginning for initial loop/square @@ -178,7 +179,7 @@ def _get_expaning_square_pts(self, angle, width): out.append(self._make_waypoint(angle, radius)) angle += 90 radius += increment - + # Add extra points for better coverage at edge of circle radius += increment for i in range(0,4): @@ -186,7 +187,7 @@ def _get_expaning_square_pts(self, angle, width): angle += 90 return out - + def _make_waypoint(self, angle, radius): x = self.area_center.x + math.cos(math.radians(angle))*radius y = self.area_center.y + math.sin(math.radians(angle))*radius @@ -199,5 +200,5 @@ def _make_waypoint(self, angle, radius): # trans_x = x*c - y*s + area_center.x # trans_y = y*c - x*s + area_center.y # return Point(trans_x,trans_y) - + diff --git a/boat_utilities/src/boat_utilities/__init__.py b/boat_utilities/src/boat_utilities/__init__.py index 3f007b7..6993616 100644 --- a/boat_utilities/src/boat_utilities/__init__.py +++ b/boat_utilities/src/boat_utilities/__init__.py @@ -4,3 +4,4 @@ import points import angles +import units diff --git a/boat_utilities/src/boat_utilities/units.py b/boat_utilities/src/boat_utilities/units.py new file mode 100644 index 0000000..0b64520 --- /dev/null +++ b/boat_utilities/src/boat_utilities/units.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python +"""Conversions to/from SI units.""" + +# Mass +LB_TO_KG = 0.453592 + +# Length +INCHES_TO_METERS = 0.0254 +FEET_TO_METERS = 0.3048 +YARDS_TO_METERS = 0.9144 +MILES_TO_METERS = 1609.34 +NAUT_MILES_TO_METERS = 1852 + +# Speed +MPH_TO_MPS = 0.44704 +KNOTS_TO_MPS = 0.514444 + +# Coordinates +# NOTE: This is always accurate for latitude. However, longitude varies with latitude, +# meaning this is only accurate for longitude at 0deg N +DEGREES_TO_METERS = 111319.492188 + + +def from_lbs(lbs): + """Convert pounds to kilograms.""" + return lbs * LB_TO_KG + + +def to_lbs(kilograms): + """Convert kilograms to pounds.""" + return kilograms / LB_TO_KG + + +def from_inches(inches): + """Convert inches to meters.""" + return inches * INCHES_TO_METERS + + +def to_inches(meters): + """Convert meters to inches.""" + return meters / INCHES_TO_METERS + + +def from_feet(feet): + """Convert feet to meters.""" + return feet * FEET_TO_METERS + + +def to_feet(meters): + """Convert meters to feet.""" + return meters / FEET_TO_METERS + + +def from_yards(yards): + """Convert yards to meters.""" + return yards * YARDS_TO_METERS + + +def to_yards(meters): + """Convert meters to yards.""" + return meters / YARDS_TO_METERS + + +def from_miles(miles): + """Convert miles to meters.""" + return miles * MILES_TO_METERS + + +def to_miles(meters): + """Convert meters to miles.""" + return meters / MILES_TO_METERS + + +def from_naut_miles(naut_miles): + """Convert nautical miles to meters.""" + return naut_miles * NAUT_MILES_TO_METERS + + +def to_naut_miles(meters): + """Convert meters to nautical miles.""" + return meters / NAUT_MILES_TO_METERS + + +def from_mph(mph): + """Convert miles/hour to meters/second.""" + return mph * MPH_TO_MPS + + +def to_mph(m_per_sec): + """Convert meters/second to miles/hour.""" + return m_per_sec / MPH_TO_MPS + + +def from_knots(knots): + """Convert knots to meters/second.""" + return knots * KNOTS_TO_MPS + + +def to_knots(m_per_sec): + """Convert meters/second to knots.""" + return m_per_sec / KNOTS_TO_MPS + + +def from_degrees(degrees): + """Convert degrees lat/long to meters.""" + return degrees * DEGREES_TO_METERS + + +def to_degrees(meters): + """Convert meters to degrees lat/long.""" + return meters / DEGREES_TO_METERS From 50c106fd39cfe64990fcf0663cb1e9519511a4f8 Mon Sep 17 00:00:00 2001 From: Matthew Reynolds Date: Mon, 26 Nov 2018 20:52:15 -0500 Subject: [PATCH 12/17] Used proper "private" member naming in planner_base --- boat_nav/src/layline_action.py | 1 + boat_nav/src/path_planners/long.py | 16 +-- boat_nav/src/path_planners/nav.py | 12 +-- boat_nav/src/path_planners/planner_base.py | 16 +-- boat_nav/src/path_planners/search.py | 4 +- boat_nav/src/path_planners/station.py | 110 ++++++++++----------- 6 files changed, 77 insertions(+), 82 deletions(-) diff --git a/boat_nav/src/layline_action.py b/boat_nav/src/layline_action.py index 51eecee..d7a9cfe 100755 --- a/boat_nav/src/layline_action.py +++ b/boat_nav/src/layline_action.py @@ -59,6 +59,7 @@ def compass_callback(self, compass): self.compass = compass.data self.update_apparent_wind() + def update_apparent_wind(self): self.apparent_wind_heading = angles.normalize(self.ane_reading + self.compass) self.wind_coming = angles.opposite(self.apparent_wind_heading) diff --git a/boat_nav/src/path_planners/long.py b/boat_nav/src/path_planners/long.py index 9180b64..40a9d89 100644 --- a/boat_nav/src/path_planners/long.py +++ b/boat_nav/src/path_planners/long.py @@ -8,24 +8,24 @@ class LongPlanner(Planner): """Simple Planner implementation that traverses the waypoints in waypoints_raw, but does not destroy achieved waypoints, allowing infinite looping. """ - + @overrides def setup(self): if len(self.waypoints) is 0: rospy.loginfo(rospy.get_caller_id() + " No points to nav") self.set_minor_state(BoatState.MIN_COMPLETE) return - + self.publish_target(self.waypoints[0]) self.set_minor_state(BoatState.MIN_PLANNING) - - + + @overrides def planner(self): - + # If we've reached a waypoint, add it to the end of the list to continue looping forever - if len(self.waypoints) > 0 and self.boat_reached_target(): + if len(self.waypoints) > 0 and self._boat_reached_target(): self.waypoints.append(self.waypoints[0]) - + # Run the default traverse_waypoints planner - self.traverse_waypoints_planner(); + self._traverse_waypoints_planner() diff --git a/boat_nav/src/path_planners/nav.py b/boat_nav/src/path_planners/nav.py index 5d4d5ee..c1956d8 100644 --- a/boat_nav/src/path_planners/nav.py +++ b/boat_nav/src/path_planners/nav.py @@ -6,20 +6,20 @@ class NavPlanner(Planner): """Simple Planner implementation that traverses the waypoints in waypoints_raw once each.""" - + @overrides def setup(self): if len(self.waypoints) is 0: rospy.loginfo(rospy.get_caller_id() + " No points to nav") self.set_minor_state(BoatState.MIN_COMPLETE) return - + self.publish_target(self.waypoints[0]) self.set_minor_state(BoatState.MIN_PLANNING) - - + + @overrides def planner(self): - + # Run the default traverse_waypoints planner - self.traverse_waypoints_planner(); + self._traverse_waypoints_planner() diff --git a/boat_nav/src/path_planners/planner_base.py b/boat_nav/src/path_planners/planner_base.py index 83fadb5..696c4b4 100644 --- a/boat_nav/src/path_planners/planner_base.py +++ b/boat_nav/src/path_planners/planner_base.py @@ -70,7 +70,7 @@ def planner(self): NotImplementedError("Class %s doesn't implement planner()" % (self.__class__.__name__)) - def traverse_waypoints_planner(self): + def _traverse_waypoints_planner(self): """Navigate through the waypoints on the waypoints_raw topic sequentially.""" waypoints = self.waypoints @@ -78,7 +78,7 @@ def traverse_waypoints_planner(self): if len(waypoints) > 0: # If the boat is close enough to the waypoint, start navigating towards the next waypoint in the path - if self.boat_reached_target(): + if self._boat_reached_target(): rospy.loginfo(rospy.get_caller_id() + " Reached intermediate waypoint (lat: %.2f, long: %.2f)", waypoints[0].pt.y, waypoints[0].pt.x) del waypoints[0] @@ -112,10 +112,7 @@ def _compass_callback(compass): @staticmethod def set_minor_state(minor): - """Set the minor state of the BoatState. - - @param minor: The state to set - """ + """Set the minor state of the BoatState.""" Planner.state.minor = minor _boat_state_pub.publish(Planner.state) @@ -126,10 +123,7 @@ def clear_waypoints(): @staticmethod def update_waypoints(new_pts): - """Set and publish the specified waypoints. - - @param new_pts: The waypoints to set - """ + """Set and publish the specified waypoints.""" Planner.waypoints = new_pts _waypoints_pub.publish(Planner.waypoints) @@ -186,7 +180,7 @@ def _calc_wind_coming(): Planner.wind_coming = angles.opposite(Planner.new_wind_heading) - def boat_reached_target(self): + def _boat_reached_target(self): """Determine if the boat is within BUOY_TOL meters of the target_waypoint. @return True if the boat is within the tolerance diff --git a/boat_nav/src/path_planners/search.py b/boat_nav/src/path_planners/search.py index 24edc20..370e3aa 100755 --- a/boat_nav/src/path_planners/search.py +++ b/boat_nav/src/path_planners/search.py @@ -61,7 +61,7 @@ def planner(self): if self.vision_target is (): if len(self.waypoints) > 0: - self.traverse_waypoints_planner() + self._traverse_waypoints_planner() # If we ran through all the waypoints set without finding anything, restart the planner else: @@ -72,7 +72,7 @@ def planner(self): self.publish_target(Waypoint(self.vision_target, Waypoint.TYPE_INTERSECT)) #Once we intercept the vision target, publish and complete - if self.boat_reached_target(): #TODO: Add additional 'complete' criteria to ensure contact + if self._boat_reached_target(): #TODO: Add additional 'complete' criteria to ensure contact self.found_pub.publish(True) self.set_minor_state(BoatState.MIN_COMPLETE) #TODO: Add MIN_SHUTDOWN?? diff --git a/boat_nav/src/path_planners/station.py b/boat_nav/src/path_planners/station.py index 2215d19..b3a41ab 100644 --- a/boat_nav/src/path_planners/station.py +++ b/boat_nav/src/path_planners/station.py @@ -12,39 +12,39 @@ class StationPlanner(Planner): """Planner implementation to stay within a location for 5 minutes, and then exit.""" - + def __init__(self): self.box = [] self.start_station = Point() self.end_station = Point() self.station_timeout = False - + rospy.Subscriber('bounding_box', PointArray, self._bounding_box_callback) - - + + # =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= Main Behaviour =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= - + @overrides def setup(self): - + if len(self.box) is not 4: rospy.loginfo(rospy.get_caller_id() + " Box is invalid") self.set_minor_state(BoatState.MIN_COMPLETE) return - + rospy.loginfo(rospy.get_caller_id() + " Beginning station challenge path planner routine") - + rospy.Timer(rospy.Duration(5*60), self._timer_callback, oneshot=True) - + # Clear previous points self.clear_waypoints() - + # Determine wall angles and box widths, as well as slope of the bottom of the box, as long as it's not vertical m_bottom = 0 vert = False - + box = self.box - + if abs(box[3].x - box[0].x) > 0.0001: m_bottom = (box[3].y - box[0].y)/(box[3].x - box[0].x) else: @@ -57,7 +57,7 @@ def setup(self): # Determine width at the height we will be traveling station_width = (bottom_box_width + (top_box_width-bottom_box_width) * HEIGHT_TO_TRAVEL) * MAX_WIDTH station_height = (left_box_height + (right_box_height-left_box_height) * ((1 - MAX_WIDTH) / 2.0)) * HEIGHT_TO_TRAVEL - + # Determine the two points that will alternate, first find centre point of box y_sum = 0 x_sum = 0 @@ -66,7 +66,7 @@ def setup(self): y_sum += p.y x_avr = x_sum / 4.0 y_avr = y_sum / 4.0 - + y_int = box[0].y - m_bottom * box[0].x self.start_station = start_station = Point() self.end_station = end_station = Point() @@ -101,28 +101,28 @@ def setup(self): start_station.y = m_bottom * start_station.x + y_int + station_height end_station.x = start_station.x + station_width end_station.y = m_bottom * end_station.x + y_int + station_height - + self.publish_target(Waypoint(start_station, Waypoint.TYPE_INTERSECT)) self.set_minor_state(BoatState.MIN_PLANNING) - - + + @overrides def planner(self): - + # If time is up and we've fully exited the box, stop if self.station_timeout: - if self.boat_reached_target(): + if self._boat_reached_target(): self.set_minor_state(BoatState.MIN_COMPLETE) self.station_timeout = False rospy.loginfo(rospy.get_caller_id() + " Exited box. Boat State = 'Autonomous - Complete'") return - + # If the box is ever invalid, stop if len(self.box) is not 4 and self.state.minor is not BoatState.MIN_COMPLETE: self.set_minor_state(BoatState.MIN_COMPLETE) rospy.loginfo(rospy.get_caller_id() + " Box is invalid. Boat State = 'Autonomous - Complete'") return - + #TODO: Make this not spam (Eg. if we're already headed for the center, don't re-publish if not self._within_box(): x_avr = 0 @@ -132,38 +132,38 @@ def planner(self): y_avr += p.y / float(len(self.box)) self.publish_target(Waypoint(Point(x_avr, y_avr), Waypoint.TYPE_INTERSECT)) return - + # If we've reached the set waypoint, flip around - if self.boat_reached_target(): + if self._boat_reached_target(): if points.is_within_dist(self.target_waypoint.pt, self.start_station, 0.0001): self.publish_target(Waypoint(self.end_station, Waypoint.TYPE_INTERSECT)) else: self.publish_target(Waypoint(self.start_station, Waypoint.TYPE_INTERSECT)) - - + + # =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= Callbacks =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= - + def _timer_callback(self, event): """Exit the station once the 5 minutes of the challenge are complete.""" self.station_timeout = True rospy.loginfo(rospy.get_caller_id() + " Reached end of station timer") - + box = self.box - - # Determine distance from each side of the box + + # Determine distance from each side of the box l_dist = self._dist_from_line(box[0], box[1]) t_dist = self._dist_from_line(box[1], box[2]) r_dist = self._dist_from_line(box[3], box[2]) b_dist = self._dist_from_line(box[0], box[3]) - + dist_list = (l_dist, t_dist, r_dist, b_dist) i = dist_list.index(min(dist_list)) - + # Use service to determine 12m dist in gps # Use this distance on how far to put the buoy outside the box tol_dist = Services.to_gps(Point(20,0)).x cur_pos_gps = Services.to_gps(self.cur_pos) - + final_point = {0 : Point(box[0].x - tol_dist, cur_pos_gps.y), 1 : Point(cur_pos_gps.x, box[1].y + tol_dist), 2 : Point(box[3].x + tol_dist, cur_pos_gps.y), @@ -171,14 +171,14 @@ def _timer_callback(self, event): } final_direction = {0 : "Left", 1: "Top", 2: "Right", 3: "Bottom"} rospy.loginfo(rospy.get_caller_id() + " Exiting bounding box through: " + final_direction[i]) - + self.publish_target(Waypoint(final_point[i], Waypoint.TYPE_INTERSECT)) - - + + def _bounding_box_callback(self, bounding_box): """Callback for station bounding box.""" wind_coming = self.wind_coming - + # Reorganize the local points to create a box when drawn, if there are four if len(bounding_box.points) == 4: x_sum = 0 @@ -196,7 +196,7 @@ def _bounding_box_callback(self, bounding_box): if p.x < x_avr: if p.y < y_avr: temp[0] = p - else: + else: temp[1] = p else: if p.y < y_avr: @@ -207,7 +207,7 @@ def _bounding_box_callback(self, bounding_box): if p.x < x_avr: if p.y < y_avr: temp[1] = p - else: + else: temp[2] = p else: if p.y < y_avr: @@ -218,7 +218,7 @@ def _bounding_box_callback(self, bounding_box): if p.x < x_avr: if p.y < y_avr: temp[2] = p - else: + else: temp[3] = p else: if p.y < y_avr: @@ -229,7 +229,7 @@ def _bounding_box_callback(self, bounding_box): if p.x < x_avr: if p.y < y_avr: temp[3] = p - else: + else: temp[0] = p else: if p.y < y_avr: @@ -239,17 +239,17 @@ def _bounding_box_callback(self, bounding_box): self.box = temp else: self.box = bounding_box.points - + # For if bounding box is added after we are in station mode if self.state.challenge is BoatState.CHA_STATION: self.setup() - - + + # =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= Utility Functions =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= - + def _dist_from_line(self, start_point, end_point): """Calculate the distance of the boat from the line. - + @param start_point: The initial point on the line @param end_point: The second point on the line @return The distance in coordinates @@ -260,11 +260,11 @@ def _dist_from_line(self, start_point, end_point): m = (end_point.y - start_point.y)/(end_point.x - start_point.x) b = start_point.y - m * start_point.x return abs(b + m * cur_pos_gps.x - cur_pos_gps.y)/(math.sqrt(1 + m*m)) - - + + def _within_box(self): """Determine if the boat is within the inner box. - + @return True if the boat is within the box """ # Find centre of box @@ -282,21 +282,21 @@ def _within_box(self): pt.y = (self.box[i].y - y_avr)*INNER_BOX_SCALE + y_avr pt.x = (self.box[i].x - x_avr)*INNER_BOX_SCALE + x_avr inner_box[i] = pt - + # Create horizontal vector from the cur_pos towards +x start_point = Services.to_gps(self.cur_pos) m = 0 b = start_point.y intersections = 0 - - # Cast the vector out of the box, if the boat is inside the box, the ray will only intersect with a side once. If outside of the box, it + + # Cast the vector out of the box, if the boat is inside the box, the ray will only intersect with a side once. If outside of the box, it # will intersect an odd number of times. for i in range(4): # Make sure line is not vertical if abs(inner_box[(i+1)%4].x - inner_box[i].x) > 0.0001: m_box = (inner_box[(i+1)%4].y - inner_box[i].y)/(inner_box[(i+1)%4].x - inner_box[i].x) b_box = inner_box[i].y - m_box * inner_box[i].x - + # If the slopes are the same they will never intersect if abs(m - m_box) > 0.0001: x_int = (b_box - b)/(m_box - m) @@ -308,10 +308,10 @@ def _within_box(self): intersections += 1 # Perfectly vertical line, will intersect as long as it is on the right side of the starting point and in the right y range else: - if (inner_box[i].x >= start_point.x and start_point.y <= max(inner_box[i].y,inner_box[(i+1)%4].y) + if (inner_box[i].x >= start_point.x and start_point.y <= max(inner_box[i].y,inner_box[(i+1)%4].y) and start_point.y >= min(inner_box[i].y,inner_box[(i+1)%4].y)): intersections += 1 - - + + return (intersections % 2) is not 0 From 09a312e911f884ba226a3a9a2d6862499fc5a379 Mon Sep 17 00:00:00 2001 From: Matthew Reynolds Date: Sun, 2 Dec 2018 22:59:25 -0500 Subject: [PATCH 13/17] Label geographic coordinate conversions more clearly --- boat_nav/src/path_planners/planner_base.py | 2 +- boat_nav/src/path_planners/search.py | 2 +- boat_utilities/src/boat_utilities/units.py | 16 ++++++++-------- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/boat_nav/src/path_planners/planner_base.py b/boat_nav/src/path_planners/planner_base.py index 696c4b4..9973ab4 100644 --- a/boat_nav/src/path_planners/planner_base.py +++ b/boat_nav/src/path_planners/planner_base.py @@ -148,7 +148,7 @@ def publish_target(*argv): # Perform the neccessary computation to allow rounding of buoys if target_waypoint.type is Waypoint.TYPE_ROUND and target_waypoint in waypoints and waypoints.index(target_waypoint) < len(waypoints)-1: - r = units.to_degrees(ROUND_DIST) + r = units.to_geo_coords(ROUND_DIST) k = ROUND_FACTOR # Use the heading from the boat to the buoy and from the buoy to the next to calculate where around the target to place the waypoint. diff --git a/boat_nav/src/path_planners/search.py b/boat_nav/src/path_planners/search.py index 370e3aa..ae681e1 100755 --- a/boat_nav/src/path_planners/search.py +++ b/boat_nav/src/path_planners/search.py @@ -140,7 +140,7 @@ def _get_expaning_square_pts(self, angle, width): @param angle: The heading of the incoming wind, in degrees. @param width: The width of the search path, in meters """ - width = units.to_degrees(width) + width = units.to_geo_coords(width) angle += 90 out = [] radius = width *1.5 diff --git a/boat_utilities/src/boat_utilities/units.py b/boat_utilities/src/boat_utilities/units.py index 0b64520..f5cb0ca 100644 --- a/boat_utilities/src/boat_utilities/units.py +++ b/boat_utilities/src/boat_utilities/units.py @@ -15,10 +15,10 @@ MPH_TO_MPS = 0.44704 KNOTS_TO_MPS = 0.514444 -# Coordinates +# Geographic coordinates # NOTE: This is always accurate for latitude. However, longitude varies with latitude, # meaning this is only accurate for longitude at 0deg N -DEGREES_TO_METERS = 111319.492188 +GEO_DEGREES_TO_METERS = 111319.492188 def from_lbs(lbs): @@ -101,11 +101,11 @@ def to_knots(m_per_sec): return m_per_sec / KNOTS_TO_MPS -def from_degrees(degrees): - """Convert degrees lat/long to meters.""" - return degrees * DEGREES_TO_METERS +def from_geo_coords(degrees): + """Convert geographic coordinates (degrees lat/long) to meters.""" + return degrees * GEO_DEGREES_TO_METERS -def to_degrees(meters): - """Convert meters to degrees lat/long.""" - return meters / DEGREES_TO_METERS +def to_geo_coords(meters): + """Convert meters to geographic coordinates (degrees lat/long).""" + return meters / GEO_DEGREES_TO_METERS From d26a2b52a0af9af2309ad8db1e60c2f9dcd31b20 Mon Sep 17 00:00:00 2001 From: Matthew Reynolds Date: Sun, 2 Dec 2018 23:16:34 -0500 Subject: [PATCH 14/17] Prefix private methods w/ underscore --- boat_nav/src/path_planners/avoid.py | 18 +++++++++--------- boat_nav/src/path_planners/long.py | 6 +++--- boat_nav/src/path_planners/nav.py | 6 +++--- boat_nav/src/path_planners/planner_base.py | 22 +++++++++++----------- boat_nav/src/path_planners/search.py | 18 +++++++++--------- boat_nav/src/path_planners/station.py | 20 ++++++++++---------- 6 files changed, 45 insertions(+), 45 deletions(-) diff --git a/boat_nav/src/path_planners/avoid.py b/boat_nav/src/path_planners/avoid.py index cf6169f..ea2d820 100644 --- a/boat_nav/src/path_planners/avoid.py +++ b/boat_nav/src/path_planners/avoid.py @@ -7,30 +7,30 @@ class AvoidPlanner(Planner): """Not a real planner just some BS to get partial points.""" - + def __init__(self): self.has_obstacle = False self.rudder_pub = rospy.Publisher('rudder', Float32, queue_size=10) self.pid_enable_pub = rospy.Publisher('rudder_pid/enable', Bool, queue_size=10) rospy.Subscriber('vision', PointArray, self._obstacle_callback) - - + + @overrides def setup(self): self.pid_enable_pub.publish(Bool(False)) - self.set_minor_state(BoatState.MIN_PLANNING) - - + self._set_minor_state(BoatState.MIN_PLANNING) + + @overrides def planner(self): if self.has_obstacle: self.rudder_pub.publish(Float32(150)) else: self.rudder_pub.publish(Float32(90)) - - + + def _obstacle_callback(self, targets): self.has_obstacle = len(targets.points) > 0 - + if Planner.state.challenge is BoatState.CHA_AVOID: self.planner() diff --git a/boat_nav/src/path_planners/long.py b/boat_nav/src/path_planners/long.py index 40a9d89..7163667 100644 --- a/boat_nav/src/path_planners/long.py +++ b/boat_nav/src/path_planners/long.py @@ -13,11 +13,11 @@ class LongPlanner(Planner): def setup(self): if len(self.waypoints) is 0: rospy.loginfo(rospy.get_caller_id() + " No points to nav") - self.set_minor_state(BoatState.MIN_COMPLETE) + self._set_minor_state(BoatState.MIN_COMPLETE) return - self.publish_target(self.waypoints[0]) - self.set_minor_state(BoatState.MIN_PLANNING) + self._publish_target(self.waypoints[0]) + self._set_minor_state(BoatState.MIN_PLANNING) @overrides diff --git a/boat_nav/src/path_planners/nav.py b/boat_nav/src/path_planners/nav.py index c1956d8..2392aa9 100644 --- a/boat_nav/src/path_planners/nav.py +++ b/boat_nav/src/path_planners/nav.py @@ -11,11 +11,11 @@ class NavPlanner(Planner): def setup(self): if len(self.waypoints) is 0: rospy.loginfo(rospy.get_caller_id() + " No points to nav") - self.set_minor_state(BoatState.MIN_COMPLETE) + self._set_minor_state(BoatState.MIN_COMPLETE) return - self.publish_target(self.waypoints[0]) - self.set_minor_state(BoatState.MIN_PLANNING) + self._publish_target(self.waypoints[0]) + self._set_minor_state(BoatState.MIN_PLANNING) @overrides diff --git a/boat_nav/src/path_planners/planner_base.py b/boat_nav/src/path_planners/planner_base.py index 9973ab4..5d7d3a4 100644 --- a/boat_nav/src/path_planners/planner_base.py +++ b/boat_nav/src/path_planners/planner_base.py @@ -30,7 +30,7 @@ class Planner: - Utilities Implementations should also use the \@overrides (\@overrides.overrides) decorator - to ensure proper inheritence on the abstract setup() and planner() methods + to ensure proper inheritance on the abstract setup() and planner() methods Note that implementations must implement the setup() and planner() methods """ @@ -82,13 +82,13 @@ def _traverse_waypoints_planner(self): rospy.loginfo(rospy.get_caller_id() + " Reached intermediate waypoint (lat: %.2f, long: %.2f)", waypoints[0].pt.y, waypoints[0].pt.x) del waypoints[0] - self.update_waypoints(waypoints) + self._update_waypoints(waypoints) if len(waypoints) > 0: - self.publish_target(waypoints[0]) + self._publish_target(waypoints[0]) # If there are no waypoints left to navigate to, exit else: - self.set_minor_state(BoatState.MIN_COMPLETE) + self._set_minor_state(BoatState.MIN_COMPLETE) rospy.loginfo(rospy.get_caller_id() + " No waypoints left. Boat State = 'Autonomous - Complete'") @@ -111,24 +111,24 @@ def _compass_callback(compass): # =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= Setters =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= @staticmethod - def set_minor_state(minor): + def _set_minor_state(minor): """Set the minor state of the BoatState.""" Planner.state.minor = minor _boat_state_pub.publish(Planner.state) @staticmethod - def clear_waypoints(): - """Clear all the waypoints. Equivalent to update_waypoints([])""" - Planner.update_waypoints([]) + def _clear_waypoints(): + """Clear all the waypoints. Equivalent to _update_waypoints([])""" + Planner._update_waypoints([]) @staticmethod - def update_waypoints(new_pts): + def _update_waypoints(new_pts): """Set and publish the specified waypoints.""" Planner.waypoints = new_pts _waypoints_pub.publish(Planner.waypoints) @staticmethod - def publish_target(*argv): + def _publish_target(*argv): """Publish the target waypoint. Publish the target waypoint, performing additional computation if the waypoint is @@ -145,7 +145,7 @@ def publish_target(*argv): target_waypoint = Planner.target_waypoint cur_pos = Planner.cur_pos - # Perform the neccessary computation to allow rounding of buoys + # Perform the necessary computation to allow rounding of buoys if target_waypoint.type is Waypoint.TYPE_ROUND and target_waypoint in waypoints and waypoints.index(target_waypoint) < len(waypoints)-1: r = units.to_geo_coords(ROUND_DIST) diff --git a/boat_nav/src/path_planners/search.py b/boat_nav/src/path_planners/search.py index ae681e1..3b064e3 100755 --- a/boat_nav/src/path_planners/search.py +++ b/boat_nav/src/path_planners/search.py @@ -46,9 +46,9 @@ def setup(self): sweep_width = VISION_WIDTH # Calculate an expanding square pattern and begin traversing the pattern - self.update_waypoints(self._get_expaning_square_pts(wind_heading, sweep_width)) - self.publish_target(self.waypoints[0]) - self.set_minor_state(BoatState.MIN_PLANNING) + self._update_waypoints(self._get_expanding_square_pts(wind_heading, sweep_width)) + self._publish_target(self.waypoints[0]) + self._set_minor_state(BoatState.MIN_PLANNING) self.started_pattern = True @overrides @@ -69,12 +69,12 @@ def planner(self): # If we've seen the target at least once, navigate towards its last seen position else: - self.publish_target(Waypoint(self.vision_target, Waypoint.TYPE_INTERSECT)) + self._publish_target(Waypoint(self.vision_target, Waypoint.TYPE_INTERSECT)) #Once we intercept the vision target, publish and complete if self._boat_reached_target(): #TODO: Add additional 'complete' criteria to ensure contact self.found_pub.publish(True) - self.set_minor_state(BoatState.MIN_COMPLETE) #TODO: Add MIN_SHUTDOWN?? + self._set_minor_state(BoatState.MIN_COMPLETE) #TODO: Add MIN_SHUTDOWN?? self.irons_client.send_goal(IronsGoal()) @@ -92,7 +92,7 @@ def _search_area_callback(self, search_area): """Callback for search area. The first Point is the center of the circular area, the second is a point on the edge of the area. - That is, the distance between the two points describes the radius of the circlular area, centered on Point 1. + That is, the distance between the two points describes the radius of the circular area, centred on Point 1. @param search_area: The PointArray describing the search area. """ @@ -106,7 +106,7 @@ def _search_area_callback(self, search_area): dy = (search_area.points[1].y - self.area_center.y) self.area_radius = math.sqrt(dx*dx+dy*dy) - # Start seach routine + # Start search routine if self.state.challenge is BoatState.CHA_SEARCH: self.setup() @@ -131,7 +131,7 @@ def _vision_callback(self, targets): # =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= Utility Functions =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= - def _get_expaning_square_pts(self, angle, width): + def _get_expanding_square_pts(self, angle, width): """Calculate the points required to search the given area, using an expanding square algorithm. The square will be oriented such that the first point will be directly downwind. This @@ -164,7 +164,7 @@ def _get_expaning_square_pts(self, angle, width): radius += increment # do some sweeping lines while going downwind to cover the areas around center - # these points are arbituarily made based on simulator and can be modified based on current needs + # these points are arbitrarily made based on simulator and can be modified based on current needs out.append(self._make_waypoint(angle-90, radius*0.6)) out.append(self._make_waypoint(angle-45, radius*0.707)) out.append(self._make_waypoint(angle-90, radius*0.4)) diff --git a/boat_nav/src/path_planners/station.py b/boat_nav/src/path_planners/station.py index b3a41ab..d21ce90 100644 --- a/boat_nav/src/path_planners/station.py +++ b/boat_nav/src/path_planners/station.py @@ -29,7 +29,7 @@ def setup(self): if len(self.box) is not 4: rospy.loginfo(rospy.get_caller_id() + " Box is invalid") - self.set_minor_state(BoatState.MIN_COMPLETE) + self._set_minor_state(BoatState.MIN_COMPLETE) return rospy.loginfo(rospy.get_caller_id() + " Beginning station challenge path planner routine") @@ -37,7 +37,7 @@ def setup(self): rospy.Timer(rospy.Duration(5*60), self._timer_callback, oneshot=True) # Clear previous points - self.clear_waypoints() + self._clear_waypoints() # Determine wall angles and box widths, as well as slope of the bottom of the box, as long as it's not vertical m_bottom = 0 @@ -102,8 +102,8 @@ def setup(self): end_station.x = start_station.x + station_width end_station.y = m_bottom * end_station.x + y_int + station_height - self.publish_target(Waypoint(start_station, Waypoint.TYPE_INTERSECT)) - self.set_minor_state(BoatState.MIN_PLANNING) + self._publish_target(Waypoint(start_station, Waypoint.TYPE_INTERSECT)) + self._set_minor_state(BoatState.MIN_PLANNING) @overrides @@ -112,14 +112,14 @@ def planner(self): # If time is up and we've fully exited the box, stop if self.station_timeout: if self._boat_reached_target(): - self.set_minor_state(BoatState.MIN_COMPLETE) + self._set_minor_state(BoatState.MIN_COMPLETE) self.station_timeout = False rospy.loginfo(rospy.get_caller_id() + " Exited box. Boat State = 'Autonomous - Complete'") return # If the box is ever invalid, stop if len(self.box) is not 4 and self.state.minor is not BoatState.MIN_COMPLETE: - self.set_minor_state(BoatState.MIN_COMPLETE) + self._set_minor_state(BoatState.MIN_COMPLETE) rospy.loginfo(rospy.get_caller_id() + " Box is invalid. Boat State = 'Autonomous - Complete'") return @@ -130,15 +130,15 @@ def planner(self): for p in self.box: x_avr += p.x / float(len(self.box)) y_avr += p.y / float(len(self.box)) - self.publish_target(Waypoint(Point(x_avr, y_avr), Waypoint.TYPE_INTERSECT)) + self._publish_target(Waypoint(Point(x_avr, y_avr), Waypoint.TYPE_INTERSECT)) return # If we've reached the set waypoint, flip around if self._boat_reached_target(): if points.is_within_dist(self.target_waypoint.pt, self.start_station, 0.0001): - self.publish_target(Waypoint(self.end_station, Waypoint.TYPE_INTERSECT)) + self._publish_target(Waypoint(self.end_station, Waypoint.TYPE_INTERSECT)) else: - self.publish_target(Waypoint(self.start_station, Waypoint.TYPE_INTERSECT)) + self._publish_target(Waypoint(self.start_station, Waypoint.TYPE_INTERSECT)) # =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= Callbacks =*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*= @@ -172,7 +172,7 @@ def _timer_callback(self, event): final_direction = {0 : "Left", 1: "Top", 2: "Right", 3: "Bottom"} rospy.loginfo(rospy.get_caller_id() + " Exiting bounding box through: " + final_direction[i]) - self.publish_target(Waypoint(final_point[i], Waypoint.TYPE_INTERSECT)) + self._publish_target(Waypoint(final_point[i], Waypoint.TYPE_INTERSECT)) def _bounding_box_callback(self, bounding_box): From 7437c5bd01032dd3f286c684dc232323af55bc5d Mon Sep 17 00:00:00 2001 From: Matthew Reynolds Date: Sun, 2 Dec 2018 23:22:08 -0500 Subject: [PATCH 15/17] Fix spelling & typos --- .../tools/calibration/calibrate.py | 12 ++-- boat_nav/src/layline_action.py | 2 +- boat_nav/src/max_vmg_action.py | 60 +++++++++---------- boat_nav/src/navigator_node.py | 6 +- 4 files changed, 40 insertions(+), 40 deletions(-) diff --git a/boat_interfaces/tools/calibration/calibrate.py b/boat_interfaces/tools/calibration/calibrate.py index b86338d..bc28a46 100644 --- a/boat_interfaces/tools/calibration/calibrate.py +++ b/boat_interfaces/tools/calibration/calibrate.py @@ -33,7 +33,7 @@ def read_line(): def wait_for_string(s): global cur_index - + count = 0 while True: @@ -44,9 +44,9 @@ def wait_for_string(s): else: cur_index = substr_index return - + read_line() - + if count > 1000: raise Exception("Looped too many times, please check output above for abnormalities.") else: @@ -54,7 +54,7 @@ def wait_for_string(s): def get_next_number(): global cur_index - + count = 0 while True: @@ -70,9 +70,9 @@ def get_next_number(): cur_index += len(num_str) return float(num_str) else: - break; + break cur_index += 1 - + read_line() if count > 1000: diff --git a/boat_nav/src/layline_action.py b/boat_nav/src/layline_action.py index d7a9cfe..281546d 100755 --- a/boat_nav/src/layline_action.py +++ b/boat_nav/src/layline_action.py @@ -110,7 +110,7 @@ def layline_callback(self, goal): # If we reach approximately half of the perpendicular distance, we know we must cancel the layline action # and return to our original tack. We use 48% rather than 50% here since the boat cannot respond instantly - # due to intertia, etc. So this provides us a bit of buffer to get back on track + # due to inertia, etc. So this provides us a bit of buffer to get back on track # TODO: Tune this constant pos = self.cur_pos cur_d_perp = abs(a*pos.x + b*pos.y + c)/math.sqrt(a*a+b*b) diff --git a/boat_nav/src/max_vmg_action.py b/boat_nav/src/max_vmg_action.py index 28707e7..3b8da87 100755 --- a/boat_nav/src/max_vmg_action.py +++ b/boat_nav/src/max_vmg_action.py @@ -15,56 +15,56 @@ def __init__(self, name): self._name = name self._as = SimpleActionServer(self._name, MaxVMGActionMsg, execute_cb=self.execute, auto_start = False) self._as.start() - + self.layline = rospy.get_param('/boat/nav/layline') - + self.ane_reading = 0 self.heading = 0 self.speed = 0 self.pos = Point() self.max_vmg = 0 - + # Subscribers rospy.Subscriber('anemometer', Float32, self.anemometer_callback) rospy.Subscriber('lps', Point, self.pos_callback) rospy.Subscriber('compass', Float32, self.heading_callback) rospy.Subscriber('gps_raw', GPS, self.speed_callback) - + # Publishers self.target_heading_pub = rospy.Publisher('rudder_pid/setpoint', Float32, queue_size=10) self.pid_enable_pub = rospy.Publisher('rudder_pid/enable', Bool, queue_size=10) #self.state_pub = rospy.Publisher('boat_state', BoatState, queue_size=10) - + self.rate = rospy.Rate(100) - + def speed_callback(self, gps): self.speed = gps.speed - + def heading_callback(self, compass): self.heading = compass.data - + def pos_callback(self, pos): self.pos = pos - + def anemometer_callback(self, anemometer): self.ane_reading = anemometer.data - + def vmg(self): return angles.cosd(self.heading - self.direct_heading()) * self.speed - + def direct_heading(self): orig = self.pos dest = self.goal.target return angles.atan2d(dest.y-orig.y, dest.x-orig.x) - + def execute(self, goal): self.goal = goal - + success = False preempted = False - + self._feedback.status = "Entered Action Callback" - + # Publish info to the console for the user if goal.direction is 1: rospy.loginfo('Max VMG Action: Startup. Finding VMG on right of target') @@ -72,45 +72,45 @@ def execute(self, goal): rospy.loginfo('Max VMG Action: Startup. Finding VMG on left of target') else: rospy.loginfo('Max VMG Action: Illegal Arguments') - + # Set state and stuff if needed - + # Make sure PID is enabled self.target_heading_pub.publish(Float32(self.heading)) self.pid_enable_pub.publish(Bool(True)) - + # If we're pointing towards the wrong side of the target, start by pointing towards the target if (goal.direction is 1 and self.heading < self.direct_heading())\ or (goal.direction is -1 and self.heading > self.direct_heading()): self.target_heading_pub.publish(self.direct_heading()) - + # Start executing the action while not success and not preempted: - + # If we are trying to find the max VMG to the right of the target and we're pointing on the correct side of the target: if goal.direction is 1 and self.heading > self.direct_heading()-2.5: - - cur_vmg = self.vmg(); - + + cur_vmg = self.vmg() + # Keep turning further right until we find the max vmg if cur_vmg > self.max_vmg: self.target_heading_pub.publish(self.heading+2) self.max_vmg = cur_vmg else: success = True - + # If we are trying to find the max VMG to the left of the target and we're pointing on the correct side of the target: elif goal.direction is -1 and self.heading < self.direct_heading()+2.5: - - cur_vmg = self.vmg(); - + + cur_vmg = self.vmg() + # Keep turning further left until we find the max vmg if cur_vmg > self.max_vmg: self.target_heading_pub.publish(self.heading-2) self.max_vmg = cur_vmg else: success = True - + # If the action was preempted... if self._as.is_preempt_requested(): rospy.loginfo('Max VMG Action: Preempted') @@ -119,10 +119,10 @@ def execute(self, goal): self._feedback.status = "Preempted" self._as.set_preempted() preempted = True - + # Sleep self.rate.sleep() - + # Once we're outside of the loop, if we succeeded... if success: self._result.success = success diff --git a/boat_nav/src/navigator_node.py b/boat_nav/src/navigator_node.py index 91e7855..bbfd0da 100755 --- a/boat_nav/src/navigator_node.py +++ b/boat_nav/src/navigator_node.py @@ -83,7 +83,7 @@ def boat_state_callback(new_state): ## Callback for setting the apparent wind heading when the `/anemometer` topic is updated # -# @param new_heading The new anemometer reading, 180 is directly infront of boat increasing CCW +# @param new_heading The new anemometer reading, 180 is directly in front of boat increasing CCW # def anemometer_callback(new_heading): global ane_reading @@ -236,7 +236,7 @@ def calc_cur_tack_max_vmg(wind_coming): vmg_heading = direct_heading # If none of the above, the best heading will be on the opposite tack - # however it is extremely unfavorable because clearly no best heading lies on this tack + # however it is extremely unfavourable because clearly no best heading lies on this tack else: theoretic_boat_speed = 0 vmg_heading = apparent_wind_heading @@ -381,7 +381,7 @@ def awa_algorithm(): target_heading = global_vmg_heading heading_pub.publish(Float32(target_heading)) - #Final leg of the course, traveling on an optimal vmg course, time to get to layline. Second condition to make sure this doesn't run if we are already on the layline + # Final leg of the course, traveling on an optimal vmg course, time to get to layline. Second condition to make sure this doesn't run if we are already on the layline # TODO: Tolerance correctly, make sure this isnt called on a downwind cuz we mistolerance it. Shouldnt be because target_heading will constantly be updating to be equal to global_vmg_heading above # TODO: Ensure this is only called once per run. Currently if we have a short laylineaction, it can get repeated elif per_course_left <= 40 and not on_layline(wind_coming, 1.0) and boat_speed >= min_tacking_speed: From ae9c7052417db3af804e69eafb45b1fc0bc01bcd Mon Sep 17 00:00:00 2001 From: Matthew Reynolds Date: Sun, 2 Dec 2018 23:29:49 -0500 Subject: [PATCH 16/17] Use utilities in boat_vision package --- boat_vision/src/stereo_vision_node.py | 155 ++++++++++++-------------- boat_vision/src/vision_parser_node.py | 23 ++-- 2 files changed, 82 insertions(+), 96 deletions(-) diff --git a/boat_vision/src/stereo_vision_node.py b/boat_vision/src/stereo_vision_node.py index 511e4ca..c76c522 100755 --- a/boat_vision/src/stereo_vision_node.py +++ b/boat_vision/src/stereo_vision_node.py @@ -1,21 +1,21 @@ #!/usr/bin/env python import rospy -import rospkg import math import cv2 -import numpy as np +from numpy import array as nparray import threading from boat_msgs.msg import Point, PolarPoint, VisionTarget from sensor_msgs.msg import Image from cv_bridge import CvBridge, CvBridgeError - +from boat_utilities.angles import cosd, sind + # Assume that both cams are facing forwards, 0deg -CAM_SPACING = 0.30; # 0.3m -#CAM_F_LEN = 4.0; # 4.0mm ** Not needed because FOV was specified -CAM_FOV_HORIZ = 67; # Degrees +CAM_SPACING = 0.30 # 0.3m +#CAM_F_LEN = 4.0 # 4.0mm ** Not needed because FOV was specified +CAM_FOV_HORIZ = 67 # Degrees #CAM_FOV_VERT = CAM_FOV_HORIZ * 0.75 # Not used and tbh probably not accurate -CAM_HEIGHT = 720; -CAM_WIDTH = 1280; +CAM_HEIGHT = 720 +CAM_WIDTH = 1280 left_img = () right_img = () @@ -24,12 +24,6 @@ vision_pub = rospy.Publisher('vision_target', VisionTarget, queue_size=10) debug = rospy.get_param("stereo_vision/debug") -def sind(angle): - return math.sin(math.radians(angle)) - -def cosd(angle): - return math.cos(math.radians(angle)) - # angle is in CCW degrees from x+ axis def get_x_displacement(x_init, rad, angle): return cosd(angle) * rad + x_init @@ -39,52 +33,52 @@ def get_y_displacement(y_init, rad, angle): return sind(angle) * rad + y_init def process_cams(left, right, spacing): - + target = PolarPoint() - + # Output the request #rospy.loginfo(rospy.get_caller_id() + " Request: left=%.2f,%.2f, right=%.2f,%.2f", left.x, left.y, right.x, right.y) - + # Calculate the average y value and the angle to that y val #avg_y = (left.y + right.y)/2.0 #vert_angle = ((avg_y / CAM_HEIGHT) * 2 - 1) * CAM_FOV_VERT / 2 - + # Determine the angle to the object in each camera, from -30 to 30deg (0deg is straight ahead) - angle1 = ((left.x / CAM_WIDTH) * 2 - 1) * CAM_FOV_HORIZ / 2; # -30 to 30deg - angle2 = ((right.x / CAM_WIDTH) * 2 - 1) * CAM_FOV_HORIZ / 2; # -30 to 30deg - + angle1 = ((left.x / CAM_WIDTH) * 2 - 1) * CAM_FOV_HORIZ / 2 # -30 to 30deg + angle2 = ((right.x / CAM_WIDTH) * 2 - 1) * CAM_FOV_HORIZ / 2 # -30 to 30deg + # If the lines are parallel or diverging, there is no solution if (angle1 <= angle2 or angle2 >= angle1): rospy.logwarn("Invalid input, angles are diverging") return False - + # Convert the angles from 0deg = straight to the internal angle of the triangle bound by the rays angle1 = -angle1 + 90 angle2 += 90 - + # Determine the third angle through simple geometrical relationships angle3 = 180 - angle1 - angle2 - + # Determine the length from each camera to the buoy, using the sine law dist1 = spacing / sind(angle3) * sind(angle1) dist2 = spacing / sind(angle3) * sind(angle2) - + # Determine the coordinates of the buoy relative to the center of the two cams. # x is horiz position, y is depth, in meters x_final = get_x_displacement(-spacing/2.0, dist1, angle1) y_final = get_y_displacement(0, dist1, angle1) - + # Perhaps useful if the cameras are tilted up or down? #depth_2d = sqrt(x_final * x_final + y_final * y_final) #z_final = depth_2d / sinDeg(90-vert_angle) * sinDeg(vert_angle) - + # Use simple trig to determine the distance and angle from the center of the # two cameras to the buoy's coordinates. - + target.dist = math.sqrt(x_final * x_final + y_final * y_final) #res.buoy.dist = sqrt(x_final * x_final + y_final * y_final + z_final * z_final) target.heading = math.degrees(math.atan2(y_final, x_final)) - + #rospy.loginfo(rospy.get_caller_id() + "Returning response: [dist:%.2f, heading:%.2f]", res.target.dist, res.target.heading) return target @@ -92,95 +86,95 @@ def process_cams(left, right, spacing): def get_buoy_coords(img, pub): p = Point() found = False - + #Filter filtered = cv2.bilateralFilter(img, 5, 500, 500) - + # HSV Threshold - hsv_img = cv2.cvtColor(filtered, cv2.COLOR_BGR2HSV); - #threshold_img = cv2.inRange(hsv_img, np.array([0, 165, 130]), np.array([15, 255, 255])); # Good, old - #threshold_img = cv2.inRange(hsv_img, np.array([0, 125, 100]), np.array([25, 255, 255])); # Wide - threshold_img = cv2.inRange(hsv_img, np.array([0, 165, 200]), np.array([14, 245, 255])); # Overcast - + hsv_img = cv2.cvtColor(filtered, cv2.COLOR_BGR2HSV) + #threshold_img = cv2.inRange(hsv_img, nparray([0, 165, 130]), nparray([15, 255, 255])) # Good, old + #threshold_img = cv2.inRange(hsv_img, nparray([0, 125, 100]), nparray([25, 255, 255])) # Wide + threshold_img = cv2.inRange(hsv_img, nparray([0, 165, 200]), nparray([14, 245, 255])) # Overcast + # Blur - threshold_img = cv2.medianBlur(threshold_img, 3); - threshold_img = cv2.GaussianBlur(threshold_img, (5,5), 2, 2); - + threshold_img = cv2.medianBlur(threshold_img, 3) + threshold_img = cv2.GaussianBlur(threshold_img, (5,5), 2, 2) + # Get center via Hough Circles circles = cv2.HoughCircles(threshold_img, cv2.HOUGH_GRADIENT, 1, 2, param1=200,param2=15,minRadius=0,maxRadius=0) - + if circles is not None: - + for c in circles[0]: center = (c[0], c[1]) radius = c[2] - + if debug: cv2.circle(filtered, center, radius, (0,255,0), 1, 1) - + p.x += float(c[0])/len(circles[0]) p.y += float(c[1])/len(circles[0]) found = True - + # Get center via contours edge_detected_image = cv2.convertScaleAbs(cv2.Canny(threshold_img, 75, 200)) _,contours,_ = cv2.findContours(edge_detected_image, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - + maxArea = 0 bestContour = () if contours is not None: for c in contours: - + # Approximate as polygon approx = cv2.approxPolyDP(c, 0.012 * cv2.arcLength(c, True), True) - + # Area area = cv2.contourArea(c) - + # Find the largest semi-circular ( if len(approx) > 3 and area > maxArea: maxArea = area bestContour = c - + # Average the two measurements if bestContour is not (): m = cv2.moments(bestContour) - + if found: p.x = (p.x + m['m10']/m['m00'])/2 p.y = (p.y + m['m01']/m['m00'])/2 else: p.x = m['m10']/m['m00'] p.y = m['m01']/m['m00'] - + found = True - + # Draw stuff for debug if debug: if bestContour is not (): cv2.drawContours(filtered, [bestContour], 0, (255,255,255), 1) - + c = (int(p.x), int(p.y)) - cv2.drawMarker(filtered, c, (0,255,0), cv2.MARKER_CROSS, 150, 3); - cv2.drawMarker(filtered, c, (0,0,255), cv2.MARKER_CROSS, 125, 3); - cv2.drawMarker(filtered, c, (0,255,0), cv2.MARKER_CROSS, 100, 3); - cv2.drawMarker(filtered, c, (0,0,255), cv2.MARKER_CROSS, 75, 3); - cv2.drawMarker(filtered, c, (0,255,0), cv2.MARKER_CROSS, 50, 3); - cv2.drawMarker(filtered, c, (0,0,255), cv2.MARKER_CROSS, 25, 3); - cv2.drawMarker(filtered, c, (0,255,0), cv2.MARKER_CROSS, 20, 3); - cv2.drawMarker(filtered, c, (0,0,255), cv2.MARKER_CROSS, 15, 3); - cv2.drawMarker(filtered, c, (0,255,0), cv2.MARKER_CROSS, 10, 3); - cv2.drawMarker(filtered, c, (0,0,255), cv2.MARKER_CROSS, 5, 3); - + cv2.drawMarker(filtered, c, (0,255,0), cv2.MARKER_CROSS, 150, 3) + cv2.drawMarker(filtered, c, (0,0,255), cv2.MARKER_CROSS, 125, 3) + cv2.drawMarker(filtered, c, (0,255,0), cv2.MARKER_CROSS, 100, 3) + cv2.drawMarker(filtered, c, (0,0,255), cv2.MARKER_CROSS, 75, 3) + cv2.drawMarker(filtered, c, (0,255,0), cv2.MARKER_CROSS, 50, 3) + cv2.drawMarker(filtered, c, (0,0,255), cv2.MARKER_CROSS, 25, 3) + cv2.drawMarker(filtered, c, (0,255,0), cv2.MARKER_CROSS, 20, 3) + cv2.drawMarker(filtered, c, (0,0,255), cv2.MARKER_CROSS, 15, 3) + cv2.drawMarker(filtered, c, (0,255,0), cv2.MARKER_CROSS, 10, 3) + cv2.drawMarker(filtered, c, (0,0,255), cv2.MARKER_CROSS, 5, 3) + image_message = CvBridge().cv2_to_imgmsg(filtered, encoding="bgr8") pub.publish(image_message) - + if found: return p return False # This thread simply grabs the most recent frame as fast as possible from the cameras. -# This in combination with a synchronized retrieve() procides the most in-sync images. +# This in combination with a synchronized retrieve() provides the most in-sync images. class cam_thread(threading.Thread): def __init__(self, cam): @@ -188,50 +182,47 @@ def __init__(self, cam): self.cam = cam self.running = True self.has = False - + def stop(self): self.running = False - + def run(self): - + rate = rospy.Rate(50) #50 hz while self.running: self.has = self.cam.grab() rate.sleep() - + self.cam.release def initialize_node(): """Initialize the node. Setup the node handle and subscribers for ROS.""" rospy.init_node('stereo_vision') - - r = rospkg.RosPack() - path = r.get_path('boat_vision') + "/debug/18" - + #left = cam_thread(cv2.VideoCapture(1)) #right = cam_thread(cv2.VideoCapture(2)) left = cam_thread(cv2.VideoCapture(rospy.get_param("/stereo_vision/left_cam"))) right = cam_thread(cv2.VideoCapture(rospy.get_param("/stereo_vision/right_cam"))) - + left.start() right.start() - + # Constantly process cams rate = rospy.Rate(rospy.get_param("/stereo_vision/rate")) while not rospy.is_shutdown(): if not left.has or not right.has: continue - + _, left_img = left.cam.retrieve() _, right_img = right.cam.retrieve() - + # Make sure the resolution is accurate global CAM_WIDTH global CAM_HEIGHT CAM_WIDTH = left.cam.get(cv2.CAP_PROP_FRAME_WIDTH) CAM_HEIGHT = left.cam.get(cv2.CAP_PROP_FRAME_HEIGHT) - + points = map(get_buoy_coords, [left_img, right_img], [left_pub, right_pub]) if points[0] is not False and points[1] is not False: polar = process_cams(points[0], points[1], CAM_SPACING) @@ -249,13 +240,13 @@ def initialize_node(): vision_pub.publish(msg) rate.sleep() - + left.stop() right.stop() - + left.join() right.join() - cv2.destroyAllWindows() + cv2.destroyAllWindows() if __name__ == '__main__': diff --git a/boat_vision/src/vision_parser_node.py b/boat_vision/src/vision_parser_node.py index 6313bfd..8f90a26 100755 --- a/boat_vision/src/vision_parser_node.py +++ b/boat_vision/src/vision_parser_node.py @@ -4,6 +4,7 @@ from boat_msgs.msg import Point, PointArray, VisionTarget from boat_msgs.srv import ConvertPoint from std_msgs.msg import Float32 +from boat_utilities.angles import cosd, sind pub = rospy.Publisher('vision', PointArray, queue_size=10) to_gps = rospy.ServiceProxy('lps_to_gps', ConvertPoint) @@ -19,45 +20,39 @@ def vision_callback(data): def position_callback(data): global cur_pos - cur_pos = data + cur_pos = data def compass_callback(data): global compass compass = data.data -def sind(angle): - return math.sin(math.radians(angle)) - -def cosd(angle): - return math.cos(math.radians(angle)) - def initialize_node(): rospy.init_node('vision_parser') - + rospy.Subscriber('vision_target', VisionTarget, vision_callback, queue_size=1) rospy.Subscriber('lps', Point, position_callback, queue_size=1) # Only want it to receive the most recent position rospy.Subscriber('compass', Float32, compass_callback, queue_size=1) - + rospy.wait_for_service("lps_to_gps") - + # Read vision topic and boat_pos and aggregate data into waypoints #TODO: Maybe move to vision callback?? rate = rospy.Rate(10) while not rospy.is_shutdown(): points.points = [] - + if target.has: # Calculate the vision target's pos relative to the boat point = cur_pos cur_pos.x += target.pt.dist*cosd(compass-target.pt.heading) cur_pos.y += target.pt.dist*sind(compass-target.pt.heading) points.points.append(to_gps(cur_pos).pt) - + # Publish it pub.publish(points) - + rate.sleep() - + if __name__ == '__main__': try: From 47bb6f78b7c54abbda02f481e2233b452f594f49 Mon Sep 17 00:00:00 2001 From: Matthew Reynolds Date: Mon, 3 Dec 2018 19:19:50 -0500 Subject: [PATCH 17/17] Improve inclusive/exclusive angle comparisons --- boat_utilities/src/boat_utilities/angles.py | 53 ++++++++++++++++++--- 1 file changed, 47 insertions(+), 6 deletions(-) diff --git a/boat_utilities/src/boat_utilities/angles.py b/boat_utilities/src/boat_utilities/angles.py index 30f509f..67f7c0d 100644 --- a/boat_utilities/src/boat_utilities/angles.py +++ b/boat_utilities/src/boat_utilities/angles.py @@ -1,10 +1,20 @@ #!/usr/bin/env python +from enum import Enum + # NOTE: All operations in this module use degrees CCW from East, as in a unit circle # TODO: Switch to rads __UNITS = 360.0 +class ComparisonResult(Enum): + """The result of comparing two angles using `get_side()`.""" + EQUAL=0 + LEFT=1 + RIGHT=2 + OPPOSITE=3 + + def cosd(angle): """Calculate the cosine of the following angle, in degrees.""" from math import cos, radians @@ -53,13 +63,14 @@ def opposite_signed(angle): return normalize_signed(angle + __UNITS/2) -def is_within_bounds(val, lower, upper): +def is_within_bounds(val, lower, upper, inclusive=False): """Determine whether lower < val < upper. Args: val The value to check lower The lower bound upper The upper bound + inclusive Whether to include exact equality with the limits (Default: False) Returns: `True` if the value is between the specified bounds """ @@ -72,15 +83,21 @@ def is_within_bounds(val, lower, upper): if lower > upper: lower -= __UNITS - return lower <= val and val <= upper + if inclusive: + return lower <= val and val <= upper + else: + return lower < val < upper -def is_on_right(angle, ref): +def is_on_right(angle, ref, inclusive=False): """Determine whether the specified angle is within the 180 degrees to the right of the reference. + Note: It is recommended to use `get_side()` in most cases instead. + Args: angle The angle to check ref The reference angle to compare against + inclusive Whether to include exact equality with the reference and opposite angles (Default: False) Returns: `True` if `angle` is within 180 degrees to the right of `ref` """ @@ -89,15 +106,18 @@ def is_on_right(angle, ref): ref_opp = normalize(ref + __UNITS/2) # We check if ref_opp < angle < ref - return is_within_bounds(angle, ref_opp, ref) + return is_within_bounds(angle, ref_opp, ref, inclusive) -def is_on_left(angle, ref): +def is_on_left(angle, ref, inclusive=False): """Determine whether the specified angle is within the 180 degrees to the left of the reference. + Note: It is recommended to use `get_side()` in most cases instead. + Args: angle The angle to check ref The reference angle to compare against + inclusive Whether to include exact equality with the reference and opposite angles (Default: False) Returns: `True` if `angle` is within 180 degrees to the left of `ref` """ @@ -106,4 +126,25 @@ def is_on_left(angle, ref): ref_opp = normalize(ref + __UNITS/2) # We check if ref < angle < ref_opp - return is_within_bounds(angle, ref, ref_opp) + return is_within_bounds(angle, ref, ref_opp, inclusive) + + +def get_side(angle, ref): + """Determine on which side of a reference the specified angle lies. + + Args: + angle The angle to check + ref The reference angle to compare against + Returns: + The corresponding `ComparisonResult` enum value + """ + if angle == ref: + return ComparisonResult.EQUAL + + if is_on_left(angle, ref, False): + return ComparisonResult.LEFT + + if is_on_right(angle, ref, False): + return ComparisonResult.RIGHT + + return ComparisonResult.OPPOSITE