in robot_ws/src/rl_agent/markov/environments/meiro_runner_env.py [0:0]
def __init__(self):
self.on_track = 0
self.progress = 0
self.yaw = 0
self.x = INITIAL_POS_X
self.y = INITIAL_POS_Y
self.steps = 0
self.ranges = None
self.footsteps_marker = np.zeros((int((STAGE_X_MAX - STAGE_X_MIN) / FOOTSTEPS_MARKER_SIZE)+1, int((STAGE_Y_MAX - STAGE_Y_MIN) / FOOTSTEPS_MARKER_SIZE)+1))
# actions -> steering angle, throttle
self.action_space = spaces.Box(low=np.array([-1, 0]), high=np.array([+1, +1]), dtype=np.float32)
# environment -> lider scan
high = np.array([LIDAR_SCAN_MAX_DISTANCE] * TRAINING_IMAGE_SIZE)
low = np.array([0.0] * TRAINING_IMAGE_SIZE)
self.observation_space = spaces.Box(low, high, dtype=np.float32)
#ROS initialization
self.ack_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=100)
self.gazebo_model_state_service = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
rospy.init_node('rl_coach', anonymous=True)
#Subscribe to ROS topics and register callbacks
rospy.Subscriber('/odom', Odometry, self.callback_pose)
rospy.Subscriber('/scan', LaserScan, self.callback_scan)
self.aws_region = rospy.get_param('ROS_AWS_REGION')
self.reward_in_episode = 0
self.steps = 0
self.last_min_distance = sys.maxsize
self.last_position_x = self.x
self.last_position_y = self.y
self.last_footsteps_mark_position = self.calc_footsteps_mark_position(self.x, self.y)