#!/usr/bin/env python

import pyexotica as exo
import exotica_ompl_solver_py as ompl
import numpy as np
from pyexotica.publish_trajectory import publish_trajectory

exo.Setup.init_ros()
prm = exo.Setup.load_solver(
    '{exotica_examples}/resources/configs/example_prm.xml')
# This PRM has been setup for multi-query operation.
# You have to call prm.clear_query() between runs.
# Call prm.clear() to discard the roadmap.

# Temporary fix: call solve() to setup the OMPL problem,
# otherwise grow_roadmap() worn't work (issue #456).
solution = prm.solve()
prm.clear()

# Grow roadmap for 1s
prm.grow_roadmap(1)

solution = None
for i in range(10):
    # Solve
    solution = prm.solve()

    # Grow the roadmap some more
    prm.grow_roadmap(1)
    # Clear previous start/goal
    prm.clear_query()

publish_trajectory(solution, 3.0, prm.get_problem())
