Utility module

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

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

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.

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,15 +179,15 @@ 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):
 			out.append(self._make_waypoint(angle, radius))
 			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)
-	
+
 

boat_utilities/src/boat_utilities/__init__.py

@@ -4,3 +4,4 @@
 
 import points
 import angles
+import units