update: Voronoi-based planner

README.md

@@ -123,6 +123,8 @@ Below is an example of `user_config.yaml`
 ```yaml
 map: "warehouse"
 world: "warehouse"
+rviz_file: "sim_env.rviz"
+
 robots_config:
   - robot1_type: "turtlebot3_burger"
     robot1_global_planner: "astar"
@@ -138,32 +140,122 @@ robots_config:
     robot2_y_pos: "-7.5"
     robot2_z_pos: "0.0"
     robot2_yaw: "0.0"
-robots_init: "robots_rviz_init.yaml"
-rviz_file: "sim_env.rviz"
-pedestrians: "pedestrian_config.yaml"
+
+plugins:
+  pedestrians: "pedestrian_config.yaml"
+  obstacles: "obstacles_config.yaml"
 ```
 
 Explanation:
 
-- map: static map，located in `src/sim_env/map/`, if `map: ""`, map_server will not publish map message which often used in SLAM.
+- `map`: static map，located in `src/sim_env/map/`, if `map: ""`, map_server will not publish map message which often used in SLAM.
+
+- `world`: gazebo world，located in `src/sim_env/worlds/`, if `world: ""`, Gazebo will be disabled which often used in real world.
 
-- world: gazebo world，located in `src/sim_env/worlds/`, if `world: ""`, Gazebo will be disabled which often used in real world.
+- `rviz_file`: RVIZ configure, automatically generated if `rviz_file` is not set.
 
-- robots_config: robotic configuration.
+- `robots_config`: robotic configuration.
 
-  - type: robotic type，such as `turtlebot3_burger`, `turtlebot3_waffle` and `turtlebot3_waffle_pi`.
+  - `type`: robotic type，such as *`turtlebot3_burger`, `turtlebot3_waffle` and `turtlebot3_waffle_pi`*.
 
-  - global_planner: global algorithm, details in Section `Version`.
+  - `global_planner`: global algorithm, details in Section `Version`.
 
-  - local_planner: local algorithm, details in Section `Version`.
+  - `local_planner`: local algorithm, details in Section `Version`.
 
-  - xyz_pos and yaw: initial pose.
+  - `xyz_pos and yaw`: initial pose.
 
-- robots_init: initial pose in RVIZ.
+- `plugins`: other applications using in simulation
+  - `pedestrians`: configure file to add dynamic obstacles(e.g. pedestrians).
+  - `obstacles`: configure file to add static obstacles.
+
+For *pedestrians* and *obstacles* configuration files, the examples are shown below
+
+```yaml
+## pedestrians_config.yaml
+
+# sfm algorithm configure
+social_force:
+  animation_factor: 5.1
+  # only handle pedestrians within `people_distance`
+  people_distance: 6.0
+  # weights of social force model
+  goal_weight: 2.0
+  obstacle_weight: 80.0
+  social_weight: 15
+  group_gaze_weight: 3.0
+  group_coh_weight: 2.0
+  group_rep_weight: 1.0
+
+# pedestrians setting
+pedestrians:
+  update_rate: 5
+  ped_property:
+    - name: human_1
+      pose: 5 -2 1 0 0 1.57
+      velocity: 0.9
+      radius: 0.4
+      cycle: true
+      time_delay: 5
+      ignore:
+        model_1: ground_plane
+        model_2: turtlebot3_waffle
+      trajectory:
+        goal_point_1: 5 -2 1 0 0 0
+        goal_point_2: 5 2 1 0 0 0
+    - name: human_2
+      pose: 6 -3 1 0 0 0
+      velocity: 1.2
+      radius: 0.4
+      cycle: true
+      time_delay: 3
+      ignore:
+        model_1: ground_plane
+        model_2: turtlebot3_waffle
+      trajectory:
+        goal_point_1: 6 -3 1 0 0 0
+        goal_point_2: 6 4 1 0 0 0
+```
+Explanation:
+
+
+- `social_force`: The weight factors that modify the navigation behavior. See the [Social Force Model](https://github.com/robotics-upo/lightsfm) for further information.
+- `pedestrians/update_rate`: Update rate of pedestrains presentation. The higher `update_rate`, the more sluggish the environment becomes.
+- `pedestrians/ped_property`: Pedestrians property configuration.
+  - `name`: The id for each human.
+  - `pose`: The initial pose for each human.
+  - `velocity`: Maximum velocity (*m/s*) for each human.
+  - `radius`: Approximate radius of the human's body (m).
+  - `cycle`: If *true*, the actor will start the goal point sequence when the last goal point is reached.
+  - `time_delay`: This is time in seconds to wait before starting the human motion.
+  - `ignore_obstacles`: All the models that must be ignored as obstacles, must be indicated here. The other actors in the world are included automatically.
+  - `trajectory`. The list of goal points that the actor must reach must be indicated here. The goals will be post into social force model.
+
+```yaml
+## obstacles_config.yaml 
+
+# static obstacles
+obstacles:
+  - type: BOX
+    pose: 5 2 0 0 0 0
+    color: Grey
+    props:
+      m: 1.00
+      w: 0.25
+      d: 0.50
+      h: 0.80
+```
+Explanation:
+- `type`: model type of specific obstacle, *Optional: `BOX`, `CYLINDER` or `SPHERE`*
+- `pose`: fixed pose of the obstacle
+- `color`: color of the obstacle
+- `props`: property of the obstacle
+  - `m`: mass
+  - `w`: width
+  - `d`: depth
+  - `h`: height
+  - `r`: radius
 
-- rviz_file: RVIZ configure, automatically generated if `rviz_file` is not set.
 
-- pedestrians: configure file to add dynamic obstacles(e.g. pedestrians).
 
 ## <span id="3">03. Version
 
@@ -178,6 +270,7 @@ Explanation:
 |     **D\***      |     [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)]((https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/graph_planner/src/d_star.cpp))      |       ![d_star_ros.gif](assets/d_star_ros.gif)       |
 |    **LPA\***     |    [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)]((https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/graph_planner/src/lpa_star.cpp))     |     ![lpa_star_ros.gif](assets/lpa_star_ros.gif)     |
 |   **D\* Lite**   |   [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)]((https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/graph_planner/src/d_star_lite.cpp))   |  ![d_star_lite_ros.gif](assets/d_star_lite_ros.gif)  |
+|   **Voronoi**   |   [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)]((https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/graph_planner/src/voronoi.cpp))   |  ![voronoi_ros.gif](assets/voronoi_ros.gif)  |
 |     **RRT**      |       [![Status](https://img.shields.io/badge/done-v1.1-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/sample_planner/src/rrt.cpp)        |          ![rrt_ros.gif](assets/rrt_ros.gif)          |
 |    **RRT\***     |     [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/sample_planner/src/rrt_star.cpp)     |     ![rrt_star_ros.gif](assets/rrt_star_ros.gif)     |
 | **Informed RRT** |   [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/sample_planner/src/informed_rrt.cpp)   | ![informed_rrt_ros.gif](assets/informed_rrt_ros.gif) |
@@ -219,6 +312,9 @@ Explanation:
 * RRT*: [Sampling-based algorithms for optimal motion planning](https://journals.sagepub.com/doi/abs/10.1177/0278364911406761).
 * Informed RRT*: [Optimal Sampling-based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal heuristic](https://arxiv.org/abs/1404.2334).
 
+### Evolutionary-based Planning
+* [ACO: ](http://www.cs.yale.edu/homes/lans/readings/routing/dorigo-ants-1999.pdf) Ant Colony Optimization: A New Meta-Heuristic
+
 ### Local Planning
 
 * DWA: [The Dynamic Window Approach to Collision Avoidance](https://www.ri.cmu.edu/pub_files/pub1/fox_dieter_1997_1/fox_dieter_1997_1.pdf).
@@ -314,6 +410,7 @@ We use another [gazebo simulation](https://github.com/ZhanyuGuo/ackermann_ws) as
 Dataset-of-Gazebo-Worlds-Models-and-Maps](https://github.com/mlherd/Dataset-of-Gazebo-Worlds-Models-and-Maps) and [
 aws-robomaker-small-warehouse-world](https://github.com/aws-robotics/aws-robomaker-small-warehouse-world). Thanks for these open source models sincerely.
 * Pedestrians in this environment are using social force model(sfm), which comes from [https://github.com/robotics-upo/lightsfm](https://github.com/robotics-upo/lightsfm).
+* A ROS costmap plugin for [dynamicvoronoi](http://www2.informatik.uni-freiburg.de/~lau/dynamicvoronoi/) presented by Boris Lau.
 
 ## <span id="8">08. License
 

src/planner/global_planner/evolutionary_planner/CMakeLists.txt

@@ -12,7 +12,7 @@ find_package(catkin REQUIRED COMPONENTS
   pluginlib
   tf2_geometry_msgs
   tf2_ros
-  utils
+  global_planner
 )
 
 catkin_package(
@@ -32,5 +32,5 @@ add_library(${PROJECT_NAME}
 
 target_link_libraries(${PROJECT_NAME}
   ${catkin_LIBRARIES}
-  utils
+  global_planner
 )

src/planner/global_planner/evolutionary_planner/include/aco.h

@@ -19,7 +19,7 @@
 
 #include "global_planner.h"
 
-namespace aco_planner
+namespace global_planner
 {
 struct Ant
 {
@@ -41,7 +41,7 @@ struct Ant
 /**
  * @brief Class for objects that plan using the ACO algorithm
  */
-class ACO : public global_planner::GlobalPlanner
+class ACO : public GlobalPlanner
 {
 public:
   /**
@@ -61,17 +61,17 @@ class ACO : public global_planner::GlobalPlanner
 
   /**
    * @brief ACO implementation
-   * @param gloal_costmap global costmap
+   * @param global_costmap global costmap
    * @param start         start node
    * @param goal          goal node
    * @param path          optimal path consists of Node
    * @param expand        containing the node been search during the process(unused)
    * @return  true if path found, else false
    */
-  bool plan(const unsigned char* gloal_costmap, const Node& start, const Node& goal, std::vector<Node>& path,
+  bool plan(const unsigned char* global_costmap, const Node& start, const Node& goal, std::vector<Node>& path,
             std::vector<Node>& expand);
 
-  void antSearch(const unsigned char* gloal_costmap, const Node& start, const Node& goal);
+  void antSearch(const unsigned char* global_costmap, const Node& start, const Node& goal);
 
 protected:
   int n_ants_;           // number of ants
@@ -89,5 +89,5 @@ class ACO : public global_planner::GlobalPlanner
 private:
   std::mutex lock_;
 };
-}  // namespace aco_planner
+}  // namespace global_planner
 #endif

src/planner/global_planner/evolutionary_planner/include/evolutionary_planner.h

@@ -102,7 +102,7 @@ class EvolutionaryPlanner : public nav_core::BaseGlobalPlanner
    * @param plan  plan transfromed from path, i.e. [start, ..., goal]
    * @return  bool true if successful, else false
    */
-  bool _getPlanFromPath(std::vector<Node>& path, std::vector<geometry_msgs::PoseStamped>& plan);
+  bool _getPlanFromPath(std::vector<global_planner::Node>& path, std::vector<geometry_msgs::PoseStamped>& plan);
 
   /**
    * @brief Tranform from costmap(x, y) to world map(x, y)

src/planner/global_planner/evolutionary_planner/src/aco.cpp

@@ -15,9 +15,10 @@
 #include <unordered_map>
 #include <algorithm>
 #include <iostream>
+
 #include "aco.h"
 
-namespace aco_planner
+namespace global_planner
 {
 /**
  * @brief Construct a new ACO object
@@ -51,14 +52,14 @@ ACO::~ACO()
 
 /**
  * @brief ACO implementation
- * @param gloal_costmap global costmap
+ * @param global_costmap global costmap
  * @param start         start node
  * @param goal          goal node
  * @param path          optimal path consists of Node
  * @param expand        containing the node been search during the process(unused)
  * @return  true if path found, else false
  */
-bool ACO::plan(const unsigned char* gloal_costmap, const Node& start, const Node& goal, std::vector<Node>& path,
+bool ACO::plan(const unsigned char* global_costmap, const Node& start, const Node& goal, std::vector<Node>& path,
                std::vector<Node>& expand)
 {
   // pheromone initialization
@@ -74,7 +75,7 @@ bool ACO::plan(const unsigned char* gloal_costmap, const Node& start, const Node
   {
     std::vector<std::thread> ants_list = std::vector<std::thread>(n_ants_);
     for (size_t j = 0; j < n_ants_; j++)
-      ants_list[j] = std::thread(&ACO::antSearch, this, gloal_costmap, start, goal);
+      ants_list[j] = std::thread(&ACO::antSearch, this, global_costmap, start, goal);
     for (size_t j = 0; j < n_ants_; j++)
       ants_list[j].join();
 
@@ -91,7 +92,7 @@ bool ACO::plan(const unsigned char* gloal_costmap, const Node& start, const Node
   return false;
 }
 
-void ACO::antSearch(const unsigned char* gloal_costmap, const Node& start, const Node& goal)
+void ACO::antSearch(const unsigned char* global_costmap, const Node& start, const Node& goal)
 {
   int max_steps = nx_ * ny_ / 2;
   Ant ant(start);
@@ -111,7 +112,7 @@ void ACO::antSearch(const unsigned char* gloal_costmap, const Node& start, const
       node_n.id_ = grid2Index(node_n.x_, node_n.y_);
 
       // next node hit the boundary or obstacle
-      if ((node_n.id_ < 0) || (node_n.id_ >= ns_) || (gloal_costmap[node_n.id_] >= lethal_cost_ * factor_))
+      if ((node_n.id_ < 0) || (node_n.id_ >= ns_) || (global_costmap[node_n.id_] >= lethal_cost_ * factor_))
         continue;
 
       // current node exists in history path
@@ -146,7 +147,7 @@ void ACO::antSearch(const unsigned char* gloal_costmap, const Node& start, const
     ant.cur_node_ = next_positions[dist(engine)];
     ant.steps_ += 1;
   }
-  
+
   // pheromone update based on successful ants
   lock_.lock();
   if (ant.found_goal_)
@@ -187,4 +188,4 @@ void ACO::antSearch(const unsigned char* gloal_costmap, const Node& start, const
   lock_.unlock();
 }
 
-}  // namespace aco_planner
+}  // namespace global_planner

src/planner/global_planner/evolutionary_planner/src/evolutionary_planner.cpp

@@ -106,7 +106,7 @@ void EvolutionaryPlanner::initialize(std::string name, costmap_2d::Costmap2D* co
       private_nh.param("Q", Q, 1.0);                // pheromone gain
       private_nh.param("max_iter", max_iter, 100);  // maximum iterations
 
-      g_planner_ = new aco_planner::ACO(nx_, ny_, resolution_, n_ants, alpha, beta, rho, Q, max_iter);
+      g_planner_ = new global_planner::ACO(nx_, ny_, resolution_, n_ants, alpha, beta, rho, Q, max_iter);
     }
 
     ROS_INFO("Using global graph planner: %s", planner_name.c_str());
@@ -197,8 +197,8 @@ bool EvolutionaryPlanner::makePlan(const geometry_msgs::PoseStamped& start, cons
   g_planner_->map2Grid(m_goal_x, m_goal_y, g_goal_x, g_goal_y);
 
   // NOTE: how to init start and goal?
-  Node start_node(g_start_x, g_start_y, 0, 0, g_planner_->grid2Index(g_start_x, g_start_y), 0);
-  Node goal_node(g_goal_x, g_goal_y, 0, 0, g_planner_->grid2Index(g_goal_x, g_goal_y), 0);
+  global_planner::Node start_node(g_start_x, g_start_y, 0, 0, g_planner_->grid2Index(g_start_x, g_start_y), 0);
+  global_planner::Node goal_node(g_goal_x, g_goal_y, 0, 0, g_planner_->grid2Index(g_goal_x, g_goal_y), 0);
 
   // clear the cost of robot location
   costmap_->setCost(g_start_x, g_start_y, costmap_2d::FREE_SPACE);
@@ -208,8 +208,8 @@ bool EvolutionaryPlanner::makePlan(const geometry_msgs::PoseStamped& start, cons
     g_planner_->outlineMap(costmap_->getCharMap());
 
   // calculate path
-  std::vector<Node> path;
-  std::vector<Node> expand;
+  std::vector<global_planner::Node> path;
+  std::vector<global_planner::Node> expand;
   bool path_found = g_planner_->plan(costmap_->getCharMap(), start_node, goal_node, path, expand);
 
   if (path_found)
@@ -281,7 +281,7 @@ bool EvolutionaryPlanner::makePlanService(nav_msgs::GetPlan::Request& req, nav_m
  * @param plan  plan transfromed from path, i.e. [start, ..., goal]
  * @return  bool true if successful, else false
  */
-bool EvolutionaryPlanner::_getPlanFromPath(std::vector<Node>& path, std::vector<geometry_msgs::PoseStamped>& plan)
+bool EvolutionaryPlanner::_getPlanFromPath(std::vector<global_planner::Node>& path, std::vector<geometry_msgs::PoseStamped>& plan)
 {
   if (!initialized_)
   {

src/planner/global_planner/global_planner/CMakeLists.txt

@@ -0,0 +1,28 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(global_planner)
+
+find_package(catkin REQUIRED COMPONENTS
+  angles
+  roscpp
+  costmap_2d
+  geometry_msgs
+)
+
+catkin_package(
+ INCLUDE_DIRS include
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+add_library(${PROJECT_NAME}
+  src/global_planner.cpp
+  src/nodes.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)