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_utilities
+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.
 
@@ -52,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)
+If you want to download the python phidgets API [See here](https://www.phidgets.com/docs/Language_-_Python#Install_Phidget_Python_module_for_Linux)

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
@@ -29,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
@@ -55,20 +56,20 @@ 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(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
 
 
 # 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)
 
@@ -79,21 +80,21 @@ 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(conform_angle(target_heading.data))
+	pid_setpoint = Float32(angles.normalize_signed(target_heading.data))
 	pid_setpoint_pub.publish(pid_setpoint)
 
 
@@ -103,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))
 
@@ -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 -->