From 8f862a32f8bb3c253ee66ede668efb8518c16112 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?G=C3=B6ktu=C4=9F=20Karaka=C5=9Fl=C4=B1?= <20567087+goktug97@users.noreply.github.com> Date: Wed, 1 Jan 2020 21:09:35 +0300 Subject: [PATCH] add C and Python demo code refactor code add examples add more python bindings --- CMakeLists.txt | 9 +-- examples/demo.c | 175 +++++++++++++++++++++++++++++++++++++++++++++++ examples/demo.py | 113 ++++++++++++++++++++++++++++++ python/cdwa.pxd | 17 +++-- python/dwa.pyx | 100 +++++++++++++++++++++------ setup.py | 9 ++- src/demo.c | 53 -------------- src/dwa.c | 14 ++-- src/dwa.h | 9 +-- 9 files changed, 404 insertions(+), 95 deletions(-) create mode 100644 examples/demo.c create mode 100644 examples/demo.py delete mode 100644 src/demo.c diff --git a/CMakeLists.txt b/CMakeLists.txt index 4098b1c..325a803 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/examples/demo.c b/examples/demo.c new file mode 100644 index 0000000..b28ef62 --- /dev/null +++ b/examples/demo.c @@ -0,0 +1,175 @@ +#include "SDL/SDL.h" +#include +#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; +} diff --git a/examples/demo.py b/examples/demo.py new file mode 100644 index 0000000..f94ff75 --- /dev/null +++ b/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() diff --git a/python/cdwa.pxd b/python/cdwa.pxd index 5ddb445..4e5bcd4 100644 --- a/python/cdwa.pxd +++ b/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); diff --git a/python/dwa.pyx b/python/dwa.pyx index 3a91d2c..99c26c7 100644 --- a/python/dwa.pyx +++ b/python/dwa.pyx @@ -117,10 +117,10 @@ cdef class Config: self.thisptr.heading = heading self.thisptr.clearance = clearance self.thisptr.velocity = velocity - self.thisptr.base.xtop = base[0] - self.thisptr.base.yleft = base[1] - self.thisptr.base.xbottom = base[2] - self.thisptr.base.yright = base[3] + self.thisptr.base.xmin = base[0] + self.thisptr.base.ymin = base[1] + self.thisptr.base.xmax = base[2] + self.thisptr.base.ymax = base[3] if self.thisptr is NULL: raise MemoryError @@ -225,22 +225,22 @@ cdef class Config: self.thisptr.velocity = value def __str__(self): - string = f'maxSpeed {self.thisptr.maxSpeed}' - string = f'{string}\nminSpeed {self.thisptr.minSpeed}' - string = f'{string}\nmaxYawrate {self.thisptr.maxYawrate}' - string = f'{string}\nmaxAccel {self.thisptr.maxAccel}' - string = f'{string}\nmaxdYawrate {self.thisptr.maxdYawrate}' - string = f'{string}\nvelocityResolution {self.thisptr.velocityResolution}' - string = f'{string}\nyawrateResolution {self.thisptr.yawrateResolution}' - string = f'{string}\ndt {self.thisptr.dt}' - string = f'{string}\npredictTime {self.thisptr.predictTime}' - string = f'{string}\nheading {self.thisptr.heading}' - string = f'{string}\nclearance {self.thisptr.clearance}' - string = f'{string}\nvelocity {self.thisptr.velocity}' - string = f'{string}\nbase.xtop {self.thisptr.base.xtop}' - string = f'{string}\nbase.yleft {self.thisptr.base.yleft}' - string = f'{string}\nbase.xbottom {self.thisptr.base.xbottom}' - string = f'{string}\nbase.yright {self.thisptr.base.yright}' + string = f'maxSpeed {self.thisptr.maxSpeed}' + string = f'{string}\nminSpeed {self.thisptr.minSpeed}' + string = f'{string}\nmaxYawrate {self.thisptr.maxYawrate}' + string = f'{string}\nmaxAccel {self.thisptr.maxAccel}' + string = f'{string}\nmaxdYawrate {self.thisptr.maxdYawrate}' + string = f'{string}\nvelocityResolution {self.thisptr.velocityResolution}' + string = f'{string}\nyawrateResolution {self.thisptr.yawrateResolution}' + string = f'{string}\ndt {self.thisptr.dt}' + string = f'{string}\npredictTime {self.thisptr.predictTime}' + string = f'{string}\nheading {self.thisptr.heading}' + string = f'{string}\nclearance {self.thisptr.clearance}' + string = f'{string}\nvelocity {self.thisptr.velocity}' + string = f'{string}\nbase.xtop {self.thisptr.base.xtop}' + string = f'{string}\nbase.yleft {self.thisptr.base.yleft}' + string = f'{string}\nbase.xbottom {self.thisptr.base.xbottom}' + string = f'{string}\nbase.yright {self.thisptr.base.yright}' return string cdef class PointCloud: @@ -263,6 +263,66 @@ cdef class PointCloud: if self.thisptr is not NULL: cdwa.freePointCloud(self.thisptr) +cdef class DynamicWindow: + cdef cdwa.DynamicWindow* thisptr + + def __cinit__(self, tuple velocity, Config config): + cdef float v , w + v, w = velocity + cdef Velocity _velocity = Velocity(v, w) + cdwa.createDynamicWindow(_velocity.thisptr[0], config.thisptr[0], &self.thisptr) + if self.thisptr is NULL: + raise MemoryError + + property possible_v: + def __get__(self): + assert self.thisptr is not NULL + return np.array( + self.thisptr.possibleV, + dtype=np.float32) + + property possible_w: + def __get__(self): + assert self.thisptr is not NULL + return np.array( + self.thisptr.possibleW, + dtype=np.float32) + + def __dealloc__(self): + if self.thisptr is not NULL: + cdwa.freeDynamicWindow(self.thisptr) + + def __str__(self): + string = f'Possible Linear Velocities: {self.possible_v}' + string = f'{string}\nPossible Angular Velocities: {self.possible_w}' + return string + +def calculate_velocity_cost(tuple velocity, Config config): + cdef float v , w + v, w = velocity + cdef Velocity _velocity = Velocity(v, w) + return cdwa.calculateVelocityCost(_velocity.thisptr[0], config.thisptr[0]); + +def calculate_heading_cost(tuple pose, tuple goal): + cdef float x, y, yaw, gx, gy + x, y, yaw = pose + gx, gy = goal + cdef Pose _pose = Pose(Point(x, y), yaw) + cdef Point _goal = Point(gx, gy) + return cdwa.calculateHeadingCost(_pose.thisptr[0], _goal.thisptr[0]) + +def calculate_clearance_cost(tuple pose, tuple velocity, + np.ndarray[np.float32_t, ndim=2] point_cloud, + Config config): + cdef float x, y, yaw, v , w + cdef PointCloud _point_cloud = PointCloud(point_cloud) + x, y, yaw = pose + v, w = velocity + cdef Pose _pose = Pose(Point(x, y), yaw) + cdef Velocity _velocity = Velocity(v, w) + return cdwa.calculateClearanceCost(_pose.thisptr[0], _velocity.thisptr[0], + _point_cloud.thisptr, config.thisptr[0]) + def motion(tuple pose, tuple velocity, float dt): cdef float x, y, yaw, v , w x, y, yaw = pose diff --git a/setup.py b/setup.py index 033da8c..812ec24 100644 --- a/setup.py +++ b/setup.py @@ -12,23 +12,26 @@ setup( name='dynamic-window-approach', description='Dynamic Window Approach algorithm written in C with Python Bindings', - version='1.0.3', + version='1.1.0', author='Göktuğ Karakaşlı', author_email='karakasligk@gmail.com', license='MIT', long_description=long_description, long_description_content_type='text/markdown', ext_modules = cythonize([Extension("dwa", ["python/dwa.pyx"], - include_dirs=[numpy.get_include(), 'src'])]), + include_dirs=[numpy.get_include(), 'src'], + define_macros = [('NPY_NO_DEPRECATED_API', 'NPY_1_7_API_VERSION')])], + compiler_directives={'language_level' : "3"}), url='https://github.com/goktug97/DynamicWindowApproach', download_url=( - 'https://github.com/goktug97/DynamicWindowApproach/archive/v1.0.3.tar.gz'), + 'https://github.com/goktug97/DynamicWindowApproach/archive/v1.1.0.tar.gz'), classifiers=[ "Programming Language :: Python :: 3", "License :: OSI Approved :: MIT License", "Operating System :: POSIX :: Linux", ], install_requires=[ + 'cython', 'numpy', ], python_requires='>=3.6', diff --git a/src/demo.c b/src/demo.c deleted file mode 100644 index 84af3cd..0000000 --- a/src/demo.c +++ /dev/null @@ -1,53 +0,0 @@ -#include "dwa.h" - -int main() { - Config config; - Rect rect; - - rect.xtop = 0.6; - rect.yleft = 0.5; - rect.xbottom = -0.6; - rect.yright = -0.5; - - config.maxSpeed = 0.5; - config.minSpeed = 0.0; - config.maxYawrate = 40.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 = 1.0; - config.heading = 0.5; - config.clearance = 0.5; - config.velocity = 0.5; - config.base = rect; - - 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; - - Point goal; - goal.x = 10.0; - goal.y = 10.0; - - PointCloud *pointCloud = createPointCloud(3); - pointCloud->points[0].x = -1.0; pointCloud->points[0].y = 1.0; - pointCloud->points[1].x = 0.0; pointCloud->points[1].y = 2.0; - pointCloud->points[2].x = 4.0; pointCloud->points[2].y = 2.0; - - while(1){ - velocity = planning(pose, velocity, goal, pointCloud, config); - pose = motion(pose, velocity, config.dt); - printf("x: %f y: %f yaw: %f\n", pose.point.x, pose.point.y, pose.yaw); - if (sqrtf(powf(pose.point.x-goal.x, 2) + powf(pose.point.y-goal.y, 2)) < 1.0) - break; - } - freePointCloud(pointCloud); - return 0; -} diff --git a/src/dwa.c b/src/dwa.c index 88c1d4c..f2d58ed 100644 --- a/src/dwa.c +++ b/src/dwa.c @@ -2,8 +2,10 @@ void createDynamicWindow(Velocity velocity, Config config, DynamicWindow **dynamicWindow) { - float minV = max(config.minSpeed, velocity.linearVelocity - config.maxAccel * config.dt); - float maxV = min(config.maxSpeed, velocity.linearVelocity + config.maxAccel * config.dt); + float + minV = max(config.minSpeed, velocity.linearVelocity - config.maxAccel * config.dt); + float + maxV = min(config.maxSpeed, velocity.linearVelocity + config.maxAccel * config.dt); float minW = max(-config.maxYawrate, velocity.angularVelocity - config.maxdYawrate * config.dt); float maxW = @@ -86,10 +88,10 @@ calculateClearanceCost dy = pPose.point.y - pointCloud->points[i].y; x = -dx * cos(pPose.yaw) + -dy * sin(pPose.yaw); y = -dx * -sin(pPose.yaw) + -dy * cos(pPose.yaw); - if (x <= config.base.xtop && - x >= config.base.xbottom && - y <= config.base.yleft && - y >= config.base.yright){ + if (x <= config.base.xmax && + x >= config.base.xmin && + y <= config.base.ymax && + y >= config.base.ymin){ return FLT_MAX; } r = sqrtf(dx*dx + dy*dy); diff --git a/src/dwa.h b/src/dwa.h index 6c6c93b..412bfc7 100644 --- a/src/dwa.h +++ b/src/dwa.h @@ -15,11 +15,12 @@ __typeof__ (b) _b = (b); \ _a < _b ? _a : _b; }) + typedef struct { - float xtop; - float yleft; - float xbottom; - float yright; + float xmin; + float ymin; + float xmax; + float ymax; } Rect; typedef struct {