Utility module

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.
 

boat_2018/package.xml

@@ -19,6 +19,7 @@
   <exec_depend>boat_msgs</exec_depend>
   <exec_depend>boat_nav</exec_depend>
   <exec_depend>boat_vision</exec_depend>
+  <exec_depend>boat_utilities</exec_depend>
 
   <export>
     <metapackage/>

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

boat_interfaces/package.xml

@@ -15,6 +15,7 @@
   <depend>actionlib</depend>
   <depend>std_msgs</depend>
   <depend>boat_msgs</depend>
+  <depend>boat_utilities</depend>
 
   <!-- Launch file dependencies -->
   <exec_depend>rosserial_python</exec_depend>

boat_interfaces/src/pid.py

@@ -1,33 +1,24 @@
 #!/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
 kd = 0.0
 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 

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')

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
@@ -83,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)
 
@@ -95,9 +71,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:
@@ -109,12 +87,12 @@ 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:
 			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:
@@ -126,19 +104,18 @@ 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:
 					self.state.minor = BoatState.MIN_PLANNING
 				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()
@@ -151,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__':

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

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

boat_nav/package.xml

@@ -15,6 +15,7 @@
   <depend>actionlib</depend>
   <depend>std_msgs</depend>
   <depend>boat_msgs</depend>
+  <depend>boat_utilities</depend>
   <depend>tf</depend>
 
   <!-- Launch file dependencies -->

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