complete code

README.md

@@ -1,3 +1,79 @@
-# Car-like-Robotic-swarm
-Coming soon.
-Preprint paper link: https://arxiv.org/abs/2210.05863
+# Decentralized Car-Like Robotic Swarm
+
+## 0. Overview
+Code in this repo is about the decentralized car-like robotic swarm in a simulation environment. Due to the authority, we replace the Carla map with a simple environment, where the MPC controller is also removed. 
+
+**Related Paper**: 
+
+[Decentralized Planning for Car-Like Robotic Swarm in Cluttered Environments](https://arxiv.org/abs/2210.05863), Changjia Ma, Zhichao Han, Tingrui Zhang, Jingping Wang, Long Xu, Chengyang Li, Chao Xu and Fei Gao, Accepted by **IROS 2023**.
+
+The video link: [youtube](https://www.youtube.com/watch?v=qdYr3BKHsdM) and [bilibili](https://www.bilibili.com/video/BV11e4y1n7JL/?spm_id_from=333.999.0.0&vd_source=52c6d27efb21131ce8de5028bf3873c7).
+
+<p align="center">
+  <img src="figs/header.gif" width = "1200"/>
+</p>
+
+
+## 1. Setup
+All the Simulations are conducted in the Linux environment on a PC equipped with an Intel Core i7-10700 CPU and a GeForce RTX 3070 GPU.
+
+Our software is developed and tested in Ubuntu 20.04 with ROS installed.
+
+**Attention!** Map generator in this source code requires a higher version of python3, hence we do not recommend using Ubuntu 18 since the default version of python is python2.
+
+## 2. Build on ROS
+
+1. Create an empty new workspace and clone this repository to your workspace: 
+
+```
+git clone https://github.com/ZJU-FAST-Lab/Car-like-Robotic-swarm.git
+cd car_swarm
+```
+
+2. Compile it.
+
+```
+catkin_make -DCMAKE_BUILD_TYPE=Release
+```
+
+**Attention!** When compiling, be sure to use **release** mode!(Just follow the above command)
+
+## 3. Run
+
+```
+source devel/setup.bash
+```
+
+Then, run the command:
+
+```
+roslaunch traj_planner swarm.launch
+```
+
+Wait for about 2 seconds until the global map comes out.
+
+Then, you can click the **2D Nav Goal** at **anywhere** in **RVIZ** to trigger the planning.
+
+Here is an example:
+
+<p align="center">
+  <img src="figs/demo.gif" width = "1200"/>
+</p>
+**Tip:** We recommend developers to use **rosmon** to replace the **roslaunch** since rosmon is more friendly to mulit-robot systems.
+
+Please refer to [swarm formation](https://github.com/ZJU-FAST-Lab/Swarm-Formation) for more details of rosmon.
+
+## 4. Licence
+The source code is released under [GPLv3](http://www.gnu.org/licenses/) license.
+
+## 5. Acknowledgement
+
+The trajectory representation is modified from [MINCO](https://github.com/ZJU-FAST-Lab/GCOPTER).
+
+For a more detailed version of car-like robot planning, please see [Dftpav](https://github.com/ZJU-FAST-Lab/Dftpav).
+
+## 6. Maintaince
+
+For any technical issue, please contact Changjia Ma([email protected]).
+
+For commercial inquiries, please contact [Fei GAO](http://zju-fast.com/fei-gao/) ([email protected]).

src/.vscode/c_cpp_properties.json

@@ -0,0 +1,17 @@
+{
+    "configurations": [
+        {
+            "browse": {
+                "databaseFilename": "",
+                "limitSymbolsToIncludedHeaders": true
+            },
+            "includePath": [
+                "/opt/ros/noetic/include/**",
+                "/usr/include/**",
+                "${workspaceFolder}/simulator/kinematics_simulator/include"
+            ],
+            "name": "ROS"
+        }
+    ],
+    "version": 4
+}

src/.vscode/settings.json

@@ -0,0 +1,11 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ],
+    "files.associations": {
+        "core": "cpp"
+    }
+}

src/CMakeLists.txt

@@ -0,0 +1 @@
+/opt/ros/noetic/share/catkin/cmake/toplevel.cmake

src/Minco/.vscode/c_cpp_properties.json

@@ -0,0 +1,21 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "",
+        "limitSymbolsToIncludedHeaders": true
+      },
+      "includePath": [
+        "/home/aolai/car_swarm_carla/devel/include/**",
+        "/opt/ros/noetic/include/**",
+        "/home/aolai/car_swarm_carla/src/Minco/utils/DecompROS/decomp_ros_utils/include/**",
+        "/home/aolai/car_swarm_carla/src/mapping/include/**",
+        "/home/aolai/car_swarm_carla/src/mpc_control/mpc/include/**",
+        "/home/aolai/car_swarm_carla/src/Minco/traj_planner/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS"
+    }
+  ],
+  "version": 4
+}

src/Minco/.vscode/settings.json

@@ -0,0 +1,6 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/home/aolai/car_swarm_carla/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ]
+}

src/Minco/traj_planner/CMakeLists.txt

@@ -0,0 +1,118 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(traj_planner)
+
+set(CMAKE_VERBOSE_MAKEFILE "true")
+set(CMAKE_BUILD_TYPE "Release")
+set(CMAKE_CXX_FLAGS "-std=c++14 -g")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -fPIC")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -Wall")
+
+find_package(OpenMP REQUIRED)
+find_package(Protobuf REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(catkin_simple REQUIRED)
+find_package(ompl REQUIRED)
+# find_package(SUITESPARSE REQUIRED)
+# find_package(G2O REQUIRED)
+
+
+set(THIRD_PARTY_INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/thirdparty)
+set(PLAN_UTILS_INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/include/plan_utils)
+
+find_package(PCL REQUIRED)
+find_package(catkin REQUIRED COMPONENTS
+    roscpp
+    visualization_msgs
+    std_msgs
+    sensor_msgs
+    geometry_msgs
+    message_generation
+    cv_bridge
+    mapping
+    decomp_ros_utils
+    # mpc
+    kinematics_simulator
+    swarm_bridge
+    # costmap_2d
+    # base_local_planner
+    # move_base_msgs
+    # nav_core
+    # pluginlib
+)
+
+include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
+    ${CMAKE_CURRENT_BINARY_DIR}
+    ${EIGEN3_INCLUDE_DIRS}
+    ${DECOMP_UTIL_INCLUDE_DIRS}
+    ${THIRD_PARTY_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+
+
+)
+
+PROTOBUF_GENERATE_CPP(PROTO_SRCS PROTO_HDRS
+    proto/minco_config.proto
+)
+
+
+# add_message_files(
+#   FILES
+#   DataDisp.msg
+#   PolyTraj.msg
+#   )
+
+
+# Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+  geometry_msgs
+)
+
+#if this catkin packge's header is used by other packages, use catkin_package to
+#declare the include directories of this package.
+catkin_package(
+    INCLUDE_DIRS include ${THIRD_PARTY_INCLUDE_DIRS}
+    ${CMAKE_CURRENT_BINARY_DIR}
+    ${PLAN_UTILS_INCLUDE_DIRS}
+    CATKIN_DEPENDS decomp_ros_utils #mpc 
+    kinematics_simulator mapping
+    # traj_server_ros
+)
+
+add_executable(planner_node
+    src/planner_node.cpp
+    src/replan_fsm.cpp
+    src/traj_manager.cpp
+    src/kino_astar.cpp
+    src/raycast.cpp
+    src/traj_optimizer.cpp
+    ${PROTO_SRCS}
+)
+
+# add_library(planner_utils
+#     plan_utils/poly_traj_utils.hpp
+#     plan_utils/traj_container.hpp
+# )
+
+target_link_libraries(planner_node PRIVATE
+    # hkust_pl_common
+    # hkust_pl_smm
+    ompl
+    "${OpenMP_CXX_FLAGS}"
+    ${catkin_LIBRARIES}
+    ${Protobuf_LIBRARIES}
+)
+
+# target_compile_options(traj_planner PRIVATE "${OpenMP_CXX_FLAGS}")
+
+# add_library(traj_server_ros
+#     src/traj_server_ros.cpp
+#     src/traj_visualizer.cpp
+# )
+# target_link_libraries(traj_server_ros PRIVATE
+#     traj_planner
+#     ${OpenMP_CXX_FLAGS}
+# )

src/Minco/traj_planner/config/minco_config.pb.txt

@@ -0,0 +1,95 @@
+name: "planning.minco.Config"
+version: ""
+status: "SET"
+
+planner_cfg {
+  velocity_singularity_eps: 0.01
+  low_speed_threshold: 2.0
+  weight_proximity: 1.0
+  is_fitting_only: false
+}
+
+
+map_cfg {
+  map_size_x: 2000
+  map_size_y: 2000
+  map_size_z: 3
+
+
+  s_back_len: 20.0
+
+  dyn_bounds {
+    max_lon_vel: 50.0
+    min_lon_vel: 0.0
+    max_lon_acc: 3.0
+    max_lon_dec: -6.0
+    max_lat_vel: 2.5
+    max_lat_acc: 1.25
+  }
+
+  max_grids_along_time: 2
+
+  infl_steps {
+    x_p: 5
+    x_n: 5
+    y_p: 5
+    y_n: 5
+    z_p: 1
+    z_n: 1
+  }
+
+  max_tau: 0.8
+  init_max_tau: 0.8
+  w_time: 5
+  horizon: 50
+  resolution_dp: 0.5
+  time_resolution: 0.2
+  lambda_heu: 5.0
+  allocate_num: 100000
+  check_num: 5
+  max_search_time: 1000.1
+  phi_grid_resolution: 0.3
+  traj_forward_penalty: 1.0
+  traj_back_penalty: 2.5
+  traj_gear_switch_penalty: 15.0
+  traj_steer_penalty: 0.5
+  traj_steer_change_penalty: 0.0
+  map_resl: 0.3
+  step_arc: 0.9
+  checkl: 0.2
+  onedv_res: 0.2
+  onedl_res: 0.1
+  onedt_res: 0.1
+}
+
+opt_cfg {
+ traj_resolution: 8
+  des_traj_resolution: 20
+  wei_sta_obs: 4000.0
+  wei_dyn_obs: 7000.0
+  wei_feas: 1000.0
+  wei_sqrvar: 1000.0
+  wei_time: 1000.0
+  dyn_obs_clearance: 1.0
+  max_vel: 15.0
+  min_vel: -15.0
+  max_acc: 2.0
+  min_acc: -2.0
+  max_cur: 0.3
+  traj_piece_duration: 1.0
+  max_low_vel: 16.0
+  max_low_acc: 2.5
+  half_margin: 0.25
+}
+
+
+
+
+
+
+
+
+
+
+
+