3 ]G@sddlmZddlZddlmZddlZddlZddlZddlZddlm Z ddl Z ddl Z ddl m Z ddlmZddlmZddlmZdd lmZdd lmZdd lmZdd lmZddlZejd ejddZdZdZdZ dZ!dZ"dZ#dZ$dZ%dZ&d%Z'd&Z(dZ)dZ*dZ+d'Z,dZ-e.e j/e$e'de%e(dZ0d(Z1d)Z2dZ3dZ4dZ5dZ6d Z7Gd!d"d"ej8eZ9Gd#d$d$e9eZ:dS)*)print_functionN)ABC)spaces)Odometry)Twist) SetModelState) ModelState) LaserScan) ContactsState)Point)Float64zwtf.log)filenamelevelz0.0.3ihg@gK7A`?g?i'g!@g@g?g`l8~.?gaI?gڷlHb?ghKcR?g$@g?g333333?g{Gz?c@sdeZdZddZddZddZddZd d Zd d Zd dZ ddZ ddZ ddZ ddZ dS)RoverTrainingGroundsEnvcCstjdjtt|_tjdj|jtd|_tjdj|jd|_d|_t |_ tjdj|j t |_ tjdj|j d|_ d |_d|_d|_d |_d|_d|_tjtjdd gtjd d gtjd |_tjdtjtgt}tjdgt}tj||tjd|_tjdtjdtdd|_ tjdt!dd|_"tj#dt$|_%tj&ddtj'dtj(dt)|j*tj(dt+|j,tj(dt-|j.d|_/tjddS)Nz3Initializing environment variables for version # {}z#current_distance_to_checkpoint = {}g?zremaining_range = {}gFz self.x = {}z self.y = {}rg?g?)lowhighdtypezAction Space declared)rzObservation Space declaredz/cmd_vel ) queue_sizez/current_positionz/gazebo/set_model_stateZrl_coachT)Z anonymous log_levelz/odomz/scanz /robot_bumperz us-east-1zInitialization complete.)0logginginfoformatVERSIONINITIAL_DISTANCE_TO_CHECKPOINTcurrent_distance_to_checkpointremaining_rangedistance_travelledmoving_toward_checkpoint INITIAL_POS_Xx INITIAL_POS_Yycollidereward_in_episodedone next_statesteps step_rewardrangesrBoxnparrayfloat32 action_spaceLIDAR_SCAN_MAX_DISTANCETRAINING_IMAGE_SIZEobservation_spacerospyZ Publisherr ack_publisherr Zcurrent_position_pubZ ServiceProxyrgazebo_model_state_serviceZ init_nodeDEBUGZ Subscriberr callback_poser callback_scanr callback_collision aws_region)selfrrr>F/home/chris/code/rl_agent2/markov/environments/training_grounds_env.py__init__EsF     z RoverTrainingGroundsEnv.__init__cCsnd|_d|_d|_t|d}t|d}|jd7_|j||tjt|j |i}|j|j|j|fS)NFrr) r+r(r)floatr* send_actiontimesleep:SLEEP_BETWEEN_ACTION_AND_REWARD_CALCULATION_TIME_IN_SECONDinfer_reward_state)r=actionsteeringthrottlerr>r>r?steps     zRoverTrainingGroundsEnv.stepcCs\y&t}||j_||j_|jj|Wn0tk rV}ztdj |WYdd}~XnXdS)Nz%Error in the send_action function: {}) rlinearr#angularzr6publish Exceptionprintr)r=rHrIZspeederrr>r>r?rBsz#RoverTrainingGroundsEnv.send_actioncCs|y@tjdtd|jd|j|j|jd|_t|_td|_ d|_ d|_ d|_ d|_t |_t|_d|_|jddtjdtjd t}t |jj_t|jj_t|jj_t|jj_t|jj_t|jj_t|jj_d|jj _d|jj _d|jj _d|jj!_d|jj!_d|jj!_d |_"|j#|t$j%t&|j'ddgWn2t(k rt}ztd j)|WYdd}~XnX|j*S) NzResetting episodic variableszTotal Reward=%.2fzTotal Steps=%.2fgg?FrzResetting Rover in Gazebozgazebo/set_model_stateZroverzError in the reset function: {})+rrrPr'r*send_reward_to_cloudwatchr rrrr!r&r,r"r#r$r%rBr5Zwait_for_servicerposeposition INITIAL_POS_ZrMINITIAL_ORIENT_X orientationINITIAL_ORIENT_YINITIAL_ORIENT_ZINITIAL_ORIENT_WwZtwistrKrL model_namer7rCrD SLEEP_AFTER_RESET_TIME_IN_SECONDrFrOrr))r=Z model_staterQr>r>r?resetsP                       zRoverTrainingGroundsEnv.resetcCs"t|j}t|j}t||}|S)N)intr#r%r )r=fxZfymarkr>r>r?calc_footsteps_mark_positions   z4RoverTrainingGroundsEnv.calc_footsteps_mark_positioncCs |j|_dS)N)r,)r=datar>r>r?r:sz%RoverTrainingGroundsEnv.callback_scancCs|jjj}t|j|j|j}t|j|j}t|j|j}|dksL|dkrtj|j|j|j|j}|j |7_ t }ttj |jdd|jdd|_ |j |j krd|_nd|_|j |_ |j|j 8_|j|_|j|_dS)NgMbP?g!@rTF)rSrTr r#r%rMabsmathhypotr r sqrtrcrr!r)r=rcZ new_positionpZx_diffZy_diffdistZnew_distance_to_checkpointr>r>r?r9s"  z%RoverTrainingGroundsEnv.callback_posecCs|j}t|dkrd|_dS)NrT)stateslenr&)r=rcrjr>r>r?r;s z*RoverTrainingGroundsEnv.callback_collisioncCs`t|j}tjd|dt}tj|}tjtj|||jdt}t|tj |<tj |}||fS)Nrr) rkr,r.linspacer3arangeclipinterpr2isnanamin)r=sizer#xpstate min_distancer>r>r?get_min_distance_to_objects   z2RoverTrainingGroundsEnv.get_min_distance_to_objectcCsly6tjj}|jd|jd}|jdd|dgddWn0tk rf}ztdj|WYdd}~XnXdS) N cloudwatch) region_nameZTraining_GroundsNone)Z MetricNameZUnitValueZAWSRoboMakerSimulation)Z MetricData Namespacez3Error in the send_reward_to_cloudwatch function: {}) boto3sessionSessionclientr<Zput_metric_datarOrPr)r=rewardr}Zcloudwatch_clientrQr>r>r?rR(s  z1RoverTrainingGroundsEnv.send_reward_to_cloudwatchc Csyd}d}x|js tjtqWt|d}t|d}|j\}}|jtdksb|jtdkrxt dt |_ d|_ n|j tdks|j tdkrt dt |_ d|_ n|jrt dt |_ d|_ n|tkrt dt |_ d|_ n|j tko|jtkr,t d|jdkrd |_ d }nt|j|_ d}n<|jdkrNt |_ t d d|_ n|jr`|}d }n|}d}|||_ |j|j 7_t td |jd|j d|jd|j||_Wn6tk r} zt tdj| WYdd} ~ XnXdS)Nrrg?zRover has left the mission map!Tz&The Rover has collided with an object!z"Unrecoverable contact with object.z-Congratulations! You reached the checkpoint!gFz,Your power supply has reached zero capacity!g@z Step No=%.2fz Reward=%.2fzEpisode_Reward=%.2fzDistance from finish line=%fz) Error in infer_reward_state function: {}g)r,rCrD&SLEEP_WAITING_FOR_IMAGE_TIME_IN_SECONDrArvr# STAGE_X_MIN STAGE_X_MAXrPCOLLISION_REWARDr+r(r% STAGE_Y_MIN STAGE_Y_MAXr&CRASH_DISTANCE CHECKPOINT_Y CHECKPOINT_Xr*FINISHEDrr!r'rrr)rOr) r=rGrZreward_multiplierrHrIrurtr(rQr>r>r?rF9sb         z*RoverTrainingGroundsEnv.infer_reward_stateN)__name__ __module__ __qualname__r@rJrBr^rbr:r9r;rvrRrFr>r>r>r?rDsM 0 &rcs$eZdZddZfddZZS)RoverTrainingGroundsDiscreteEnvcCstj|tjd|_dS)N)rr@rDiscreter1)r=r>r>r?r@s z(RoverTrainingGroundsDiscreteEnv.__init__csR|dkrd}d}n,|dkr$d}d}n|dkr6d}d}ntd||g}tj|S)Nrg?g@rrzInvalid actiong) ValueErrorsuperrJ)r=rGrHrIZcontinuous_action) __class__r>r?rJsz$RoverTrainingGroundsDiscreteEnv.step)rrrr@rJ __classcell__r>r>)rr?rsrg!g!gڷlHbg$g$); __future__rrCabcrr|gymnumpyr.rrer5Z nav_msgs.msgrZgeometry_msgs.msgrZgazebo_msgs.srvrZgazebo_msgs.msgrZsensor_msgs.msgr r r Z std_msgs.msgr r basicConfigr8rr3r2rZFOOTSTEPS_MARKER_SIZEr MAX_STEPSrrrr"r$rUrVrXrYrZrdrgrrrrrr]rErEnvrrr>r>r>r?sb           c