import pyhop2
from a_star import astar
from gbfs import gbfs
from map import Map
from renderer import Renderer
from robot_path_controller.srv import Path, PathResponse
from robot_path_controller.msg import WayPoint
import rospy
from gazebo_msgs.srv import GetModelState
from check_result import check_result, pause, set_trace
from products import Products
import numpy as np
prod = Products()
prod.seed(1)
item_list = prod.get_random_list()
item_list.append('cashier')
renderer = Renderer()
#########################################################
## Grocery Plan #########################################
# loc = {'robot': (7.4, 12.85), 'brocoli': (1.54, 5.4), 'cucumber': (1.54, 7.91), 'salt': (8.39, 4.79), 'watermelon': (5.1, 8.44), 'strawberry': (2.31, 6.36), 'potato': (3.58, 7.44), 'hamburger': (7.4, 12.85)}
domain_name = 'groceryplan'
# Create a new domain to contain the methods and operators
pyhop2.Domain(domain_name)
import numpy as np
from map import Map
from products import Products
map = Map(radius=0.177)
collision = map.check_collision(15, 15)
prod = Products()
prod.seed(100)
new_list = prod.get_random_list()
print(new_list)
new_list1 = prod.get_random_list()
print(new_list1)
print(collision)
for product, coord in prod.product_list.items():
collision = map.check_collision(coord[0], coord[1])
if (collision == True):
print(product, " collides")