def init(node):
global mission_goal
sensors.init(node)
# This reads the list of waypoint from a file called mission_file.csv in the folder olinoboat
waypoints = read_mission.read_mission_csv()
mission_goal = MissionGoal(sensors, waypoints, node)
from think_behaviors import *
import think_behaviors
from hardware import servos, sensors
import rospy
current_node = rospy.init_node("think_v1",anonymous=True)
sensors.init(current_node)
servos.init(current_node)
rospy.Rate(10)
'''
servos:
sail - servos.sail.set_position(angle)
rudder - servos.sail.set_position(angle)
sensors:
compass - sensors.compass.angle
gps - sensors.gps.current_location
wind_angle - sensors.wind_angle.angle
leak_detector - sensors.leak_detector.leak_callback = callback_function
sensors.leak_detector.leak_detected
'''
loop_count = 0
next_tack = 100
goal = [76.2,42.1]
behaviors = think_behaviors.__all__
what_to_do = []
while not rospy.is_shutdown():
# Imports necessary sailbot code
from hardware import sensors
from hardware import servos
def sail_update_on_wind_direction_change():
wind_angle = sensors.wind_angle.angle
rospy.loginfo("fast_sail_angle.py: Wind sensor sent %f" % (wind_angle))
# This command takes an angle and converts it to 0-180 range
# necessary because it doesn't matter which side the wind is coming from for sail setting - wind 90 deg. off the starboard OR port bow both result in the same sail setting
wind_angle = abs((wind_angle + 180) % 360 - 180)
points_of_sail = [0, 45, 60, 90, 135, 180]
sail_points = [0, 0, 15, 40, 60, 80]
sail_angle = pl.interp(wind_angle, points_of_sail,sail_points)
rospy.loginfo("fast_sail_angle.py: sail angle should be:" + str(sail_angle))
# Sets the sail servo
servos.sail.set_position(sail_angle)
rospy.sleep(10)
print 'init maintaing_fast_sail_position node'
current_node = rospy.init_node("maintain_fast_sail_position",anonymous=True)
sensors.init(current_node)
servos.init(current_node)
# This line calls sail_update_on_wind_direction_change() every time the wind direction changes
sensors.wind_angle.set_callback(sail_update_on_wind_direction_change)
rospy.spin()
def init(think_node):
sensors.init(think_node)
sensors.wind_angle.set_callback(go_fast_cb)
def init(think_node):
sensors.init(think_node)
sensors.gps.set_callback(go_short_cb)
mission_catcher.init(think_node)
