def __init__(self, config={}):
self._config.update(config)
self._stepper = Stepper()
self._stepper.disable()
self._servo = Servo()
self.set_vertical_position(0)
self._lidar = Lidar()
def __init__(self, config, level):
self._log = Logger('follower', Level.INFO)
self._config = config
if self._config:
self._log.info('configuration provided.')
_config = self._config['ros'].get('wall_follower')
self._port_angle = _config.get('port_angle')
self._stbd_angle = _config.get('starboard_angle')
_range_value = _config.get('tof_range')
_range = Range.from_str(_range_value)
_servo_number = _config.get('servo_number')
else:
self._log.warning('no configuration provided.')
self._port_angle = -90.0
self._stbd_angle = 90.0
_range = Range.PERFORMANCE
_servo_number = 2
# _range = Range.LONG
# _range = Range.MEDIUM
# _range = Range.SHORT
self._log.info(
'wall follower port angle: {:>5.2f}°; starboard angle: {:>5.2f}°'.
format(self._port_angle, self._stbd_angle))
self._servo = Servo(self._config, _servo_number, level)
self._tof = TimeOfFlight(_range, Level.WARN)
self._max_retries = 10
self._enabled = False
self._closed = False
self._log.info('ready.')
def __init__(self, cfg):
self.STEERING_NEUTRAL = cfg['servo_neutral_angle']
self.MIN_STEERING_ANGLE = cfg[
'servo_min_angle_limit'] # Steering left/right max angle.
self.MAX_STEERING_ANGLE = cfg[
'servo_max_angle_limit'] # Steering left/right max angle.
self.STEERING_RATE = cfg[
'steering_rate'] # Server steering angle is fomula angle. But car_client depends on car. Drift type steering, normal steering, etc. Therefore, use this rate for ajust good angle.
self.MOTOR_NEUTRAL_SPEED = cfg['motor_neutral_speed']
self.MOTOR_FORWARD_SPEED_RATE = float(
cfg['motor_max_speed_limit']
) / float(
100
) # Server max speed is 100. But car_client depends on config parameter.
self.MOTOR_BACK_SPEED_RATE = float(
cfg['motor_min_speed_limit']
) / float(
-100
) # Server min speed is -100. But car_client depends on config parameter.
self.steering = Servo(cfg)
self.motor = Motor(cfg)
self.steering.set_angle(self.STEERING_NEUTRAL, delay=0)
self.motor.set_speed(self.MOTOR_NEUTRAL_SPEED, delay=0)
self.speed = self.MOTOR_NEUTRAL_SPEED
self.back_in = False
self.back_start_time = None
return
def __init__(self, config, level):
self._log = Logger('uscanner', Level.INFO)
self._config = config
if self._config:
self._log.info('configuration provided.')
_config = self._config['ros'].get('ultrasonic_scanner')
self._min_angle = _config.get('min_angle')
self._max_angle = _config.get('max_angle')
self._degree_step = _config.get('degree_step')
self._use_raw_distance = _config.get('use_raw_distance')
self._read_delay_sec = _config.get('read_delay_sec')
self._log.info('read delay: {:>4.1f} sec'.format(
self._read_delay_sec))
_servo_number = _config.get('servo_number')
else:
self._log.warning('no configuration provided.')
self._min_angle = -60.0
self._max_angle = 60.0
self._degree_step = 3.0
self._use_raw_distance = False
self._read_delay_sec = 0.1
_servo_number = 2
self._log.info(
'scan from {:>5.2f} to {:>5.2f} with step of {:>4.1f}°'.format(
self._min_angle, self._max_angle, self._degree_step))
self._servo = Servo(self._config, _servo_number, level)
self._ub = self._servo.get_ultraborg()
# self._tof = TimeOfFlight(_range, Level.WARN)
self._error_range = 0.067
self._enabled = False
self._closed = False
self._log.info('ready.')
def main():
_log = Logger("servo-test", Level.INFO)
try:
_log.info('starting ultrasonic scan...')
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_number = 1
_servo = Servo(_config, _number, Level.INFO)
# _servo.enable()
# _thread = threading.Thread(target=_servo.sweep, args=[])
# _thread.start()
for i in numpy.arange(-90.0, 90.0, 5.0):
_servo.set_position(i)
_log.info('position: {}'.format(i))
time.sleep(0.5)
_servo.set_position(0.0)
_servo.disable()
_thread.join()
_log.info('servo test complete.')
except Exception:
_log.info(traceback.format_exc())
sys.exit(1)
def __init__(self, config, level):
self._log = Logger('lidar', Level.INFO)
self._config = config
if self._config:
self._log.info('configuration provided.')
_config = self._config['ros'].get('lidar')
self._reverse_movement = _config.get('reverse_movement')
self._double_sweep = _config.get('double_sweep')
self._min_angle = _config.get('min_angle')
self._max_angle = _config.get('max_angle')
self._degree_step = _config.get('degree_step')
self._step_delay_sec = _config.get('step_delay_sec')
_range_value = _config.get('tof_range')
_range = Range.from_str(_range_value)
_servo_number = _config.get('servo_number')
else:
self._log.warning('no configuration provided.')
self._log.info('play sound disabled.')
self._reverse_movement = False
self._double_sweep = False
self._min_angle = -60.0
self._max_angle = 60.0
self._degree_step = 3.0
self._step_delay_sec = 0.01
_range = Range.PERFORMANCE
# _range = Range.LONG
# _range = Range.MEDIUM
# _range = Range.SHORT
_servo_number = -1
self._log.info(
'scan range of {} from {:>4.1f}° to {:>4.1f}° with step of {:>4.1f}°'
.format(_range, self._min_angle, self._max_angle,
self._degree_step))
if _servo_number > 0:
self._servo = Servo(self._config, _servo_number, level)
else:
self._servo = None
_range = Range.PERFORMANCE
# _range = Range.LONG
# _range = Range.MEDIUM
# _range = Range.SHORT
self._tof = TimeOfFlight(_range, Level.WARN)
self._error_range = 0.067
self._enabled = False
self._closed = False
self._log.info('ready.')
def HW_Func():
status = LogicFunctionDisplay([0x70], 1)
psf = LogicFunctionDisplay([0x71, 0x72])
psb = LogicFunctionDisplay([0x73, 0x74, 0x75])
# psb.setBrightness(7) # can be a value between [off] 0-15 [brightest]
imu = IMU() # inerial measurement unit
sub = zmq.Sub(['servos'], ('localhost', 9004))
js_sub = zmq.Sub(['cmd'], ('localhost', 9006))
# you can monitor the publishing of this with:
# topic_echo.py localhost 9005 imu
pub = zmq.Pub(('localhost', 9005))
# create servos
servos = {}
servos['door0'] = Servo(0)
servos['door1'] = Servo(1)
servos['door2'] = Servo(2)
servos['door3'] = Servo(3)
servos['door4'] = Servo(4)
servos['js'] = Servo(7) # this is just for demo
pprint(servos)
while True:
status.update()
psf.update()
psb.update()
time.sleep(0.5)
accel, mag, gyro = imu.get()
msg = Msg.IMU()
msg.linear_acceleration.set(*accel)
# msg.angular_velocity.set(*gyro)
msg.orientation.set(1, 0, 0, 0)
pub.pub('imu', msg)
topic, msg = sub.recv()
if msg:
if topic == 'servos':
print('Topic, Msg:', topic, msg)
angle = msg['angle']
name = msg['name']
servos[name].angle = angle
elif topic == 'cmd':
print('<<< crap cmd in wrong place >>>')
# this is a joystick test
topic, msg = js_sub.recv()
if msg:
if topic == 'cmd':
print('Topic, Msg:', topic, msg)
angle = 90 * msg.angular.x + 90
servos['js'].angle = angle
time.sleep(0.025)
def __init__(self, config):
self.is_running = True
self.is_talking = False
# attach audio player
self.audio = AudioPlayer(config)
# add list of supported phrases
self.phrases = config.phrases
# bind mouth and eye servos based on pins defined in config
self.servos = {
"eyes": Servo(
PinSet(
config.getint('pins', 'pwma'),
config.getint('pins', 'ain1'),
config.getint('pins', 'ain2')
),
duration=config.getfloat('settings', 'eyes_duration'),
speed=config.getint('settings', 'eyes_speed'),
label='eyes'
),
"mouth": Servo(
PinSet(
config.getint('pins', 'pwmb'),
config.getint('pins', 'bin1'),
config.getint('pins', 'bin2')
),
duration=config.getfloat('settings', 'mouth_duration'),
speed=config.getint('settings', 'mouth_speed'),
label='mouth'
)
}
self.character = {
'name': config.get('character', 'name'),
'prefix': config.get('character', 'prefix')
}
logging.debug("character is: %s", self.character['name'])
logging.debug("prefix is: %s", self.character['prefix'])
self.talk_thread = Thread(target=self. __talk_monitor, daemon=True)
self.blink_thread = Thread(target=self. __blink_monitor, daemon=True)
logging.debug("bear constructor finished")
def __init__(self, comm=None):
if comm is None or not comm.is_open:
self.comm = serial.Serial(DEFAULT_COM_PORT, DEFAULT_COM_BAUD)
else:
self.comm = comm
self.servos = DEFAULT_CHANNEL_MAPPINGS
for row, columns in self.servos.items():
for column, channel in columns.items():
self.servos[row][column] = Servo(channel, comm)
def __init__(self, config, queue, level):
self._log = Logger('cat-scan', Level.INFO)
self._config = config
self._queue = queue
if self._config:
self._log.info('configuration provided.')
_config = self._config['ros'].get('cat_scan')
self._active_angle = _config.get('active_angle')
self._inactive_angle = _config.get('inactive_angle')
_pir_pin = _config.get('pir_pin')
_servo_number = _config.get('servo_number')
else:
self._log.warning('no configuration provided.')
self._active_angle = -90.0
self._inactive_angle = 90.0
_pir_pin = 15
_servo_number = 3
_threshold_value = 0.1 # the value above which the device will be considered “on”
self._log.info(
'cat scan active angle: {:>5.2f}°; inactive angle: {:>5.2f}°'.
format(self._active_angle, self._inactive_angle))
self._servo = Servo(self._config, _servo_number, level)
# set up pin where PIR is connected
self._sensor = MotionSensor(_pir_pin,
threshold=_threshold_value,
pull_up=False)
self._sensor.when_motion = self._activated
self._sensor.when_no_motion = None #self._deactivated
self._scan_enabled = False
self._disabling = False
self._enabled = False
self._closed = False
# arm behaviour
self._arm_movement_degree_step = 5.0
self._arm_up_delay = 0.09
self._arm_down_delay = 0.04
self._rgbmatrix = RgbMatrix(Level.INFO)
self._rgbmatrix.set_display_type(DisplayType.SCAN)
self._log.info('ready.')
def __init__(self):
# create servos
self.servos = {}
self.servos['door0'] = Servo(0)
self.servos['door1'] = Servo(1)
self.servos['door2'] = Servo(2)
self.servos['door3'] = Servo(3)
self.servos['door4'] = Servo(4)
self.servos['js'] = Servo(7) # this is just for demo
def test_servo_init_sets_position_to_start(mock_comm):
servo = Servo('TEST', mock_comm)
assert mock_comm.readline() == '#TESTP1000\r'
assert servo.position == -1.0
class UltrasonicScanner():
def __init__(self, config, level):
self._log = Logger('uscanner', Level.INFO)
self._config = config
if self._config:
self._log.info('configuration provided.')
_config = self._config['ros'].get('ultrasonic_scanner')
self._min_angle = _config.get('min_angle')
self._max_angle = _config.get('max_angle')
self._degree_step = _config.get('degree_step')
self._use_raw_distance = _config.get('use_raw_distance')
self._read_delay_sec = _config.get('read_delay_sec')
self._log.info('read delay: {:>4.1f} sec'.format(
self._read_delay_sec))
_servo_number = _config.get('servo_number')
else:
self._log.warning('no configuration provided.')
self._min_angle = -60.0
self._max_angle = 60.0
self._degree_step = 3.0
self._use_raw_distance = False
self._read_delay_sec = 0.1
_servo_number = 2
self._log.info(
'scan from {:>5.2f} to {:>5.2f} with step of {:>4.1f}°'.format(
self._min_angle, self._max_angle, self._degree_step))
self._servo = Servo(self._config, _servo_number, level)
self._ub = self._servo.get_ultraborg()
# self._tof = TimeOfFlight(_range, Level.WARN)
self._error_range = 0.067
self._enabled = False
self._closed = False
self._log.info('ready.')
# ..........................................................................
def _in_range(self, p, q):
return p >= (q - self._error_range) and p <= (q + self._error_range)
# ..........................................................................
def scan(self):
if not self._enabled:
self._log.warning('cannot scan: disabled.')
return
start = time.time()
_min_mm = 9999.0
_angle_at_min = 0.0
_max_mm = 0
_angle_at_max = 0.0
_max_retries = 10
self._servo.set_position(self._min_angle)
time.sleep(0.3)
_slack = 0.1
self._log.info(
Fore.BLUE + Style.BRIGHT +
'scanning from {:>5.2f} to {:>5.2f} with step of {:>4.1f}°...'.
format(self._min_angle, self._max_angle, self._degree_step))
for degrees in numpy.arange(self._min_angle, self._max_angle + _slack,
self._degree_step):
self._log.debug('loop set to : {:>5.2f}°...'.format(degrees))
if self._servo.is_in_range(degrees):
self._servo.set_position(degrees)
wait_count = 0
while not self._in_range(self._servo.get_position(degrees),
degrees) and wait_count < 10:
time.sleep(0.0025)
wait_count += 1
self._log.debug(Fore.GREEN + Style.BRIGHT + 'measured degrees: {:>5.2f}°: \ttarget: {:>5.2f}°; waited: {:d}'.format(\
self._servo.get_position(degrees), degrees, wait_count))
_mm = self._servo.get_distance(_max_retries,
self._use_raw_distance)
# if we get a zero we keep trying...
if _mm is None:
self._log.info(
'failed to read distance at {:>5.2f}°.'.format(
degrees))
else:
if _mm <= 0.01:
retry_count = 0
while _mm == 0.0 and retry_count < _max_retries:
_mm = self._servo.get_distance(
_max_retries, self._use_raw_distance)
if _mm is None:
self._log.info(
'failed to read distance at {:>5.2f}°: '.
format(degrees) + Fore.RED +
'\t retries: {:d}'.format(retry_count))
else:
if _mm <= 0.01:
self._log.info(
'distance at {:>5.2f}°: '.format(
degrees) + Fore.RED +
'\t{:>6.0f}mm'.format(_mm) +
'\t retries: {:d}'.format(retry_count))
else:
self._log.info(
'distance at {:>5.2f}°: \t{:>6.0f}mm'.
format(degrees, _mm) + Fore.BLACK +
' (on retry)')
retry_count += 1
time.sleep(0.1)
else:
self._log.info(
'distance at {:>5.2f}°: \t{:>6.0f}mm'.format(
degrees, _mm))
if _mm is not None:
# capture min and max at angles
_min_mm = min(_min_mm, _mm)
if _mm == _min_mm:
_angle_at_min = degrees
_max_mm = max(_max_mm, _mm)
if _mm == _max_mm:
_angle_at_max = degrees
time.sleep(self._read_delay_sec)
# time.sleep(0.1)
else:
self._log.warning(
'requested position: {:>5.2f}° out range of servo movement.'
.format(degrees))
time.sleep(0.1)
# self._log.info('complete.')
elapsed = time.time() - start
self._log.info('scan complete: {:>5.2f}sec elapsed.'.format(elapsed))
self._log.info(
Fore.CYAN + Style.BRIGHT +
'mix. distance at {:>5.2f}°:\t{}mm'.format(_angle_at_min, _min_mm))
self._log.info(
Fore.CYAN + Style.BRIGHT +
'max. distance at {:>5.2f}°:\t{}mm'.format(_angle_at_max, _max_mm))
self._servo.set_position(0.0)
return [_angle_at_min, _min_mm, _angle_at_max, _max_mm]
# ..........................................................................
def enable(self):
if self._closed:
self._log.warning('cannot enable: closed.')
return
# self._tof.enable()
self._enabled = True
# ..........................................................................
def disable(self):
self._enabled = False
self._servo.disable()
# self._tof.disable()
# ..........................................................................
def close(self):
self.disable()
self._servo.close()
self._closed = True
class RosCar():
def __init__(self, cfg):
self.STEERING_NEUTRAL = cfg['servo_neutral_angle']
self.MIN_STEERING_ANGLE = cfg[
'servo_min_angle_limit'] # Steering left/right max angle.
self.MAX_STEERING_ANGLE = cfg[
'servo_max_angle_limit'] # Steering left/right max angle.
self.STEERING_RATE = cfg[
'steering_rate'] # Server steering angle is fomula angle. But car_client depends on car. Drift type steering, normal steering, etc. Therefore, use this rate for ajust good angle.
self.MOTOR_NEUTRAL_SPEED = cfg['motor_neutral_speed']
self.MOTOR_FORWARD_SPEED_RATE = float(
cfg['motor_max_speed_limit']
) / float(
100
) # Server max speed is 100. But car_client depends on config parameter.
self.MOTOR_BACK_SPEED_RATE = float(
cfg['motor_min_speed_limit']
) / float(
-100
) # Server min speed is -100. But car_client depends on config parameter.
self.steering = Servo(cfg)
self.motor = Motor(cfg)
self.steering.set_angle(self.STEERING_NEUTRAL, delay=0)
self.motor.set_speed(self.MOTOR_NEUTRAL_SPEED, delay=0)
return
def listener(self):
"""
READ ROS TOPICS AND RUN FUNCTION BY HZ.
angle_deg: -45 to 45
speed_deg: -100 to 100
brake_bool: True or False
"""
rospy.init_node('RoboCarROS', anonymous=True)
rospy.Subscriber('/steer/angle_deg', Int8, self.set_angle)
rospy.Subscriber('/motor/speed_deg', Int8, self.set_speed)
rospy.Subscriber('/motor/brake_bool', Bool, self.brake)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
return
def set_angle(self, angle):
"""
angle.data: -45 to 45 degree.
"""
steering_angle = angle.data
steering_angle = int(float(steering_angle) * float(self.STEERING_RATE))
steering_angle = self.STEERING_NEUTRAL + steering_angle
"""
Adjust within operable angle
"""
if steering_angle > self.MAX_STEERING_ANGLE:
steering_angle = self.MAX_STEERING_ANGLE
if steering_angle < self.MIN_STEERING_ANGLE:
steering_angle = self.MIN_STEERING_ANGLE
self.steering.set_angle(steering_angle)
def set_speed(self, speed):
motor_speed = speed.data
if motor_speed > 0:
motor_speed = int(
float(motor_speed) * float(self.MOTOR_FORWARD_SPEED_RATE))
elif motor_speed < 0:
motor_speed = int(
float(motor_speed) * float(self.MOTOR_BACK_SPEED_RATE))
self.motor.set_speed(motor_speed)
def brake(self, value):
if value.data:
self.motor.set_speed(self.MOTOR_NEUTRAL_SPEED, delay=0)
class CatScan():
'''
Uses an infrared PIR sensor to scan for cats. This involves raising a
servo arm holding the sensor.
'''
def __init__(self, config, queue, level):
self._log = Logger('cat-scan', Level.INFO)
self._config = config
self._queue = queue
if self._config:
self._log.info('configuration provided.')
_config = self._config['ros'].get('cat_scan')
self._active_angle = _config.get('active_angle')
self._inactive_angle = _config.get('inactive_angle')
_pir_pin = _config.get('pir_pin')
_servo_number = _config.get('servo_number')
else:
self._log.warning('no configuration provided.')
self._active_angle = -90.0
self._inactive_angle = 90.0
_pir_pin = 15
_servo_number = 3
_threshold_value = 0.1 # the value above which the device will be considered “on”
self._log.info(
'cat scan active angle: {:>5.2f}°; inactive angle: {:>5.2f}°'.
format(self._active_angle, self._inactive_angle))
self._servo = Servo(self._config, _servo_number, level)
# set up pin where PIR is connected
self._sensor = MotionSensor(_pir_pin,
threshold=_threshold_value,
pull_up=False)
self._sensor.when_motion = self._activated
self._sensor.when_no_motion = None #self._deactivated
self._scan_enabled = False
self._disabling = False
self._enabled = False
self._closed = False
# arm behaviour
self._arm_movement_degree_step = 5.0
self._arm_up_delay = 0.09
self._arm_down_delay = 0.04
self._rgbmatrix = RgbMatrix(Level.INFO)
self._rgbmatrix.set_display_type(DisplayType.SCAN)
self._log.info('ready.')
# ..........................................................................
def _warning_display(self):
self._rgbmatrix.disable()
self._rgbmatrix.set_display_type(DisplayType.RANDOM)
self._rgbmatrix.enable()
# ..........................................................................
def _activated(self):
'''
The default function called when the sensor is activated.
'''
if self._enabled and self._scan_enabled:
self._log.info('cat sensed.')
self._log.info(Fore.RED + Style.BRIGHT + 'detected CAT!')
self._queue.add(Message(Event.CAT_SCAN))
self.set_mode(False)
self._warning_display()
else:
self._log.info('[DISABLED] activated catscan.')
# ..........................................................................
def _deactivated(self):
'''
The default function called when the sensor is deactivated.
'''
if self._enabled:
self._log.info('deactivated catscan.')
else:
self._log.debug('[DISABLED] deactivated catscan.')
# ..........................................................................
def is_waiting(self):
'''
Returns true if the scan is in progress.
'''
return self._scan_enabled
# ..........................................................................
def set_mode(self, active):
'''
Sets the mode to active or inactive. This raises or lowers the servo arm.
'''
if not self._enabled:
self._log.warning('cannot scan: disabled.')
return
_current_angle = self._servo.get_position(-999.0)
self._log.critical('current angle: {:>4.1f}°'.format(_current_angle))
if active:
if _current_angle != -999.0 and _current_angle is not self._active_angle:
with _mutex:
# for degrees in numpy.arange(self._inactive_angle, self._active_angle + 0.01, -1.0 * self._arm_movement_degree_step):
for degrees in numpy.arange(
_current_angle, self._active_angle + 0.01,
-1.0 * self._arm_movement_degree_step):
self._log.debug(
'position set to: {:>4.1f}°'.format(degrees))
self._servo.set_position(degrees)
time.sleep(self._arm_up_delay)
self._log.info('position set to active.')
else:
self._log.warning('position already set to active.')
# wait until it settles
self._log.info('waiting for motion detector to settle...')
time.sleep(1.0)
while self._sensor.motion_detected:
self._log.debug('still waiting...')
time.sleep(0.1)
# time.sleep(5.0)
self._scan_enabled = True
self._rgbmatrix.enable()
else:
self._scan_enabled = False
self._rgbmatrix.disable()
if _current_angle != -999.0 and _current_angle is not self._inactive_angle:
with _mutex:
for degrees in numpy.arange(
self._active_angle, self._inactive_angle + 0.1,
self._arm_movement_degree_step):
self._log.debug(
'position set to: {:>4.1f}°'.format(degrees))
self._servo.set_position(degrees)
time.sleep(self._arm_down_delay)
self._log.info('position set to inactive.')
else:
self._log.warning('position already set to inactive.')
# ..........................................................................
def enable(self):
self._log.debug('enabling...')
if self._closed:
self._log.warning('cannot enable: closed.')
return
# self._tof.enable()
self._enabled = True
self._log.debug('enabled.')
# ..........................................................................
def disable(self):
if self._disabling:
self._log.warning('already disabling.')
else:
self._disabling = True
self._enabled = False
self._rgbmatrix.disable()
self._log.debug('disabling...')
self.set_mode(False)
self._servo.disable()
self._servo.close()
self._disabling = False
self._log.debug('disabled.')
# ..........................................................................
def close(self):
self.disable()
self._closed = True
import time
from lib.movement import Movement
from lib.dualshock import DualShock
from lib.servo import Servo
import signal
import sys
import atexit
ps3 = DualShock()
movement = Movement()
servo1 = Servo(17)
servo2 = Servo(26)
def shutdown(signal, frame):
print('Stopping Robot...')
servo1.stop()
servo2.stop()
movement.cleanup()
sys.exit(0)
# atexit.register(shutdown)
signal.signal(signal.SIGTERM, shutdown)
signal.signal(signal.SIGINT, shutdown)
y = 0.00
x = 0.00
for event in ps3.controller.read_loop():
if event.code == 02 and event.type == 3:
x = ps3.scale_stick(event.value)
class WallFollower():
def __init__(self, config, level):
self._log = Logger('follower', Level.INFO)
self._config = config
if self._config:
self._log.info('configuration provided.')
_config = self._config['ros'].get('wall_follower')
self._port_angle = _config.get('port_angle')
self._stbd_angle = _config.get('starboard_angle')
_range_value = _config.get('tof_range')
_range = Range.from_str(_range_value)
_servo_number = _config.get('servo_number')
else:
self._log.warning('no configuration provided.')
self._port_angle = -90.0
self._stbd_angle = 90.0
_range = Range.PERFORMANCE
_servo_number = 2
# _range = Range.LONG
# _range = Range.MEDIUM
# _range = Range.SHORT
self._log.info(
'wall follower port angle: {:>5.2f}°; starboard angle: {:>5.2f}°'.
format(self._port_angle, self._stbd_angle))
self._servo = Servo(self._config, _servo_number, level)
self._tof = TimeOfFlight(_range, Level.WARN)
self._max_retries = 10
self._enabled = False
self._closed = False
self._log.info('ready.')
# ..........................................................................
def scan(self):
mm = 0.0
wait_count = 0
while mm <= 0.1 and wait_count < self._max_retries:
mm = self._tof.read_distance()
time.sleep(0.0025)
wait_count += 1
self._log.info('distance: {:>5.0f}mm'.format(mm))
return mm
# ..........................................................................
def set_position(self, orientation):
if not self._enabled:
self._log.warning('cannot scan: disabled.')
return
if orientation == Orientation.PORT:
self._servo.set_position(self._port_angle)
elif orientation == Orientation.STBD:
self._servo.set_position(self._stbd_angle)
else:
raise ValueError(
'only port or starboard acceptable for wall follower orientation'
)
self._log.info('wall follower position set to {}.'.format(orientation))
# ..........................................................................
def enable(self):
if self._closed:
self._log.warning('cannot enable: closed.')
return
self._tof.enable()
self._enabled = True
# ..........................................................................
def disable(self):
self._enabled = False
self._servo.set_position(0.0)
self._servo.disable()
self._tof.disable()
# ..........................................................................
def close(self):
self.disable()
self._servo.close()
self._closed = True
class Lidar():
'''
A combination of a VL53L1X laser distance sensor and a micro servo as a LIDAR.
The zero position is straight forward, with negative (minimum servo position)
sweeping to port, positive (maximum servo position) to starboard.
The 'reverse_movement' parameter is used in case the servo movement is backwards.
'''
def __init__(self, config, level):
self._log = Logger('lidar', Level.INFO)
self._config = config
if self._config:
self._log.info('configuration provided.')
_config = self._config['ros'].get('lidar')
self._reverse_movement = _config.get('reverse_movement')
self._double_sweep = _config.get('double_sweep')
self._min_angle = _config.get('min_angle')
self._max_angle = _config.get('max_angle')
self._degree_step = _config.get('degree_step')
self._step_delay_sec = _config.get('step_delay_sec')
_range_value = _config.get('tof_range')
_range = Range.from_str(_range_value)
_servo_number = _config.get('servo_number')
else:
self._log.warning('no configuration provided.')
self._log.info('play sound disabled.')
self._reverse_movement = False
self._double_sweep = False
self._min_angle = -60.0
self._max_angle = 60.0
self._degree_step = 3.0
self._step_delay_sec = 0.01
_range = Range.PERFORMANCE
# _range = Range.LONG
# _range = Range.MEDIUM
# _range = Range.SHORT
_servo_number = -1
self._log.info(
'scan range of {} from {:>4.1f}° to {:>4.1f}° with step of {:>4.1f}°'
.format(_range, self._min_angle, self._max_angle,
self._degree_step))
if _servo_number > 0:
self._servo = Servo(self._config, _servo_number, level)
else:
self._servo = None
_range = Range.PERFORMANCE
# _range = Range.LONG
# _range = Range.MEDIUM
# _range = Range.SHORT
self._tof = TimeOfFlight(_range, Level.WARN)
self._error_range = 0.067
self._enabled = False
self._closed = False
self._log.info('ready.')
# ..........................................................................
def _in_range(self, p, q):
return p >= (q - self._error_range) and p <= (q + self._error_range)
# ..........................................................................
def _set_servo_position(self, angle):
if self._servo:
if self._reverse_movement:
self._servo.set_position(-1.0 * angle)
else:
self._servo.set_position(angle)
# ..........................................................................
def _get_servo_position(self, default_angle):
if self._servo:
if self._reverse_movement:
return -1.0 * self._servo.get_position(-1.0 * default_angle)
else:
return self._servo.get_position(default_angle)
# ..........................................................................
def scan(self):
if not self._enabled:
self._log.warning('cannot scan: disabled.')
return None
try:
if self._servo:
start = time.time()
_min_mm = 9999.0
_angle_at_min = 0.0
_max_mm = 0
_angle_at_max = 0.0
self._set_servo_position(self._min_angle)
time.sleep(0.3)
# wait_count = 0
# while ( not self._in_range(self._get_servo_position(self._min_angle), self._min_angle) ) and ( wait_count < 20 ):
# wait_count += 1
# self._set_servo_position(self._min_angle)
# self._log.info(Fore.MAGENTA + Style.BRIGHT + 'waiting for match at degrees: {:>5.2f}°: waited: {:d}'.format(self._get_servo_position(-1), wait_count))
# time.sleep(1.0)
# time.sleep(0.05)
# self._log.info(Fore.YELLOW + Style.BRIGHT + 'starting scan from degrees: {:>5.2f}°: waited: {:d}'.format(self._get_servo_position(-1), wait_count))
# sweep from minimum to maximum ........................................................
self._log.info('sweep fore...')
# self._log.info('sweep min to max...')
for degrees in numpy.arange(self._min_angle,
self._max_angle + 0.01,
self._degree_step):
self._set_servo_position(degrees)
wait_count = 0
while not self._in_range(self._get_servo_position(degrees),
degrees) and wait_count < 10:
time.sleep(0.0025)
wait_count += 1
self._log.debug(Fore.GREEN + Style.BRIGHT + 'measured degrees: {:>5.2f}°: \ttarget: {:>5.2f}°; waited: {:d}'.format(\
self._get_servo_position(degrees), degrees, wait_count))
mm = self._tof.read_distance()
self._log.info('distance at {:>5.2f}°: \t{}mm'.format(
degrees, mm))
# capture min and max at angles
_min_mm = min(_min_mm, mm)
if mm == _min_mm:
_angle_at_min = degrees
_max_mm = max(_max_mm, mm)
if mm == _max_mm:
_angle_at_max = degrees
time.sleep(self._step_delay_sec)
if self._double_sweep:
self._log.info('sweep back...')
# self._log.info('sweep max to min...')
# sweep from maximum to minimum ........................................................
for degrees in numpy.arange(self._max_angle,
self._min_angle + 0.01,
-1.0 * self._degree_step):
self._set_servo_position(degrees)
wait_count = 0
while not self._in_range(
self._get_servo_position(degrees),
degrees) and wait_count < 10:
time.sleep(0.0025)
wait_count += 1
self._log.debug(Fore.GREEN + Style.BRIGHT + 'measured degrees: {:>5.2f}°: \ttarget: {:>5.2f}°; waited: {:d}'.format(\
self._get_servo_position(degrees), degrees, wait_count))
mm = self._tof.read_distance()
self._log.info('distance at {:>5.2f}°: \t{}mm'.format(
degrees, mm))
# capture min and max at angles
_min_mm = min(_min_mm, mm)
if mm == _min_mm:
_angle_at_min = degrees
_max_mm = max(_max_mm, mm)
if mm == _max_mm:
_angle_at_max = degrees
time.sleep(self._step_delay_sec)
time.sleep(0.1)
# self._log.info('complete.')
elapsed = time.time() - start
self._log.info('min. distance at {:>5.2f}°:\t{}mm'.format(
_angle_at_min, _min_mm))
self._log.info('max. distance at {:>5.2f}°:\t{}mm'.format(
_angle_at_max, _max_mm))
self._log.info(
'scan complete: {:>5.2f}sec elapsed.'.format(elapsed))
self._set_servo_position(0.0)
# self._servo.set_position(_angle_at_max)
# self.close()
return [_angle_at_min, _min_mm, _angle_at_max, _max_mm]
else:
_degrees = 0
_start = dt.now()
mm = self._tof.read_distance()
_elapsed_ms = (dt.now() - _start).total_seconds() * 1000.0
self._log.info('distance at {:>5.2f}°: \t{}mm'.format(
_degrees, mm))
self._log.info(
'scan complete: {:>5.2f}ms elapsed.'.format(_elapsed_ms))
return [-1.0, mm, -1.0, mm]
except KeyboardInterrupt:
self._log.info('caught Ctrl-C.')
self.close()
self._log.info('interrupted: incomplete.')
# ..........................................................................
def enable(self):
if self._closed:
self._log.warning('cannot enable: closed.')
return
self._tof.enable()
self._enabled = True
# ..........................................................................
def disable(self):
self._enabled = False
if self._servo:
self._servo.disable()
self._tof.disable()
# ..........................................................................
def close(self):
self.disable()
if self._servo:
self._servo.close()
self._closed = True
def test_servo_write_raises_if_position_out_of_bounds(mock_comm):
servo = Servo('TEST', mock_comm)
with pytest.raises(ValueError):
servo.write(-1.1)
class RosCar():
def __init__(self, cfg):
self.STEERING_NEUTRAL = cfg['servo_neutral_angle']
self.MIN_STEERING_ANGLE = cfg[
'servo_min_angle_limit'] # Steering left/right max angle.
self.MAX_STEERING_ANGLE = cfg[
'servo_max_angle_limit'] # Steering left/right max angle.
self.STEERING_RATE = cfg[
'steering_rate'] # Server steering angle is fomula angle. But car_client depends on car. Drift type steering, normal steering, etc. Therefore, use this rate for ajust good angle.
self.MOTOR_NEUTRAL_SPEED = cfg['motor_neutral_speed']
self.MOTOR_FORWARD_SPEED_RATE = float(
cfg['motor_max_speed_limit']
) / float(
100
) # Server max speed is 100. But car_client depends on config parameter.
self.MOTOR_BACK_SPEED_RATE = float(
cfg['motor_min_speed_limit']
) / float(
-100
) # Server min speed is -100. But car_client depends on config parameter.
self.steering = Servo(cfg)
self.motor = Motor(cfg)
self.steering.set_angle(self.STEERING_NEUTRAL, delay=0)
self.motor.set_speed(self.MOTOR_NEUTRAL_SPEED, delay=0)
self.speed = self.MOTOR_NEUTRAL_SPEED
return
def __del__(self):
self.steering.set_angle(self.STEERING_NEUTRAL, delay=0)
self.motor.set_speed(self.MOTOR_NEUTRAL_SPEED, delay=0)
return
def listener(self):
"""
ROS Topicの更新時に処理を行う設定
rosnode list
rostopic echo /cmd_vel
rostopic echo /ackermann_cmd
"""
rospy.init_node('ROSCarNano', anonymous=True)
"""
アッカーマン運動力学
"""
ackermann = AckermannPublisher()
ackermann.add()
rospy.Subscriber('/ackermann_cmd', AckermannDriveStamped,
self.ackermann_cmd)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
return
def ackermann_cmd(self, values):
"""
ステアリング・速度のターゲット値の処理
ステアリング値のsteering_angleは角度(rad)なので、degreeに変換する
speed: m/s
target_speed: motor power %
# map(指示を受けた速度(m/s), 指示速度の後方最高速度m/s, 指示速度の最高速度m/s, モーター後方最高出力%, モーター前方最高出力%)
# ここでは前後モーター最高出力は100%として、デバイス設定時に車両設定の速度制限を適用する。
"""
print("/ackermann_cmd: {}".format(values))
#speed = abs(values.drive.speed) # no backwords
speed = values.drive.speed
target_speed = map(speed, -0.28, 0.28, -100.0, 100.0)
"""
ステアリング角
アッカーマン運動力学のステアリング角を利用する
"""
angle_rad = values.drive.steering_angle
self.set_speed(target_speed)
self.set_angle_rad(angle_rad)
def rad_to_angle(self, angle_rad):
"""
angle_radをサーボ可動域に変換する。
angle_radは右カーブがマイナス、左カーブがプラス。
サーボは右カーブが90-180度、左カーブが0-90度となるため、
angle_radの正負を逆にしてdegreeに変換してから90度加える。
ω(rad/s)
θ= angle_rad*180/pi (deg/s)
rad = θ*pi/180
"""
angle_rad = -1.0 * angle_rad
theta = float(angle_rad) * float(180) / math.pi
angle = 90.0 - (180.0 - theta) / 2.0
return angle
def set_angle_rad(self, angle_rad):
"""
omega: rad/s
"""
steering_angle = self.rad_to_angle(angle_rad)
#print("omega to angle: {} -> {}".format(omega, steering_angle))
steering_angle = int(float(steering_angle) * float(self.STEERING_RATE))
steering_angle = self.STEERING_NEUTRAL + steering_angle
"""
Adjust within operable angle
"""
if steering_angle > self.MAX_STEERING_ANGLE:
steering_angle = self.MAX_STEERING_ANGLE
if steering_angle < self.MIN_STEERING_ANGLE:
steering_angle = self.MIN_STEERING_ANGLE
print("steering: {}".format(steering_angle))
self.steering.set_angle(steering_angle)
def set_speed(self, speed):
"""
speed: Analog int value for PCA9685
"""
motor_speed = speed
if motor_speed > 0:
motor_speed = int(
float(motor_speed) * float(self.MOTOR_FORWARD_SPEED_RATE))
elif motor_speed < 0:
motor_speed = int(
float(motor_speed) * float(self.MOTOR_BACK_SPEED_RATE))
print("motor: {}".format(motor_speed))
self.motor.set_speed(motor_speed)
def brake(self, value):
if value.data:
self.motor.set_speed(self.MOTOR_NEUTRAL_SPEED, delay=0)
class RoofMount:
_config = {
'servo': {
'enabled': True,
# for calibration
'level_degrees': 35,
'max_degrees': 70, # down
'min_degrees': -48, # up
},
'lidar': {
'enabled': True,
'updateRatePerSecond': 270
}
}
def __init__(self, config={}):
self._config.update(config)
self._stepper = Stepper()
self._stepper.disable()
self._servo = Servo()
self.set_vertical_position(0)
self._lidar = Lidar()
def up(self, degrees=10):
'''Move up relative to current position'''
try:
self._servo.set_position(self._servo.position - degrees)
except:
pass
def down(self, degrees=10):
'''Move down relative to current position'''
try:
self._servo.set_position(self._servo.position + degrees)
except:
pass
def level(self):
'''Vertically level the mount'''
self._servo.set_position(self._config['servo']['level_degrees'])
def clockwise(self, degrees=10):
'''Move clockwise relative to current position'''
self.__rotate(Stepper.CLOCKWISE,
(self._stepper.position() + degrees) % 360)
def counter_clockwise(self, degrees=10):
'''Move counter-clockwise relative to current position'''
self.__rotate(Stepper.COUNTER_CLOCKWISE,
(self._stepper.position() - degrees) % 360)
def __rotate(self, dir, degrees):
self._stepper.enable()
self._stepper.set_direction(dir)
self._stepper.set_position(degrees)
self._stepper.disable()
def home(self):
'''Move stepper and servo to home positions'''
self._stepper.home()
self.level()
def horizontal_position(self):
return self._stepper.position()
def vertical_position(self):
return self._config['servo']['level_degrees'] - self._servo.position()
def set_horizontal_position(self, degrees):
'''Take shortest path to target degrees'''
diff = degrees - self._stepper.position()
if diff < 0:
diff += 360
if diff <= 180:
self.__rotate(Stepper.CLOCKWISE, degrees)
else:
self.__rotate(Stepper.COUNTER_CLOCKWISE, degrees)
def set_vertical_position(self, degrees):
'''Position relative to the horizon'''
min = -(self._config['servo']['max_degrees'] -
self._config['servo']['level_degrees'])
max = -(self._config['servo']['min_degrees'] -
self._config['servo']['level_degrees'])
if degrees < min or degrees > max:
print('Position ', degrees, 'out of range (', min, ',', max, ')')
return
pos = -degrees + self._config['servo']['level_degrees']
self._servo.set_position(pos)
def get_readings(self):
return {
'vertical_position': self.vertical_position(),
'horizontal_position': self.horizontal_position(),
'lidar': self._lidar.distance(),
}
def horizontal_scan(self, vertical_degrees, resolution=1.0, callback=None):
'''Performs a 360 scan at a specified angle. Resolution is a
percentage and dictates how often a readings is performed during
the scan.'''
print("Performing scan.")
readings = []
self.set_vertical_position(vertical_degrees)
self._stepper.enable()
for step in range(self._stepper._stepsPerRevolution):
self._stepper.step()
if step % (1 / resolution) == 0:
reading = self.get_readings()
readings.append(reading)
if callback is not None:
callback(reading)
self._stepper.disable()
return readings
class RosCar():
def __init__(self, cfg):
self.STEERING_NEUTRAL = cfg['servo_neutral_angle']
self.MIN_STEERING_ANGLE = cfg[
'servo_min_angle_limit'] # Steering left/right max angle.
self.MAX_STEERING_ANGLE = cfg[
'servo_max_angle_limit'] # Steering left/right max angle.
self.STEERING_RATE = cfg[
'steering_rate'] # Server steering angle is fomula angle. But car_client depends on car. Drift type steering, normal steering, etc. Therefore, use this rate for ajust good angle.
self.MOTOR_NEUTRAL_SPEED = cfg['motor_neutral_speed']
self.MOTOR_FORWARD_SPEED_RATE = float(
cfg['motor_max_speed_limit']
) / float(
100
) # Server max speed is 100. But car_client depends on config parameter.
self.MOTOR_BACK_SPEED_RATE = float(
cfg['motor_min_speed_limit']
) / float(
-100
) # Server min speed is -100. But car_client depends on config parameter.
self.steering = Servo(cfg)
self.motor = Motor(cfg)
self.steering.set_angle(self.STEERING_NEUTRAL, delay=0)
self.motor.set_speed(self.MOTOR_NEUTRAL_SPEED, delay=0)
self.speed = self.MOTOR_NEUTRAL_SPEED
self.back_in = False
self.back_start_time = None
return
def __del__(self):
self.steering.set_angle(self.STEERING_NEUTRAL, delay=0)
self.motor.set_speed(self.MOTOR_NEUTRAL_SPEED, delay=0)
return
def listener(self):
"""
READ ROS TOPICS AND RUN FUNCTION BY HZ.
rosnode list
rosnode info /twist_filter
rostopic echo /cmd_vel
"""
rospy.init_node('ROSCarNano', anonymous=True)
rospy.Subscriber('/cmd_vel', Twist, self.cmd_vel)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
return
def cmd_vel(self, values):
"""
ステアリング・速度のターゲット値の処理
ステアリング値のomegaはターゲット角速度である。現在の速度を加味してomegaを算出しなければならない
self.target_speed: target speed (m/s)
"""
"""
Autoware 1.9.1にバック機能は無い。
そのため速度は負の値は正の値として扱う。
"""
print("/cmd_vel: {}".format(values))
speed = abs(values.linear.x)
target_speed = map(speed, 0.0, 0.22, 0.0, 30.0)
"""
Autowareの角速度は右カーブがマイナス、左カーブがプラス。
omega: rad/s and 10 Hz
1 processing = 1 Hz = 0.1 * omega
left/right = -1 * omega
"""
omega = values.angular.z
"""
omegaはtarget_speedが時速2kmの時、ベストマッチ。
速度が上がった時、それに応じてomegaを減算させる。
"""
print("omega: {} speed: {}".format(omega, speed))
self.set_speed(target_speed)
self.set_angle_rad(omega)
return
def omega_to_angle(self, omega):
"""
角速度ωをサーボ可動域に変換する。
omegaは右カーブがマイナス、左カーブがプラス。
サーボは右カーブが90-180度、左カーブが0-90度となるため、
omegaの正負を逆にしてdegreeに変換してから90度加える。
ω(rad/s)
θ= ω*180/pi (deg/s)
rad = θ*pi/180
"""
omega = -1.0 * omega
theta = float(omega) * float(180) / math.pi
angle = 90.0 - (180.0 - theta) / 2.0
return angle
def set_angle_rad(self, omega):
"""
omega: rad/s
"""
steering_angle = self.omega_to_angle(omega)
#print("omega to angle: {} -> {}".format(omega, steering_angle))
steering_angle = int(float(steering_angle) * float(self.STEERING_RATE))
steering_angle = self.STEERING_NEUTRAL + steering_angle
"""
Adjust within operable angle
"""
if steering_angle > self.MAX_STEERING_ANGLE:
steering_angle = self.MAX_STEERING_ANGLE
if steering_angle < self.MIN_STEERING_ANGLE:
steering_angle = self.MIN_STEERING_ANGLE
print("steering: {}".format(steering_angle))
self.steering.set_angle(steering_angle)
def set_speed(self, speed):
"""
speed: Analog int value for PCA9685
"""
motor_speed = speed
if motor_speed > 0:
motor_speed = int(
float(motor_speed) * float(self.MOTOR_FORWARD_SPEED_RATE))
elif motor_speed < 0:
motor_speed = int(
float(motor_speed) * float(self.MOTOR_BACK_SPEED_RATE))
self.motor.set_speed(motor_speed)
def brake(self, value):
if value.data:
self.motor.set_speed(self.MOTOR_NEUTRAL_SPEED, delay=0)
def test_servo_write_sets_desired_position(mock_comm):
servo = Servo('TEST', mock_comm)
mock_comm.reset_output_buffer()
servo.write(0.4)
assert mock_comm.readline() == '#TESTP1700\r'
assert servo.position == 0.4
def main():
print("start")
steering = None
try:
"""
LOAD SETUP VARIABLES
"""
cfg = load_config()
steering = Servo(cfg)
delay = 0.1
angle = 85
steering.set_angle(angle)
time.sleep(1)
for i in range(10):
angle = 70
steering.set_angle(angle, delay)
time.sleep(1)
angle = 85
steering.set_angle(angle, delay)
time.sleep(1)
angle = 100
steering.set_angle(angle)
time.sleep(1)
angle = 85
steering.set_angle(angle, delay)
time.sleep(1)
except:
import traceback
traceback.print_exc()
finally:
if steering is not None:
angle = 85
steering.set_angle(angle)
print("end")
class RosCar():
def __init__(self, cfg):
self.STEERING_NEUTRAL = cfg['servo_neutral_angle']
self.MIN_STEERING_ANGLE = cfg['servo_min_angle_limit'] # Steering left/right max angle.
self.MAX_STEERING_ANGLE = cfg['servo_max_angle_limit'] # Steering left/right max angle.
self.STEERING_RATE = cfg['steering_rate'] # Server steering angle is fomula angle. But car_client depends on car. Drift type steering, normal steering, etc. Therefore, use this rate for ajust good angle.
self.MOTOR_NEUTRAL_SPEED = cfg['motor_neutral_speed']
self.MOTOR_FORWARD_SPEED_RATE = float(cfg['motor_max_speed_limit'])/float(100) # Server max speed is 100. But car_client depends on config parameter.
self.MOTOR_BACK_SPEED_RATE = float(cfg['motor_min_speed_limit'])/float(-100) # Server min speed is -100. But car_client depends on config parameter.
self.steering = Servo(cfg)
self.motor = Motor(cfg)
self.steering.set_angle(self.STEERING_NEUTRAL, delay=0)
self.motor.set_speed(self.MOTOR_NEUTRAL_SPEED, delay=0)
self.speed = self.MOTOR_NEUTRAL_SPEED
self.back_in = False
self.back_start_time = None
self.target_speed = self.MOTOR_NEUTRAL_SPEED
return
def __del__(self):
self.steering.set_angle(self.STEERING_NEUTRAL, delay=0)
self.motor.set_speed(self.MOTOR_NEUTRAL_SPEED, delay=0)
return
def listener(self):
"""
READ ROS TOPICS AND RUN FUNCTION BY HZ.
rosnode list
rosnode info /twist_filter
rostopic echo /twist_cmd
"""
rospy.init_node('RoboCarROS', anonymous=True)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd)
# current_velocity
rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
return
def current_velocity(self, values):
"""
現在速度と目標速度を比較して、モーター出力を調整する。
self.speed: モーター出力
current_velocity: 現在速度
self.target_speed: 目標速度
"""
current_velocity = values.twist.linear.x
print("speed now: {} target: {}".format(current_velocity, self.target_speed))
if current_velocity < self.target_speed:
self.speed += 1
elif current_velocity > self.target_speed:
self.speed -= 1
if self.speed >= 100:
self.speed = 100
elif self.speed <= 0:
self.speed = 0
"""
現在速度が0.1(m/s)未満の時、急発進を抑えるためにモーター出力を制限する。
"""
if self.speed > 25 and current_velocity < 0.1:
self.speed = 25
self.set_speed(self.speed)
return
def twist_cmd(self, values):
"""
ステアリング・速度のターゲット値の処理
ステアリング値はここで適用する
速度は変数で保持し、current_velocityによる現在速度取得時に適用する
ステアリング値のomegaはターゲット角速度である。現在の速度を加味してomegaを算出しなければならない
self.target_speed: target speed (m/s)
"""
#print(values)
"""
Autoware 1.9.1にバック機能は無い。
そのため速度は負の値は正の値として扱う。
"""
speed = abs(values.twist.linear.x)
self.target_speed = speed
"""
Autowareの角速度は右カーブがマイナス、左カーブがプラス。
omega: rad/s and 10 Hz
1 processing = 1 Hz = 0.1 * omega
left/right = -1 * omega
"""
omega = values.twist.angular.z
"""
omegaはtarget_speedが時速2kmの時、ベストマッチ。
速度が上がった時、それに応じてomegaを減算させる。
"""
print("before omega: {} speed: {}".format(omega, speed))
if self.target_speed >= 0.56:
"""
目標速度が時速2km(0.56m/s)以上の時、比率に応じてomegaを小さくする
"""
ratio = map(self.target_speed, 2.0, 1000.0, 1.0, 1000.0)
omega = omega*ratio
print("after omega: {} speed: {}".format(omega, speed))
self.set_angle_rad(omega)
return
def omega_to_angle(self, omega):
"""
角速度ωをサーボ可動域に変換する。
omegaは右カーブがマイナス、左カーブがプラス。
サーボは右カーブが90-180度、左カーブが0-90度となるため、
omegaの正負を逆にしてdegreeに変換してから90度加える。
ω(rad/s)
θ= ω*180/pi (deg/s)
rad = θ*pi/180
"""
omega = -1.0 * omega
theta = float(omega)*float(180)/math.pi
angle = 90.0 - (180.0 - theta)/2.0
return angle
def set_angle_rad(self, omega):
"""
omega: rad/s
"""
steering_angle = self.omega_to_angle(omega)
#print("omega to angle: {} -> {}".format(omega, steering_angle))
steering_angle = int(float(steering_angle) * float(self.STEERING_RATE))
steering_angle = self.STEERING_NEUTRAL + steering_angle
"""
Adjust within operable angle
"""
if steering_angle > self.MAX_STEERING_ANGLE:
steering_angle = self.MAX_STEERING_ANGLE
if steering_angle < self.MIN_STEERING_ANGLE:
steering_angle = self.MIN_STEERING_ANGLE
print("steering: {}".format(steering_angle))
self.steering.set_angle(steering_angle)
def set_speed(self, speed):
"""
speed: Analog int value for PCA9685
"""
motor_speed = speed
if motor_speed > 0:
motor_speed = int(float(motor_speed) * float(self.MOTOR_FORWARD_SPEED_RATE))
elif motor_speed < 0:
motor_speed = int(float(motor_speed) * float(self.MOTOR_BACK_SPEED_RATE))
self.motor.set_speed(motor_speed)
def brake(self, value):
if value.data:
self.motor.set_speed(self.MOTOR_NEUTRAL_SPEED, delay=0)