Initial checkin of Cython-based Python bindings.

micadam · Jul 30, 2015 · 390381c · 390381c
1 parent f8ed86e
commit 390381c
Show file tree

Hide file tree

Showing 5 changed files with 314 additions and 0 deletions.
diff --git a/.gitignore b/.gitignore
@@ -11,3 +11,7 @@
 *.lai
 *.la
 *.a
+
+# Python & Cython-generated files
+build/
+src/rvo2.cpp
diff --git a/README.md b/README.md
@@ -1,3 +1,56 @@
+Python bindings for Optimal Reciprocal Collision Avoidance
+==========================================================
+
+This repository contains the RVO2 framework, as described below, along with
+[Cython](http://cython.org/)-based Python bindings.
+
+Building & installing
+----------------------
+
+Run `python setup.py build` to build, and `python setup.py install` to install.
+
+Only tested with Python 3.4 on Ubuntu Linux. If you have success (or failure)
+stories, please share them!
+
+Differences with the C++ version
+--------------------------------
+
+The `Vector2` and `Line` classes from the RVO2 library are //not// wrapped. Instead,
+vectors are passed as tuples `(x, y)` from/to Python. Lines are passed as tuples
+`(point x, point y, direction x, direction y)`.
+
+An example:
+
+```python
+#!/usr/bin/env python
+
+import rvo2
+
+sim = rvo2.PyRVOSimulator(1/60, 1.5, 5, 1.5, 2, 0.4, 2)
+a0 = sim.addAgent((0, 0), 0.4)
+a1 = sim.addAgent((1, 0), 0.4)
+a2 = sim.addAgent((1, 1), 0.4)
+a3 = sim.addAgent((0, 1), 0.4)
+
+sim.setAgentPrefVelocity(a0, (1, 1))
+sim.setAgentPrefVelocity(a1, (-1, 1))
+sim.setAgentPrefVelocity(a2, (-1, -1))
+sim.setAgentPrefVelocity(a3, (1, -1))
+
+print('Simulation has %i agents and %i obstacle vertices in it.' %
+      (sim.getNumAgents(), sim.getNumObstacleVertices()))
+
+print('Running simulation')
+
+for step in range(20):
+    sim.doStep()
+
+    positions = ['(%5.3f, %5.3f)' % sim.getAgentPosition(agent_no)
+                 for agent_no in (a0, a1, a2, a3)]
+    print('step=%2i  t=%.3f  %s' % (step, sim.getGlobalTime(), '  '.join(positions)))
+```
+
+
 Optimal Reciprocal Collision Avoidance
 ======================================
 

diff --git a/requirements.txt b/requirements.txt
@@ -0,0 +1 @@
+Cython==0.22.1
diff --git a/setup.py b/setup.py
@@ -0,0 +1,40 @@
+from setuptools import setup, Extension
+from setuptools.command.build_ext import build_ext as _build_ext
+from Cython.Build import cythonize
+
+
+class build_ext(_build_ext):
+    """Builds RVO2 before our module."""
+
+    def run(self):
+        # Build RVO2
+        import os
+        import os.path
+        import subprocess
+
+        build_dir = os.path.abspath('build/RVO2')
+        if not os.path.exists(build_dir):
+            os.makedirs(build_dir)
+            subprocess.check_call(['cmake', '../..', '-DCMAKE_CXX_FLAGS=-fPIC'],
+                                  cwd=build_dir)
+        subprocess.check_call(['make', '-j8'], cwd=build_dir)
+
+        super().run()
+
+
+extensions = [
+    Extension('rvo2', ['src/*.pyx', 'src/*.pxd'],
+              include_dirs=['src'],
+              libraries=['RVO'],
+              library_dirs=['build/RVO2/src'],
+              extra_compile_args=['-fPIC']),
+]
+
+setup(
+    name="pyrvo2",
+    ext_modules=cythonize(extensions),
+    cmdclass={'build_ext': build_ext},
+    install_requires=[
+        'Cython>=0.22.1',
+    ],
+)
diff --git a/src/rvo2.pyx b/src/rvo2.pyx
@@ -0,0 +1,216 @@
+# distutils: language = c++
+from libcpp.vector cimport vector
+from libcpp cimport bool
+from cython.operator cimport dereference as deref
+
+
+cdef extern from "Vector2.h" namespace "RVO":
+    cdef cppclass Vector2:
+        Vector2() except +
+        Vector2(float x, float y) except +
+        float x() const
+        float y() const
+
+
+cdef extern from "RVOSimulator.h" namespace "RVO":
+    cdef const size_t RVO_ERROR
+
+
+cdef extern from "RVOSimulator.h" namespace "RVO":
+    cdef cppclass Line:
+        Vector2 point
+        Vector2 direction
+
+
+cdef extern from "RVOSimulator.h" namespace "RVO":
+    cdef cppclass RVOSimulator:
+        RVOSimulator()
+        RVOSimulator(float timeStep, float neighborDist, size_t maxNeighbors,
+                     float timeHorizon, float timeHorizonObst, float radius,
+                     float maxSpeed, const Vector2 & velocity)
+        size_t addAgent(const Vector2 & position)
+        size_t addAgent(const Vector2 & position, float neighborDist,
+                        size_t maxNeighbors, float timeHorizon,
+                        float timeHorizonObst, float radius, float maxSpeed,
+                        const Vector2 & velocity)
+        size_t addObstacle(const vector[Vector2] & vertices)
+        void doStep()
+        size_t getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const
+        size_t getAgentMaxNeighbors(size_t agentNo) const
+        float getAgentMaxSpeed(size_t agentNo) const
+        float getAgentNeighborDist(size_t agentNo) const
+        size_t getAgentNumAgentNeighbors(size_t agentNo) const
+        size_t getAgentNumObstacleNeighbors(size_t agentNo) const
+        size_t getAgentNumORCALines(size_t agentNo) const
+        size_t getAgentObstacleNeighbor(size_t agentNo, size_t neighborNo) const
+        const Line & getAgentORCALine(size_t agentNo, size_t lineNo) const
+        const Vector2 & getAgentPosition(size_t agentNo) const
+        const Vector2 & getAgentPrefVelocity(size_t agentNo) const
+        float getAgentRadius(size_t agentNo) const
+        float getAgentTimeHorizon(size_t agentNo) const
+        float getAgentTimeHorizonObst(size_t agentNo) const
+        const Vector2 & getAgentVelocity(size_t agentNo) const
+        float getGlobalTime() const
+        size_t getNumAgents() const
+        size_t getNumObstacleVertices() const
+        const Vector2 & getObstacleVertex(size_t vertexNo) const
+        size_t getNextObstacleVertexNo(size_t vertexNo) const
+        size_t getPrevObstacleVertexNo(size_t vertexNo) const
+        float getTimeStep() const
+        void processObstacles()
+        bool queryVisibility(const Vector2 & point1, const Vector2 & point2,
+                             float radius) const
+        void setAgentDefaults(float neighborDist, size_t maxNeighbors,
+                              float timeHorizon, float timeHorizonObst,
+                              float radius, float maxSpeed,
+                              const Vector2 & velocity)
+        void setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors)
+        void setAgentMaxSpeed(size_t agentNo, float maxSpeed)
+        void setAgentNeighborDist(size_t agentNo, float neighborDist)
+        void setAgentPosition(size_t agentNo, const Vector2 & position)
+        void setAgentPrefVelocity(size_t agentNo, const Vector2 & prefVelocity)
+        void setAgentRadius(size_t agentNo, float radius)
+        void setAgentTimeHorizon(size_t agentNo, float timeHorizon)
+        void setAgentTimeHorizonObst(size_t agentNo, float timeHorizonObst)
+        void setAgentVelocity(size_t agentNo, const Vector2 & velocity)
+        void setTimeStep(float timeStep)
+
+
+cdef class PyRVOSimulator:
+    cdef RVOSimulator *thisptr
+    cdef float default_neighborDist
+    cdef size_t default_maxNeighbors
+    cdef float default_timeHorizon
+    cdef float default_timeHorizonObst
+    cdef float default_radius
+    cdef float default_maxSpeed
+
+    def __cinit__(self, float timeStep, float neighborDist, size_t maxNeighbors,
+                  float timeHorizon, float timeHorizonObst, float radius,
+                  float maxSpeed, tuple velocity=(0, 0)):
+        cdef Vector2 c_velocity = Vector2(velocity[0], velocity[1])
+
+        self.thisptr = new RVOSimulator(timeStep, neighborDist, maxNeighbors,
+                                        timeHorizon, timeHorizonObst, radius,
+                                        maxSpeed, c_velocity)
+        self.default_neighborDist = neighborDist
+        self.default_maxNeighbors = maxNeighbors
+        self.default_timeHorizon = timeHorizon
+        self.default_timeHorizonObst = timeHorizonObst
+        self.default_radius = radius
+        self.default_maxSpeed = maxSpeed
+
+    def addAgent(self, tuple pos, float radius):
+        cdef Vector2 c_pos = Vector2(pos[0], pos[1])
+        agent_nr = self.thisptr.addAgent(c_pos,
+                                         self.default_neighborDist,
+                                         self.default_maxNeighbors,
+                                         self.default_timeHorizon,
+                                         self.default_timeHorizonObst,
+                                         radius or self.default_radius,
+                                         self.default_maxSpeed,
+                                         Vector2())
+        if agent_nr == RVO_ERROR:
+            raise RuntimeError('Error adding agent to RVO simulation')
+        return agent_nr
+
+    def addObstacle(self, list vertices):
+        cdef vector[Vector2] c_vertices
+
+        for x, y in vertices:
+            c_vertices.push_back(Vector2(x, y))
+
+        obstacle_nr = self.thisptr.addObstacle(c_vertices)
+        if obstacle_nr == RVO_ERROR:
+            raise RuntimeError('Error adding obstacle to RVO simulation')
+        return obstacle_nr
+
+    def doStep(self):
+        self.thisptr.doStep()
+    def getAgentAgentNeighbor(self, size_t agent_no, size_t neighbor_no):
+        return self.thisptr.getAgentAgentNeighbor(agent_no, neighbor_no)
+    def getAgentMaxNeighbors(self, size_t agent_no):
+        return self.thisptr.getAgentMaxNeighbors(agent_no)
+    def getAgentMaxSpeed(self, size_t agent_no):
+        return self.thisptr.getAgentMaxSpeed(agent_no)
+    def getAgentNeighborDist(self, size_t agent_no):
+        return self.thisptr.getAgentNeighborDist(agent_no)
+    def getAgentNumAgentNeighbors(self, size_t agent_no):
+        return self.thisptr.getAgentNumAgentNeighbors(agent_no)
+    def getAgentNumObstacleNeighbors(self, size_t agent_no):
+        return self.thisptr.getAgentNumObstacleNeighbors(agent_no)
+    def getAgentNumORCALines(self, size_t agent_no):
+        return self.thisptr.getAgentNumORCALines(agent_no)
+    def getAgentObstacleNeighbor(self, size_t agent_no, size_t obstacle_no):
+        return self.thisptr.getAgentObstacleNeighbor(agent_no, obstacle_no)
+    def getAgentORCALine(self, size_t agent_no, size_t line_no):
+        cdef Line line = self.thisptr.getAgentORCALine(agent_no, line_no)
+        return line.point.x(), line.point.y(), line.direction.x(), line.direction.y()
+    def getAgentPosition(self, size_t agent_no):
+        cdef Vector2 pos = self.thisptr.getAgentPosition(agent_no)
+        return pos.x(), pos.y()
+    def getAgentPrefVelocity(self, size_t agent_no):
+        cdef Vector2 velocity = self.thisptr.getAgentPrefVelocity(agent_no)
+        return velocity.x(), velocity.y()
+    def getAgentRadius(self, size_t agent_no):
+        return self.thisptr.getAgentRadius(agent_no)
+    def getAgentTimeHorizon(self, size_t agent_no):
+        return self.thisptr.getAgentTimeHorizon(agent_no)
+    def getAgentTimeHorizonObst(self, size_t agent_no):
+        return self.thisptr.getAgentTimeHorizonObst(agent_no)
+    def getAgentVelocity(self, size_t agent_no):
+        cdef Vector2 velocity = self.thisptr.getAgentVelocity(agent_no)
+        return velocity.x(), velocity.y()
+    def getGlobalTime(self):
+        return self.thisptr.getGlobalTime()
+    def getNumAgents(self):
+        return self.thisptr.getNumAgents()
+    def getNumObstacleVertices(self):
+        return self.thisptr.getNumObstacleVertices()
+    def getObstacleVertex(self, size_t vertex_no):
+        cdef Vector2 pos = self.thisptr.getObstacleVertex(vertex_no)
+        return pos.x(), pos.y()
+    def getNextObstacleVertexNo(self, size_t vertex_no):
+        return self.thisptr.getNextObstacleVertexNo(vertex_no)
+    def getPrevObstacleVertexNo(self, size_t vertex_no):
+        return self.thisptr.getPrevObstacleVertexNo(vertex_no)
+    def getTimeStep(self):
+        return self.thisptr.getTimeStep()
+    def processObstacles(self):
+        self.thisptr.processObstacles()
+    def queryVisibility(self, tuple point1, tuple point2, float radius=0.0):
+        return self.thisptr.queryVisibility(Vector2(point1[0], point1[1]),
+                                            Vector2(point2[0], point2[1]),
+                                            radius)
+    def setAgentDefaults(self, float neighbor_dist, size_t max_neighbors, float time_horizon,
+                         float time_horizon_obst, float radius, float max_speed,
+                         tuple velocity=(0, 0)):
+        cdef Vector2 c_velocity = Vector2(velocity[0], velocity[1])
+        self.thisptr.setAgentDefaults(neighbor_dist, max_neighbors, time_horizon,
+                                      time_horizon_obst, radius, max_speed, c_velocity)
+
+    def setAgentMaxNeighbors(self, size_t agent_no, size_t max_neighbors):
+        self.thisptr.setAgentMaxNeighbors(agent_no, max_neighbors)
+    def setAgentMaxSpeed(self, size_t agent_no, float max_speed):
+        self.thisptr.setAgentMaxSpeed(agent_no, max_speed)
+    def setAgentNeighborDist(self, size_t agent_no, float neighbor_dist):
+        self.thisptr.setAgentNeighborDist(agent_no, neighbor_dist)
+    def setAgentNeighborDist(self, size_t agent_no, float neighbor_dist):
+        self.thisptr.setAgentNeighborDist(agent_no, neighbor_dist)
+    def setAgentPosition(self, size_t agent_no, tuple position):
+        cdef Vector2 c_pos = Vector2(position[0], position[1])
+        self.thisptr.setAgentPosition(agent_no, c_pos)
+    def setAgentPrefVelocity(self, size_t agent_no, tuple velocity):
+        cdef Vector2 c_velocity = Vector2(velocity[0], velocity[1])
+        self.thisptr.setAgentPrefVelocity(agent_no, c_velocity)
+    def setAgentRadius(self, size_t agent_no, float radius):
+        self.thisptr.setAgentRadius(agent_no, radius)
+    def setAgentTimeHorizon(self, size_t agent_no, float time_horizon):
+        self.thisptr.setAgentTimeHorizon(agent_no, time_horizon)
+    def setAgentTimeHorizonObst(self, size_t agent_no, float timeHorizonObst):
+        self.thisptr.setAgentTimeHorizonObst(agent_no, timeHorizonObst)
+    def setAgentVelocity(self, size_t agent_no, tuple velocity):
+        cdef Vector2 c_velocity = Vector2(velocity[0], velocity[1])
+        self.thisptr.setAgentVelocity(agent_no, c_velocity)
+    def setTimeStep(self, float time_step):
+        return self.thisptr.setTimeStep(time_step)
-Original file line number
+Diff line change
@@ Expand Up / @@ -11,3 +11,7 @@ @@
     *.lai
     *.la
     *.a
+    # Python & Cython-generated files
+    build/
+    src/rvo2.cpp