From 023d7c357702d11d122f6c4e6ef8b4d13d6287fd 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: Tue, 24 Dec 2019 18:58:33 +0300 Subject: [PATCH] add python bindings - python bindings - refactor - fix bugs --- .gitignore | 8 ++ CMakeLists.txt | 33 ++++++ makefile | 3 - python/cdwa.pxd | 44 ++++++++ python/dwa.pyx | 293 ++++++++++++++++++++++++++++++++++++++++++++++++ setup.py | 34 ++++++ src/demo.c | 53 +++++++++ src/dwa.c | 130 ++++++++------------- src/dwa.h | 38 ++++--- 9 files changed, 535 insertions(+), 101 deletions(-) create mode 100644 .gitignore create mode 100644 CMakeLists.txt delete mode 100644 makefile create mode 100644 python/cdwa.pxd create mode 100644 python/dwa.pyx create mode 100644 setup.py create mode 100644 src/demo.c diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..1e601b2 --- /dev/null +++ b/.gitignore @@ -0,0 +1,8 @@ +bin +*.npy +build +dist +*.egg-info +__pycache__ +python/dwa.c +test.py diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..8ca00d9 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 2.8) +project(DynamicWindowApproach) + +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +set(CMAKE_CXX_FLAGS "-Wall -Wextra") +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 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) +set_target_properties(demo + PROPERTIES + RUNTIME_OUTPUT_DIRECTORY "../bin") +target_link_libraries(demo m) + +# Build shared library +add_library(dwa SHARED src/dwa.c) +target_link_libraries(dwa m) +set_target_properties(dwa PROPERTIES PUBLIC_HEADER "src/dwa.h" + VERSION ${DWA_VERSION_STRING} + SOVERSION ${DWA_VERSION_MAJOR}) +INSTALL(TARGETS dwa + LIBRARY DESTINATION lib + PUBLIC_HEADER DESTINATION include) diff --git a/makefile b/makefile deleted file mode 100644 index 8f902ec..0000000 --- a/makefile +++ /dev/null @@ -1,3 +0,0 @@ -all: ./src/dwa.c ./src/dwa.h - gcc -o dwa ./src/dwa.c -I./src -lm - diff --git a/python/cdwa.pxd b/python/cdwa.pxd new file mode 100644 index 0000000..5ddb445 --- /dev/null +++ b/python/cdwa.pxd @@ -0,0 +1,44 @@ +cdef extern from "dwa.h": + ctypedef struct Rect: + float xtop + float yleft + float xbottom + float yright + ctypedef struct Config: + float maxSpeed + float minSpeed + float maxYawrate + float maxAccel + float maxdYawrate + float velocityResolution + float yawrateResolution + float dt + float predictTime + float heading + float clearance + float velocity + Rect base + ctypedef struct Point: + float x + float y + ctypedef struct PointCloud: + int size; + Point *points; + ctypedef struct Pose: + Point point + float yaw + ctypedef struct Velocity: + float linearVelocity + float angularVelocity + ctypedef struct DynamicWindow: + int nPossibleV; + float *possibleV; + int nPossibleW; + float *possibleW; + + Pose motion(Pose pose, Velocity velocity, float dt); + Velocity planning( + Pose pose, Velocity velocity, Point goal, + PointCloud *point_cloud, Config config); + PointCloud* createPointCloud(int size); + void freePointCloud(PointCloud* pointCloud); diff --git a/python/dwa.pyx b/python/dwa.pyx new file mode 100644 index 0000000..2fd614c --- /dev/null +++ b/python/dwa.pyx @@ -0,0 +1,293 @@ +# distutils: sources = ./src/dwa.c +# distutils: include_dirs = src + +from libc.stdlib cimport malloc, free + +import numpy as np +cimport numpy as np + +cimport cdwa + +cdef class Velocity: + cdef cdwa.Velocity* thisptr + + def __cinit__(self, float linear_velocity, float angular_velocity): + self.thisptr = malloc(sizeof(cdwa.Velocity)) + self.thisptr.linearVelocity = linear_velocity + self.thisptr.angularVelocity = angular_velocity + if self.thisptr is NULL: + raise MemoryError + + def __dealloc__(self): + if self.thisptr is not NULL: + free(self.thisptr) + + property linear_velocity: + def __get__(self): + assert self.thisptr is not NULL + return self.thisptr.linearVelocity + + property angular_velocity: + def __get__(self): + assert self.thisptr is not NULL + return self.thisptr.angularVelocity + + def __str__(self): + string = f'Linear Velocity: {self.linear_velocity}' + string = f'{string}, Angular Velocity: {self.angular_velocity}' + return string + +cdef class Point: + cdef cdwa.Point* thisptr + + def __cinit__(self, float x, float y): + self.thisptr = malloc(sizeof(cdwa.Point)) + self.thisptr.x = x + self.thisptr.y = y + if self.thisptr is NULL: + raise MemoryError + + def __dealloc__(self): + if self.thisptr is not NULL: + free(self.thisptr) + + property x: + def __get__(self): + assert self.thisptr is not NULL + return self.thisptr.x + + property y: + def __get__(self): + assert self.thisptr is not NULL + return self.thisptr.y + + def __str__(self): + string = f'x: {self.thisptr.x}, y: {self.thisptr.y}' + return string + +cdef class Pose: + cdef cdwa.Pose* thisptr + + def __cinit__(self, Point point, float yaw): + self.thisptr = malloc(sizeof(cdwa.Pose)) + self.thisptr.point.x = point.x + self.thisptr.point.y = point.y + self.thisptr.yaw = yaw + if self.thisptr is NULL: + raise MemoryError + + def __dealloc__(self): + if self.thisptr is not NULL: + free(self.thisptr) + + property point: + def __get__(self): + assert self.thisptr is not NULL + return Point(self.thisptr.point.x, self.thisptr.point.y) + + property yaw: + def __get__(self): + assert self.thisptr is not NULL + return self.thisptr.yaw + + def __str__(self): + string = f'x: {self.thisptr.point.x}, y: {self.thisptr.point.y}' + string = f'{string}, yaw: {self.thisptr.yaw}' + return string + +cdef class Config: + cdef cdwa.Config* thisptr + + def __cinit__(self, float max_speed, float min_speed, + float max_yawrate, float max_accel, float max_dyawrate, + float velocity_resolution, float yawrate_resolution, float dt, + float predict_time, float heading, float clearance, float velocity, + list base): + + self.thisptr = malloc(sizeof(cdwa.Config)) + self.thisptr.maxSpeed = max_speed + self.thisptr.minSpeed = min_speed + self.thisptr.maxYawrate = max_yawrate + self.thisptr.maxAccel = max_accel + self.thisptr.maxdYawrate = max_dyawrate + self.thisptr.velocityResolution = velocity_resolution + self.thisptr.yawrateResolution = yawrate_resolution + self.thisptr.dt = dt + self.thisptr.predictTime = predict_time + 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] + if self.thisptr is NULL: + raise MemoryError + + def __dealloc__(self): + if self.thisptr is not NULL: + free(self.thisptr) + + property max_speed: + def __get__(self): + assert self.thisptr is not NULL + return self.thisptr.maxSpeed + def __set__(self, value): + assert self.thisptr is not NULL + self.thisptr.maxSpeed = value + + property min_speed: + def __get__(self): + assert self.thisptr is not NULL + return self.thisptr.minSpeed + def __set__(self, value): + assert self.thisptr is not NULL + self.thisptr.minSpeed = value + + property dt: + def __get__(self): + assert self.thisptr is not NULL + return self.thisptr.dt + def __set__(self, value): + assert self.thisptr is not NULL + self.thisptr.dt = value + + property max_yawrate: + def __get__(self): + assert self.thisptr is not NULL + return self.thisptr.maxYawrate + def __set__(self, value): + assert self.thisptr is not NULL + self.thisptr.maxYawrate = value + + property max_accel: + def __get__(self): + assert self.thisptr is not NULL + return self.thisptr.maxAccel + def __set__(self, value): + assert self.thisptr is not NULL + self.thisptr.maxAccel = value + + property max_dyawrate: + def __get__(self): + assert self.thisptr is not NULL + return self.thisptr.maxdYawrate + def __set__(self, value): + assert self.thisptr is not NULL + self.thisptr.maxdYawrate = value + + property velocity_resolution: + def __get__(self): + assert self.thisptr is not NULL + return self.thisptr.velocityResolution + def __set__(self, value): + assert self.thisptr is not NULL + self.thisptr.velocityResolution = value + + property yawrate_resolution: + def __get__(self): + assert self.thisptr is not NULL + return self.thisptr.yawrateResolution + def __set__(self, value): + assert self.thisptr is not NULL + self.thisptr.yawrateResolution = value + + property predict_time: + def __get__(self): + assert self.thisptr is not NULL + return self.thisptr.predictTime + def __set__(self, value): + assert self.thisptr is not NULL + self.thisptr.predictTime = value + + property heading: + def __get__(self): + assert self.thisptr is not NULL + return self.thisptr.heading + def __set__(self, value): + assert self.thisptr is not NULL + self.thisptr.heading = value + + property clearance: + def __get__(self): + assert self.thisptr is not NULL + return self.thisptr.clearance + def __set__(self, value): + assert self.thisptr is not NULL + self.thisptr.clearance = value + + property velocity: + def __get__(self): + assert self.thisptr is not NULL + return self.thisptr.velocity + def __set__(self, value): + assert self.thisptr is not NULL + 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}' + return string + +cdef class PointCloud: + cdef cdwa.PointCloud* thisptr + + def __cinit__(self, np.ndarray[np.float32_t, ndim=2] point_cloud): + cdef int size = len(point_cloud) + self.thisptr = cdwa.createPointCloud(size) + self.thisptr.size = size + cdef float x + cdef float y + for i in range(size): + x, y = point_cloud[i] + self.thisptr.points[i].x = x + self.thisptr.points[i].y = y + if self.thisptr is NULL: + raise MemoryError + + def __dealloc__(self): + if self.thisptr is not NULL: + cdwa.freePointCloud(self.thisptr) + +def motion(tuple pose, tuple velocity, float dt): + cdef float x, y, yaw, v , w + x, y, yaw = pose + v, w = velocity + cdef Pose _pose = Pose(Point(x, y), yaw) + cdef Velocity _velocity = Velocity(v, w) + cdef cdwa.Pose new_pose + new_pose = cdwa.motion(_pose.thisptr[0], _velocity.thisptr[0], dt) + return (new_pose.point.x, new_pose.point.y, new_pose.yaw) + +def planning(tuple pose, tuple velocity, tuple goal, + np.ndarray[np.float32_t, ndim=2] point_cloud, + Config config): + + cdef float x, y, yaw, v , w, gx, gy + cdef PointCloud _point_cloud = PointCloud(point_cloud) + x, y, yaw = pose + v, w = velocity + gx, gy = goal + cdef Pose _pose = Pose(Point(x, y), yaw) + cdef Velocity _velocity = Velocity(v, w) + cdef Point _goal = Point(gx, gy) + cdef cdwa.Velocity best_velocity + best_velocity = cdwa.planning(_pose.thisptr[0], _velocity.thisptr[0], + _goal.thisptr[0], _point_cloud.thisptr, + config.thisptr[0]); + + return (best_velocity.linearVelocity, best_velocity.angularVelocity) diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..d9162e9 --- /dev/null +++ b/setup.py @@ -0,0 +1,34 @@ +#!/usr/bin/env python3 + +import os +from setuptools import setup +from distutils.extension import Extension +from Cython.Build import cythonize + +directory = os.path.abspath(os.path.dirname(__file__)) +with open(os.path.join(directory, 'README.md'), encoding='utf-8') as f: + long_description = f.read() + +setup( + name='dynamic-window-approach', + description='Dynamic Window Approach algorithm written in C wiht Python Bindings', + version='1.0.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"])]), + url='https://github.com/goktug97/DynamicWindowApproach', + download_url=( + 'https://github.com/goktug97/DynamicWindowApproach/archive/v1.0.0.tar.gz'), + classifiers=[ + "Programming Language :: Python :: 3", + "License :: OSI Approved :: MIT License", + "Operating System :: POSIX :: Linux", + ], + install_requires=[ + 'numpy', + ], + python_requires='>=3.6', + include_package_data=True) diff --git a/src/demo.c b/src/demo.c new file mode 100644 index 0000000..84af3cd --- /dev/null +++ b/src/demo.c @@ -0,0 +1,53 @@ +#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 e4ce5d0..88c1d4c 100644 --- a/src/dwa.c +++ b/src/dwa.c @@ -1,35 +1,50 @@ #include "dwa.h" void -createDynamicWindow(Velocity velocity, Robot robot, DynamicWindow **dynamicWindow) { - float minV = max(robot.minSpeed, velocity.linearVelocity - robot.maxAccel * robot.dt); - float maxV = min(robot.maxSpeed, velocity.linearVelocity + robot.maxAccel * robot.dt); +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 minW = - max(-robot.maxYawrate, velocity.angularVelocity - robot.maxdYawrate * robot.dt); + max(-config.maxYawrate, velocity.angularVelocity - config.maxdYawrate * config.dt); float maxW = - max(robot.maxYawrate, velocity.angularVelocity + robot.maxdYawrate * robot.dt); + max(config.maxYawrate, velocity.angularVelocity + config.maxdYawrate * config.dt); - int nPossibleV = (maxV - minV) / robot.velocityResolution; - int nPossibleW = (maxW - minW) / robot.yawrateResolution; - *dynamicWindow = malloc(sizeof(DynamicWindow) + - nPossibleV * sizeof(float) + - nPossibleW * sizeof(float) + - 2 * sizeof(int)); + int nPossibleV = (maxV - minV) / config.velocityResolution; + int nPossibleW = (maxW - minW) / config.yawrateResolution; + *dynamicWindow = malloc(sizeof(DynamicWindow)); (*dynamicWindow)->possibleV = malloc(nPossibleV * sizeof(float)); (*dynamicWindow)->possibleW = malloc(nPossibleW * sizeof(float)); (*dynamicWindow)->nPossibleV = nPossibleV; (*dynamicWindow)->nPossibleW = nPossibleW; - for(int i=0; i < (maxV - minV) / robot.velocityResolution; i++) { - (*dynamicWindow)->possibleV[i] = minV + (float)i * robot.velocityResolution; + for(int i=0; i < nPossibleV; i++) { + (*dynamicWindow)->possibleV[i] = minV + (float)i * config.velocityResolution; } - for(int i=0; i < (maxW - minW) / robot.yawrateResolution; i++) { - (*dynamicWindow)->possibleW[i] = minW + (float)i * robot.yawrateResolution; + for(int i=0; i < nPossibleW; i++) { + (*dynamicWindow)->possibleW[i] = minW + (float)i * config.yawrateResolution; } } +void freeDynamicWindow(DynamicWindow *dynamicWindow){ + free(dynamicWindow->possibleV); + free(dynamicWindow->possibleW); + free(dynamicWindow); +} + +PointCloud* createPointCloud(int size){ + PointCloud* pointCloud = malloc(sizeof(PointCloud)); + pointCloud->points = malloc(size * sizeof(Point)); + pointCloud->size = size; + return pointCloud; +} + +void freePointCloud(PointCloud* pointCloud){ + free(pointCloud->points); + free(pointCloud); +} + Pose motion(Pose pose, Velocity velocity, float dt){ Pose new_pose; new_pose.yaw = pose.yaw + velocity.angularVelocity * dt; @@ -38,7 +53,7 @@ Pose motion(Pose pose, Velocity velocity, float dt){ return new_pose; } -float calculateVelocityCost(Velocity velocity, Robot config) { +float calculateVelocityCost(Velocity velocity, Config config) { return config.maxSpeed - velocity.linearVelocity; } @@ -51,7 +66,8 @@ float calculateHeadingCost(Pose pose, Point goal) { } float -calculateClearanceCost(Pose pose, Velocity velocity, Point *pointCloud, Robot config) { +calculateClearanceCost +(Pose pose, Velocity velocity, PointCloud *pointCloud, Config config) { Pose pPose = pose; float time = 0.0; float minr = FLT_MAX; @@ -65,17 +81,18 @@ calculateClearanceCost(Pose pose, Velocity velocity, Point *pointCloud, Robot co while (time < config.predictTime) { pPose = motion(pPose, velocity, config.dt); - for(int i = 0; i < sizeof(pointCloud)/sizeof(pointCloud[0]); ++i) { - dx = pPose.point.x - pointCloud[i].x; - dy = pPose.point.y - pointCloud[i].y; - x = -dx * cos(pPose.yaw) + -dy * -sin(pPose.yaw); - y = -dx * sin(pPose.yaw) + -dy * cos(pPose.yaw); + for(int i = 0; i < pointCloud->size; ++i) { + dx = pPose.point.x - pointCloud->points[i].x; + 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) + y >= config.base.yright){ return FLT_MAX; - r = sqrt(dx*dx + dy*dy); + } + r = sqrtf(dx*dx + dy*dy); if (r < minr) minr = r; } @@ -85,7 +102,8 @@ calculateClearanceCost(Pose pose, Velocity velocity, Point *pointCloud, Robot co } Velocity -planning(Pose pose, Velocity velocity, Point goal, Point *pointCloud, Robot config){ +planning(Pose pose, Velocity velocity, Point goal, + PointCloud *pointCloud, Config config){ DynamicWindow *dw; createDynamicWindow(velocity, config, &dw); Velocity pVelocity; @@ -95,10 +113,10 @@ planning(Pose pose, Velocity velocity, Point goal, Point *pointCloud, Robot conf Velocity bestVelocity; for (int i = 0; i < dw->nPossibleV; ++i) { for (int j = 0; j < dw->nPossibleW; ++j) { - pPose = motion(pPose, pVelocity, config.predictTime); + pPose = pose; pVelocity.linearVelocity = dw->possibleV[i]; pVelocity.angularVelocity = dw->possibleW[j]; - + pPose = motion(pPose, pVelocity, config.predictTime); cost = config.velocity * calculateVelocityCost(pVelocity, config) + config.heading * calculateHeadingCost(pPose, goal) + @@ -109,62 +127,6 @@ planning(Pose pose, Velocity velocity, Point goal, Point *pointCloud, Robot conf } } } - free(dw); + freeDynamicWindow(dw); return bestVelocity; } - -void main() { - Robot 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 = 0.0; - - Point pointCloud[10]; - pointCloud[0].x = 0.0; pointCloud[0].y = 1.0; - pointCloud[1].x = 2.0; pointCloud[1].y = 2.0; - pointCloud[2].x = 0.0; pointCloud[2].y = 3.0; - pointCloud[3].x = 3.0; pointCloud[3].y = 1.0; - pointCloud[4].x = 3.5; pointCloud[4].y = 4.0; - pointCloud[5].x = 5.0; pointCloud[5].y = 4.0; - pointCloud[6].x = 7.0; pointCloud[6].y = 1.0; - pointCloud[7].x = 9.0; pointCloud[7].y = 1.0; - pointCloud[8].x = 8.0; pointCloud[8].y = 8.0; - pointCloud[9].x = 10.0; pointCloud[9].y = 3.0; - - pose = motion(pose, velocity, config.dt); - printf("%f %f %f\n", pose.point.x, pose.point.y, pose.yaw); - velocity = planning(pose, velocity, goal, pointCloud, config); - printf("%f %f\n", velocity.linearVelocity, velocity.angularVelocity); - pose = motion(pose, velocity, config.dt); - printf("%f %f %f\n", pose.point.x, pose.point.y, pose.yaw); -} diff --git a/src/dwa.h b/src/dwa.h index 44d8631..6c6c93b 100644 --- a/src/dwa.h +++ b/src/dwa.h @@ -5,16 +5,15 @@ #include /* https://stackoverflow.com/a/3437484 */ - #define max(a,b) \ - ({ __typeof__ (a) _a = (a); \ - __typeof__ (b) _b = (b); \ - _a > _b ? _a : _b; }) +#define max(a,b) \ + ({ __typeof__ (a) _a = (a); \ + __typeof__ (b) _b = (b); \ + _a > _b ? _a : _b; }) -/* https://stackoverflow.com/a/3437484 */ - #define min(a,b) \ - ({ __typeof__ (a) _a = (a); \ - __typeof__ (b) _b = (b); \ - _a < _b ? _a : _b; }) +#define min(a,b) \ + ({ __typeof__ (a) _a = (a); \ + __typeof__ (b) _b = (b); \ + _a < _b ? _a : _b; }) typedef struct { float xtop; @@ -37,7 +36,7 @@ typedef struct { float clearance; float velocity; Rect base; -} Robot; +} Config; typedef struct { float linearVelocity; @@ -49,6 +48,11 @@ typedef struct { float y; } Point; +typedef struct { + int size; + Point *points; +} PointCloud; + typedef struct { Point point; float yaw; @@ -61,11 +65,17 @@ typedef struct { float *possibleW; } DynamicWindow; -void createDynamicWindow(Velocity velocity, Robot robot, DynamicWindow **dynamicWindow); +void +createDynamicWindow(Velocity velocity, Config config, DynamicWindow **dynamicWindow); Pose motion(Pose pose, Velocity velocity, float dt); -float calculateVelocityCost(Velocity velocity, Robot config); +float calculateVelocityCost(Velocity velocity, Config config); float calculateHeadingCost(Pose pose, Point goal); float -calculateClearanceCost(Pose pose, Velocity velocity, Point *pointCloud, Robot config); +calculateClearanceCost +(Pose pose, Velocity velocity, PointCloud *pointCloud, Config config); Velocity -planning(Pose pose, Velocity velocity, Point goal, Point *pointCloud, Robot config); +planning +(Pose pose, Velocity velocity, Point goal, PointCloud *pointCloud, Config config); +PointCloud* createPointCloud(int size); +void freePointCloud(PointCloud* pointCloud); +void freeDynamicWindow(DynamicWindow *dynamicWindow);