Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Utility module #21

Merged
merged 17 commits into from
Dec 4, 2018
Merged
Show file tree
Hide file tree
Changes from 11 commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 4 additions & 1 deletion README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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.

Expand All @@ -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)
1 change: 1 addition & 0 deletions boat_2018/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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/>
Expand Down
3 changes: 2 additions & 1 deletion boat_interfaces/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
1 change: 1 addition & 0 deletions boat_interfaces/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
17 changes: 4 additions & 13 deletions boat_interfaces/src/pid.py
Original file line number Diff line number Diff line change
@@ -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
Expand Down
39 changes: 18 additions & 21 deletions boat_interfaces/src/rudder_node.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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)

Expand All @@ -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)


Expand All @@ -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))

Expand All @@ -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')
Expand Down
50 changes: 14 additions & 36 deletions boat_interfaces/src/tacking_action.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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:
matthew-reynolds marked this conversation as resolved.
Show resolved Hide resolved
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)

Expand All @@ -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:
Expand All @@ -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:
Expand All @@ -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()
Expand All @@ -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__':
Expand Down
3 changes: 2 additions & 1 deletion boat_msgs/action/Tacking.action
Original file line number Diff line number Diff line change
@@ -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
Expand Down
3 changes: 2 additions & 1 deletion boat_nav/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
1 change: 1 addition & 0 deletions boat_nav/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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 -->
Expand Down
Loading