steering.py
Go to the documentation of this file.00001 """ 00002 * 00003 * Description: common steering constants and calculations 00004 * 00005 * Copyright (C) 2005, 2007, 2009 Austin Robot Technology 00006 * 00007 * License: Modified BSD Software License Agreement 00008 * 00009 * $Id: steering.h 2 2010-01-17 01:54:03Z jack.oquin $ 00010 *""" 00011 00012 # This code originally imported angles.h 00013 # Angles.h has no python equivalent in ROS, but the math library 00014 # has the functions this file needs to do its job. 00015 00016 import math 00017 from art_msgs.msg import ArtVehicle 00018 00019 ## @brief 00020 # 00021 # Common steering constants and calculations. 00022 # 00023 00024 # Constants: # 00025 steer_speed_min = 3.8 # minimum speed for calculation # 00026 # (experimentally verified) # 00027 00028 # Determine steering angle (in degrees) for a given speed and yawrate. 00029 # 00030 # This computation is based on a simplified "bicycle model" of 00031 # vehicle steering geometery. We ignore the slightly different 00032 # angles of the two front wheels, abstracting them into a single 00033 # wheel at the midline of the vehicle (like a bicycle or tricycle). 00034 # 00035 # Consider a radius from the center of the vehicle's turning circle 00036 # to the midpoint of the rear axle. The vehicle's wheelbase is at 00037 # a right angle to this radius, since the rear wheels do not pivot. 00038 # The hypotenuse of this right triangle is the slightly longer 00039 # distance from the midpoint of the front axle back to the center 00040 # of the circle. As long as the wheels do not slip, the acute 00041 # angle between the radius and the hypotenuse will be equal to the 00042 # angle of the front wheel relative to the midline of the vehicle. 00043 # 00044 # Thus, the tangent of the steering angle is the wheelbase (w) 00045 # divided by the radius (r). We estimate r from the desired 00046 # yawspeed (y), assuming a constant velocity (v): 00047 # 00048 # y = (2*pi radians) / (2*pi*r/v seconds) 00049 # y = v/r radians/second 00050 # r = v/y 00051 # 00052 def steering_angle(v, y) : 00053 steer_radians = math.atan(ArtVehicle.wheelbase * y,v) 00054 steer_degrees = math.degrees(steer_radians) 00055 00056 steer_degrees = min(steer_degrees, ArtVehicle.max_steer_degrees) 00057 steer_degrees = max(steer_degrees, -ArtVehicle.max_steer_degrees) 00058 00059 return steer_degrees 00060 00061 # Determine yaw rate (in radians/second) for a given speed and 00062 # steering angle (in degrees). Inverse of steering_angle(). 00063 # 00064 00065 def angle_to_yaw(v, angle) : 00066 return v * math.tan(math.radians(angle)) / ArtVehicle.wheelbase 00067 00068 maximum_yaw = angle_to_yaw(steer_speed_min, ArtVehicle.max_steer_degrees)