<s> '''<STR_LIT>''' <EOL> import imp <EOL> import math <EOL> from os . path import exists , join <EOL> import threading <EOL> import logging <EOL> logger = logging . getLogger ( '<STR_LIT>' ) <EOL> from hal_impl . data import hal_data <EOL> class PhysicsInitException ( Exception ) : <EOL> pass <EOL> class PhysicsEngine : <EOL> '''<STR_LIT>''' <EOL> def __init__ ( self , physics_controller ) : <EOL> '''<STR_LIT>''' <EOL> self . physics_controller = physics_controller <EOL> def initialize ( self , hal_data ) : <EOL> '''<STR_LIT>''' <EOL> pass <EOL> def update_sim ( self , hal_data , now , tm_diff ) : <EOL> '''<STR_LIT>''' <EOL> pass <EOL> class PhysicsInterface : <EOL> '''<STR_LIT>''' <EOL> def __init__ ( self , robot_path , fake_time , config_obj ) : <EOL> self . last_tm = None <EOL> self . _lock = threading . Lock ( ) <EOL> self . x = config_obj [ '<STR_LIT>' ] [ '<STR_LIT>' ] [ '<STR_LIT>' ] <EOL> self . y = config_obj [ '<STR_LIT>' ] [ '<STR_LIT>' ] [ '<STR_LIT>' ] <EOL> self . angle = math . radians ( config_obj [ '<STR_LIT>' ] [ '<STR_LIT>' ] [ '<STR_LIT>' ] ) <EOL> self . vx = <NUM_LIT:0> <EOL> self . vy = <NUM_LIT:0> <EOL> self . fake_time = fake_time <EOL> self . robot_enabled = False <EOL> self . config_obj = config_obj <EOL> self . engine = None <EOL> self . analog_gyro_channels = [ ] <EOL> self . device_gyro_channels = [ ] <EOL> self . hal_data = hal_data <EOL> physics_module_path = join ( robot_path , '<STR_LIT>' ) <EOL> if exists ( physics_module_path ) : <EOL> try : <EOL> physics_module = imp . load_source ( '<STR_LIT>' , physics_module_path ) <EOL> except : <EOL> logger . exception ( "<STR_LIT>" ) <EOL> raise PhysicsInitException ( ) <EOL> if not hasattr ( physics_module , '<STR_LIT>' ) : <EOL> logger . error ( "<STR_LIT>" ) <EOL> raise PhysicsInitException ( ) <EOL> try : <EOL> self . engine = physics_module . PhysicsEngine ( self ) <EOL> if hasattr ( self . engine , '<STR_LIT>' ) : <EOL> self . engine . initialize ( self . hal_data ) <EOL> except : <EOL> logger . exception ( "<STR_LIT>" ) <EOL> raise PhysicsInitException ( ) <EOL> logger . info ( "<STR_LIT>" ) <EOL> else : <EOL> logger . warning ( "<STR_LIT>" , physics_module_path ) <EOL> def setup_main_thread ( self ) : <EOL> if self . engine is not None : <EOL> self . fake_time . set_physics_fn ( self . _on_increment_time ) <EOL> def __repr__ ( self ) : <EOL> return '<STR_LIT>' <EOL> def _on_increment_time ( self , now ) : <EOL> last_tm = self . last_tm <EOL> if last_tm is None : <EOL> self . last_tm = now <EOL> else : <EOL> tm_diff = now - last_tm <EOL> if tm_diff > <NUM_LIT> : <EOL> self . engine . update_sim ( self . hal_data , now , tm_diff ) <EOL> self . last_tm = now <EOL> def _set_robot_enabled ( self , enabled ) : <EOL> self . robot_enabled = enabled <EOL> def _has_engine ( self ) : <EOL> return self . engine is not None <EOL> def add_analog_gyro_channel ( self , ch ) : <EOL> '''<STR_LIT>''' <EOL> self . analog_gyro_channels . append ( ch ) <EOL> add_gyro_channel = add_analog_gyro_channel <EOL> def add_device_gyro_channel ( self , angle_key ) : <EOL> '''<STR_LIT>''' <EOL> hal_data [ '<STR_LIT>' ] [ angle_key ] = <NUM_LIT:0> <EOL> self . device_gyro_channels . append ( angle_key ) <EOL> def drive ( self , speed , rotation_speed , tm_diff ) : <EOL> '''<STR_LIT>''' <EOL> if not self . robot_enabled : <EOL> return <EOL> distance = speed * tm_diff <EOL> angle = rotation_speed * tm_diff <EOL> x = distance * math . cos ( angle ) <EOL> y = distance * math . sin ( angle ) <EOL> self . _move ( x , y , angle ) <EOL> def vector_drive ( self , vx , vy , vw , tm_diff ) : <EOL> '''<STR_LIT>''' <EOL> if not self . robot_enabled : <EOL> return <EOL> angle = vw * tm_diff <EOL> vx = ( vx * tm_diff ) <EOL> vy = ( vy * tm_diff ) <EOL> x = vx * math . sin ( angle ) + vy * math . cos ( angle ) <EOL> y = vx * math . cos ( angle ) + vy * math . sin ( angle ) <EOL> self . _move ( x , y , angle ) <EOL> def _move ( self , x , y , angle ) : <EOL> with self . _lock : <EOL> self . vx += x <EOL> self . vy += y <EOL> self . angle += angle <EOL> c = math . cos ( self . angle ) <EOL> s = math . sin ( self . angle ) <EOL> self . x += ( x * c - y * s ) <EOL> self . y += ( x * s + y * c ) <EOL> self . _update_gyros ( angle ) <EOL> def _update_gyros ( self , angle ) : <EOL> angle = math . degrees ( angle ) <EOL> for k in self . device_gyro_channels : <EOL> self . hal_data [ '<STR_LIT>' ] [ k ] += angle <EOL> gyro_value = angle / <NUM_LIT> <EOL> for gyro_ch in self . analog_gyro_channels : <EOL> self . hal_data [ '<STR_LIT>' ] [ gyro_ch ] [ '<STR_LIT>' ] += gyro_value <EOL> def get_position ( self ) : <EOL> '''<STR_LIT>''' <EOL> with self . _lock : <EOL> return self . x , self . y , self . angle <EOL> def _get_vector ( self ) : <EOL> '''<STR_LIT>''' <EOL> return self . vx , self . vy , self . angle </s>
