add C and Python demo code

refactor code
add examples
add more python bindings

CMakeLists.txt

@@ -10,17 +10,18 @@ set(CMAKE_CXX_FLAGS_DEBUG "-g")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3")
 
 set(DWA_VERSION_MAJOR 1)
-set(DWA_VERSION_MINOR 0)
-set(DWA_VERSION_PATCH 3)
+set(DWA_VERSION_MINOR 1)
+set(DWA_VERSION_PATCH 0)
 set(DWA_VERSION_STRING ${DWA_VERSION_MAJOR}.${DWA_VERSION_MINOR}.${DWA_VERSION_PATCH})
 
 # Build demo executable
 include_directories(src)
-add_executable(demo src/demo.c src/dwa.c)
+add_executable(demo examples/demo.c src/dwa.c)
 set_target_properties(demo
   PROPERTIES
   RUNTIME_OUTPUT_DIRECTORY "../bin")
-target_link_libraries(demo m)
+target_link_libraries(demo m SDL GL glut)
+set_target_properties(demo PROPERTIES EXCLUDE_FROM_ALL TRUE)
 
 # Build shared library
 add_library(dwa SHARED src/dwa.c)

examples/demo.c

@@ -0,0 +1,175 @@
+#include "SDL/SDL.h"
+#include <GL/gl.h>
+#include "dwa.h"
+
+#define RESOLUTION_WIDTH  600
+#define RESOLUTION_HEIGHT 600
+#define M_PI 3.14159265358979323846
+#define M_PI2 M_PI * 2.0
+
+void initGL(void);
+void renderRectangle(Rect rect, Pose pose);    
+void renderPointCloud(PointCloud *pointCloud, int size);
+void drawCircle(GLfloat x, GLfloat y, GLfloat radius);
+
+void initGL(void) {
+  glMatrixMode(GL_PROJECTION);
+  glLoadIdentity();
+  glOrtho(-RESOLUTION_WIDTH/2, RESOLUTION_WIDTH/2,
+          -RESOLUTION_HEIGHT/2, RESOLUTION_HEIGHT/2,
+          1, -1);
+  glMatrixMode(GL_MODELVIEW);
+  glLoadIdentity();
+}
+
+void drawCircle(GLfloat x, GLfloat y, GLfloat radius){
+  int nTriangle = 20;
+  glBegin(GL_TRIANGLE_FAN);
+  glVertex2f(x, y);
+  for (int i = 0; i <= nTriangle; i++) { 
+    glVertex2f(x + (radius * cos(i * M_PI2 / nTriangle)), 
+	       y + (radius * sin(i * M_PI2 / nTriangle)));
+  }
+  glEnd();
+}
+
+void renderPointCloud(PointCloud *pointCloud, int size){
+  glBegin(GL_POINTS);
+  for (size_t i = 0; i < size; i++){
+    glColor3f(0, 0, 1);
+    drawCircle(pointCloud->points[i].x*10, pointCloud->points[i].y*10, 3.0);
+  }
+  glEnd();
+}
+
+void renderRectangle(Rect rect, Pose pose) {
+  glPushMatrix();
+  glTranslatef(pose.point.x*10.0,
+	       pose.point.y*10.0,
+	       0.0f);
+  glRotatef(pose.yaw * 180.0/M_PI, 0, 0, 1);
+  glBegin(GL_POLYGON);
+  glColor3f(1, 0, 0);
+  glVertex2f(rect.xmin*10, rect.ymax*10);
+  glVertex2f(rect.xmin*10, rect.ymin*10);
+  glVertex2f(rect.xmax*10, rect.ymin*10);
+  glVertex2f(rect.xmax*10, rect.ymax*10);
+  glEnd();
+  glPopMatrix();
+}
+
+int main() {
+  int running = 1;
+  int drawing = 0;
+
+  Point goal;
+  goal.x = 0.0;
+  goal.y = 0.0;
+
+  PointCloud *tmpPointCloud;
+  PointCloud *pointCloud = createPointCloud(100);
+  int currentIdx = 0;
+
+  Velocity velocity;
+  velocity.linearVelocity = 0.0;
+  velocity.angularVelocity = 0.0;
+
+  Pose pose;
+  pose.point.x = 0.0;
+  pose.point.y = 0.0;
+  pose.yaw = 0.0;
+
+  Config config;
+  Rect rect;
+
+  rect.xmin = -3.0;
+  rect.ymin = -2.5;
+  rect.xmax = +3.0;
+  rect.ymax = +2.5;
+
+  config.maxSpeed = 5.0;
+  config.minSpeed = 0.0;
+  config.maxYawrate = 60.0 * M_PI / 180.0;
+  config.maxAccel = 15.0;
+  config.maxdYawrate = 110.0 * M_PI / 180.0;
+  config.velocityResolution = 0.1;
+  config.yawrateResolution = 1.0 * M_PI / 180.0;
+  config.dt = 0.1;
+  config.predictTime = 3.0;
+  config.heading = 0.15;
+  config.clearance = 1.0;
+  config.velocity = 1.0;
+  config.base = rect;
+
+  if (SDL_Init(SDL_INIT_EVERYTHING) == -1) return 1;
+
+  SDL_WM_SetCaption("test", NULL);
+  SDL_SetVideoMode(RESOLUTION_WIDTH, RESOLUTION_HEIGHT, 0, SDL_OPENGL);
+
+  initGL();
+
+  while (running) {
+    SDL_Event sdlEvent;
+    glClear(GL_COLOR_BUFFER_BIT);
+    while (SDL_PollEvent(&sdlEvent) > 0) {
+      switch (sdlEvent.type) {
+      case SDL_QUIT:
+        running = 0;
+        break;
+      case SDL_KEYDOWN:
+	switch (sdlEvent.key.keysym.sym) {
+	case SDLK_ESCAPE:
+	  running = 0;
+	case SDLK_r:
+	  freePointCloud(pointCloud);
+	  pointCloud = createPointCloud(100);
+	  currentIdx = 0;
+	  pose.point.x = 0.0;
+	  pose.point.y = 0.0;
+	  pose.yaw = 0.0;
+	}
+	break;
+      case SDL_MOUSEBUTTONDOWN:
+	drawing = 1;
+        break;
+      case SDL_MOUSEBUTTONUP:
+	drawing = 0;
+	break;
+      case SDL_MOUSEMOTION:
+	if (drawing) {
+	  pointCloud->points[currentIdx].x = (sdlEvent.button.x - RESOLUTION_WIDTH/2.0)/10.0;
+	  pointCloud->points[currentIdx].y = (-sdlEvent.button.y + RESOLUTION_HEIGHT/2.0)/10.0;
+	  currentIdx++;
+	  if (!(currentIdx % 100)) {
+	    tmpPointCloud = createPointCloud(currentIdx);
+	    memcpy(tmpPointCloud->points, pointCloud->points, currentIdx*sizeof(Point));
+	    freePointCloud(pointCloud);
+	    pointCloud = createPointCloud(currentIdx+100);
+	    memcpy(pointCloud->points, tmpPointCloud->points, currentIdx*sizeof(Point));
+	    freePointCloud(tmpPointCloud);
+	  }
+	} else {
+	  goal.x = (sdlEvent.button.x - RESOLUTION_WIDTH/2.0)/10.0;
+	  goal.y = (-sdlEvent.button.y + RESOLUTION_HEIGHT/2.0)/10.0;
+	  glColor3f(0, 1, 0);
+	  drawCircle(goal.x*10, goal.y*10, 3);
+	}
+	break;
+      }
+    }
+    if (currentIdx) {
+      tmpPointCloud = createPointCloud(currentIdx);
+      memcpy(tmpPointCloud->points, pointCloud->points, currentIdx*sizeof(Point));
+      velocity = planning(pose, velocity, goal, tmpPointCloud, config);
+      pose = motion(pose, velocity, config.dt);
+      freePointCloud(tmpPointCloud);
+      renderPointCloud(pointCloud, currentIdx);
+    }
+    renderRectangle(config.base, pose);
+    SDL_GL_SwapBuffers();
+  }
+
+  SDL_Quit();
+  freePointCloud(pointCloud);
+  return 0;
+}

examples/demo.py

@@ -0,0 +1,113 @@
+#!/usr/bin/env python3
+
+import time
+
+import numpy as np
+import cv2
+import dwa
+
+class Demo(object):
+    def __init__(self):
+        # 1 px = 0.1 m
+        cv2.namedWindow('cvwindow')
+        cv2.setMouseCallback('cvwindow', self.callback)
+        self.drawing = False
+
+        self.point_cloud = []
+        self.draw_points = []
+
+        # Planner Settings
+        self.vel = (0.0, 0.0)
+        self.pose = (30.0, 30.0, 0)
+        self.goal = None
+        self.base = [-3.0, -2.5, +3.0, +2.5]
+        self.config = dwa.Config(
+                max_speed = 3.0,
+                min_speed = -1.0,
+                max_yawrate = np.radians(40.0),
+                max_accel = 15.0,
+                max_dyawrate = np.radians(110.0),
+                velocity_resolution = 0.1,
+                yawrate_resolution = np.radians(1.0),
+                dt = 0.1,
+                predict_time = 3.0,
+                heading = 0.15,
+                clearance = 1.0,
+                velocity = 1.0,
+                base = self.base)
+
+    def callback(self, event, x, y, flags, param):
+        if event == cv2.EVENT_LBUTTONDOWN:
+            self.drawing = True
+        elif event == cv2.EVENT_MOUSEMOVE:
+            if self.drawing:
+                if [x, y] not in self.draw_points:
+                    self.draw_points.append([x, y])
+                    self.point_cloud.append([x/10, y/10])
+                    self.goal = None
+            else:
+                self.goal = (x/10, y/10)
+        elif event == cv2.EVENT_LBUTTONUP:
+            self.drawing = False
+
+    def main(self):
+        import argparse
+        parser = argparse.ArgumentParser(description='DWA Demo')
+        parser.add_argument('--save', dest='save', action='store_true')
+        parser.set_defaults(save=False)
+        args = parser.parse_args()
+        if args.save:
+            import imageio
+            writer = imageio.get_writer('./dwa.gif', mode='I', duration=0.05)
+        while True:
+            prev_time = time.time()
+            self.map = np.zeros((600, 600, 3), dtype=np.uint8)
+            for point in self.draw_points:
+                cv2.circle(self.map, tuple(point), 4, (255, 255, 255), -1)
+            if self.goal is not None:
+                cv2.circle(self.map, (int(self.goal[0]*10), int(self.goal[1]*10)),
+                        4, (0, 255, 0), -1)
+                if len(self.point_cloud):
+                    # Planning
+                    self.vel = dwa.planning(self.pose, self.vel, self.goal,
+                            np.array(self.point_cloud, np.float32), self.config)
+                    # Simulate motion
+                    self.pose = dwa.motion(self.pose, self.vel, self.config.dt)
+
+            pose = np.ndarray((3,))
+            pose[0:2] = np.array(self.pose[0:2]) * 10
+            pose[2] = self.pose[2]
+
+            base = np.array(self.base) * 10
+            base[0:2] += pose[0:2]
+            base[2:4] += pose[0:2]
+
+            # Not the correct rectangle but good enough for the demo
+            width = base[2] - base[0]
+            height = base[3] - base[1]
+            rect = ((pose[0], pose[1]), (width, height), np.degrees(pose[2]))
+            box = cv2.boxPoints(rect)
+            box = np.int0(box)
+            cv2.drawContours(self.map,[box],0,(0,0,255),-1)
+
+            fps = int(1.0 / (time.time() - prev_time))
+            cv2.putText(self.map, f'FPS: {fps}', (20, 30),
+                    cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
+
+            cv2.putText(self.map, f'Point Cloud Size: {len(self.point_cloud)}',
+                    (20, 60), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
+
+            if args.save:
+                writer.append_data(self.map)
+            cv2.imshow('cvwindow', self.map)
+            key = cv2.waitKey(1)
+            if key == 27:
+                break
+            elif key == ord('r'):
+                self.point_cloud = []
+                self.draw_points = []
+        if args.save:
+            writer.close()
+
+if __name__ == '__main__':
+    Demo().main()

python/cdwa.pxd

@@ -1,9 +1,9 @@
 cdef extern from "dwa.h":
     ctypedef struct Rect:
-        float xtop
-        float yleft
-        float xbottom
-        float yright
+        float xmin
+        float ymin
+        float xmax
+        float ymax
     ctypedef struct Config:
         float maxSpeed
         float minSpeed
@@ -39,6 +39,13 @@ cdef extern from "dwa.h":
     Pose motion(Pose pose, Velocity velocity, float dt);
     Velocity planning(
         Pose pose, Velocity velocity, Point goal,
-        PointCloud *point_cloud, Config config);
+        PointCloud *pointCloud, Config config);
     PointCloud* createPointCloud(int size);
     void freePointCloud(PointCloud* pointCloud);
+    void freeDynamicWindow(DynamicWindow *dynamicWindow);
+    void createDynamicWindow(
+        Velocity velocity, Config config, DynamicWindow **dynamicWindow);
+    float calculateVelocityCost(Velocity velocity, Config config);
+    float calculateHeadingCost(Pose pose, Point goal);
+    float calculateClearanceCost(
+        Pose pose, Velocity velocity, PointCloud *pointCloud, Config config);