Bezier splines for non-holonomic mobile robot formations

README.md

@@ -61,4 +61,4 @@ To merge your changes/added files into the official match-ROS repository, we nee
 | [004](student_code/004_ips_sensor_fusion/README.md) | Entwicklung und Evaluierung von Konzepten zur Nutzung von Ultraschall Indoor Positioning Systemen in der mobilen Robotik | concept for fusing ultrasonic ips data with amcl and imu using kalman filters |
 | [005](student_code/005_Recursive-Least-Squares-Algorithm/README.md) | Entwicklung eines Algorithmus zur Schätzung der dynamischen Parameter unbekannter Objekte in einem Multirobotersystem | In this work we developed an Recursive-Least-Squares Algorithm to estimate the dynamic parameters of an unknown object |
 | [006](student_code/006_FormationLayer/README.md) | Conception and implementation of an algorithm for the dynamic generation of the costmap of a multi-robot-system | A plugin costmap layer for a multi-robot-system |
-
+| [007](student_code/007_bezierSplines/README.md) | Evaluierung von Bahnglättungsalgorithmen für Formationen bestehend aus nicht-holonomen mobilen Robotern | An algorithm that generates bezier Splines on the basis of any path planner for the use of formations of non-holonomic mobile robots |

student_code/007_FromPathplanerToBezierSpline/README.md

@@ -0,0 +1,69 @@
+# Creating Bézier-Splines on the basis of path planners
+## Overview
+This folder contains all the code used in this work. The ROS Package is implemented in ROS Noetic on Ubuntu 20.04. Python3 and C++ is used. 
+
+**Author:** Henrik Wonnemann
+
+**E-Mail:** [email protected]
+
+## Packages
+### bezier_path 
+Contains the whole project
+
+## Launch files
+### test.launch
+Starts everything including the algorithm. Launching this file will generate a spline.
+
+### other launch files in launch directory of bezier_path
+slightly modified launch files from match_mobile_robotics according to currently used path planner and start/end pose
+
+## class
+### bezier_splines
+base-, cubic- and quintic-bezier-spline provide functions to calculate bezier-splines. 
+Code was copied from "formation-path-planning/fp_utils/src" and modified according to the algorithm. Folders geometry_info and visualization_helper need to be found but are not used.
+
+## Source and header files
+### main
+Started by test.launch. Callback methods and calls functions from other source files to calculate waypoints and Bézier-splines.
+To change the operating mode of the algorithm, change the variable "size_t detail" in the function findBestPath(). Explanation of the modes is commented. Other parameters that influence the spline can also be changed in this function.
+
+### waypoints
+Functions involved in calculation of waypoints. Functions called by main.cpp and itself.
+
+### bezierpath
+Functions involved in calculation of the parameters of the spline segments. Functions called by main.cpp and itself.
+
+### rviz
+Visualization functions for RVIZ
+
+### utils
+'other' functions
+There has been the problem that all used path planners planned part of the paths through the very outer area of the costmap.
+A function has been implemented to reduce the costmap after the path has already been planned as far as necessary for the path to be free of the costmap. That function is called in main.cpp in the mapCallback.
+
+### matlab
+Functions that were used to collect and export data to .csv
+
+## Scripts
+### activate_vscode.py 
+VSCode always minimized when the program starts so this maximizes it again. Obviously not essential. Called by test.launch
+
+### set_pose.py
+Sets start and end pose, can be changed here, but also needs to be changed in mir_100_changed and general_mir_changed launch. Called by test.launch.
+
+## topdown.rviz
+Saved rviz configuration.
+
+## RaStar
+Free to use Relaxed A-Star path planner downloaded from github. Used to test another planner besides navfnRos.
+
+## Dependencies
+### match_gazebo
+From match_mobile_robotics
+Due to the issue described in 'utils' the costmap has to be received multiple times. In order for the callback method to work, 'always_use_full_costmap = true' has to be added in mir/mir_navigation/config/Costmap/costmap_common_params.yaml
+
+### mir_launch_sim
+From match_mobile_robotics/mir
+
+### mir_navigation
+From match_mobile_robotics/mir

student_code/007_FromPathplanerToBezierSpline/src/RaStar/relaxed_astar/CMakeLists.txt

@@ -0,0 +1,216 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(relaxed_astar VERSION 0.0.0)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## 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
+  nav_core
+  roscpp
+  rospy
+  std_msgs
+  pluginlib
+)
+
+## 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 tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_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
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## 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 your 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 relaxed_astar
+#  CATKIN_DEPENDS nav_core roscpp rospy std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/relaxed_astar.cpp
+# )
+
+add_library(relaxed_astar_lib src/RAstar_ros.cpp)
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/relaxed_astar_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_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 ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+
+install(TARGETS relaxed_astar_lib
+  # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+
+
+install(DIRECTORY config/
+   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config
+)
+
+## 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
+  relaxed_astar_planner_plugin.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_relaxed_astar.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)

student_code/007_FromPathplanerToBezierSpline/src/RaStar/relaxed_astar/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2019 Unmanned-Surface-Vehicle
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

student_code/007_FromPathplanerToBezierSpline/src/RaStar/relaxed_astar/README.md

@@ -0,0 +1,4 @@
+# relaxed_astar
+ROS plugin to replace the move_base global planner. Based on http://wiki.ros.org/navigation/Tutorials/Writing%20A%20Global%20Path%20Planner%20As%20Plugin%20in%20ROS .  
+
+Adapted to USV_SIM_LSA.

student_code/007_FromPathplanerToBezierSpline/src/RaStar/relaxed_astar/config/relaxed_astar_planner_plugin.xml

@@ -0,0 +1,5 @@
+<library path="lib/librelaxed_astar_lib">
+<class name="RAstar_planner/RAstarPlannerROS" type="RAstar_planner::RAstarPlannerROS" base_class_type="nav_core::BaseGlobalPlanner">
+<description>This is RA∗ global planner plugin by iroboapp project.</description>
+</class>
+</library>