add dynamic voronoi

dynamicvoronoi/CMakeLists.txt

@@ -0,0 +1,161 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(dynamicvoronoi)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependencies might have been
+##     pulled in transitively but can be declared for certainty nonetheless:
+##     * add a build_depend tag for "message_generation"
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES dynamicvoronoi
+#  CATKIN_DEPENDS roscpp
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(include)
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a cpp library
+add_library(dynamicvoronoi
+  src/bucketedqueue.cpp src/dynamicvoronoi.cpp
+)
+
+## Declare a cpp executable
+# add_executable(dynamicvoronoi_node src/dynamicvoronoi_node.cpp)
+
+## Add cmake target dependencies of the executable/library
+## as an example, message headers may need to be generated before nodes
+# add_dependencies(dynamicvoronoi_node dynamicvoronoi_generate_messages_cpp)
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(dynamicvoronoi_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS dynamicvoronoi dynamicvoronoi_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_dynamicvoronoi.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

dynamicvoronoi/README.md

@@ -0,0 +1 @@
+# dynamic_voronoi

dynamicvoronoi/include/bucketedqueue.h

@@ -0,0 +1,45 @@
+#ifndef _PRIORITYQUEUE2_H_
+#define _PRIORITYQUEUE2_H_
+
+#define MAXDIST 1000
+#define RESERVE 64
+
+#include <vector>
+#include <set>
+#include <queue>
+#include <assert.h>
+#include "point.h"
+
+//! Priority queue for integer coordinates with squared distances as priority.
+/** A priority queue that uses buckets to group elements with the same priority.
+ *  The individual buckets are unsorted, which increases efficiency if these groups are large.
+ *  The elements are assumed to be integer coordinates, and the priorities are assumed
+ *  to be squared euclidean distances (integers).
+ */
+class BucketPrioQueue {
+
+public:
+  //! Standard constructor
+  /** Standard constructor. When called for the first time it creates a look up table 
+   *  that maps square distanes to bucket numbers, which might take some time... 
+   */
+  BucketPrioQueue(); 
+  //! Checks whether the Queue is empty
+  bool empty();
+  //! push an element
+  void push(int prio, INTPOINT t);
+  //! return and pop the element with the lowest squared distance */
+  INTPOINT pop();
+
+private:
+
+  static void initSqrIndices();
+  static std::vector<int> sqrIndices;
+  static int numBuckets;
+  int count;
+  int nextBucket;
+
+  std::vector<std::queue<INTPOINT> > buckets;
+};
+
+#endif

dynamicvoronoi/include/dynamicvoronoi.h

@@ -0,0 +1,106 @@
+#ifndef _DYNAMICVORONOI_H_
+#define _DYNAMICVORONOI_H_
+
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <limits.h>
+#include <queue>
+
+#include "bucketedqueue.h"
+
+//! A DynamicVoronoi object computes and updates a distance map and Voronoi diagram.
+class DynamicVoronoi {
+
+public:
+
+  DynamicVoronoi();
+  ~DynamicVoronoi();
+
+  //! Initialization with an empty map
+  void initializeEmpty(int _sizeX, int _sizeY, bool initGridMap=true);
+  //! Initialization with a given binary map (false==free, true==occupied)
+  void initializeMap(int _sizeX, int _sizeY, bool** _gridMap);
+
+  //! add an obstacle at the specified cell coordinate
+  void occupyCell(int x, int y);
+  //! remove an obstacle at the specified cell coordinate
+  void clearCell(int x, int y);
+  //! remove old dynamic obstacles and add the new ones
+  void exchangeObstacles(std::vector<INTPOINT> newObstacles);
+
+  //! update distance map and Voronoi diagram to reflect the changes
+  void update(bool updateRealDist=true);
+  //! prune the Voronoi diagram
+  void prune();
+
+  //! returns the obstacle distance at the specified location
+  float getDistance( int x, int y );
+  //! returns whether the specified cell is part of the (pruned) Voronoi graph
+  bool isVoronoi( int x, int y );
+  //! checks whether the specficied location is occupied
+  bool isOccupied(int x, int y);
+  //! write the current distance map and voronoi diagram as ppm file
+  void visualize(const char* filename="result.ppm");
+
+  //! returns the horizontal size of the workspace/map
+  unsigned int getSizeX() {return sizeX;}
+  //! returns the vertical size of the workspace/map
+  unsigned int getSizeY() {return sizeY;}
+
+private:  
+  struct dataCell {
+    float dist;
+    char voronoi;
+    char queueing;
+    int obstX;
+    int obstY;
+    bool needsRaise;
+    int sqdist;
+  };
+
+  typedef enum {voronoiKeep=-4, freeQueued = -3, voronoiRetry=-2, voronoiPrune=-1, free=0, occupied=1} State;
+  typedef enum {fwNotQueued=1, fwQueued=2, fwProcessed=3, bwQueued=4, bwProcessed=1} QueueingState;
+  typedef enum {invalidObstData = SHRT_MAX/2} ObstDataState;
+  typedef enum {pruned, keep, retry} markerMatchResult;
+
+
+
+  // methods
+  void setObstacle(int x, int y);
+  void removeObstacle(int x, int y);
+  inline void checkVoro(int x, int y, int nx, int ny, dataCell& c, dataCell& nc);
+  void recheckVoro();
+  void commitAndColorize(bool updateRealDist=true);
+  inline void reviveVoroNeighbors(int &x, int &y);
+
+  inline bool isOccupied(int &x, int &y, dataCell &c);
+  inline markerMatchResult markerMatch(int x, int y);
+
+  // queues
+
+  BucketPrioQueue open;
+  std::queue<INTPOINT> pruneQueue;
+
+  std::vector<INTPOINT> removeList;
+  std::vector<INTPOINT> addList;
+  std::vector<INTPOINT> lastObstacles;
+
+  // maps
+  int sizeY;
+  int sizeX;
+  dataCell** data;
+  bool** gridMap;
+
+  // parameters
+  int padding;
+  double doubleThreshold;
+
+  double sqrt2;
+
+  //  dataCell** getData(){ return data; }
+};
+
+
+#endif
+

dynamicvoronoi/include/point.h

@@ -0,0 +1,14 @@
+#ifndef _VOROPOINT_H_
+#define _VOROPOINT_H_
+
+#define INTPOINT IntPoint
+
+/*! A light-weight integer point with fields x,y */
+class IntPoint {
+public:
+  IntPoint() : x(0), y(0) {}
+  IntPoint(int _x, int _y) : x(_x), y(_y) {}
+  int x,y;
+};
+
+#endif