diff --git a/setup.py b/setup.py index f53ed6f..16e5dfd 100644 --- a/setup.py +++ b/setup.py @@ -1,3 +1,4 @@ +import numpy from setuptools import setup, Extension from setuptools.command.build_ext import build_ext as _build_ext from Cython.Build import cythonize @@ -48,4 +49,5 @@ def run(self): 'Topic :: Games/Entertainment :: Simulation', 'Topic :: Software Development :: Libraries :: Python Modules', ], + include_dirs=[numpy.get_include()], ) diff --git a/src/RVOSimulator.cpp b/src/RVOSimulator.cpp index e5feee4..7844ed4 100644 --- a/src/RVOSimulator.cpp +++ b/src/RVOSimulator.cpp @@ -366,4 +366,13 @@ namespace RVO { { timeStep_ = timeStep; } + + void RVOSimulator::clearAgents() + { + for (size_t i = 0; i < agents_.size(); ++i) { + delete agents_[i]; + } + agents_.clear(); + kdTree_->agents_.clear(); + } } diff --git a/src/RVOSimulator.h b/src/RVOSimulator.h index bfe715d..6212472 100644 --- a/src/RVOSimulator.h +++ b/src/RVOSimulator.h @@ -575,6 +575,11 @@ namespace RVO { */ void setTimeStep(float timeStep); + /** + * \brief Clears all agents from the simulator, but retains obstacles. + */ + void clearAgents(); + private: std::vector agents_; Agent *defaultAgent_; diff --git a/src/rvo2.pyx b/src/rvo2.pyx index dcbdded..7314370 100644 --- a/src/rvo2.pyx +++ b/src/rvo2.pyx @@ -73,6 +73,7 @@ cdef extern from "RVOSimulator.h" namespace "RVO": void setAgentTimeHorizonObst(size_t agentNo, float timeHorizonObst) void setAgentVelocity(size_t agentNo, const Vector2 & velocity) void setTimeStep(float timeStep) + void clearAgents() cdef class PyRVOSimulator: @@ -87,6 +88,9 @@ cdef class PyRVOSimulator: timeHorizon, timeHorizonObst, radius, maxSpeed, c_velocity) + def __dealloc__(self): + del self.thisptr + def addAgent(self, tuple pos, neighborDist=None, maxNeighbors=None, timeHorizon=None, timeHorizonObst=None, radius=None, maxSpeed=None, @@ -222,3 +226,5 @@ cdef class PyRVOSimulator: self.thisptr.setAgentVelocity(agent_no, c_velocity) def setTimeStep(self, float time_step): self.thisptr.setTimeStep(time_step) + def clearAgents(self): + self.thisptr.clearAgents() diff --git a/test/positions_log_for_test_scenario.npz b/test/positions_log_for_test_scenario.npz new file mode 100644 index 0000000..7607af3 Binary files /dev/null and b/test/positions_log_for_test_scenario.npz differ diff --git a/test/reproduce_a_scenario.py b/test/reproduce_a_scenario.py new file mode 100644 index 0000000..f877bd9 --- /dev/null +++ b/test/reproduce_a_scenario.py @@ -0,0 +1,124 @@ +import rvo2 +import numpy as np + +def reproduce_test_scenario(plotting=False): + sim = rvo2.PyRVOSimulator(1/60., 1.5, 5, 1.5, 2, 0.4, 2) + + obstacles = [ + [(0.3, 0.1), (0.1, 0.1), (0.1, -0.1)] + ] + for obstacle in obstacles: + sim.addObstacle(obstacle) + sim.processObstacles() + + a0 = sim.addAgent((-1, -1)) + a1 = sim.addAgent((1, -1)) + a2 = sim.addAgent((1, 1)) + a3 = sim.addAgent((-1, 1), 1.5, 5, 1.5, 2, 0.4, 2, (0, 0)) + sim.setAgentPrefVelocity(a0, (1, 1)) + sim.setAgentPrefVelocity(a1, (-1, 1)) + sim.setAgentPrefVelocity(a2, (-1, -1)) + sim.setAgentPrefVelocity(a3, (1, -1)) + + positions_log = [] + for step in range(200): + sim.doStep() + positions = [sim.getAgentPosition(agent_no) for agent_no in (a0, a1, a2, a3)] + positions_log.append(positions) + + positions_log = np.array(positions_log) +# np.savez("positions_log_for_test_scenario.npz", +# positions_log=positions_log, +# obstacles=np.array(obstacles)) + + archive = np.load("positions_log_for_test_scenario.npz") + + test_passed = np.allclose(archive['positions_log'], positions_log) + + if plotting: + plot_obstacles(np.array(obstacles)) + plot_positions_log(positions_log) + plot_positions_log(archive['positions_log']) + + if test_passed: + print("Test passed!") + else: + print("Test failed!") + return test_passed + +def test_clear_agents(plotting=False): + sim = rvo2.PyRVOSimulator(1/60., 1.5, 5, 1.5, 2, 0.4, 2) + + obstacles = [ + [(0.3, 0.1), (0.1, 0.1), (0.1, -0.1)] + ] + for obstacle in obstacles: + sim.addObstacle(obstacle) + sim.processObstacles() + + a0 = sim.addAgent((-1, -1)) + a1 = sim.addAgent((1, -1)) + a2 = sim.addAgent((1, 1)) + a3 = sim.addAgent((-1, 1), 1.5, 5, 1.5, 2, 0.4, 2, (0, 0)) + sim.setAgentPrefVelocity(a0, (1, 1)) + sim.setAgentPrefVelocity(a1, (-1, 1)) + sim.setAgentPrefVelocity(a2, (-1, -1)) + sim.setAgentPrefVelocity(a3, (1, -1)) + + positions_log = [] + for step in range(200): + sim.doStep() + positions = [sim.getAgentPosition(agent_no) for agent_no in (a0, a1, a2, a3)] + positions_log.append(positions) + positions_log = np.array(positions_log) + + # clear agents, reset agents, run same scenario + sim.clearAgents() + + a0 = sim.addAgent((-1, -1)) + a1 = sim.addAgent((1, -1)) + a2 = sim.addAgent((1, 1)) + a3 = sim.addAgent((-1, 1), 1.5, 5, 1.5, 2, 0.4, 2, (0, 0)) + sim.setAgentPrefVelocity(a0, (1, 1)) + sim.setAgentPrefVelocity(a1, (-1, 1)) + sim.setAgentPrefVelocity(a2, (-1, -1)) + sim.setAgentPrefVelocity(a3, (1, -1)) + + print('Running simulation') + new_positions_log = [] + for step in range(200): + sim.doStep() + positions = [sim.getAgentPosition(agent_no) for agent_no in (a0, a1, a2, a3)] + new_positions_log.append(positions) + new_positions_log = np.array(new_positions_log) + + test_passed = np.allclose(new_positions_log, positions_log) + + if plotting: + plot_obstacles(np.array(obstacles)) + plot_positions_log(positions_log) + plot_positions_log(new_positions_log) + + if test_passed: + print("Test passed!") + else: + print("Test failed!") + return test_passed + + +def plot_positions_log(positions_log): + n_traj = positions_log.shape[1] + for i in range(n_traj): + plt.plot(positions_log[:,i,0], positions_log[:,i,1]) + +def plot_obstacles(obstacles): + for obstacle in obstacles: + c = np.concatenate([obstacle, obstacle[:1]], axis=0) + plt.plot(c[:,0], c[:,1], 'k') + + +if __name__ == "__main__": + from matplotlib import pyplot as plt + reproduce_test_scenario(plotting=True) + test_clear_agents() + plt.show()