<s> from __future__ import division <EOL> import numpy as np <EOL> import drc as lcmdrc <EOL> from director import irisUtils <EOL> from director import applogic as app <EOL> from director import objectmodel as om <EOL> from director import transformUtils <EOL> from director import lcmUtils <EOL> from director import robotstate <EOL> from director . terrainitem import TerrainRegionItem <EOL> from director . lcmframe import positionMessageFromFrame <EOL> from director . utime import getUtime <EOL> class IRISDriver ( object ) : <EOL> def __init__ ( self , jointController , <EOL> footstepParams , <EOL> request_channel = '<STR_LIT>' , <EOL> response_channel = '<STR_LIT>' ) : <EOL> self . jointController = jointController <EOL> self . params = footstepParams <EOL> self . map_mode_map = [ <EOL> lcmdrc . footstep_plan_params_t . FOOT_PLANE , <EOL> lcmdrc . footstep_plan_params_t . TERRAIN_HEIGHTS_AND_NORMALS , <EOL> lcmdrc . footstep_plan_params_t . TERRAIN_HEIGHTS_Z_NORMALS , <EOL> lcmdrc . footstep_plan_params_t . HORIZONTAL_PLANE <EOL> ] <EOL> self . request_channel = request_channel <EOL> self . response_channel = response_channel <EOL> self . regions = { } <EOL> self . sub = lcmUtils . addSubscriber ( self . response_channel , lcmdrc . iris_region_response_t , self . onIRISRegionResponse ) <EOL> self . segmentation_sub = lcmUtils . addSubscriber ( '<STR_LIT>' , lcmdrc . iris_region_response_t , self . onIRISRegionResponse ) <EOL> def getNewUID ( self ) : <EOL> return max ( self . regions . keys ( ) + [ <NUM_LIT:0> ] ) + <NUM_LIT:1> <EOL> def getNewUIDs ( self , num_ids = <NUM_LIT:1> ) : <EOL> return [ max ( self . regions . keys ( ) + [ <NUM_LIT:0> ] ) + <NUM_LIT:1> + i for i in range ( num_ids ) ] <EOL> def newTerrainItem ( self , tform , uid = None , region = None ) : <EOL> if uid is None : <EOL> uid = self . getNewUID ( ) <EOL> elif uid in self . regions : <EOL> return self . regions [ uid ] <EOL> view = app . getCurrentRenderView ( ) <EOL> item = TerrainRegionItem ( uid , view , tform , self , region ) <EOL> parentObj = om . getOrCreateContainer ( '<STR_LIT>' ) <EOL> om . addToObjectModel ( item , parentObj ) <EOL> self . regions [ uid ] = item <EOL> return item <EOL> def requestIRISRegion ( self , tform , uid , bounding_box_width = <NUM_LIT:2> ) : <EOL> start = np . asarray ( tform . GetPosition ( ) ) <EOL> A_bounds , b_bounds = self . getXYBounds ( start , bounding_box_width ) <EOL> msg = lcmdrc . iris_region_request_t ( ) <EOL> msg . utime = getUtime ( ) <EOL> msg . initial_state = robotstate . drakePoseToRobotState ( self . jointController . q ) <EOL> msg . map_mode = self . map_mode_map [ self . params . properties . map_mode ] <EOL> msg . num_seed_poses = <NUM_LIT:1> <EOL> msg . seed_poses = [ positionMessageFromFrame ( tform ) ] <EOL> msg . region_id = [ uid ] <EOL> msg . xy_bounds = [ irisUtils . encodeLinCon ( A_bounds , b_bounds ) ] <EOL> lcmUtils . publish ( self . request_channel , msg ) <EOL> def onIRISRegionResponse ( self , msg ) : <EOL> for i in range ( msg . num_iris_regions ) : <EOL> new_region = irisUtils . SafeTerrainRegion . from_iris_region_t ( msg . iris_regions [ i ] ) <EOL> uid = msg . region_id [ i ] <EOL> if uid not in self . regions : <EOL> tform = new_region . tform <EOL> item = self . newTerrainItem ( tform , uid , new_region ) <EOL> else : <EOL> item = self . regions [ uid ] <EOL> item . setRegion ( new_region ) <EOL> def autoIRISSegmentation ( self , <EOL> xy_lb = [ - <NUM_LIT:2> , - <NUM_LIT:2> ] , <EOL> xy_ub = [ <NUM_LIT:2> , <NUM_LIT:2> ] , <EOL> max_num_regions = <NUM_LIT:10> , <EOL> default_yaw = np . nan , <EOL> max_slope_angle = np . nan , <EOL> max_height_variation = np . nan , <EOL> plane_distance_tolerance = np . nan , <EOL> plane_angle_tolerance = np . nan ) : <EOL> msg = lcmdrc . auto_iris_segmentation_request_t ( ) <EOL> msg . utime = getUtime ( ) <EOL> A = np . vstack ( ( np . eye ( <NUM_LIT:2> ) , - np . eye ( <NUM_LIT:2> ) ) ) <EOL> b = np . hstack ( ( xy_ub , - np . asarray ( xy_lb ) ) ) <EOL> msg . xy_bounds = irisUtils . encodeLinCon ( A , b ) <EOL> msg . initial_state = robotstate . drakePoseToRobotState ( self . jointController . q ) <EOL> msg . map_mode = self . map_mode_map [ self . params . properties . map_mode ] <EOL> msg . num_seed_poses = <NUM_LIT:0> <EOL> msg . max_num_regions = max_num_regions <EOL> msg . region_id = self . getNewUIDs ( max_num_regions ) <EOL> msg . default_yaw = default_yaw <EOL> msg . max_slope_angle = max_slope_angle <EOL> msg . max_height_variation = max_height_variation <EOL> msg . plane_distance_tolerance = plane_distance_tolerance <EOL> msg . plane_angle_tolerance = plane_angle_tolerance <EOL> lcmUtils . publish ( '<STR_LIT>' , msg ) <EOL> def getXYBounds ( self , start , bounding_box_width = <NUM_LIT:2> ) : <EOL> start = np . asarray ( start ) <EOL> lb = start [ : <NUM_LIT:2> ] - bounding_box_width / <NUM_LIT:2> <EOL> ub = start [ : <NUM_LIT:2> ] + bounding_box_width / <NUM_LIT:2> <EOL> A = np . vstack ( ( - np . eye ( <NUM_LIT:2> ) , np . eye ( <NUM_LIT:2> ) ) ) <EOL> b = np . hstack ( ( - lb , ub ) ) <EOL> return A , b </s>
