diff --git a/zju-notes/UPDATE/8.17/heading_planning_laylines.py b/zju-notes/UPDATE/8.17/heading_planning_laylines.py new file mode 100644 index 00000000..9ba31710 --- /dev/null +++ b/zju-notes/UPDATE/8.17/heading_planning_laylines.py @@ -0,0 +1,151 @@ +from collections import deque +import LatLon as ll +import math +from shapely.geometry import Point, Polygon +import rospy +from .navigation import angleSum +from .taskbase import TaskBase +from .heading_planning import TackVoting + +# For calculations, lay lines don't extend to infinity. +# This is in m; 10km should be plenty for our purposes. +LAYLINE_EXTENT = 10000 + +class HeadingPlan(TaskBase): + def __init__(self, nav, + waypoint=ll.LatLon(50.742810, 1.014469), # somewhere in the solent + target_radius=2, tack_voting_radius=15, waypoint_id=None, + ): + """Sail towards a waypoint. + + *nav* is a sailing_robot.navigation.Navigation instance. + + *waypoint* is a LatLon object telling us where to go. + + *target_radius* is how close we need to get to the waypoint, in metres. + + *tack_voting_radius* is the distance within which we use tack voting, to + avoid too frequent tacks close to the waypoint. + """ + self.nav = nav + self.waypoint = waypoint + x, y = self.nav.latlon_to_utm(waypoint.lat.decimal_degree, waypoint.lon.decimal_degree) + self.waypoint_xy = Point(x, y) + self.target_area = self.waypoint_xy.buffer(target_radius) + self.sailing_state = 'normal' # sailing state can be 'normal','switch_to_port_tack' or 'switch_to_stbd_tack' + self.tack_voting = TackVoting(50, 35) + self.tack_voting_radius = tack_voting_radius + self.waypoint_id = waypoint_id + + def start(self): + pass + + def check_end_condition(self): + """Are we there yet?""" + return self.nav.position_xy.within(self.target_area) + + debug_topics = [ + ('dbg_heading_to_waypoint', 'Float32'), + ('dbg_distance_to_waypoint', 'Float32'), + ('dbg_goal_wind_angle', 'Float32'), + ('dbg_latest_waypoint_id', 'String'), + ] + + def calculate_state_and_goal(self): + """Work out what we want the boat to do + """ + dwp, hwp = self.nav.distance_and_heading(self.waypoint_xy) + self.debug_pub('dbg_distance_to_waypoint', dwp) + self.debug_pub('dbg_heading_to_waypoint', hwp) + self.debug_pub('dbg_latest_waypoint_id', self.waypoint_id) + + boat_wind_angle = self.nav.angle_to_wind() + wp_wind_angle = self.nav.heading_to_wind_angle(hwp) + rospy.logwarn('wp_wind_angle = ' + str(wp_wind_angle)) + if self.sailing_state != 'normal': + # A tack/jibe is in progress + if self.sailing_state == 'switch_to_port_tack': + goal_angle = self.nav.beating_angle + continue_tack = boat_wind_angle < goal_angle or boat_wind_angle > 120 + else: # 'switch_to_stbd_tack' + goal_angle = -self.nav.beating_angle + continue_tack = boat_wind_angle > goal_angle or boat_wind_angle < -120 + + if (wp_wind_angle % 360) > 60 and (wp_wind_angle % 360) < 300: + goal_wind_angle = wp_wind_angle + self.debug_pub('dbg_goal_wind_angle', goal_wind_angle) + state = 'normal' + return state, self.nav.wind_angle_to_heading(goal_wind_angle) + + if continue_tack: + self.debug_pub('dbg_goal_wind_angle', goal_angle) + return self.sailing_state, self.nav.wind_angle_to_heading(goal_angle) + else: + # Tack completed + self.log('info', 'Finished tack (%s)', self.sailing_state) + self.tack_voting.reset(boat_wind_angle > 0) + self.sailing_state = 'normal' + + on_port_tack = boat_wind_angle > 0 + + + # Detect if the waypoint is downwind, if so head directly to it + if (wp_wind_angle % 360) > 60 and (wp_wind_angle % 360) < 300: + goal_wind_angle = wp_wind_angle + self.debug_pub('dbg_goal_wind_angle', goal_wind_angle) + state = 'normal' + return state, self.nav.wind_angle_to_heading(goal_wind_angle) + + + tack_now = False + if wp_wind_angle * boat_wind_angle > 0: + # These two have the same sign, so we're on the better tack already + self.tack_voting.vote(on_port_tack) + elif self.nav.position_xy.within(self.lay_triangle()): + # We're between the laylines; stick to our current tack for now + self.tack_voting.vote(on_port_tack) + else: + tack_now = True + self.tack_voting.vote(not on_port_tack) + + if dwp < self.tack_voting_radius: + # Close to the waypoint, use tack voting so we're not constantly + # tacking. + tack_now = self.tack_voting.tack_now(on_port_tack) + + if tack_now: + # Ready about! + if on_port_tack: + state = 'switch_to_stbd_tack' + goal_wind_angle = -self.nav.beating_angle + else: + state = 'switch_to_port_tack' + goal_wind_angle = self.nav.beating_angle + self.sailing_state = state + self.log('info', 'Starting tack/jibe (%s)', state) + else: + # Stay on our current tack + if on_port_tack: + goal_wind_angle = max(wp_wind_angle, self.nav.beating_angle) + else: + goal_wind_angle = min(wp_wind_angle, -self.nav.beating_angle) + state = 'normal' + + self.debug_pub('dbg_goal_wind_angle', goal_wind_angle) + return state, self.nav.wind_angle_to_heading(goal_wind_angle) + + def lay_triangle(self): + """Calculate the lay lines for the current waypoint. + + This returns a shapely Polygon with the two lines extended to + LAYLINE_EXTENT (10km). + """ + downwind = angleSum(self.nav.absolute_wind_direction(), 180) + x0, y0 = self.waypoint_xy.x, self.waypoint_xy.y + l1 = math.radians(angleSum(downwind, -self.nav.beating_angle)) + x1 = x0 + (LAYLINE_EXTENT * math.sin(l1)) + y1 = y0 + (LAYLINE_EXTENT * math.cos(l1)) + l2 = math.radians(angleSum(downwind, self.nav.beating_angle)) + x2 = x0 + (LAYLINE_EXTENT * math.sin(l2)) + y2 = y0 + (LAYLINE_EXTENT * math.cos(l2)) + return Polygon([(x0, y0), (x1, y1), (x2, y2)]) diff --git a/zju-notes/UPDATE/Read Me.md b/zju-notes/UPDATE/Read Me.md new file mode 100644 index 00000000..8cf40b55 --- /dev/null +++ b/zju-notes/UPDATE/Read Me.md @@ -0,0 +1,49 @@ +# Read Me + +主要记录每天对代码修改的部分 + +## 8.17 + +1. tasks节点中顺风判定的wp范围变大 + + 修改文件：heading_planning_laylines.py，第93行 + + ```python + if (wp_wind_angle % 360) > 60 and (wp_wind_angle % 360) < 300: + ``` + + + +2. 增加功能：如果在tack过程中发现了需要壁障操作，直接跳出tack + + 修改文件：heading_planning_laylines.py，第74-78行 + + 说明：在对continue_tack判断前增加了对wp是否是顺风方向目标的判定 + + ```python + if self.sailing_state != 'normal': + # A tack/jibe is in progress + if self.sailing_state == 'switch_to_port_tack': + goal_angle = self.nav.beating_angle + continue_tack = boat_wind_angle < goal_angle or boat_wind_angle > 120 + else: # 'switch_to_stbd_tack' + goal_angle = -self.nav.beating_angle + continue_tack = boat_wind_angle > goal_angle or boat_wind_angle < -120 + + if (wp_wind_angle % 360) > 60 and (wp_wind_angle % 360) < 300: + goal_wind_angle = wp_wind_angle + self.debug_pub('dbg_goal_wind_angle', goal_wind_angle) + state = 'normal' + return state, self.nav.wind_angle_to_heading(goal_wind_angle) + + if continue_tack: + self.debug_pub('dbg_goal_wind_angle', goal_angle) + return self.sailing_state, self.nav.wind_angle_to_heading(goal_angle) + else: + # Tack completed + self.log('info', 'Finished tack (%s)', self.sailing_state) + self.tack_voting.reset(boat_wind_angle > 0) + self.sailing_state = 'normal' + ``` + + \ No newline at end of file