#! /usr/bin/env python

"""Implements navigation for the Robo Magellan rover."""

# A note about speeds for movement and turning when approaching and
# driving away from cones. The calculated speed and turning are
# fractions from 0 to 1 which are multipled by either the nominal
# speed and max turning, resp., when in AUTO mode, or by the PWM
# ranges when in MANUAL mode.
#
# Minimum and maximum speeds are also fractions between 0 and 1,
# and are used to constrain the calculated fractional speed before
# multiplying by the nominal speed or PWM range.
#
# Minimum and maximum turnings rates are not used to constrain
# the turning, since the rover won't stall because of insufficient
# turning.
#
# Encoding of cone altitudes
#
# The cone altitude is encoded into 4 digits as follows:
#   Nxxx: 1=cone, 2=last cone, 0=not a cone
#   xNxx's: percentage of normal cruise speed 0=100%, 1=10%, 2=20%, etc.
#   xxNN's: min speed toward cone as pct of nominal, 00=0%, 23=23%, etc.
#
# In GUIDED mode, max speed is ~normal_speed, in m/s.
# In MANUAL mode, max speed is maximum throtle PWM.
# When backing away from cone, nominal speed is maximum reverse throttle PWM.

import time
from math import pi, tan, atan2, sqrt
import rospy
import tf
from tf.transformations import quaternion_from_euler, euler_from_quaternion
import tf2_ros
from geodesy.utm import fromLatLong
import std_msgs.msg as std_msgs
from std_msgs.msg import Bool
from geometry_msgs.msg import Pose, PoseStamped, Twist, TwistStamped
from mavros_msgs.msg import State, OverrideRCIn, ParamValue, WaypointList
from mavros_msgs.srv import ParamGet, ParamSet, SetMode, CommandBool
from mavros_msgs.srv import WaypointSetCurrent
from robo_magellan.constants import Topics, ExecCmds
from robo_magellan.msg import location_msgs as location_data
from robo_magellan.msg import LocalWaypointList, LocalWaypoint


class Modes:
    MANUAL = 'MANUAL'
    HOLD = 'HOLD'
    AUTO = 'AUTO'
    GUIDED = 'GUIDED'
    RTL = 'RTL'

class MavrosCmds:
    MAV_CMD_DO_CHANGE_SPEED = 178
    MAV_CMD_DO_SET_HOME = 179


class RCChannels:
    SPEED = 2
    TURNING = 0


class NavigatorState:

    def __init__(self, name):
        self.name = name

    def get_name(self):
        return self.name


class States:
    WAITING_FOR_START = NavigatorState('WAITING_FOR_START')
    FOLLOWING_WAYPOINTS = NavigatorState('FOLLOWING_WAYPOINTS')
    CIRCLING_BACK = NavigatorState('CIRCLING_BACK')
    CIRCLING_FORWARD = NavigatorState('CIRCLING_FORWARD')
    DRIVING_TO_CONE = NavigatorState('DRIVING_TO_CONE')
    ESCAPING_CONE = NavigatorState('ESCAPING_CONE')
    FINISHED = NavigatorState('FINISHED')
    FAILED = NavigatorState('FAILED')
    # A special state for ignoring messages while delaying for a bit.
    LIMBO = NavigatorState('LIMBO')


class Navigator:

    def run(self):
        self.state = None
        self.last_wp_speed = None
        self.cone_wp_index = None
        self.cone_is_close = False
        self.cone_pose = None
        self.waypoint_list = WaypointList()
        self.waypoint_list.waypoints = []
        self.map_waypoint_list = None
        self.target_heading = None
        self.robot_pose = PoseStamped()
        self.cone_lost_count = 0

        rospy.init_node('navigator')

        rate = rospy.Rate(rospy.get_param('~rate', 10))

        self.normal_speed = rospy.get_param('~normal_speed', 2.0)
        self.min_speed = rospy.get_param('~min_speed', 0.1)
        self.max_turning = rospy.get_param('~max_turning', 5.0)

        self.throttle_neutral = rospy.get_param(
            '~throttle_neutral', 1500)
        self.throttle_reverse_min = rospy.get_param(
            '~throttle_reverse_min', 1500)
        self.throttle_reverse_max = rospy.get_param(
            '~throttle_reverse_max', 1000)
        self.throttle_fwd_min = rospy.get_param(
            '~throttle_fwd_min', 1500)
        self.throttle_fwd_max = rospy.get_param(
            '~throttle_fwd_max', 2000)

        self.steering_neutral = rospy.get_param(
            '~steering_neutral', 1500)
        self.steering_left_max = rospy.get_param(
            '~steering_left_max', 1000)
        self.steering_right_max = rospy.get_param(
            '~steering_right_max', 2000)

        self.cone_normal_min_area = rospy.get_param('~cone_normal_min_area',
                                                    100)
        self.cone_recovery_min_area = rospy.get_param(
            '~cone_recovery_min_area', 400)

        self.cone_close_distance = rospy.get_param('~cone_close_distance', 2.0)

        self.cone_approach_use_throttle = rospy.get_param(
            '~cone_approach_use_throttle', False)
        self.cone_approach_k_speed = rospy.get_param(
            '~cone_approach_k_speed', 0.25)
        self.cone_approach_k_turning = rospy.get_param(
            '~cone_approach_k_turning', 0.5)
        self.cone_timeout = rospy.get_param(
            '~cone_timeout_seconds', 60)

        self.escape_strategy = rospy.get_param('~escape_strategy',
                                               'simple_backup')
        self.escape_backup_duration = rospy.get_param(
            '~escape_backup_duration', 1.0)

        self.escape_k_throttle = rospy.get_param('~escape_k_speed', 2.0)
        self.escape_k_turning = rospy.get_param('~escape_k_turning', 2.0)
        self.escape_min_speed = rospy.get_param('~escape_min_speed', 0.7)
        if self.escape_min_speed == 0:
            self.escape_min_speed = self.min_speed
        self.escape_angle_tolerance = rospy.get_param(
            '~escape_angle_tolerance', 0.15)

        self.direction_change_reverse_speed = rospy.get_param(
            '~direction_change_reverse_speed', -1.0)
        self.direction_change_reverse_duration = rospy.get_param(
            '~direction_change_reverse_duration', 1.5)
        self.direction_change_idle_duration = rospy.get_param(
            '~direction_change_idle_duration', 1.0)

        self.circling_relative_speed = rospy.get_param(
            '~circling_relative_speed', 1.5)
        self.circling_angle_tolerance = rospy.get_param(
            '~circling_angle_tolerance', 0.15)

        self.camera_horz_fov = rospy.get_param('~horz_fov', 70.0) / 180.0 * pi
        self.camera_horz_pixels = rospy.get_param('~horz_pixels', 640)

        self.cone_lost_limit = rospy.get_param('~cone_lost_limit', 15)

        self.tf_buffer = tf2_ros.Buffer()
        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)

        self.rc_pub = rospy.Publisher(Topics.RC_OVERRIDE, OverrideRCIn,
                                      queue_size=1)

        self.vel_pub = rospy.Publisher(Topics.SETPOINT_VELOCITY,
                                       TwistStamped, queue_size=1)

        self.state_pub = rospy.Publisher(Topics.NAVIGATOR_STATE,
                                         std_msgs.String, queue_size=1,
                                         latch=True)

        self.waypoints_pub = rospy.Publisher(Topics.UPDATE_WAYPOINTS,
                                             LocalWaypointList, queue_size=1)

        rospy.Subscriber(Topics.EXEC_CMD, std_msgs.String, self.on_exec_cmd)
        rospy.Subscriber(Topics.ROBOT_STATE, State, self.on_state)
        rospy.Subscriber(Topics.WAYPOINTS, WaypointList,
                         self.on_waypoints_changed)
        rospy.Subscriber(Topics.ROBOT_POSE, PoseStamped, self.on_robot_pose)
        rospy.Subscriber(Topics.CONE_LOCATIONS, location_data,
                         self.on_cone_locations)
        rospy.Subscriber(Topics.TOUCH, Bool, self.on_touch)
        rospy.Subscriber(Topics.LOCAL_WAYPOINTS, LocalWaypointList,
                         self.on_map_waypoints_changed)

        self._mavros_set_mode = self.get_proxy(Topics.SET_MODE, SetMode)
        self._mavros_param_get = self.get_proxy(Topics.GET_PARAM, ParamGet)
        self._mavros_param_set = self.get_proxy(Topics.SET_PARAM, ParamSet)
        self._mavros_arming = self.get_proxy(Topics.ARMING, CommandBool)
        self._mavros_set_current_wp = self.get_proxy(
            Topics.SET_CURRENT_WAYPOINT, WaypointSetCurrent)

        self.last_wp_speed = None

        # If gcs_id is set, set SYSID_MYGCS parameter.
        if rospy.has_param("~gcs_id"):
            gcs_id = int(rospy.get_param("~gcs_id"))
            self.set_parameter('SYSID_MYGCS', gcs_id)

        self.begin_state(States.WAITING_FOR_START)

        while not rospy.is_shutdown():
            self.state_pub.publish(self.state.get_name())
            rate.sleep()

    def get_proxy(self, topic, type):
        rospy.wait_for_service(topic)
        return rospy.ServiceProxy(topic, type)

    def begin_state(self, new_state):
        old_state = self.state
        self.state = new_state
        if old_state is None:
            rospy.loginfo('Initial state: %s', new_state.get_name())
        else:
            rospy.loginfo('State change: %s --> %s', old_state.get_name(),
                          new_state.get_name())

    def show_event(self, event, value=None):
        if self.state is None:
            rospy.loginfo('No state, event: %s', event)
        elif value is None:
            rospy.loginfo('In state %s, event: %s', self.state.get_name(),
                          event)
        else:
            rospy.loginfo('In state %s, event: %s, value: %s',
                          self.state.get_name(), event, value)

    def on_exec_cmd(self, msg):
        cmd = msg.data

        if cmd == ExecCmds.RESET:
            self.show_event('on_exec_cmd', cmd)
            self.set_manual_speed(0, 0)
            self.begin_state(States.WAITING_FOR_START)
        elif self.state is States.WAITING_FOR_START and cmd==ExecCmds.START:
            self.show_event('on_exec_cmd', cmd)
            # Make sure we have waypoints.
            if len(self.waypoint_list.waypoints) < 2:
                rospy.logerr('No waypoints loaded')
                return
            self.arm_robot(True)
            time.sleep(1.0)
            self.follow_waypoints(1)
        elif self.state is States.WAITING_FOR_START \
             and cmd==ExecCmds.ADJUST_WAYPOINTS:
            # Adjust waypoints w/ wp index zero.
            self.adjust_waypoints(0)

    def follow_waypoints(self, index):
            self.set_current_waypoint(index)
            # Handle case where waypoint is a cone.
            if self.is_cone_waypoint(index):
                self.cone_wp_index = index
            else:
                self.cone_wp_index = None
            self.cone_is_close = False
            self.set_waypoint_speed(index)
            self.begin_state(States.FOLLOWING_WAYPOINTS)

    def on_state(self, state):
        if self.state is States.FOLLOWING_WAYPOINTS and state.mode==Modes.HOLD:
            self.show_event('on_state', state.mode)
            self.begin_circling_back()

    def begin_circling_back(self):
        robot_heading = self.pose_heading(self.robot_pose.pose)
        self.target_heading = robot_heading + 175.0/180.0*pi
        self.set_manual_speed(0, 0)
        self.set_mode(Modes.MANUAL)
        self.turn_to_heading(self.target_heading)
        self.begin_state(States.CIRCLING_BACK)

    def on_robot_pose(self, position):
        self.robot_pose = position

        if self.state is States.ESCAPING_CONE:
            self.show_event('on_robot_pose')
            pose = position.pose;
            robot_heading = self.pose_heading(position.pose)
            diff = self.normalize_angle(self.target_heading - robot_heading)
            if abs(diff) > self.escape_angle_tolerance:
                self.escape_backward(diff)
            else:
                self.set_manual_speed(0, 0)
                self.begin_state(States.LIMBO)
                self.target_heading = None
                time.sleep(self.direction_change_idle_duration)
                self.follow_waypoints(self.cone_wp_index + 1)

    def on_waypoints_changed(self, waypoint_list):
        self.waypoint_list = waypoint_list
        if len(self.waypoint_list.waypoints) < 2:
            rospy.logerr('No waypoints loaded')
            return

        if self.state is States.FOLLOWING_WAYPOINTS:
            self.show_event('on_waypoints_changed')
            if self.cone_wp_index is not None \
               and self.waypoint_list.current_seq > self.cone_wp_index:
                # Missed a cone - need to circle to find it.
                self.begin_circling_back()
            else:
                self.set_waypoint_speed(self.waypoint_list.current_seq)
                if self.is_cone_waypoint(self.waypoint_list.current_seq):
                    self.cone_wp_index = self.waypoint_list.current_seq
                self.cone_is_close = False

    def on_map_waypoints_changed(self, list):
        self.map_waypoint_list = list

    def on_touch(self, msg):
        # Very quickly change state if touching the cone.
        if self.state is States.DRIVING_TO_CONE and msg.data:
            self.state = States.LIMBO
            touching = True
        else:
            touching = False

        if touching:
            self.show_event('on_touch', touching)
            self.set_manual_speed(0, 0)
            if self.cone_wp_index == len(self.waypoint_list.waypoints) - 1:
                self.set_mode(Modes.HOLD)
                self.begin_state(States.FINISHED)
            else:
                self.set_mode(Modes.MANUAL)

                # Must pause for a bit so braking isn't actuated when we
                # reverse. Go into limbo state so we won't respond to
                # messages while delaying.
                self.begin_state(States.LIMBO)
                if self.direction_change_reverse_duration > 0:
                    self.set_manual_speed(self.direction_change_reverse_speed,
                                          0)
                    time.sleep(self.direction_change_reverse_duration)
                self.set_manual_speed(0, 0)
                time.sleep(self.direction_change_idle_duration)
                # Should update waypoints for cone location, but skip for now.
                cone_wp \
                    = self.map_waypoint_list.waypoints[self.cone_wp_index]
                escape_wp \
                    = self.map_waypoint_list.waypoints[self.cone_wp_index + 1]
                self.target_heading \
                    = self.angle_to_point(cone_wp.point, escape_wp.point)

                if self.escape_strategy == 'simple_backup':
                    self.simple_backup(self.escape_backup_duration)
                    self.follow_waypoints(self.cone_wp_index + 1)
                    rospy.loginfo('After escaping, state=%s',
                                  self.state.get_name())
                else:
                    # New backup strategy, back up toward next waypoint.
                    self.begin_state(States.ESCAPING_CONE)

    # Escape by backing up straight for a duration, then pausing to setting
    # before moving.
    def simple_backup(self, duration):
        rospy.loginfo('Simple backup strategy, duration=%f', duration)
        self.set_manual_speed(-self.escape_min_speed, 0)
        time.sleep(duration)
        self.set_manual_speed(0, 0)
        time.sleep(2.0)

    def on_cone_locations(self, cone_locations):
        if (self.state is States.FOLLOWING_WAYPOINTS \
            or self.state is States.CIRCLING_BACK \
            or self.state is States.CIRCLING_FORWARD) \
           and self.cone_wp_index is not None \
           and self.cone_detected(cone_locations.poses,
                                  self.cone_recovery_min_area):
            cone_pose = self.cone_detected(cone_locations.poses,
                                           self.cone_recovery_min_area)
            self.show_event('on_cone_locations', len(cone_locations.poses))
            self.begin_driving_to_cone(cone_pose)
        elif self.state is States.DRIVING_TO_CONE \
             and self.cone_detected(cone_locations.poses,
                                    self.cone_normal_min_area):
            cone_pose = self.cone_detected(cone_locations.poses,
                                           self.cone_normal_min_area)
            self.cone_lost_count = 0
            self.show_event('on_cone_locations', len(cone_locations.poses))
            self.drive_to_cone(cone_pose)
        elif self.state is States.DRIVING_TO_CONE:
            self.cone_lost_count += 1
            if self.cone_lost_count > self.cone_lost_limit:
                # Lost cone, must recover.
                self.begin_circling_back()
        elif self.state is States.CIRCLING_FORWARD:
            robot_heading = self.pose_heading(self.robot_pose.pose)
            diff = self.normalize_angle(self.target_heading - robot_heading)
            if abs(diff) > self.circling_angle_tolerance:
                self.circle_forward(diff)
            elif self.cone_wp_index+1 < len(self.waypoint_list.waypoints):
                # More waypoints - skip to next waypoint
                self.set_manual_speed(0,0)
                self.set_current_waypoint(self.cone_wp_index + 1)
                self.follow_waypoints(self.cone_wp_index + 1)
                rospy.loginfo('Missed cone - moving to next waypoint')
            else:
                self.set_manual_speed(0, 0)
                self.set_mode(Modes.HOLD)
                self.begin_state(States.FAILED)
        elif self.state is States.CIRCLING_BACK:
            robot_heading = self.pose_heading(self.robot_pose.pose)
            diff = self.normalize_angle(self.target_heading - robot_heading)
            if abs(diff) > self.circling_angle_tolerance:
                self.circle_forward(diff)
            else:
                self.target_heading += 175.0/180.0*pi
                self.turn_to_heading(self.target_heading)
                self.begin_state(States.CIRCLING_FORWARD)

    def begin_driving_to_cone(self, cone_pose):
        # Found the cone.
        self.set_manual_speed(0, 0)
        if self.cone_approach_use_throttle:
            self.set_mode(Modes.MANUAL)
        else:
            self.set_mode(Modes.GUIDED)
            self.drive_to_cone(cone_pose)
        self.begin_state(States.DRIVING_TO_CONE)

    def drive_to_cone(self, cone_pose):
        # A pseudo-distance used to determine the pixel width of a cone
        # heading angle.
        fov_dist = self.camera_horz_pixels/2 / tan(self.camera_horz_fov/2)
        cone_heading = atan2(cone_pose.x, fov_dist)
        distance = self.get_cone_distance(cone_pose.area)

        if distance <= self.cone_close_distance:
            self.cone_is_close = True

        min_speed, max_speed = self.get_cone_speed_factor(self.cone_wp_index)

        # Drive at minimum speed if cone is already close.
        if self.cone_is_close:
            max_speed = min_speed

        speed = min(max(self.cone_approach_k_speed*distance, min_speed),
                    max_speed)
        turning = min(self.cone_approach_k_turning*abs(cone_heading),
                      self.max_turning)
        if cone_heading < 0:
            turning = -turning

        rospy.loginfo('[Navigator] d=%f heading=%f speed=%f turning=%f',
                      distance, cone_heading*180/pi, speed, turning)

        if self.cone_approach_use_throttle:
            self.set_manual_speed(speed, turning)
        else:
            linear_speed = max(self.min_speed, speed*self.normal_speed)
            twist = TwistStamped()
            twist.twist.linear.x = linear_speed
            twist.twist.angular.z = turning * self.max_turning
            self.vel_pub.publish(twist)
            rospy.loginfo('[GUIDED] speed=%f turning=%f',
                          twist.twist.linear.x, twist.twist.angular.z)

    def get_cone_distance(self, area):
        # Area should be proportional to the square of distance.
        # Assume approx. 3900 pixels at 3 meters and a 2.0
        # aspect ratio.
        ratio = sqrt(3900.0 / area)
        return 3 * ratio

    def turn_to_heading(self, heading):
        robot_heading = self.pose_heading(self.robot_pose.pose)
        diff = self.normalize_angle(self.target_heading - robot_heading)
        self.circle_forward(diff)

    def circle_forward(self, heading_diff):
        min_speed, max_speed = self.get_cone_speed_factor(self.cone_wp_index)

        speed = min(min_speed*self.circling_relative_speed, max_speed)
        turning = self.max_turning

        rospy.loginfo('[circling] heading_diff=%f speed=%f turning=%f',
                      heading_diff*180/pi, speed, turning)

        self.set_manual_speed(speed, turning)

    def escape_backward(self, heading_diff):
        min_speed, max_speed = self.get_cone_speed_factor(self.cone_wp_index)

        # Override minimum speed based on testing results.
        min_speed = max(min_speed, self.escape_min_speed)

        speed = - min(max(self.escape_k_throttle*abs(heading_diff), min_speed),
                      max_speed)
        turning = min(self.escape_k_turning*abs(heading_diff),
                      self.max_turning)

        rospy.loginfo('[escaping] heading_diff=%f speed=%f turning=%f',
                      heading_diff*180/pi, speed, turning)

        self.set_manual_speed(speed, turning)

    def angle_to_point(self, from_pt, to_pt):
        return atan2(to_pt.y - from_pt.y, to_pt.x - from_pt.x)

    def pose_heading(self, pose):
        q = [pose.orientation.x,
             pose.orientation.y,
             pose.orientation.z,
             pose.orientation.w]
        roll, pitch, heading = euler_from_quaternion(q)
        return heading

    def normalize_angle(self, theta):
        """Gets an equivalent angle to a specified angle within the
           range plus pi to minus pi."""
        while theta >= 2*pi:
            theta -= 2*pi
        while theta < 0:
            theta += 2*pi

        # Now 0 <= theta < 2*pi. Convert to -pi < theta <= pi
        if theta > pi:
            return theta - 2*pi
        else:
            return theta

    # Finds a cone pose that has an area greater than a specified minimum,
    # or returns None if none found.
    def cone_detected(self, poses, min_area):
        for pose in poses:
            if pose.area >= min_area:
                return pose
        return None

    # Adjust the waypoints so that they are all relative to the current
    # robot location.
    #
    # This method is used to adjust the waypoints for GPS drift by making
    # the waypoints relative to the current, known position.
    def adjust_waypoints(self, index):
        if self.map_waypoint_list is None:
            rospy.logerr('No map waypoints are available')
            return

        new_waypoints = LocalWaypointList()
        new_waypoints.frame = LocalWaypointList.FRAME_ROBOT_RELATIVE
        new_waypoints.current_seq = index + 1
        new_waypoints.waypoints = []

        base_wp = self.map_waypoint_list.waypoints[index]
        rospy.loginfo('Base wp: x=%f y=%f', base_wp.point.x, base_wp.point.y)

        for wp in self.map_waypoint_list.waypoints:
            new_wp = LocalWaypoint()
            new_wp.point.x = wp.point.x - base_wp.point.x
            new_wp.point.y = wp.point.y - base_wp.point.y
            new_wp.point.z = wp.point.z

            rospy.loginfo('New relative wp %d: x=%f y=%f z=%f',
                          len(new_waypoints.waypoints),
                          new_wp.point.x, new_wp.point.y, new_wp.point.z)

            new_waypoints.waypoints.append(new_wp)

        rospy.loginfo('Adjusting waypoints, new size is %d',
                      len(new_waypoints.waypoints))
        self.waypoints_pub.publish(new_waypoints)

    def arm_robot(self, is_armed):
        self._mavros_arming(is_armed)
        # Need to ensure that the command finishes before doing other
        # mavros calls, or we get an error. 10ms should be enough.
        rospy.sleep(0.2)

    def set_waypoint_speed(self, index):
        waypoint_speed = self.get_waypoint_speed(index)
        speed = self.normal_speed * waypoint_speed

        if speed != self.last_wp_speed:
            self.last_wp_speed = speed
            self.set_mode(Modes.HOLD)
            self.set_parameter('CRUISE_SPEED', speed)

        # Make sure to go back into AUTO mode so that we continue to the
        # next waypoint.
        self.set_mode(Modes.AUTO)
        rospy.loginfo('Speed %f', speed)
        
    # Sets the R/C override speeds. Turning is positive
    # to the left, while the R/C PWM value for turning
    # decreases to the left.
    def set_manual_speed(self, speed, turning):
        channels = [OverrideRCIn.CHAN_NOCHANGE, # 0
                    OverrideRCIn.CHAN_NOCHANGE, # 1
                    OverrideRCIn.CHAN_NOCHANGE, # 2
                    OverrideRCIn.CHAN_NOCHANGE, # 3
                    OverrideRCIn.CHAN_NOCHANGE, # 4
                    OverrideRCIn.CHAN_NOCHANGE, # 5
                    OverrideRCIn.CHAN_NOCHANGE, # 6
                    OverrideRCIn.CHAN_NOCHANGE] # 7

        if speed >= 0:
            throttle_limit = self.throttle_fwd_max
            throttle_min = self.throttle_fwd_min
        else:
            throttle_limit = self.throttle_reverse_max
            throttle_min = self.throttle_reverse_min

        if speed==0:
            throttle = self.throttle_neutral
        else:
            throttle = abs(speed) * (throttle_limit - throttle_min) \
                       + throttle_min
        if throttle_limit > self.throttle_neutral:
            throttle = min(throttle, throttle_limit)
        else:
            throttle = max(throttle, throttle_limit)

        # If we are backing up, negate the turning, since the wheels
        # must be turned opposite the turning.
        if speed < 0:
            wheel_turning = turning
        else:
            wheel_turning = -turning

        if wheel_turning >= 0:
            steering_limit = self.steering_left_max
        else:
            steering_limit = self.steering_right_max

        steering = abs(turning) * (steering_limit - self.steering_neutral) \
                   + self.steering_neutral
        if steering_limit >= self.steering_neutral:
            steering = min(steering, steering_limit)
        else:
            steering = max(steering, steering_limit)
            
        channels[RCChannels.SPEED] = int(throttle)
        channels[RCChannels.TURNING] = int(steering)

        msg = OverrideRCIn()
        msg.channels = channels
        self.rc_pub.publish(msg)
        rospy.loginfo('[MANUAL] speed=%f turning=%f: throttle=%d steering=%d', speed, turning, throttle, steering)

    def set_mode(self, mode):
        self._mavros_set_mode(0, mode)

    def set_current_waypoint(self, index):
        self._mavros_set_current_wp(index)
        # Need to ensure that the command finishes before doing other
        # mavros calls, or we get an error.
        rospy.sleep(0.2)

    def set_parameter(self, param_name, x):
        value = ParamValue()
        if type(x) is int:
            value.integer = x
        else:
            value.real = x
        self._mavros_param_set(param_name, value)

    def is_cone_waypoint(self, wp_index):
        wp = self.waypoint_list.waypoints[wp_index]
        return wp.z_alt >= 1000

    def get_waypoint_speed(self, wp_index):
        wp = self.waypoint_list.waypoints[wp_index]
        speed = int(wp.z_alt) / 100 % 10
        if speed == 0:
            return 1.0
        else:
            return 0.1 * speed

    def get_cone_speed_factor(self, wp_index):
        wp = self.waypoint_list.waypoints[wp_index]
        min_speed = 0.01 * (int(wp.z_alt) % 100)
        max_speed = self.get_waypoint_speed(wp_index)
        rospy.loginfo('get_cone_speed_factor %f --> %f, %f', wp.z_alt,
                      min_speed, max_speed)

        return (min_speed, max_speed)

if __name__ == "__main__":
    try:
        Navigator().run()

    except rospy.ROSInterruptException:
        pass
