"""Run dstar given hdf5 data

Author: Antoni Rosinol
Email : tonirosinol
"""
from __future__ import print_function
import rlvision.utils as utils
import subprocess as sp
import os
import numpy as np

# let's load split 3 of grid 16x16
db, imsize = utils.load_grid16(split=3)

# call batch_im_data
batch_im_data = db["batch_im_data"]

# Get current working directory
# TODO I don't know if there is a better way?
dir = os.getcwd()

# Let's take a random grid
grid_id = 1000;

# Get grid
grid= batch_im_data[grid_id, :]

# Get value data containing the reward values.
# The database puts a number 10 wherever the goal is (I think)
value_data = db["value_data"]

# Select a start position
start = [2, 5]

# Get start index
start_index = np.ravel_multi_index(start, imsize, order='F')
if grid[start_index] == 0:
    print("[ERROR] start position falls over an obstacle")
else:
    # Color in grey the start position
    grid[start_index] = 100

# Get goal index. It is given in the datasets, no need to verify that it is not
# an obstacle (I GUESS)
# Goal has always a value of 10 in the value_data? or just positive? or what?
goal_index = int(np.nonzero(value_data[grid_id, :] == 10)[0])

# Color goal cell in grey, but slighlty more grey than start
# TODO add colors to the grid instead
grid[goal_index] = 200

# Utility function.
stringify = lambda x: str(x).strip('[()]').replace(',', '').replace(' ', '').replace('\n', '')

# Run dstar algorithm in c++
# Send start_index, goal_index, size of the grid and the grid through std input
# All inputs must be flattened, aka string of int or ints (no matrices)
# I.e. a 2x2 grid would be given as a string of 4 ints (row-major, C style).
dstar_subprocess = sp.Popen([dir+"/dstar-lite/build/dstar_from_input",
                            stringify(start_index), stringify(goal_index),
                            stringify(batch_im_data[grid_id, :])],
                            stdin=sp.PIPE, stdout=sp.PIPE, stderr=sp.PIPE)

# Answer from dstar algorithm.
# It is send through stdout but also catches stderr.
response = dstar_subprocess.communicate()

# TODO Handle errors!

#batch_im_data[grid_id, 1]
# Response == (stdout answer, None)
answer = response[0].splitlines()
errors = response[1].splitlines()

if len(errors) == 0:
    for a in answer:
        grid[int(a)] = 150
else:
    print("[ERROR] Errors found.")
    print(errors)

# Let's plot Dstar ouput on the grid
utils.plot_grid(grid, imsize)



@@ -150,53 +150,31 @@
 ort 
-subprocess as sp%0Aimport os%0Aimport numpy as np
+rlvision.dstar as dstar
 %0A%0A# 
@@ -309,105 +309,8 @@
 %22%5D%0A%0A
-# Get current working directory%0A# TODO I don't know if there is a better way?%0Adir = os.getcwd()%0A%0A
 # Le
@@ -345,17 +345,16 @@
 id = 100
-0
 ;%0A%0A# Get
@@ -363,16 +363,17 @@
 rid%0Agrid
+ 
 = batch_
@@ -397,149 +397,8 @@
 :%5D%0A%0A
-# Get value data containing the reward values.%0A# The database puts a number 10 wherever the goal is (I think)%0Avalue_data = db%5B%22value_data%22%5D%0A%0A
 # Se
@@ -432,259 +432,32 @@
  = %5B
-2, 5%5D%0A%0A# Get start index%0Astart_index = np.ravel_multi_index(start, imsize, order='F')%0Aif grid%5Bstart_index%5D == 0:%0A    print(%22%5BERROR%5D start position falls over an obstacle%22)%0Aelse:%0A    # Color in grey the start position%0A    grid%5Bstart_index%5D = 100%0A%0A# Get
+1, 1%5D%0A%0A # I think that a
  goa
@@ -467,12 +467,8 @@
 ndex
-. It
  is 
@@ -492,66 +492,9 @@
 sets
-, no need to verify that it is not%0A# an obstacle (I GUESS)
+.
 %0A# G
@@ -568,16 +568,18 @@
 r what?%0A
+# 
 goal_ind
@@ -639,1268 +639,310 @@
 %0A%0A# 
-Color goal cell in grey, but slighlty more grey than start%0A# TODO add colors to the grid instead%0Agrid%5B
+Get a random 
 goal
-_
+ 
 index
-%5D = 200%0A%0A# Utility function.%0Astringify = lambda x: str(x).strip('%5B()%5D').replace(',', '').replace(' ', '').replace('%5Cn', '')%0A%0A# Run dstar algorithm in c++%0A# Send start_index, goal_index, size of the grid and the grid through std input%0A# All inputs must be flattened, aka string of int or ints (no matrices)%0A# I.e. a 2x2 grid would be given as a string of 4 ints (row-major, C style).%0Adstar_subprocess = sp.Popen(%5Bdir+%22/dstar-lite/build/dstar_from_input%22,%0A                            stringify(start_index), stringify(goal_index),%0A                            stringify(batch_im_data%5Bgrid_id, :%5D)%5D,%0A                            stdin=sp.PIPE, stdout=sp.PIPE, stderr=sp.PIPE)%0A%0A# Answer from dstar algorithm.%0A# It is send through stdout but also catches stderr.%0Aresponse = dstar_subprocess.communicate()%0A%0A# TODO Handle errors!%0A%0A#batch_im_data%5Bgrid_id, 1%5D%0A# Response == (stdout answer, None)%0Aanswer = response%5B0%5D.splitlines()%0Aerrors = response%5B1%5D.splitlines()%0A%0Aif len(errors) == 0:%0A    for a in answer:%0A        grid%5Bint(a)%5D = 150%0Aelse:%0A    print(%22%5BERROR%5D Errors found.%22)%0A    print(errors)%0A%0A# Let's plot Dstar ouput on the grid%0Autils.plot_grid(grid, imsize
+.%0Agoal = %5B14, 14%5D%0A%0A# Run dstar%0Agrid_with_path, errors = dstar.run_dstar(start, goal, grid, imsize)%0A%0A# Let's plot dstar ouput on the grid%0Aif errors is False:%0A    utils.plot_grid(grid_with_path, imsize)%0Aelse:%0A    print(%22%5BERROR%5D Did not plot grid with path because of found errors.%22
 )%0A%0A%0A

Use new dstar module in dstar_test.py