[ { "path": ".gitignore", "content": "docker/ignition_models\ndocker/models\ncontroller/MAV/\n__pycache__/\n" }, { "path": "CMakeLists.txt", "content": "cmake_minimum_required(VERSION 3.0.2)\nproject(errt)\n\nset(CMAKE_CXX_STANDARD 14)\nset(CMAKE_CXX_STANDARD_REQUIRED ON)\nset(octomap_DIR \"/opt/ros/noetic/share/octomap\")\nset(CMAKE_MODULE_PATH \"${CMAKE_MODULE_PATH}\" \"${CMAKE_CURRENT_SOURCE_DIR}/cmake\")\n##set(CMAKE_CXX_FLAGS \"${CMAKE_CXX_FLAGS} -std=c++14 -Wall -Wextra\")\n\nfind_package(catkin_simple REQUIRED)\nfind_package(Eigen3 REQUIRED)\nfind_package (PCL 1.10 REQUIRED)\n\nset(PCL_INCLUDE_DIRS /usr/include/pcl-1.10 /usr/include/eigen3/)\n## Compile as C++11, supported in ROS Kinetic and newer\n# add_compile_options(-std=c++11)\n\n## Find catkin macros and libraries\n## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)\n## is used, also find other catkin packages\t\nfind_package(catkin REQUIRED COMPONENTS\n roscpp\n ufomap_msgs\n ufomap_ros\n tf2_ros\n roslib\n dynamic_reconfigure\n geometry_msgs\n octomap_msgs\n octomap_ros\n mav_msgs\n mav_planning_msgs\n std_srvs\n roscpp\n pcl_ros\n tf_conversions\n std_msgs\n nav_msgs\n move_base_msgs\n actionlib\n actionlib_msgs\n message_generation\n ufomap_mapping\n roscpp_serialization\n\n\n)\nfind_package(ufomap REQUIRED)\n\nfind_package(catkin_simple REQUIRED)\nfind_package(Eigen3 REQUIRED)\nfind_package(roscpp_serialization REQUIRED)\n\ninclude_directories(${catkin_INCLUDE_DIRS} MAV MAV/rrt/target/release) \ninclude_directories(${PCL_INCLUDE_DIRS})\ninclude_directories(${EIGEN3_INCLUDE_DIR} ${CHOLMOD_INCLUDE_DIR})\n## System dependencies are found with CMake's conventions\n# find_package(Boost REQUIRED COMPONENTS system)\n\ninclude_directories(\n ${catkin_INCLUDE_DIRS}\n $(OCTOMAP_INCLUDE_DIRS)\n include/\n ${Eigen_INCLUDE_DIRS}\n ${Boost_INCLUDE_DIRS}\n)\n\n## Uncomment this if the package has a setup.py. This macro ensures\n## modules and global scripts declared therein get installed\n## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html\n# catkin_python_setup()\n\n################################################\n## Declare ROS messages, services and actions ##\n################################################\n\n## To declare and build messages, services or actions from within this\n## package, follow these steps:\n## * Let MSG_DEP_SET be the set of packages whose message types you use in\n## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).\n## * In the file package.xml:\n## * add a build_depend tag for \"message_generation\"\n## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET\n## * If MSG_DEP_SET isn't empty the following dependency has been pulled in\n## but can be declared for certainty nonetheless:\n## * add a exec_depend tag for \"message_runtime\"\n## * In this file (CMakeLists.txt):\n## * add \"message_generation\" and every package in MSG_DEP_SET to\n## find_package(catkin REQUIRED COMPONENTS ...)\n## * add \"message_runtime\" and every package in MSG_DEP_SET to\n## catkin_package(CATKIN_DEPENDS ...)\n## * uncomment the add_*_files sections below as needed\n## and list every .msg/.srv/.action file to be processed\n## * uncomment the generate_messages entry below\n## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)\n\n## Generate messages in the 'msg' folder\n# add_message_files(\n# FILES\n# Message1.msg\n# Message2.msg\n# )\n\n## Generate services in the 'srv' folder\n# add_service_files(\n# FILES\n# Service1.srv\n# Service2.srv\n# )\n\n## Generate actions in the 'action' folder\n# add_action_files(\n# FILES\n# Action1.action\n# Action2.action\n# )\n\n## Generate added messages and services with any dependencies listed here\n# generate_messages(\n# DEPENDENCIES\n# std_msgs # Or other packages containing msgs\n# )\n\n################################################\n## Declare ROS dynamic reconfigure parameters ##\n################################################\n\n## To declare and build dynamic reconfigure parameters within this\n## package, follow these steps:\n## * In the file package.xml:\n## * add a build_depend and a exec_depend tag for \"dynamic_reconfigure\"\n## * In this file (CMakeLists.txt):\n## * add \"dynamic_reconfigure\" to\n## find_package(catkin REQUIRED COMPONENTS ...)\n## * uncomment the \"generate_dynamic_reconfigure_options\" section below\n## and list every .cfg file to be processed\n\n## Generate dynamic reconfigure parameters in the 'cfg' folder\n##generate_dynamic_reconfigure_options(\n## cfg/errt.cfg\n##)\n\n###################################\n## catkin specific configuration ##\n###################################\n## The catkin_package macro generates cmake config files for your package\n## Declare things to be passed to dependent projects\n## INCLUDE_DIRS: uncomment this if your package contains header files\n## LIBRARIES: libraries you create in this project that dependent projects also need\n## CATKIN_DEPENDS: catkin_packages dependent projects also need\n## DEPENDS: system dependencies of this project that dependent projects also need\ncatkin_package(\n# INCLUDE_DIRS include\n# LIBRARIES tutorials\n# CATKIN_DEPENDS other_catkin_pkg\n# DEPENDS system_lib\n)\n\n###########\n## Build ##\n###########\n\n## Specify additional locations of header files\n## Your package locations should be listed before other locations\ninclude_directories(\n# include\n# ${catkin_INCLUDE_DIRS}\n)\n\n## Declare a C++ library\n# add_library(${PROJECT_NAME}\n# src/${PROJECT_NAME}/tutorials.cpp\n# )\n\n## Add cmake target dependencies of the library\n## as an example, code may need to be generated before libraries\n## either from message generation or dynamic reconfigure\n# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})\n\n## Declare a C++ executable\n## With catkin_make all packages are built within a single CMake context\n## The recommended prefix ensures that target names across packages don't collide\n# add_executable(${PROJECT_NAME}_node src/tutorials_node.cpp)\nadd_executable(${PROJECT_NAME}_node src/errt.cpp)\n\n## Rename C++ executable without prefix\n## The above recommended prefix causes long target names, the following renames the\n## target back to the shorter version for ease of user use\n## e.g. \"rosrun someones_pkg node\" instead of \"rosrun someones_pkg someones_pkg_node\"\n# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX \"\")\n\n## Add cmake target dependencies of the executable\n## same as for the library above\n# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})\n\n## Specify libraries to link a library or executable target against\n# target_link_libraries(${PROJECT_NAME}_node\n# ${catkin_LIBRARIES}\n# )\ntarget_link_libraries(${PROJECT_NAME}_node\n ${catkin_LIBRARIES}\n ${CMAKE_DL_LIBS}\n)\ntarget_link_libraries(${PROJECT_NAME}_node UFO::Map)\n\n#############\n## Install ##\n#############\n\n# all install targets should use catkin DESTINATION variables\n# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html\n\n## Mark executable scripts (Python etc.) for installation\n## in contrast to setup.py, you can choose the destination\n# catkin_install_python(PROGRAMS\n# scripts/my_python_script\n# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n# )\n\n## Mark executables for installation\n## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html\n# install(TARGETS ${PROJECT_NAME}_node\n# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n# )\n\n## Mark libraries for installation\n## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html\n# install(TARGETS ${PROJECT_NAME}\n# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}\n# )\n\n## Mark cpp header files for installation\n# install(DIRECTORY include/${PROJECT_NAME}/\n# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}\n# FILES_MATCHING PATTERN \"*.h\"\n# PATTERN \".svn\" EXCLUDE\n# )\n\n## Mark other files for installation (e.g. launch and bag files, etc.)\n# install(FILES\n# # myfile1\n# # myfile2\n# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}\n# )\n\n#############\n## Testing ##\n#############\n\n## Add gtest based cpp test target and link libraries\n# catkin_add_gtest(${PROJECT_NAME}-test test/test_tutorials.cpp)\n# if(TARGET ${PROJECT_NAME}-test)\n# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})\n# endif()\n\n## Add folders to be run by python nosetests\n# catkin_add_nosetests(test)\n\nadd_executable(rrt ${CMAKE_SOURCE_DIR}/MAV/rrt/example_optimizer.c)\n\n# Add libraries to the executable\ntarget_link_libraries(rrt ${CMAKE_SOURCE_DIR}/MAV/rrt/target/release/librrt.a)\ntarget_link_libraries(rrt m)\ntarget_link_libraries(rrt dl)\ntarget_link_libraries(rrt pthread)\n\nadd_custom_target(run\n COMMAND rrt\n DEPENDS rrt\n WORKING_DIRECTORY ${CMAKE_PROJECT_DIR}\n)\n" }, { "path": "LICENSE", "content": "BSD 3-Clause License\n\nCopyright (c) 2024, LTU Robotics & AI \nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are met:\n\n1. Redistributions of source code must retain the above copyright notice, this\n list of conditions and the following disclaimer.\n\n2. Redistributions in binary form must reproduce the above copyright notice,\n this list of conditions and the following disclaimer in the documentation\n and/or other materials provided with the distribution.\n\n3. Neither the name of the copyright holder nor the names of its\n contributors may be used to endorse or promote products derived from\n this software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\nAND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\nDISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE\nFOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL\nDAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR\nSERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\nCAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,\nOR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE\nOF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n" }, { "path": "README.md", "content": "# ExplorationRRT\n\nA Tree-based Next-best-trajectory Method for 3D UAV Exploration\n\n![image(14)](https://github.com/LTU-RAI/ExplorationRRT/assets/49238097/98865471-765b-4a34-9b82-17dca53e53b4)\n\n##\n\nThe ERRT framework is a combined Exploration-Planning algorithm for the exploration of unknown and unstructured 3D environments - in this version set up for rotorcraft UAVs. In the framework we leverage RRT tree-expansion for rapid path generation to multiple sampled candidate goals, combined with iterative path improvement modules and a NMPC to generate dynamic paths. Candidate RRT branches are then evaluated for distance, model-based actuation, and 3D LiDAR sensor based information gain along the candidate branches - not just at the end-goals or frontiers. The fundamental goal of ERRT is to find the momentary local \"Next-best-trajectory\" for continued and efficient exploration of unknown environments.\n\n##\n\nThe framework heavily relies on the [UFOmap](https://github.com/UnknownFreeOccupied/ufomap) occupancy mapper library and requires an UFOmap topic as input, together with the robot localization/odometry. As output the user can select to use a continously updated pose reference or the full trajectory to track. \n\nThe public version of the framework provides a pre configured Dockerfile [here](https://github.com/LTU-RAI/ExplorationRRT/blob/master/docker/Dockerfile) with all dependencies already configured including but not limited to: the [UFOmap](https://github.com/UnknownFreeOccupied/ufomap), the [RotorS UAV simulator and trajectory tracker](https://github.com/ethz-asl/rotors_simulator), and the [NMPC optimizer](https://alphaville.github.io/optimization-engine/). Instructions for installing docker and building the Dockerfile are provided below, and further down are detailed instruction on running the framework inside the errt docker container. \n\nThe docker container based simulation is pre-configured to load the DARPA SubT Cave World consisting of wide interconnected tunnels and caves, and the ERRT and UFOmap tuning is specified for this environment - and as such optimal performance in other environments might require parameter tuning. The provided version relies also on a full-tracjectory tracking controller provided by RotorS, for the UAV to track the generated and selected path. \n\n![image(15)](https://github.com/LTU-RAI/ExplorationRRT/assets/49238097/ed52dec3-6133-4387-a20a-fe6a104cbb18)\n\n# Installation \n\n## Requirements\n\nThe ERRT repository only requires docker. If you do not have docker installed, please follow the docker installation instructions. If you already have docker installed, you can skip this step.\n\n```bash\n# Add Docker's official GPG key:\nsudo apt-get update\nsudo apt-get install ca-certificates curl gnupg\nsudo install -m 0755 -d /etc/apt/keyrings\ncurl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo gpg --dearmor -o /etc/apt/keyrings/docker.gpg\nsudo chmod a+r /etc/apt/keyrings/docker.gpg\n\n# Add the repository to Apt sources:\necho \\\n \"deb [arch=$(dpkg --print-architecture) signed-by=/etc/apt/keyrings/docker.gpg] https://download.docker.com/linux/ubuntu \\\n $(. /etc/os-release && echo \"$VERSION_CODENAME\") stable\" | \\\n sudo tee /etc/apt/sources.list.d/docker.list > /dev/null\n\nsudo apt-get update\n\nsudo apt-get install docker-ce docker-ce-cli containerd.io docker-buildx-plugin docker-compose-plugin python3-pip\npip install gdown\n\n```\nThe repository contains a Dockerfile that allows the user to build a docker image containing packages for exploration, planning, control and simulation environment. \n\n## Cloning the repository and building the docker\n\nClone the ERRT project\n\n```bash\n git clone https://github.com/LTU-RAI/ExplorationRRT.git\n\n```\nNavigate to the ERRT directory and download cave models for gazebo. (This speeds up the world loading process in Gazebo)\n\n```bash\n cd ExplorationRRT/docker\n gdown --id 1TDbXF9He_LXYY57Xo3tOxvEVYH3kPtS8\n gdown --id 1y7BDt0tjK9Ml7MlTxUwOTG8ZygO_dpI0\n unzip models.zip \n unzip ignition_models.zip \n```\n\nBuild the docker image with following command. The build process might take some time when building for first time. \n\n```bash\n sudo docker build --build-arg USERNAME=$(whoami) -t errt_test . \n\n```\n\n# Fundamentals & Critical launch parameters\nThis section will detail some of the critical launch parameters of interest for the user - focusing on baseline configuration params, and those that can have a large impact on using ERRT different environments. The relevant launch files launch/errt.launch, config/errt.yaml, and launch/server.launch (for UFOmap) has more details for every configuration parameter. \n\n## ROS Topics\n\nThe Following ROS topic configurations can be found in the launch/errt.launch file. \n\n**remap from=\"odometry_in_\" to=\"/hummingbird/ground_truth/odometry\"** - The robot odometry topic \n\n**remap from=\"ufomap_in_\" to=\"ufomap_mapping_server_node/map\"** - The UFOmap topic. Maps at different depths can also be used.\n\n**remap from=\"reference_out_\" to=\"/hummingbird/reference\"** - Momentary pose references along the trajectory. These are updated by the condition of the robot position being closer to the current reference than **path_update_dist**. \n\n**remap from=\"path_out_\" to=\"/hummingbird/command/trajectory\"** - Path topic as a MultiDOFJointTrajectory message - Set up with the message type to be synced with the RotorS trajectory tracking controller.\n\nThere are also many visualization topics included in the ERRT program such as the tree_expansion (whole tree), candidate_goals, predicted_info_gain (predicted exploration), candidate_branches (all paths), and selected_trajectory. These are set up in the rviz/errt.rviz.\n\n## Tuning Parameters\n\nThis section will detail a number of tuning parameters in config/errt.yaml and launch/server.launch (UFOmap). The related launch files detail all possible tuning and configuration parameters. \n\n| Parameter | Description |\n|-------------------------|-----------------------------------------------------------------------------------------------------|\n| **resolution** | Found in server.launch. The resolution of the UFOmap. |\n| **planning_depth** | The depth of the Octree in UFOmap practically means merging voxels into larger ones.
This significantly speeds up various volumetric occupancy checks.
ERRT is configured to use a small **resolution** of ~0.05-0.15m but performing computationally demanding actions such as information gain calculations at a set depth in the Octree. |\n| **info_gain_depth** | Similar to planning_depth, used for information gain calculations. |\n| **robot_size** | Approximate size-radius of the robot, used in collision checks. |\n| **v_local** | Side length of the bounding box for local sampling space in RRT. |\n| **number_of_nodes** | Size of the RRT, determining when to stop tree expansion. |\n| **number_of_goals** | Number of candidate goals and trajectories to investigate. Affects computation time. |\n| **sensor_range** | Range of the LiDAR model for computing predicted information gain.
Should be set lower than max_range in server.launch. |\n| **sensor_vertical_fov** | Vertical cutoff angle for the LiDAR model, matching the onboard LiDAR on the robot.
Example: Ouster 32-beam 45° FoV -> **sensor_vertical_fov** = 0.393 |\n| **info_calc_dist** | Distance between nodes in candidate branches where information gain calculations are performed.
Smaller values increase computation time. |\n| **k_dist** | Gain related to the distance cost along candidate branches. |\n| **k_info** | Gain related to the information gain along candidate branches. |\n| **k_u** | Gain related to the actuation cost along candidate branches. |\n| **start_from_waypoint** | *true/false* indicates if the UAV should travel to a specified initial coordinate before ERRT takes over navigation,
set by subsequent *x,y,z*-coordinates. |\n\n## Test ERRT in a docker container\n![errt_gif-ezgif com-speed](https://github.com/LTU-RAI/ExplorationRRT/assets/49238097/957df250-dddc-4bd1-b7e9-841269cb16f2)\n\nRun the docker container with NVIDIA flags. In the ERRT directory:\n\n\n```bash\n ./start_errt_gpu.sh\n``` \n\nIf you do not have NVIDIA GPU :\n\n```bash\n ./start_errt_no_gpu.sh\n```\nOnce you are inside the docker container, please run the following command to start the ERRT tmux session.\nThis session will launch the ERRT sub-modules and a Rviz window to visualize the drone exploring the cave environment. \n\n```bash\ntmuxinator errt\n```\n\n## Acknowledgement\n\nIf you find the E-RRT framework useful in your research, please consider citing the E-RRT article.\n\n```bibtex\n@ARTICLE{10582913,\n author={Lindqvist, Björn and Patel, Akash and Löfgren, Kalle and Nikolakopoulos, George},\n journal={IEEE Transactions on Robotics}, \n title={A Tree-based Next-best-trajectory Method for 3D UAV Exploration}, \n year={2024},\n volume={},\n number={},\n pages={1-18},\n keywords={Robots;Robot sensing systems;Costs;Collision avoidance;Autonomous aerial vehicles;Trajectory;Three-dimensional displays;Tree-based Exploration;RRT;Subterranean Exploration;Field Robotics;Unmanned Aerial Vehicles},\n doi={10.1109/TRO.2024.3422052}}\n}\n\n```\n\n" }, { "path": "config/errt.yaml", "content": "######## ERRT Tuning Parameters #########\n#########################################\n\nmap_frame_id: \"world\" #UFOMAP and ERRT global frame id \n\n# Candidate goals parameters\n\nmin_info_goal: 1.0 #The minimum information gain for a candidate goal. Any value but 1 increases the information gain calculations for each sampled candidate goal and can lead to higher computation time\ngoal_sensor_range: 3.0 #Separate sensor range for \"MIN_INFO_GOAL_\" calculation. Preferably set lower than SENSOR_RANGE to promote candidate goals to be generated closer to unknown space \nnumber_of_goals: 80 #Number of candidate goals to be generated\nmin_dist_to_goal: 1.0 #The minimum distance between candidate goals and the robot position\ndist_goals: 0.2 #The minimum distance between different generated candidate goals\n\n\n# RRT Parameters\nv_local: 30 #The Bounding box side length denoting the local sampling space. Check Paper for detailed explaination. \nrun_by_nodes: true #Run by number_of_nodes if true and by number_of_itterations if false. Recommended as True\nnumber_of_nodes: 2000 #Size of the tree as the exit condition for tree expansion\nnumber_of_iterations: 10000 #The number of iterations, only used if RUN_BY_NODES_ is false\nplanning_depth: 2 #The depth in the OcTree for performing volumetric collision checks in the UFOmap\ndist_nodes: 0.4 #The set distance between poses in a candidate branch - this must be synced with NMPC_DT_ for the speed profile as v_desired = DISTANCE_BETWEEN_NODES_ / NMPC_DT_\ninfo_gain_depth: 2 #The depth in the OcTree for information gain calculations\ninfo_calc_dist: 6.0 #The distance between nodes where information gain calculations are performed - small values can lead to high computations but significant overlap between checks\ngoal_connect_dist: 2 #Maximum distance for attempting to connect candidate goals to tree nodes\n\n\n\n# Robot Parameters \nrobot_size: 0.4 #The radius of volumetric collision checks for robot-safe tree generation\nsensor_range: 10 #The effective range of the lidar sensor, used for information gain calculations. Set lower than sensor range in UFOmap.\nmin_sensor_range: 0.4 #The distance at which the sensor will exclude hits, as to not include occupation hits from the drone itself\nsensor_vertical_fov: 0.393 #The vertical angle cut-off (half FoV) for the LiDAR, used for information gain calculation bounding boxes, given in radians. Ex. Ouster 45deg FoV -> 0.393rad (x2)\n\n\n# Planning tuning & path tracking parameters \n\nk_dist: 0.3 #Gain for the distance cost during candidate branch evaluation\nk_info: 0.4 #Gain for the information gain during candidate branch evaluation\nk_u: 0.1 #Gain for the actuation cost during candidate branch evaluation\nrecalc_dist: 2.0 #Distance from the end-segment of the current trajectory at which a new trajectory calculation is initiated, for smooth trajectory transistions\npath_update_dist: 0.3 #The distance from the current robot pose at which the trajectory segment updates and is sent to the pose ref topic\npath_improvement_max: 3000 #The maximum amount of micro seconds which can be spent improving a single path\n\n\n# NMPC Tuning Parameters \n\nnmpc_horizon: 50 #Horizon in the NMPC problem. If edited must then also be specified in the NMPC module builder\nnmpc_dt: 0.4 #Sampling time of the NMPC problem. This also specifies the desired dt to reach each path segment set by dist_nodes \nposition_tracking_weight_x: 0 #Qx\nposition_tracking_weight_y: 0 \nposition_tracking_weight_z: 0 \nangle_weight_roll: 0\nangle_weight_pitch: 0\ninput_weight_thrust: 5 #Qu\ninput_weight_roll: 10 \ninput_weight_pitch: 10 \ninput_rate_weight_thrust: 5 #Qdu\ninput_rate_weight_roll: 10 \ninput_rate_weight_pitch: 10 \n\n\n# Initialization parameters \n\nstart_from_waypoint: true #Denotes if there's a starting-point to travel to or not. Can be useful to start ERRT with an initial small map as opposed to from the ground for the first trajectory\ninitial_x: 24 #Initial point x \ninitial_y: 0 #Initial point y\ninitial_z: 2 #Initial point z\n\n\n\n\n\n \n \n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n" }, { "path": "controller/errt_OpEn.py", "content": "import opengen as og\nimport casadi.casadi as cs\nimport numpy as np\n\n## Problem size\n\nN = 15\ndt = 1.0/20\nnMAV = 1 #Number of MAVs to be included in the centralized scheme\n\n## Weight matrices\n# Qx = (8, 8, 40, 2, 2, 3, 8, 8) \nQx = (4,4, 40, 4, 4, 3, 5, 5) \nP = 2*Qx; #final state weight\nRu = (3, 7, 7) # input weights\nRd = (3, 7, 7) # input rate weights\n\n## Objective function generation\nnu = 3; #Number of control inputs per MAV\nns = 8; #Number of states per MAV\n\nnp = nMAV*ns + nMAV*ns + nu + nu*nMAV + 3 + 8\n#print(np)\nu = cs.SX.sym('u', nu*nMAV*N)\nz0 = cs.SX.sym('z0', np)\n#print(z0)\nx = z0[0:nMAV*ns]\n#print(x)\nx_ref = z0[nMAV*ns:nMAV*ns + nMAV*ns]\n#print(x_ref)\nu_ref = z0[nMAV*ns + nMAV*ns:nMAV*ns + nMAV*ns + nu]\n#print(u_ref)\nu_old = z0[nMAV*ns + nMAV*ns + nu:nMAV*ns + nMAV*ns + nu + nMAV*nu]\nf_nmhe = z0[nMAV*ns + nMAV*ns + nu + nu:nMAV*ns + nMAV*ns + nu + nu + 3]\nQx_adapt = z0[nMAV*ns + nMAV*ns + nu + nu + 3:nMAV*ns + nMAV*ns + nu + nu + 3 + 8]\n\n\ncost = 0\nc = 0\n\nfor i in range(0, N):\n###State Cost \n for j in range(0,nMAV):\n cost += Qx_adapt[0]*(x[ns*j]-x_ref[ns*j])**2 + Qx_adapt[1]*(x[ns*j+1]-x_ref[ns*j+1])**2 + Qx_adapt[2]*(x[ns*j+2]-x_ref[ns*j+2])**2 + Qx_adapt[3]*(x[ns*j+3]-x_ref[ns*j+3])**2 + Qx_adapt[4]*(x[ns*j+4]-x_ref[ns*j+4])**2 + Qx_adapt[5]*(x[ns*j+5]-x_ref[ns*j+5])**2 + Qx_adapt[6]*(x[ns*j+6]-x_ref[ns*j+6])**2 + Qx_adapt[7]*(x[ns*j+7]-x_ref[ns*j+7])**2\n\n####Input Cost \n u_n = u[(i*nMAV*nu):(i*nMAV*nu+nu*nMAV)]\n for j in range(0,nMAV):\n cost += Ru[0]*(u_n[nu*j] - u_ref[0])**2 + Ru[1]*(u_n[nu*j+1] - u_ref[1])**2 + Ru[2]*(u_n[nu*j+2] - u_ref[2])**2 #Input weights\n cost += Rd[0]*(u_n[nu*j] - u_old[nu*j])**2 + Rd[1]*(u_n[nu*j+1] - u_old[nu*j+1])**2 + Rd[2]*(u_n[nu*j+2] - u_old[nu*j+2])**2 #Input rate weights\n\n #Input rate constraints\n c = cs.vertcat(c, cs.fmax(0, u_n[nu*j+1] - u_old[nu*j+1] - 0.1))\n c = cs.vertcat(c, cs.fmax(0, u_old[nu*j+1] - u_n[nu*j+1] - 0.1))\n c = cs.vertcat(c, cs.fmax(0, u_n[nu*j+2] - u_old[nu*j+2] - 0.1))\n c = cs.vertcat(c, cs.fmax(0, u_old[nu*j+2] - u_n[nu*j+2] - 0.1))\n\n ####State update \n u_old = u_n\n for j in range(0,nMAV):\n x[ns*j] = x[ns*j] + dt * x[ns*j+3]\n x[ns*j+1] = x[ns*j+1] + dt * x[ns*j+4]\n x[ns*j+2] = x[ns*j+2] + dt * x[ns*j+5]\n x[ns*j+3] = x[ns*j+3] + dt * (cs.sin(x[ns*j+7]) * cs.cos(x[ns*j+6]) * u_n[nu*j] - 0.1 * x[ns*j+3] + f_nmhe[0])\n x[ns*j+4] = x[ns*j+4] + dt * (-cs.sin(x[ns*j+6]) * u_n[nu*j] - 0.1*x[ns*j+4] + f_nmhe[1])\n x[ns*j+5] = x[ns*j+5] + dt * (cs.cos(x[ns*j+7]) * cs.cos(x[ns*j+6]) * u_n[nu*j] - 0.2 * x[ns*j+5] - 9.81 + f_nmhe[2])\n x[ns*j+6] = x[ns*j+6] + dt * ((1 / 0.20) * (u_n[nu*j+1] - x[ns*j+6]))\n x[ns*j+7] = x[ns*j+7] + dt * ((1 / 0.20) * (u_n[nu*j+2] - x[ns*j+7]))\n #print(x[ns*j])\n \n\n\numin = [3, -0.4, -0.4] * (N*nMAV)\numax = [15.5, 0.4, 0.4] * (N*nMAV)\n\nbounds = og.constraints.Rectangle(umin, umax)\nproblem = og.builder.Problem(u, z0, cost).with_penalty_constraints(c) \\\n.with_constraints(bounds)\n\ntcp_config = og.config.TcpServerConfiguration(bind_port=2769) \n\nbuild_config = og.config.BuildConfiguration() \\\n.with_build_directory(\"MAV\") \\\n.with_build_mode(\"release\") \\\n.with_tcp_interface_config(tcp_config) \n#.with_build_c_bindings()\nmeta = og.config.OptimizerMeta() \\\n.with_optimizer_name(\"errt_nmpc\")\n#.with_rebuild(True) \nsolver_config = og.config.SolverConfiguration() \\\n.with_tolerance(1e-4) \\\n.with_initial_tolerance(1e-4) \\\n.with_max_duration_micros(40000) \\\n.with_max_outer_iterations(5) \\\n.with_penalty_weight_update_factor(2) \\\n.with_initial_penalty(1000.0) \nbuilder = og.builder.OpEnOptimizerBuilder(problem, meta,\n build_config, solver_config) \\\n.with_verbosity_level(1)\n\n\nbuilder.build()\n\n# Use TCP server\n# ------------------------------------\nmng = og.tcp.OptimizerTcpManager('MAV/errt_nmpc')\nmng.start()\nx0 = [2.0,2.0,1.0,0.0,0.0,0.0,0.0,0.0]\nxref= [2.0,2.0,1.0,0.0,0.0,0.0,0.0,0.0]\nuold =[9.81,0.0,0.0]\nuref =[9.81,0.0,0.0]\nqx_adapt = [5,5,20,4,4,4,5,5]\n\nz0 = x0 + xref + uref + uold + [0,0,0] + qx_adapt\nprint(z0)\nprint(len(z0)) ##Length is equal to np\n#obsdata = (0.0,0.0,1.0,1.0)\nmng.ping()\nsolution = mng.call(z0, initial_guess=[9.81,0,0.0]*(N),buffer_len = 8*4096)\nprint(solution['solution'])\nmng.kill()\n\n" }, { "path": "controller/errt_nmpc.py", "content": " #!/usr/bin/env python\n # license removed for brevity\nimport rospkg\nimport rospy\nimport opengen as og\nimport numpy \nfrom std_msgs.msg import String\nimport std_msgs.msg\nfrom nav_msgs.msg import Odometry\nfrom geometry_msgs.msg import TwistStamped\nfrom geometry_msgs.msg import PoseStamped\nfrom geometry_msgs.msg import Pose\nfrom geometry_msgs.msg import PointStamped\nfrom geometry_msgs.msg import Vector3\nfrom sensor_msgs.msg import Range\nfrom sensor_msgs.msg import Imu\n#from euler_to_quaternion import euler_to_quaternion\nimport statistics\nfrom mav_msgs.msg import RollPitchYawrateThrust\nfrom std_msgs.msg import Float64\n#from traj_msg.msg import OptimizationResult\nimport time\nimport sys\nimport math\nmng = og.tcp.OptimizerTcpManager('MAV/errt_nmpc')\nmng.start()\nxpos = 0\nypos = 0\nzpos = 0\nqx = 0\nqy = 0\nqz = 0\nqw = 0\nvx = 0\nvy = 0\nvz = 0\nroll = 0\npitch = 0\nyaw = 0\nroll_v = 0\npitch_v = 0\nyaw_v = 0\nyawrate = 0\nt0 = 8\nC = 9.81 / t0\n#obsdata = [0]*(3)\nN = 15\nustar = [9.81,0.0,0.0] * (N)\n\n\nnu = 3\ndt = 1.0/20\nx0 = [0,0,0.0,0.0,0.0,0.0,0.0,0.0]\nglobal uold\nuold = [9.81,0.0,0.0]\nuref = [9.81,0.0,0.0]\nxref = [0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0]\nv_z = [0.0, 0.0, 0.0]\np_f = [0,0,0]\nf_nmhe = [0,0,0]\nland_flag = 0\nstart_flag = 0\nsafety_counter = 0\n\n\n\n\ndef quaternion_to_euler(x, y, z, w):\n\n t0 = +2.0 * (w * x + y * z)\n t1 = +1.0 - 2.0 * (x * x + y * y)\n roll = math.atan2(t0, t1)\n t2 = +2.0 * (w * y - z * x)\n t2 = +1.0 if t2 > +1.0 else t2\n t2 = -1.0 if t2 < -1.0 else t2\n pitch = math.asin(t2)\n t3 = +2.0 * (w * z + x * y)\n t4 = +1.0 - 2.0 * (y * y + z * z)\n yaw = math.atan2(t3, t4)\n return [roll, pitch, yaw]\n\n\n\n\ndef callback_pot(data):\n global p_f\n p_f = [0,0,0]\n p_f[0] = data.point.x\n p_f[1] = data.point.y\n p_f[2] = data.point.z\n\n\n\n# def callback_vicon(data):\n# global xpos, ypos,zpos,vx, vy, vz,yaw_v\n# xpos = data.pose.pose.position.x\n# ypos = data.pose.pose.position.y\n# zpos = data.pose.pose.position.z\n# qx = data.pose.pose.orientation.x\n# qy = data.pose.pose.orientation.y\n# qz = data.pose.pose.orientation.z\n# qw = data.pose.pose.orientation.w\n# vx = data.twist.twist.linear.x\n# vy = data.twist.twist.linear.y\n# vz = data.twist.twist.linear.z\n# [roll_v, pitch_v, yaw_v] = quaternion_to_euler(qx,qy,qz,qw)\n #print([qx, qy, qz, qw])\n #print(yaw_v)\n\ndef callback_lio(data):\n global xpos, ypos,zpos,vx, vy, vz, yaw_v, start_flag\n xpos = data.pose.pose.position.x\n ypos = data.pose.pose.position.y\n zpos = data.pose.pose.position.z\n qx = data.pose.pose.orientation.x\n qy = data.pose.pose.orientation.y\n qz = data.pose.pose.orientation.z\n qw = data.pose.pose.orientation.w\n vx = data.twist.twist.linear.x\n vy = data.twist.twist.linear.y\n vz = data.twist.twist.linear.z\n [roll_v, pitch_v, yaw_v] = quaternion_to_euler(qx,qy,qz,qw)\n start_flag = 1\n #print([qx, qy, qz, qw])\n #print(yaw_v)\n\ndef callback_imu(imu_data):\n global roll,pitch,yaw, yawrate\n qx = imu_data.orientation.x\n qy = imu_data.orientation.y\n qz = imu_data.orientation.z\n qw = imu_data.orientation.w\n [roll, pitch, yaw] = quaternion_to_euler(qx,qy,qz,qw)\n pitch = pitch\n #print(yaw)\n yawrate = imu_data.angular_velocity.z\n\n\ndef callback_safety(data):\n global land_flag\n land_flag = 1\n\ndef callback_start(data):\n global xref, start_flag, yaw_ref, yaw_v\n if start_flag == 0:\n xref[0] = xpos\n xref[1] = ypos\n xref[2] = zpos + 1.3\n yaw_ref = yaw_v\n start_flag = 1\n\n\n\n #print(zpos)\ndef callback_ref(data):\n global xref, yaw_ref\n xref[0] = data.pose.pose.position.x\n xref[1] = data.pose.pose.position.y\n xref[2] = data.pose.pose.position.z\n #xref[3] = data.twist.twist.linear.x\n #xref[4] = data.twist.twist.linear.y\n #xref[5] = data.twist.twist.linear.z\n\n yaw_ref = data.pose.pose.orientation.z\n\n\n\n\n\ndef PANOC():\n rospy.init_node('PANOC', anonymous=True)\n pub = rospy.Publisher('/hummingbird/command/roll_pitch_yawrate_thrust', RollPitchYawrateThrust, queue_size=1)\n #pub_traj = rospy.Publisher('traj_1', OptimizationResult, queue_size=1)\n\n\n #sub_sonar = rospy.Subscriber('/mavros/distance_sensor/lidarlite_pub', Range, callback_sonar)\n #sub = rospy.Subscriber('/odometry/imu', Odometry, callback_lio)\n # pub_ref = rospy.Publisher('pixyy/reference', PoseStamped, queue_size=1)\n sub = rospy.Subscriber('/hummingbird/ground_truth/odometry', Odometry, callback_lio)\n sub_safety = rospy.Subscriber('hummingbird/safety_land', String, callback_safety)\n sub_start = rospy.Subscriber('hummingbird/set_start', String, callback_start)\n sub_imu = rospy.Subscriber('/hummingbird/ground_truth/imu', Imu, callback_imu)\n sub_pot = rospy.Subscriber('/potential_delta_p_hummingbird', PointStamped, callback_pot)\n sub_ref = rospy.Subscriber('/hummingbird/reference', Odometry, callback_ref)\n\n\n #sub_traj = rospy.Subscriber('traj_2', OptimizationResult, callback_traj)\n #sub_traj_2 = rospy.Subscriber('traj_3', OptimizationResult, callback_traj_2)\n \n rate = rospy.Rate(20) # 20hz\n uold = [9.81, 0.0, 0.0]\n\n ustar = [9.81,0.0,0.0] * (N)\n i = 0\n t = 0\n safety_counter = 0\n global integrator\n integrator = 0\n global xref, yaw_ref\n xref = [0.0,0.0,1.3,0.0,0.0,0.0,0.0,0.0]\n yaw_ref = 0\n\n\n ##ADAPT WEIGHT PARAMS####\n Qx = [10,10,20,1,1,1,5,5]\n\n\n\n\n\n xpos_ref = 0\n ypos_ref = 0\n zpos_ref = 1.0\n\n\n while not rospy.is_shutdown():\n global p_f, land_flag, start_flag\n\n start = time.time()\n\n\n\n ######BODY ROTATIONS####\n zpos_angle = zpos * (math.cos(roll) * math.cos(pitch))\n x0_body = [math.cos(yaw_v)*xpos + math.sin(yaw_v)*ypos, -math.sin(yaw_v)*xpos + math.cos(yaw_v)*ypos, zpos, vx, vy, vz, roll, pitch]\n if (t < 100) | (land_flag == 1):\n p_f = [0,0,0]\n\n f_nmhe = [0.0, 0, 0]\n\n\n xref_body = [(math.cos(yaw_v)*xref[0] + math.sin(yaw_v)*xref[1]) + p_f[0], (-math.sin(yaw_v)*xref[0] + math.cos(yaw_v)*xref[1]) + p_f[1], xref[2] + p_f[2], (math.cos(yaw_v)*xref[3] + math.sin(yaw_v)*xref[4]), (-math.sin(yaw_v)*xref[3] + math.cos(yaw_v)*xref[4]), xref[5], xref[6], xref[7]]\n\n p_ref_dist = math.sqrt((xref_body[0] - x0_body[0])**2 + (xref_body[1] - x0_body[1])**2 + (xref_body[2] - x0_body[2])**2)\n if p_ref_dist > 1:\n p_ref_norm = [(xref_body[0] - x0_body[0]) / p_ref_dist, (xref_body[1] - x0_body[1]) / p_ref_dist, (xref_body[2] - x0_body[2]) / p_ref_dist]\n xref_body[0:3] = [x0_body[0] + p_ref_norm[0], x0_body[1] + p_ref_norm[1], x0_body[2] + p_ref_norm[2]]\n\n \n\n\n z0 = x0_body + xref_body + uref + uold + f_nmhe + Qx\n solution = mng.call(z0, initial_guess=[9.81,0,0]*(N),buffer_len = 4*4096)\n ustar = solution['solution']\n uold = ustar[0:3]\n\n print(\"odom:\", x0_body);\n print(\"p_ref:\", xref_body[0:3]);\n\n u_r = ustar[1]\n u_p = ustar[2]\n\n rpyt = RollPitchYawrateThrust()\n\n if land_flag == 0:\n integrator = integrator + 0.005*(xref_body[2] - zpos)\n\n t0_ = t0 + integrator\n\n\n C = 9.81 / t0_\n u_t = ustar[0] / C\n\n\n if (t < 40) & (start_flag == 1):\n u_t = 0.2\n u_r = 0\n u_p = 0\n integrator = 0\n\n if start_flag == 0:\n u_t = 0\n t = 0\n u_r = 0\n u_p = 0\n integrator = 0\n\n if land_flag == 1:\n u_t = (t0_ - 0.5) - safety_counter*0.001\n safety_counter+=1\n if (u_t < 0) | (zpos < 0.2):\n u_t = 0\n mng.kill()\n\n rpyt.roll = u_r\n rpyt.pitch = u_p\n\n rpyt.thrust.x = 0\n rpyt.thrust.y = 0\n\n ang_diff = yaw_ref - yaw_v\n\n\n\n rpyt.thrust.z = u_t\n\n ang_diff = numpy.mod(ang_diff + math.pi, 2*math.pi) - math.pi\n\n\n \n u_y = 0.5 * (ang_diff) #+ yaw_integrator\n # u_y = 0.7*(ang_diff) - 0.1*yawrate\n if u_y > 1:\n u_y = 1\n\n if u_y < -1:\n u_y = -1\n \n rpyt.yaw_rate = u_y\n\n rpyt.header = std_msgs.msg.Header()\n rpyt.header.stamp = rospy.Time.now()\n rpyt.header.frame_id = 'world'\n \n pub.publish(rpyt)\n\n\n\n\n\n\n end = time.time()\n #print(end-start)\n rate.sleep()\n #end = time.time()\n\n t = t + 1\n\nif __name__ == '__main__':\n try:\n PANOC()\n except rospy.ROSInterruptException:\n pass\n" }, { "path": "controller/quaternion_to_euler.py", "content": "def quaternion_to_euler(x, y, z, w):\n\n import math\n t0 = +2.0 * (w * x + y * z)\n t1 = +1.0 - 2.0 * (x * x + y * y)\n roll = math.atan2(t0, t1)\n t2 = +2.0 * (w * y - z * x)\n t2 = +1.0 if t2 > +1.0 else t2\n t2 = -1.0 if t2 < -1.0 else t2\n pitch = math.asin(t2)\n t3 = +2.0 * (w * z + x * y)\n t4 = +1.0 - 2.0 * (y * y + z * z)\n yaw = math.atan2(t3, t4)\n return [roll, pitch, yaw]\n" }, { "path": "docker/.powerline.sh", "content": "function _update_ps1() {\n PS1=\" 🐳 $(powerline-shell $?)\"\n}\n\nif [[ $TERM != linux && ! $PROMPT_COMMAND =~ _update_ps1 ]]; then\n PROMPT_COMMAND=\"_update_ps1; $PROMPT_COMMAND\"\nfi\n" }, { "path": "docker/.tmux.conf", "content": "#set -g default-terminal \"tmux-256color\"\nset -g default-terminal \"xterm-256color\"\n#set -ga terminal-overrides \",*256col*:Tc\"\nset -ga terminal-overrides \",xterm-256color:Tc\"\n# action key\n\nset-option -g prefix C-b\nset-option -g repeat-time 0\nset-option -g focus-events on\nset -g mouse on\n#### Key bindings\n\nset-window-option -g mode-keys vi\n\n#bind t send-key C-t\n# Reload settings\nbind r source-file ~/.tmux.conf \\; display \"Reloaded!\"\n# Open current directory\nbind o run-shell \"open #{pane_current_path}\"\n# bind -r e kill-pane -a\n\n# vim-like pane switching\n# bind -r k select-pane -U \n# bind -r j select-pane -D \n# bind -r h select-pane -L \n# bind -r l select-pane -R \n\n# Moving window\nbind-key -n C-S-Left swap-window -t -1 \\; previous-window\nbind-key -n C-S-Right swap-window -t +1 \\; next-window\n\n# Resizing pane\nbind -r C-k resize-pane -U 5\nbind -r C-j resize-pane -D 5\nbind -r C-h resize-pane -L 5\nbind -r C-l resize-pane -R 5\n\n#### basic settings\n\nset-option -g status-justify \"left\"\n#set-option utf8-default on\n#set-option -g mouse-select-pane\nset-window-option -g mode-keys vi\n#set-window-option -g utf8 on\n# look'n feel\nset-option -g status-fg cyan\nset-option -g status-bg black\nset -g pane-active-border-style fg=colour166,bg=default\nset -g window-style fg=colour10,bg=default\nset -g window-active-style fg=colour12,bg=default\nset-option -g history-limit 64096\n\nset -sg escape-time 10\n\n#### COLOUR\n\n# default statusbar colors\nset-option -g status-style bg=colour235,fg=colour136,default\n\n# default window title colors\nset-window-option -g window-status-style fg=colour244,bg=colour234,dim\n\n# active window title colors\nset-window-option -g window-status-current-style fg=colour166,bg=default,bright\n\n# pane border\nset-option -g pane-border-style fg=colour235 #base02\nset-option -g pane-active-border-style fg=colour136,bg=colour235\n\n# message text\nset-option -g message-style bg=colour235,fg=colour166\n\n# pane number display\nset-option -g display-panes-active-colour colour33 #blue\nset-option -g display-panes-colour colour166 #orange\n\n# clock\nset-window-option -g clock-mode-colour colour64 #green\n\n# allow the title bar to adapt to whatever host you connect to\nset -g set-titles on\nset -g set-titles-string \"#T\"\n\n# import\n\nsource ~/statusline.conf\n# source ~/utility.conf\n" }, { "path": "docker/.vimrc", "content": "\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\n\" General\n\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\n\n\" :set spell\n\" :set spelllang=nl\n\nset t_8f=\u001b[38;2;%lu;%lu;%lum\nset t_8b=\u001b[48;2;%lu;%lu;%lum\n\n\"Get out of VI's compatible mode..\n\"gets rid of all the crap that Vim does to be vi compatible. \nset nocompatible\n\n\"Sets how many lines of history VIM has to remember\nset history=9999\n\n\" -----------------------------------------------------------------------------\n\" Each time a new or existing file is edited, Vim will try to recognize the type\n\" of the file and set the 'filetype' option. This will trigger the FileType\n\" event, which can be used to set the syntax highlighting, set options, etc.\n\" plugin\n\"\n\" This actually loads the file \"ftplugin.vim\" in 'runtimepath'.\n\"\n\" The result is that when a file is edited its plugin file is loaded (if\n\" there is one for the detected filetype).\n\"\n\" indent \n\"\n\"\n\" This actually loads the file \"indent.vim\" in 'runtimepath'.\n\"\n\" The result is that when a file is edited its indent file is loaded (if\n\" there is one for the detected filetype). indent-expression\n\"\nfiletype plugin indent on\n\n\n\" -----------------------------------------------------------------------------\n\" Set to auto read when a file is changed from the outside\n\" Autoread does not reload file unless you do something like run external\n\" command (like !ls or !sh etc) vim does not do checks periodically you can\n\" reload file manually using :e\nset autoread\n\n\n\" -----------------------------------------------------------------------------\n\" Have the mouse enabled all the time:\n\" Normally, if you try to copy text out of the xterm that vim is running in,\n\" you'll get the text as well as the numbers. The GUI version gets this right:\n\" it only selects the text, keeping the line numbers out of the picture. But I\n\" don't\n\" want the GUI version. So instead, I added this to my vimrc:\n\"\n\" :set mouse=a\n\"\n\" Much better. You can also selectively enable mouse support for specific modes\n\" only by using something other than 'a' (for 'all').\nset mouse=a\n\n\n\" -----------------------------------------------------------------------------\n\" Prevents some security exploits having to do with modelines in files. \n\" https://www.techrepublic.com/blog/it-security/turn-off-modeline-support-in-vim/\nset modelines=0\nset nomodeline\n\n\" Tabs are 4 spaces wide and are spaces\nset tabstop=4\nset shiftwidth=4\nset softtabstop=4\nset expandtab\n\nset encoding=utf-8\n\nset visualbell\nset cursorline\nset ttyfast\nset ruler\n\nset backspace=indent,eol,start\nset laststatus=2\n\nif $TERM == \"xterm-256color\"\n set t_Co=256\nendif\n\n\"show matching bracets\nset showmatch\n\n\"How many tenths of a second to blink\nset mat=1\n\n\n\"Make the new window appears below the current window.\n:se splitbelow \n\n\"Make the new window appears in right. (only 6.0 version can do a vsplit)\n:se splitright\n\n\"Improve the statusline.\n\" Normally not visible, powerline wil be used.\n\" See Vundle Plugins\n\" set statusline=%F%m%r%h%w\\ [FORMAT=%{&ff}]\\ [TYPE=%Y]\\ [ASCII=\\%03.3b]\\ [HEX=\\%02.2B]\\ [POS=%04l,%04v][%p%%]\\ [LEN=%L]\n\n\"Make place for the statusline, so it's always there.\nset laststatus=2 \n\n\"Make the menubar and toolbar toggeble.\nmap :if &guioptions =~# 'T' \n \\set guioptions-=T \n \\set guioptions-=m \n \\else \n \\set guioptions+=T \n \\set guioptions+=m \n \\endif \n\n\n\" set guioptions-=r\n\n\" Make the scrollbars toggable\n\" map :if &guioptions =~# 'r' \n\" \\set guioptions-=r \n\" \\else \n\" \\set guioptions+=r \n\" \\endif\n\n\" Turn off useless toolbar\n\" set guioptions-=T\n\n\" Turn off menu bar (toggle with CTRL+F11)\n\" set guioptions-=m\n\n\" Turn off right-hand scroll-bar (toggle with CTRL+F7)\n\" set guioptions-=r\n\n\" Turn off left-hand scroll-bar (toggle with CTRL+F7)\n\" set guioptions-=l\n\" set guioptions-=L\n\n\" CTRL+F11 to toggle the menu bar\nnmap :if &guioptions=~'m' \\| set guioptions-=m \\| else \\| set guioptions+=m \\| endif\n\n\" CTRL+F7 to toggle the right-hand scroll bar\nnmap :if &guioptions=~'r' \\| set guioptions-=r \\| else \\| set guioptions+=r \\| endif\n\n\" CTRL+F6 to toggle the left-hand scroll bar\nnmap :if &guioptions=~'lL' \\| set guioptions-=lL \\| else \\| set guioptions+=lL \\| endif\n\n\" CTRL+F5 to toggle the toolbar\nnmap :if &guioptions=~'T' \\| set guioptions-=T \\| else \\| set guioptions+=T \\| endif\n\n\n\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\n\" Files and backups\n\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\n\"Turn backup off\nset nobackup\nset nowb\nset noswapfile\n\n\n\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\n\" Text options\n\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\n \"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\n \" Indent\n \"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\n \"Auto indent\n set ai\n\n \"Smart indent\n set si\n\n \"C-style indeting\n set cindent\n\n \"Wrap lines\n set wrap\n\n\nset tabstop=4\nset expandtab\nset shiftwidth=4\n\n\"set textwidth=80\nset nu \" Turn the linenumbers on\n\"set noai \" Turn the automatic indentation off\nset showmatch \" show the matching braces, . . .\nset wildmode=longest,list \" Make tab-completion work more like bash\nset noerrorbells\nset vb t_vb=\nset ruler\nset smarttab autoindent\nset smartindent\n\nset backspace=indent,eol,start \" Provide a natural backspace\nset nocp\nfiletype plugin on\n\"let g:snip_set_textmate_cp = 1 \" Textmate compatibility \n\n\" Keyboard Shortcuts\nmap :Tlist\n\n\n\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\n\" Text options\n\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\n\"Iabbr fori for in : <:system('date')>Cursor here:<> \n\nvmap \"+y\nnmap \"+gP\nimap :noh\nnnoremap %\nvnoremap %\n\nset wrap\nset textwidth=120\nset formatoptions=qrn1\n\n\n\" Normally gives you the Help.\n\" When it gets hit on, you prob. wannted the .\n\" Let's remap to ;-)\ninoremap \nnnoremap \nvnoremap \n\n\" Hah you forgot to press the Shift, no worries, I've got your back!\nnnoremap ; :\n\nset omnifunc=syntaxcomplete#Complete\nset formatprg=par\\ -w80rj \n\nset fillchars+=vert:│\n\"autocmd ColorScheme * highlight VertSplit cterm=NONE ctermfg=NONE ctermbg=NONE guibg=NONE\nhi VertSplit ctermbg=NONE guibg=NONE\nset shortmess+=c\n\nset foldlevelstart=99\n\n\"vmap \"+y\n\"\"nmap \"+gP\n\"imap \"\n\n\"Enable colors and syntax highlighting\nset term=screen-256color\n\nset colorcolumn=120\nhighlight ColorColumn ctermbg=222 guibg=#222222\nexecute \"set colorcolumn=\" . join(range(85,335), ',')\nlet &colorcolumn=\"80,\".join(range(121,999),\",\")\n\nfunction! THEME(kind)\n if a:kind == \"Light\"\n syntax on\n set background=light\n set bg=light\n set t_Co=256\n set cursorline\n colorscheme onehalflight\n elseif a:kind == \"dark\"\n syntax on\n syntax enable\n set background=dark\n set bg=dark\n set t_Co=256\n set cursorline\n colorscheme gruvbox\n else\n echom \"Non-existing theme...\"\n endif\nendfunction\n\ncommand! THEMELIGHT :call THEME(\"light\")\ncommand! THEMEDARK :call THEME(\"dark\")\n\nif $VIM_DARK == 1\n THEMELIGHT\nelse\n THEMEDARK\nendif\n" }, { "path": "docker/Dockerfile", "content": "FROM osrf/ros:noetic-desktop-full-focal\n\nENV LANG C.UTF-8\n\nRUN echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections\n\n# Set the nvidia container runtime\nENV NVIDIA_VISIBLE_DEVICES ${NVIDIA_VISIBLE_DEVICES:-all}\nENV NVIDIA_DRIVER_CAPABILITIES ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics\n\nENV LD_LIBRARY_PATH=/usr/lib/nvidia:$LD_LIBRARY_PATH\n\n# Use build argument for the username\nARG USERNAME\nENV USR_NAME=$USERNAME\n\nRUN echo \"Current working directory after usermod:\" && pwd\n# Install some handy tools.\nRUN set -x \\\n && apt-get update \\\n && apt-get --with-new-pkgs upgrade -y \\\n && apt-get install -y mesa-utils \\\n && apt-get install -y libgl1-mesa-glx \\\n && apt-get install -y vim \\\n && apt-get install -y tmuxinator \\\n && apt-get install -y python3-catkin-tools \\\n && apt-get install -y python3-osrf-pycommon \\\n && apt-get install -y python3-pip \\\n && pip3 install opengen \\\n && pip3 install gdown \\\n && apt-get install -y libtbb-dev \\\n && apt-get install -y ros-noetic-octomap-server \\\n && apt-get install -y ros-noetic-octomap-ros \\\n && apt-get install -y ros-noetic-octomap-rviz-plugins \\\n && apt-get install -y ros-noetic-octomap-mapping \\\n && apt-get install -y libtool \\\n && apt-get install -y libgoogle-glog-dev \\\n && apt-get install -y libnlopt-dev \\\n && apt-get install -y libsuitesparse-dev \\\n && apt-get install -y ros-noetic-nlopt \\\n && apt-get install -y liblapacke-dev \\\n && apt-get install -y ros-noetic-gtsam \\\n && apt-get install -y ros-noetic-rosmon \\\n && apt-get install -y iputils-ping \\\n && apt-get install -y apt-transport-https ca-certificates \\\n && apt-get install -y openssh-server python3-pip exuberant-ctags \\\n && apt-get install -y git vim tmux nano htop sudo curl wget gnupg2 \\\n && apt-get install -y bash-completion \\\n && apt-get install -y libcanberra-gtk3-0 \\\n && apt-get install -y ros-noetic-gmapping ros-noetic-slam-gmapping ros-noetic-openslam-gmapping \\\n && apt-get install -y ros-noetic-joy \\\n && apt-get install -y ros-noetic-twist-mux \\\n && apt-get install -y ros-noetic-interactive-marker-twist-server \\\n && apt-get install -y ros-noetic-fath-pivot-mount-description \\\n && apt-get install -y ros-noetic-flir-camera-description \\\n && apt-get install -y ros-noetic-realsense2-description \\\n && apt-get install -y ros-noetic-lms1xx \\\n && apt-get install -y ros-noetic-robot-localization \\\n && apt-get install -y ros-noetic-teleop-twist-keyboard \\\n && apt-get install -y ros-noetic-teleop-twist-joy \\\n && apt-get install -y ros-noetic-rviz-imu-plugin \\\n && apt-get install -y ros-noetic-gmapping \\\n && apt-get install -y ros-noetic-mavros-msgs \\\n && rm -rf /var/lib/apt/lists/* \n \nRUN set -x \\\n && useradd -m -s /bin/bash $USERNAME \\\n && echo \"$USERNAME ALL=(ALL:ALL) NOPASSWD:ALL\" >> /etc/sudoers \\\n && echo \"$USERNAME ALL=(ALL:ALL) NOPASSWD:ALL\" \n\n# The OSRF container didn't link python3 to python, causing ROS scripts to fail.\nRUN ln -s /usr/bin/python3 /usr/bin/python\n\nUSER $root\nWORKDIR /home/$USERNAME\n\n# Install Rust using rustup\nRUN curl https://sh.rustup.rs -sSf | sh -s -- -y\n\n# Add Rust binaries to the PATH\nRUN echo \"source $HOME/.cargo/env\" >> /home/$USER_NAME/.bashrc\n\nRUN sudo usermod -a -G video $USERNAME\n\nRUN rosdep update \\\n && echo \"source /opt/ros/noetic/setup.bash\" >> /home/$USERNAME/.bashrc\n\n# Clone the necessary packages for simulation. \nRUN mkdir -p catkin_ws/src \n\nWORKDIR /home/$USERNAME/catkin_ws/src \n\nRUN catkin init\nRUN catkin config --extend /opt/ros/noetic\nRUN catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release\n\nRUN git clone https://github.com/ethz-asl/eigen_checks.git\nRUN git clone https://github.com/catkin/catkin_simple.git\nRUN git clone https://github.com/ethz-asl/eigen_catkin.git\nRUN git clone https://github.com/ntnu-arl/lidar_simulator.git\nRUN git clone https://github.com/ethz-asl/mav_comm.git\nRUN git clone https://github.com/ros-planning/navigation_msgs.git\nRUN git clone https://github.com/ethz-asl/numpy_eigen.git\nRUN git clone --branch melodic-devel https://github.com/ros-perception/perception_pcl.git\nRUN git clone https://github.com/ros/xacro.git\nRUN git clone https://github.com/ethz-asl/catkin_boost_python_buildtool.git\n\nRUN git clone https://github.com/LTU-RAI/darpa_subt_worlds.git\nRUN git clone https://github.com/LTU-RAI/rotors_simulator.git && cd rotors_simulator && git pull\nRUN git clone https://github.com/LTU-RAI/ufomap.git\n\nRUN git clone https://github.com/LTU-RAI/geometry2.git\nRUN git clone https://github.com/aakapatel/mav_control_rw.git\nUSER root\n\nWORKDIR /home/$USERNAME/catkin_ws/\nRUN /bin/bash -c 'source /opt/ros/noetic/setup.bash'\nRUN catkin init\nRUN catkin config --extend /opt/ros/noetic\nRUN catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release\n# Build all packages \n\n# Add Qt dependencies\nRUN apt-get update && apt-get install -y \\\n ros-noetic-tf2-sensor-msgs \\\n python3-catkin-tools \\\n python3-osrf-pycommon \\\n libtbb-dev \\\n qtbase5-dev \\\n qtdeclarative5-dev \\\n libqt5x11extras5-dev \\\n && rm -rf /var/lib/apt/lists/*\n\n# ENV QT_X11_NO_MITSHM 1\n\nRUN sudo catkin build \n\n#Clone Exploration package \nWORKDIR /home/$USERNAME/catkin_ws/src/\n\nRUN git clone https://github.com/LTU-RAI/ExplorationRRT.git\n\nWORKDIR /home/$USERNAME/catkin_ws/src/ExplorationRRT/\n\nUSER root\n\n# Build the cost gen for RRT solver. \nRUN /bin/bash -c 'source $HOME/.cargo/env; cd /home/$USERNAME/catkin_ws/src/ExplorationRRT; python3 rrt_costgen.py'\n\nWORKDIR /home/$USERNAME/catkin_ws/\n\nRUN sudo catkin build errt \n\nUSER $USERNAME\nWORKDIR /home/$USERNAME\nRUN mkdir -p /home/$USERNAME/.config/tmuxinator\n\nRUN git clone https://github.com/jimeh/tmux-themepack.git /home/$USERNAME/.tmux-themepack \\\n && git clone https://github.com/tmux-plugins/tmux-resurrect /home/$USERNAME/.tmux-resurrect\nCOPY --chown=$USERNAME:$USERNAME ./.tmux.conf /home/$USERNAME/.tmux.conf\nCOPY --chown=$USERNAME:$USERNAME ./statusline.conf /home/$USERNAME/statusline.conf\nCOPY --chown=$USERNAME:$USERNAME ./utility.conf /home/$USERNAME/utility.conf\nCOPY --chown=$USERNAME:$USERNAME ./errt.yml /home/$USERNAME/.config/tmuxinator/errt.yml\n\n# Set some decent colors if the container needs to be accessed via /bin/bash.\nRUN echo LS_COLORS=$LS_COLORS:\\'di=1\\;33:ln=36\\' >> ~/.bashrc \\\n && echo export LS_COLORS >> ~/.bashrc \\\n && echo 'alias tmux=\"tmux -2\"' >> ~/.bashrc \\\n && echo 'PATH=~/bin:$PATH' >> ~/.bashrc \\\n && touch ~/.sudo_as_admin_successful # To surpress the sudo message at run.\n\n# some automatic sourcing for convenience.\nRUN echo \"source /home/$USERNAME/catkin_ws/devel/setup.bash --extend\" >> /home/$USERNAME/.bashrc\nRUN echo \"export PS1='\\u@\\h:\\W\\$ '\" >> /home/$USERNAME/.bashrc \nRUN echo \"alias cbe=\\\"sudo catkin build errt\\\" \" >> /home/$USERNAME/.bashrc\nRUN echo \"alias tmkill=\\\"tmux kill-session\\\" \" >> /home/$USERNAME/.bashrc\n\nWORKDIR /home/$USERNAME/.gazebo/\nRUN mkdir -p models\nCOPY models /home/$USERNAME/.gazebo/models\n\nWORKDIR /home/$USERNAME/ \nRUN mkdir -p .ignition\nCOPY ignition_models /home/$USERNAME/.ignition/\n\nWORKDIR /home/$USERNAME/catkin_ws/\n\n# Add Rust binaries to the PATH\nRUN echo \"source $HOME/.cargo/env\" >> /home/$USERNAME/.bashrc\nSTOPSIGNAL SIGTERM\n\n# RUN groupadd -r $USERNAME && useradd -r -g $USERNAME $USERNAME\nCMD sudo service ssh start && /bin/bash\n\n" }, { "path": "docker/errt.yml", "content": "# ~/.tmuxinator/errt.yml \ntmux_options: '-2'\ndefine: &takeoff \"rosservice call /hummingbird/takeoff \\\"{}\\\" \" \nname: errt\nroot: ~/\nwindows:\n - stage_main:\n layout: even-horizontal \n panes:\n - ufomap:\n - sleep 2 \n - mon launch errt server.launch \n - errt:\n - sleep 8 \n - mon launch errt errt.launch\n\n - publishers:\n # layout: main-vertical \n layout: tiled \n panes:\n - rotors:\n - sleep 1 \n - roslaunch rotors_gazebo errt_mav.launch \n - takeoff:\n - sleep 7\n - *takeoff\n - controller: \n - sleep 3\n # - python3 errt_nmpc.py\n - roslaunch mav_linear_mpc mav_linear_mpc_sim.launch mav_name:=hummingbird\n - roscore: \n - roscore \n" }, { "path": "docker/stage_planning.yml", "content": "# ~/.tmuxinator/stage_planning.yml \ntmux_options: '-2'\ndefine: &initialize_stage \"rosservice call /initialize_planner \\\"initialize: true\\\" \" \n\nname: stage_planning\nroot: ~/\nwindows:\n - stage_main:\n layout: even-horizontal \n panes:\n - stage_interface:\n - cde\n - sleep 2 \n - mon launch stage_planner stage_interface.launch \n - graph_management:\n - cde \n - sleep 6 \n - mon launch stage_planner graph_management.launch\n\n - publishers:\n # layout: main-vertical \n layout: tiled \n panes:\n - rotors:\n - cde\n - sleep 1 \n - roslaunch rotors_gazebo custom_drone.launch \n - initialization:\n - cde \n - sleep 8 \n - *initialize_stage \n - controller: \n - cde \n - sleep 4\n - roslaunch mav_linear_mpc mav_linear_mpc_sim.launch mav_name:=hummingbird\n - roscore: \n - roscore \n" }, { "path": "docker/start_errt_gpu.sh", "content": "#!/bin/bash\nsudo xhost +si:localuser:root\nXSOCK=/tmp/.X11-unix\n\nXAUTH=/tmp/.docker.xauth\nxauth_list=$(xauth nlist :0 | sed -e 's/^..../ffff/')\nif [ ! -f $XAUTH ]\nthen\n echo XAUTH file does not exist. Creating one...\n touch $XAUTH\n chmod a+r $XAUTH\n if [ ! -z \"$xauth_list\" ]\n then\n echo $xauth_list | xauth -f $XAUTH nmerge -\n fi\nfi\n\n# Prevent executing \"docker run\" when xauth failed.\nif [ ! -f $XAUTH ]\nthen\n echo \"[$XAUTH] was not properly created. Exiting...\"\n exit 1\nfi\n\ndocker run --rm -it \\\n--env=LOCAL_USER_ID=\"$(id -u)\" \\\n-v /home/aakapatel/catkin_workspaces:/home/aakapatel/catkin_workspaces \\\n-v /home/aakapatel/.gazebo/models:/home/aakpatel/.gazebo/models \\\n--env=\"XAUTHORITY=$XAUTH\" \\\n--volume=\"$XAUTH:$XAUTH\" \\\n--volume=\"/tmp/.X11-unix:/tmp/.X11-unix:rw\" \\\n--env=\"QT_X11_NO_MITSHM=1\" \\\n--env=\"DISPLAY=$DISPLAY\" \\\n--network host \\\n--privileged \\\n--gpus all \\\n--name=errt_test \\\n errt_test bash\n\n" }, { "path": "docker/start_errt_no_gpu.sh", "content": "#!/bin/bash\n\nXAUTH=/path/to/Xauthority\n# Allow local Docker containers to access the X server\nxhost +local:docker\ndocker run -it --rm \\\n --name errt_test \\\n --gpus all \\\n --net=host \\\n -e DISPLAY=$DISPLAY \\\n --env=\"QT_X11_NO_MITSHM=1\" \\\n --env=\"XAUTHORITY=$XAUTH\" \\\n errt_test\n" }, { "path": "docker/statusline.conf", "content": "# vim: ft=tmux\nset -g mode-style \"fg=#eee8d5,bg=#073642\"\n\nset -g message-style \"fg=#eee8d5,bg=#073642\"\nset -g message-command-style \"fg=#eee8d5,bg=#073642\"\n\nset -g pane-border-style \"fg=#073642\"\nset -g pane-active-border-style \"fg=#eee8d5\"\n\nset -g status \"on\"\nset -g status-interval 1\nset -g status-justify \"left\"\n\nset -g status-style \"fg=#586e75,bg=#073642\"\n\nset -g status-bg \"#002b36\"\n\nset -g status-left-length \"100\"\nset -g status-right-length \"100\"\n\nset -g status-left-style NONE\nset -g status-right-style NONE\n\nset -g status-left \"#[fg=#073642,bg=#eee8d5,bold] #S #[fg=#eee8d5,bg=#93a1a1,nobold,nounderscore,noitalics]#[fg=#15161E,bg=#93a1a1,bold] #(whoami) #[fg=#93a1a1,bg=#002b36]\"\nset -g status-right \"#[fg=#586e75,bg=#002b36,nobold,nounderscore,noitalics]#[fg=#93a1a1,bg=#586e75]#[fg=#657b83,bg=#586e75,nobold,nounderscore,noitalics]#[fg=#93a1a1,bg=#657b83]#[fg=#93a1a1,bg=#657b83,nobold,nounderscore,noitalics]#[fg=#15161E,bg=#93a1a1,bold] #h \"\n\nsetw -g window-status-activity-style \"underscore,fg=#839496,bg=#002b36\"\nsetw -g window-status-separator \"\"\nsetw -g window-status-style \"NONE,fg=#839496,bg=#002b36\"\nsetw -g window-status-format '#[fg=#002b36,bg=#002b36]#[default] #I  #{b:pane_current_path} #[fg=#002b36,bg=#002b36,nobold,nounderscore,noitalics]'\nsetw -g window-status-current-format '#[fg=#002b36,bg=#eee8d5]#[fg=#004090,bg=#eee8d5] #I #[fg=#eee8d5,bg=#004090] #{b:pane_current_path} #[fg=#004090,bg=#002b36,nobold]'\n" }, { "path": "docker/tmux/macos.conf", "content": "# osx clipboard\nset-option -g default-command \"which reattach-to-user-namespace > /dev/null && reattach-to-user-namespace -l $SHELL || $SHELL\"\n\n# Undercurl\nset -as terminal-overrides ',*:Smulx=\\E[4::%p1%dm' # undercurl support\nset -as terminal-overrides ',*:Setulc=\\E[58::2::%p1%{65536}%/%d::%p1%{256}%/%{255}%&%d::%p1%{255}%&%d%;m' # underscore colours - needs tmux-3.0\n\n" }, { "path": "docker/tmux/statusline.conf", "content": "# vim: ft=tmux\nset -g mode-style \"fg=#eee8d5,bg=#073642\"\n\nset -g message-style \"fg=#eee8d5,bg=#073642\"\nset -g message-command-style \"fg=#eee8d5,bg=#073642\"\n\nset -g pane-border-style \"fg=#073642\"\nset -g pane-active-border-style \"fg=#eee8d5\"\n\nset -g status \"on\"\nset -g status-interval 1\nset -g status-justify \"left\"\n\nset -g status-style \"fg=#586e75,bg=#073642\"\n\nset -g status-bg \"#002b36\"\n\nset -g status-left-length \"100\"\nset -g status-right-length \"100\"\n\nset -g status-left-style NONE\nset -g status-right-style NONE\n\nset -g status-left \"#[fg=#073642,bg=#eee8d5,bold] #S #[fg=#eee8d5,bg=#93a1a1,nobold,nounderscore,noitalics]#[fg=#15161E,bg=#93a1a1,bold] #(whoami) #[fg=#93a1a1,bg=#002b36]\"\nset -g status-right \"#[fg=#586e75,bg=#002b36,nobold,nounderscore,noitalics]#[fg=#93a1a1,bg=#586e75]#[fg=#657b83,bg=#586e75,nobold,nounderscore,noitalics]#[fg=#93a1a1,bg=#657b83]#[fg=#93a1a1,bg=#657b83,nobold,nounderscore,noitalics]#[fg=#15161E,bg=#93a1a1,bold] #h \"\n\nsetw -g window-status-activity-style \"underscore,fg=#839496,bg=#002b36\"\nsetw -g window-status-separator \"\"\nsetw -g window-status-style \"NONE,fg=#839496,bg=#002b36\"\nsetw -g window-status-format '#[fg=#002b36,bg=#002b36]#[default] #I  #{b:pane_current_path} #[fg=#002b36,bg=#002b36,nobold,nounderscore,noitalics]'\nsetw -g window-status-current-format '#[fg=#002b36,bg=#eee8d5]#[fg=#004090,bg=#eee8d5] #I #[fg=#eee8d5,bg=#004090] #{b:pane_current_path} #[fg=#004090,bg=#002b36,nobold]'\n" }, { "path": "docker/tmux/tmux.conf", "content": "#set -g default-terminal \"tmux-256color\"\nset -g default-terminal \"xterm-256color\"\n#set -ga terminal-overrides \",*256col*:Tc\"\nset -ga terminal-overrides \",xterm-256color:Tc\"\n# action key\n\nset-option -g prefix C-b\nset-option -g repeat-time 0\nset-option -g focus-events on\nset -g mouse on\n#### Key bindings\n\nset-window-option -g mode-keys vi\n\n#bind t send-key C-t\n# Reload settings\nbind r source-file ~/.config/tmux/tmux.conf \\; display \"Reloaded!\"\n# Open current directory\nbind o run-shell \"open #{pane_current_path}\"\n# bind -r e kill-pane -a\n\n# vim-like pane switching\n# bind -r k select-pane -U \n# bind -r j select-pane -D \n# bind -r h select-pane -L \n# bind -r l select-pane -R \n\n# Moving window\nbind-key -n C-S-Left swap-window -t -1 \\; previous-window\nbind-key -n C-S-Right swap-window -t +1 \\; next-window\n\n# Resizing pane\nbind -r C-k resize-pane -U 5\nbind -r C-j resize-pane -D 5\nbind -r C-h resize-pane -L 5\nbind -r C-l resize-pane -R 5\n\n#### basic settings\n\nset-option -g status-justify \"left\"\n#set-option utf8-default on\n#set-option -g mouse-select-pane\nset-window-option -g mode-keys vi\n#set-window-option -g utf8 on\n# look'n feel\nset-option -g status-fg cyan\nset-option -g status-bg black\nset -g pane-active-border-style fg=colour166,bg=default\nset -g window-style fg=colour10,bg=default\nset -g window-active-style fg=colour12,bg=default\nset-option -g history-limit 64096\n\nset -sg escape-time 10\n\n#### COLOUR\n\n# default statusbar colors\nset-option -g status-style bg=colour235,fg=colour136,default\n\n# default window title colors\nset-window-option -g window-status-style fg=colour244,bg=colour234,dim\n\n# active window title colors\nset-window-option -g window-status-current-style fg=colour166,bg=default,bright\n\n# pane border\nset-option -g pane-border-style fg=colour235 #base02\nset-option -g pane-active-border-style fg=colour136,bg=colour235\n\n# message text\nset-option -g message-style bg=colour235,fg=colour166\n\n# pane number display\nset-option -g display-panes-active-colour colour33 #blue\nset-option -g display-panes-colour colour166 #orange\n\n# clock\nset-window-option -g clock-mode-colour colour64 #green\n\n# allow the title bar to adapt to whatever host you connect to\nset -g set-titles on\nset -g set-titles-string \"#T\"\n\n# import\n\nsource ~/.config/tmux/statusline.conf\nsource ~/.config/tmux/utility.conf\n" }, { "path": "docker/tmux/utility.conf", "content": "# Display lazygit\nbind -r g display-popup -d '#{pane_current_path}' -w80% -h80% -E lazygit\n\n" }, { "path": "docker/utility.conf", "content": "# Display lazygit\nbind -r g display-popup -d '#{pane_current_path}' -w80% -h80% -E lazygit\n\n" }, { "path": "launch/errt.launch", "content": "\n\t\t\t\t\t\t\t\t\t\n \n \n\n \n\n \n \n \n \n \n \t\t\t \n \t\t\t\t\t \n \t\t\t\t\n \n\n \n\n" }, { "path": "launch/octomap_gazebo.launch", "content": "\n\n\n\t\n\t\t\n\t\t\n\t\t\n\t\t\n\t\t\n\t\t\n\t\t\n\t\t\n\t\t\n\t\t\n \n\t\t\n\t\t\n\n \n\n\t\t\n\n\t\t\n\n\t\n \n\n\n \n \n\t\t\n" }, { "path": "launch/server.launch", "content": "\n\n\n\t\t\n\t\t\n\t\t\n\t\t\n\n \n\t\t\n \n\n\t\t\n\n\t\t\n\n\t\t\n\n\t\t\n\n \t\n\n\t\t\n\n\t\t\n\t\t\n\t\t\n\t\t\n\t\t\n\t\t\n\t\t\n\n\t\t\n\n\t\t\n\n\t\t\n\n\t\t\n\t\t\n\t\t\n\t\t\n\t\t\n\t\t\n\t\t\n\t\t\n \n\n \n\n\n\t\n\t\t\n \t\t\n\t\t\n\t\t\n\t\t\n\t\t\n\t\t\n\t\t\n\t\t\n\t\t\n \n\t\t\n\t\t\n\n \n\n\t\t\n\n\t\t\n\n\t\n\n\n\n\n" }, { "path": "nmpc_test.py", "content": "import opengen as og\nimport casadi.casadi as cs\n#import matplotlib.pyplot as plt\nimport numpy as np\nimport math \nfrom trajectory import trajectory\nmng = og.tcp.OptimizerTcpManager('MAV/rrt')\nmng.start()\nN = 50\ndt = 1.0 / 2\nx0 = [0,0,0,0,0,0,0,0]\nxref = [0,0,0]*N\n\nfor i in range(0,N):\n xref[3*i] = i*0.5\n xref[3*i+1] = i*0.5\n xref[3*i+2] = 0\n\n#obsdata = (0.0,0.0,1.0,0.0, 0.0, 0.0)\n#obsdata = [0]*N*3\nz0 = x0 + xref + [dt]\nprint(z0);\nmng.ping()\nsolution = mng.call(z0, initial_guess=[9.81,0,0]*(N), buffer_len = 4*4096)\n#print(solution['solution'])\nsolution_data = solution.get();\n#print(error_msg);\nustar = solution_data.solution\nprint(ustar)\n[p_traj, v_traj, cost] = trajectory(x0, ustar, 50, 0.5, xref)\n#print(len(p_traj))\nprint(cost)\n\npx = [0]*N\npy = [0]*N\nfor i in range(0,N):\n px[i] = p_traj[i][0]\n py[i] = p_traj[i][1]\n\npx_ref = [0]*N\t\npy_ref = [0]*N\nfor i in range(0,N):\n px_ref[i] = xref[3*i]\n py_ref[i] = xref[3*i+1]\n\nimport matplotlib.pyplot as plt\nplt.plot(px,py,px_ref,py_ref)\nplt.show()\n\nmng.kill()\n" }, { "path": "package.xml", "content": "\n\n errt\n 1.0.0\n Exploration-RRT Ros package\n\n \n \n \n aakapatel\n\n\n \n \n \n BSD\n\n\n catkin\n\n\n dynamic_reconfigure\n\n ufomap\n ufomap_msgs\n ufomap_ros\n roscpp\n roscpp_serialization\n tf2_ros\n tf\n std_msgs\n std_srvs\n geometry_msgs\n visualization_msgs\n pcl_ros\n eigen_catkin\n move_base_msgs\n actionlib\n actionlib_msgs\n message_generation\n catkin_simple\n\n ufomap\n ufomap_msgs\n ufomap_ros\n roscpp_serialization\n tf2_ros\n tf\n mav_msgs\n mav_planning_msgs\n move_base_msgs\n visualization_msgs\n actionlib\n actionlib_msgs\n eigen_catkin\n octomap_rviz_plugins\n octomap_ros\n message_runtime \n\n\n\n\n \n \n \n\n \n\n" }, { "path": "rrt_costgen.py", "content": "import opengen as og\nimport casadi.casadi as cs\n#import matplotlib.pyplot as plt\nimport numpy as np\nfrom trajectory import trajectory\n\n## Problem size\n\nN = 50\n#dt = 1.0 / 2\n\n## Weight matrices\nQx = (30,30,30, 2, 2,2, 4, 4) \nP = 2*Qx; #final state weight\nRu = (2,2,2); # input weights\nRd = (2, 2, 2); # input rate weights\nQT = (100,100,100)\n\n## Objective function generation\nnu = 3\nns = 3\nnp = 8 + N*ns + 1\nu = cs.SX.sym('u', nu*N)\nz0 = cs.SX.sym('z0', np)\nx = z0[0:8]\nprint(x)\nn_xref = ns*N\nx_ref_N = z0[8:(8+n_xref)]\nprint(x_ref_N)\ndt = z0[(8+n_xref):(8+n_xref+1)]\ncost = 0\nc = 0\nu_old = [9.81, 0 , 0]\n\n\nfor i in range(0, N):\n####State Cost\n #x_ref = x_ref_N[(ns*i):(ns*i+ns)]\n #print(x_ref)\n \n #State weights\n\n####Input Cost\n u_n = u[(3*i):3*i+3]\n\n cost += Ru[0]*(u_n[0] - 9.81)**2 + Ru[1]*(u_n[1])**2 + Ru[2]*(u_n[2])**2 #Input weights\n if i > 0:\n cost += Rd[0]*(u_n[0] - u_old[0])**2 + Rd[1]*(u_n[1] - u_old[1])**2 + Rd[2]*(u_n[2] - u_old[2])**2 #Input rate weights\n u_old = u_n\n\n\n #Penalty Constraint\n\n #c = cs.vertcat(c, cs.fmax(0, 0.36 - ((x[0] - obs_data[3*i])**2 + (x[1] - obs_data[3*i+1])**2 + (x[2] - obs_data[3*i+2])**2)))\n #c = cs.vertcat(c, cs.fmax(0, u_n[1] - u_old[1] - 0.05))\n #c = cs.vertcat(c, cs.fmax(0, u_old[1] - u_n[1] - 0.05))\n #c = cs.vertcat(c, cs.fmax(0, u_n[2] - u_old[2] - 0.05))\n #c = cs.vertcat(c, cs.fmax(0, u_old[2] - u_n[2]- 0.05))\n\n####State Update\n x[0] = x[0] + dt * x[3]\n x[1] = x[1] + dt * x[4]\n x[2] = x[2] + dt * x[5]\n x[3] = x[3] + dt * (cs.sin(x[7]) * cs.cos(x[6]) * u_n[0] - 1 * x[3])\n x[4] = x[4] + dt * (-cs.sin(x[6]) * u_n[0] - 1*x[4])\n x[5] = x[5] + dt * (cs.cos(x[7]) * cs.cos(x[6]) * u_n[0] - 1 * x[5] - 9.81)\n x[6] = x[6] + dt * ((1 / 0.5) * (u_n[1] - x[6]))\n x[7] = x[7] + dt * ((1 / 0.5) * (u_n[2] - x[7]))\n\n cost += Qx[0]*(x[0]-x_ref_N[3*i])**2 + Qx[1]*(x[1]-x_ref_N[3*i+1])**2 + Qx[2]*(x[2]-x_ref_N[3*i + 2])**2 + Qx[3]*(x[3])**2 + Qx[4]*(x[4])**2 + Qx[5]*(x[5])**2 + Qx[6]*(x[6])**2 + Qx[7]*(x[7])**2 \n\n #c = cs.vertcat(c, cs.fmax(0, 0.04 - ((x[0]-x_ref_N[3*i])**2 + (x[1]-x_ref_N[3*i+1])**2 + (x[2]-x_ref_N[3*i + 2])**2)))\n\n\n\ncost += QT[0]*(x[0]-x_ref_N[3*(N-1)])**2 + QT[1]*(x[1]-x_ref_N[3*(N-1)+1])**2 + QT[2]*(x[2]-x_ref_N[3*(N-1) + 2])**2\n\numin = [5, -0.7, -0.7] * (nu*N)\n#print(umin)\numax = [13.5, 0.7, 0.7] * (nu*N)\nbounds = og.constraints.Rectangle(umin, umax)\n\ntcp_config = og.config.TcpServerConfiguration(bind_port=3300)\n\nproblem = og.builder.Problem(u, z0, cost) \\\n.with_constraints(bounds)#.with_penalty_constraints(c)\nbuild_config = og.config.BuildConfiguration() \\\n.with_tcp_interface_config(tcp_config) \\\n.with_build_directory(\"MAV\") \\\n.with_build_mode(\"release\") \\\n.with_build_c_bindings() \nmeta = og.config.OptimizerMeta() \\\n.with_optimizer_name(\"rrt\") \n#.with_rebuild(True) \nsolver_config = og.config.SolverConfiguration() \\\n.with_tolerance(5e-5) \\\n.with_initial_tolerance(5e-5) \\\n.with_max_duration_micros(40000) \\\n.with_max_outer_iterations(4) \\\n.with_penalty_weight_update_factor(2) \\\n.with_initial_penalty(1000.0) \nbuilder = og.builder.OpEnOptimizerBuilder(problem, meta,\n build_config, solver_config) \\\n.with_verbosity_level(1)\n#builder.enable_tcp_interface()\n\nbuilder.build()\n# Use TCP server\n# ------------------------------------\nmng = og.tcp.OptimizerTcpManager('MAV/rrt')\nmng.start()\ndt = 1.0 / 2\nx0 = [1.0,2.0,1.0,0,0,0,0,0]\nxref = [1.0,8.0,1.0]*(N)\n#obsdata = (0.0,0.0,1.0,0.0, 0.0, 0.0)\n#obsdata = [0]*N*3\nz0 = x0 + xref + [dt]\nprint(len(z0))\nmng.ping()\nsolution = mng.call(z0, initial_guess=[9.81,0,0]*(N), buffer_len = 4*4096)\nprint(solution['solution'])\nustar = solution['solution']\n[p_hist, bla, bleh] = trajectory(x0, ustar, 50, 0.5, xref)\nprint(p_hist)\nmng.kill()\n" }, { "path": "rviz/errt.rviz", "content": "Panels:\n - Class: rviz/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /Global Options1\n - /Grid1\n - /Axes1\n - /Marker1\n - /PointCloud22\n - /Marker2\n - /Marker2/Namespaces1\n - /Marker3\n - /Path1\n - /Marker4\n - /Marker4/Namespaces1\n - /Marker5\n - /Marker5/Namespaces1\n - /Path2\n - /Path3\n - /Marker6\n Splitter Ratio: 0.5\n Tree Height: 1084\n - Class: rviz/Selection\n Name: Selection\n - Class: rviz/Tool Properties\n Expanded:\n - /2D Pose Estimate1\n - /2D Nav Goal1\n - /Publish Point1\n Name: Tool Properties\n Splitter Ratio: 0.5886790156364441\n - Class: rviz/Views\n Expanded:\n - /Current View1\n Name: Views\n Splitter Ratio: 0.5\n - Class: rviz/Time\n Name: Time\n SyncMode: 0\n SyncSource: PointCloud2\nPreferences:\n PromptSaveOnExit: true\nToolbars:\n toolButtonStyle: 2\nVisualization Manager:\n Class: \"\"\n Displays:\n - Alpha: 0.10000000149011612\n Cell Size: 1\n Class: rviz/Grid\n Color: 160; 160; 164\n Enabled: false\n Line Style:\n Line Width: 0.029999999329447746\n Value: Lines\n Name: Grid\n Normal Cell Count: 0\n Offset:\n X: 0\n Y: 0\n Z: 0\n Plane: XY\n Plane Cell Count: 100000\n Reference Frame: \n Value: false\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: Intensity\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Min Color: 0; 0; 0\n Name: PointCloud2\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.009999999776482582\n Style: Flat Squares\n Topic: /pixy/velodyne_points\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Class: rviz/Axes\n Enabled: true\n Length: 2\n Name: Axes\n Radius: 0.15000000596046448\n Reference Frame: hummingbird/base_link\n Show Trail: false\n Value: true\n - Class: rviz/Marker\n Enabled: false\n Marker Topic: /vis_next/visualization_marker\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Alpha: 0.10999999940395355\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 12.191140174865723\n Min Value: -0.3226672410964966\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: AxisColor\n Decay Time: 11110000361472\n Enabled: true\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Min Color: 0; 0; 0\n Name: PointCloud2\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.014999999664723873\n Style: Boxes\n Topic: /hummingbird/ouster/points\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /exploration_path\n Name: Marker\n Namespaces:\n points: true\n Queue Size: 100\n Value: true\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /candidate_branches\n Name: Marker\n Namespaces:\n \"\": true\n points: true\n Queue Size: 100\n Value: true\n - Alpha: 1\n Buffer Length: 4\n Class: rviz/Path\n Color: 206; 76; 230\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 0.05000000074505806\n Name: Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /tracked_trajectory\n Unreliable: false\n Value: false\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /tree_expansion\n Name: Marker\n Namespaces:\n \"\": true\n points: true\n Queue Size: 100\n Value: true\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /predicted_info_gain\n Name: Marker\n Namespaces:\n points: true\n Queue Size: 100\n Value: true\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 0; 255; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 0.20000000298023224\n Name: Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 170; 255; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /selected_trajectory\n Unreliable: false\n Value: true\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 204; 41; 204\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 0.10000000149011612\n Name: Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /state_history\n Unreliable: false\n Value: true\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /candidate_goals\n Name: Marker\n Namespaces:\n points: true\n Queue Size: 100\n Value: true\n Enabled: true\n Global Options:\n Background Color: 0; 0; 0\n Default Light: true\n Fixed Frame: world\n Frame Rate: 30\n Name: root\n Tools:\n - Class: rviz/Interact\n Hide Inactive Objects: true\n - Class: rviz/MoveCamera\n - Class: rviz/Select\n - Class: rviz/FocusCamera\n - Class: rviz/Measure\n - Class: rviz/SetInitialPose\n Theta std deviation: 0.2617993950843811\n Topic: /initialpose\n X std deviation: 0.5\n Y std deviation: 0.5\n - Class: rviz/SetGoal\n Topic: /move_base_simple/goal\n - Class: rviz/PublishPoint\n Single click: true\n Topic: /clicked_point\n Value: true\n Views:\n Current:\n Class: rviz/Orbit\n Distance: 98.66202545166016\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.05999999865889549\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Field of View: 0.7853981852531433\n Focal Point:\n X: -1.435094952583313\n Y: -1.7619433403015137\n Z: 5.213164806365967\n Focal Shape Fixed Size: true\n Focal Shape Size: 0.05000000074505806\n Invert Z Axis: false\n Name: Current View\n Near Clip Distance: 0.009999999776482582\n Pitch: 1.5697963237762451\n Target Frame: \n Yaw: 4.6976165771484375\n Saved: ~\nWindow Geometry:\n Displays:\n collapsed: false\n Height: 1376\n Hide Left Dock: false\n Hide Right Dock: true\n QMainWindow State: 000000ff00000000fd000000040000000000000156000004c7fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000005fb0000025f00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004c7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061020000023e000001d800000280000001e0fb0000000a0049006d0061006700650000000000ffffffff0000000000000000000000010000010f000003d7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003d7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a0000000039fc0100000002fb0000000800540069006d0065010000000000000a00000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000008a4000004c700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000\n Selection:\n collapsed: false\n Time:\n collapsed: false\n Tool Properties:\n collapsed: false\n Views:\n collapsed: true\n Width: 2560\n X: 0\n Y: 27\n" }, { "path": "rviz/errt_field.rviz", "content": "Panels:\n - Class: rviz/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /Global Options1\n - /Grid1\n - /Axes1\n - /PointCloud22\n - /Marker2\n - /UFOMap1\n - /UFOMap1/Voxel Rendering1\n - /UFOMap1/Voxel Rendering1/Occupied1\n - /UFOMap1/Voxel Rendering1/Free1\n - /Marker3\n - /Marker4\n - /Path1\n - /Marker5\n - /Marker6\n - /Marker6/Namespaces1\n - /Path2\n - /PointCloud23\n Splitter Ratio: 0.5\n Tree Height: 934\n - Class: rviz/Selection\n Name: Selection\n - Class: rviz/Tool Properties\n Expanded:\n - /2D Pose Estimate1\n - /2D Nav Goal1\n - /Publish Point1\n Name: Tool Properties\n Splitter Ratio: 0.5886790156364441\n - Class: rviz/Views\n Expanded:\n - /Current View1\n Name: Views\n Splitter Ratio: 0.5\n - Class: rviz/Time\n Name: Time\n SyncMode: 0\n SyncSource: PointCloud2\nPreferences:\n PromptSaveOnExit: true\nToolbars:\n toolButtonStyle: 2\nVisualization Manager:\n Class: \"\"\n Displays:\n - Alpha: 0.10000000149011612\n Cell Size: 1\n Class: rviz/Grid\n Color: 160; 160; 164\n Enabled: false\n Line Style:\n Line Width: 0.029999999329447746\n Value: Lines\n Name: Grid\n Normal Cell Count: 0\n Offset:\n X: 0\n Y: 0\n Z: 0\n Plane: XY\n Plane Cell Count: 100000\n Reference Frame: \n Value: false\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: Intensity\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Min Color: 0; 0; 0\n Name: PointCloud2\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.009999999776482582\n Style: Flat Squares\n Topic: /pixy/velodyne_points\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Class: rviz/Axes\n Enabled: true\n Length: 2\n Name: Axes\n Radius: 0.15000000596046448\n Reference Frame: shafter2/base_link\n Show Trail: false\n Value: true\n - Class: rviz/MarkerArray\n Enabled: false\n Marker Topic: /frontier_cells_vis_array\n Name: MarkerArray\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /vis_next/visualization_marker\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Alpha: 0.699999988079071\n Class: rviz/Map\n Color Scheme: map\n Draw Behind: false\n Enabled: false\n Name: Map\n Topic: /projected_map\n Unreliable: false\n Use Timestamp: false\n Value: false\n - Alpha: 0.10000000149011612\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 3.774940013885498\n Min Value: -0.9468532800674438\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: AxisColor\n Decay Time: 11110000361472\n Enabled: true\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Min Color: 0; 0; 0\n Name: PointCloud2\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.02500000037252903\n Style: Flat Squares\n Topic: /shafter2/lio_sam/mapping/cloud_registered\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Class: rviz/Marker\n Enabled: false\n Marker Topic: /RRT_PATHS\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Class: UFOMap\n Enabled: false\n Information:\n \"# Inner Nodes\": 162 665\n \"# Leaf Nodes\": 0\n Resolution: 8 cm\n Size: 3,72 MiB\n Min. Depth: 0\n Name: UFOMap\n Occupancy Thresholds:\n Free (%): 50\n Occupied (%): 50\n Queue size: 10\n UFOMap Topic: /ufomap_mapping_server_node/map_depth_1\n Use BBX:\n BBX Frame: \n Maximum:\n X: 1000\n Y: 1000\n Z: 1000\n Minimum:\n X: -1000\n Y: -1000\n Z: -1000\n Value: false\n Value: false\n Voxel Rendering:\n Free:\n Alpha: 0.014999999664723873\n Coloring:\n Color: 0; 255; 0\n Factor: 0.800000011920929\n Value: Fixed\n Scale: 1\n Style: Boxes\n Value: false\n Occupied:\n Alpha: 0.019999999552965164\n Coloring:\n Color: 0; 0; 255\n Factor: 0.5\n Value: Z-Axis\n Scale: 1\n Style: Boxes\n Value: true\n Unknown:\n Alpha: 0.029999999329447746\n Coloring:\n Color: 255; 255; 255\n Factor: 0.800000011920929\n Value: Fixed\n Scale: 1\n Style: Flat Squares\n Value: false\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /CHOSEN_RRT_PATH_VISUALIZATION\n Name: Marker\n Namespaces:\n points: true\n Queue Size: 100\n Value: true\n - Class: rviz/Marker\n Enabled: false\n Marker Topic: /RRT_GOALS\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Alpha: 1\n Buffer Length: 4\n Class: rviz/Path\n Color: 206; 76; 230\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 0.20000000298023224\n Name: Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /shafter2/lio_sam/mapping/path\n Unreliable: false\n Value: true\n - Class: rviz/Marker\n Enabled: false\n Marker Topic: /RRT_NODES\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Class: rviz/Marker\n Enabled: false\n Marker Topic: /HITS\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 0; 255; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 0.10000000149011612\n Name: Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /chosen_path\n Unreliable: false\n Value: true\n - Alpha: 0.03500000014901161\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 3.8957245349884033\n Min Value: -1.0067167282104492\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: AxisColor\n Decay Time: 1e+09\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Min Color: 0; 0; 0\n Name: PointCloud2\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.009999999776482582\n Style: Flat Squares\n Topic: /shafter2/ouster/points\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n Enabled: true\n Global Options:\n Background Color: 0; 0; 0\n Default Light: true\n Fixed Frame: shafter2/world\n Frame Rate: 30\n Name: root\n Tools:\n - Class: rviz/Interact\n Hide Inactive Objects: true\n - Class: rviz/MoveCamera\n - Class: rviz/Select\n - Class: rviz/FocusCamera\n - Class: rviz/Measure\n - Class: rviz/SetInitialPose\n Theta std deviation: 0.2617993950843811\n Topic: /initialpose\n X std deviation: 0.5\n Y std deviation: 0.5\n - Class: rviz/SetGoal\n Topic: /move_base_simple/goal\n - Class: rviz/PublishPoint\n Single click: true\n Topic: /clicked_point\n Value: true\n Views:\n Current:\n Class: rviz/Orbit\n Distance: 93.15377807617188\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.05999999865889549\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Field of View: 0.7853981852531433\n Focal Point:\n X: -2.1300721168518066\n Y: -21.92489242553711\n Z: 1.850333333015442\n Focal Shape Fixed Size: true\n Focal Shape Size: 0.05000000074505806\n Invert Z Axis: false\n Name: Current View\n Near Clip Distance: 0.009999999776482582\n Pitch: 0.7697970271110535\n Target Frame: \n Yaw: 3.9076664447784424\n Saved: ~\nWindow Geometry:\n Displays:\n collapsed: true\n Height: 1163\n Hide Left Dock: true\n Hide Right Dock: true\n QMainWindow State: 000000ff00000000fd00000004000000000000015600000431fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730200000d200000025f00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d00000431000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061020000023e000001d800000280000001e0fb0000000a0049006d0061006700650000000000ffffffff0000000000000000000000010000010f00000431fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000431000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650000000000000007800000041800fffffffb0000000800540069006d00650100000000000004500000000000000000000007480000043100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000\n Selection:\n collapsed: false\n Time:\n collapsed: false\n Tool Properties:\n collapsed: false\n Views:\n collapsed: true\n Width: 1864\n X: 3416\n Y: 206\n" }, { "path": "rviz/follow_errt.rviz", "content": "Panels:\n - Class: rviz/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /Global Options1\n - /Grid1\n - /Axes1\n - /PointCloud22\n - /Marker2\n - /Marker2/Namespaces1\n - /UFOMap1\n - /UFOMap1/Voxel Rendering1\n - /UFOMap1/Voxel Rendering1/Occupied1\n - /UFOMap1/Voxel Rendering1/Free1\n - /Marker3\n - /Marker4\n - /Path1\n - /Marker5\n - /Marker5/Namespaces1\n - /Marker6\n - /Marker6/Namespaces1\n - /Path2\n - /PointCloud23\n - /Marker7\n - /Path4\n Splitter Ratio: 0.5\n Tree Height: 871\n - Class: rviz/Selection\n Name: Selection\n - Class: rviz/Tool Properties\n Expanded:\n - /2D Pose Estimate1\n - /2D Nav Goal1\n - /Publish Point1\n Name: Tool Properties\n Splitter Ratio: 0.5886790156364441\n - Class: rviz/Views\n Expanded:\n - /Current View1\n Name: Views\n Splitter Ratio: 0.5\n - Class: rviz/Time\n Name: Time\n SyncMode: 0\n SyncSource: PointCloud2\nPreferences:\n PromptSaveOnExit: true\nToolbars:\n toolButtonStyle: 2\nVisualization Manager:\n Class: \"\"\n Displays:\n - Alpha: 0.10000000149011612\n Cell Size: 1\n Class: rviz/Grid\n Color: 160; 160; 164\n Enabled: false\n Line Style:\n Line Width: 0.029999999329447746\n Value: Lines\n Name: Grid\n Normal Cell Count: 0\n Offset:\n X: 0\n Y: 0\n Z: 0\n Plane: XY\n Plane Cell Count: 100000\n Reference Frame: \n Value: false\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: Intensity\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Min Color: 0; 0; 0\n Name: PointCloud2\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.009999999776482582\n Style: Flat Squares\n Topic: /pixy/velodyne_points\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Class: rviz/Axes\n Enabled: true\n Length: 2\n Name: Axes\n Radius: 0.15000000596046448\n Reference Frame: hummingbird/base_link\n Show Trail: false\n Value: true\n - Class: rviz/MarkerArray\n Enabled: false\n Marker Topic: /frontier_cells_vis_array\n Name: MarkerArray\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /vis_next/visualization_marker\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Alpha: 0.699999988079071\n Class: rviz/Map\n Color Scheme: map\n Draw Behind: false\n Enabled: false\n Name: Map\n Topic: /projected_map\n Unreliable: false\n Use Timestamp: false\n Value: false\n - Alpha: 0.10000000149011612\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 3.016359329223633\n Min Value: 0.003368377685546875\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: AxisColor\n Decay Time: 11110000361472\n Enabled: true\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Min Color: 0; 0; 0\n Name: PointCloud2\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.014999999664723873\n Style: Boxes\n Topic: /hummingbird/velodyne_points\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Class: rviz/Marker\n Enabled: false\n Marker Topic: /PATH_TAKEN\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Class: UFOMap\n Enabled: false\n Information:\n \"# Inner Nodes\": 56 761\n \"# Leaf Nodes\": 142 152\n Resolution: 25 cm\n Size: 2,38 MiB\n Min. Depth: 0\n Name: UFOMap\n Occupancy Thresholds:\n Free (%): 50\n Occupied (%): 50\n Queue size: 10\n UFOMap Topic: /ufomap_mapping_server_node/map\n Use BBX:\n BBX Frame: \n Maximum:\n X: 1000\n Y: 1000\n Z: 1000\n Minimum:\n X: -1000\n Y: -1000\n Z: -1000\n Value: false\n Value: false\n Voxel Rendering:\n Free:\n Alpha: 0.014999999664723873\n Coloring:\n Color: 0; 255; 0\n Factor: 0.800000011920929\n Value: Fixed\n Scale: 1\n Style: Boxes\n Value: false\n Occupied:\n Alpha: 0.019999999552965164\n Coloring:\n Color: 0; 0; 255\n Factor: 0.5\n Value: Z-Axis\n Scale: 1\n Style: Boxes\n Value: true\n Unknown:\n Alpha: 0.029999999329447746\n Coloring:\n Color: 255; 255; 255\n Factor: 0.800000011920929\n Value: Fixed\n Scale: 1\n Style: Flat Squares\n Value: false\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /CHOSEN_RRT_PATH_VISUALIZATION\n Name: Marker\n Namespaces:\n points: true\n Queue Size: 100\n Value: true\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /RRT_GOALS\n Name: Marker\n Namespaces:\n points: true\n Queue Size: 100\n Value: true\n - Alpha: 1\n Buffer Length: 4\n Class: rviz/Path\n Color: 206; 76; 230\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 0.05000000074505806\n Name: Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /tracked_trajectory\n Unreliable: false\n Value: false\n - Class: rviz/Marker\n Enabled: false\n Marker Topic: /RRT_NODES\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /HITS\n Name: Marker\n Namespaces:\n points: true\n Queue Size: 100\n Value: true\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 0; 255; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 0.20000000298023224\n Name: Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 170; 255; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /command_path\n Unreliable: false\n Value: true\n - Alpha: 0.18000000715255737\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 15.449999809265137\n Min Value: -0.75\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: AxisColor\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Min Color: 0; 0; 0\n Name: PointCloud2\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 2\n Size (m): 0.20000000298023224\n Style: Points\n Topic: /octomap_point_cloud_centers\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 25; 255; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.029999999329447746\n Name: Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: \"\"\n Unreliable: false\n Value: true\n - Class: rviz/Marker\n Enabled: false\n Marker Topic: /UNKNOWN_NODES\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 204; 41; 204\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 0.10000000149011612\n Name: Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /state_history\n Unreliable: false\n Value: true\n Enabled: true\n Global Options:\n Background Color: 0; 0; 0\n Default Light: true\n Fixed Frame: world\n Frame Rate: 30\n Name: root\n Tools:\n - Class: rviz/Interact\n Hide Inactive Objects: true\n - Class: rviz/MoveCamera\n - Class: rviz/Select\n - Class: rviz/FocusCamera\n - Class: rviz/Measure\n - Class: rviz/SetInitialPose\n Theta std deviation: 0.2617993950843811\n Topic: /initialpose\n X std deviation: 0.5\n Y std deviation: 0.5\n - Class: rviz/SetGoal\n Topic: /move_base_simple/goal\n - Class: rviz/PublishPoint\n Single click: true\n Topic: /clicked_point\n Value: true\n Views:\n Current:\n Class: rviz/Orbit\n Distance: 48.83968734741211\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.05999999865889549\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Field of View: 0.7853981852531433\n Focal Point:\n X: 10.605751991271973\n Y: -1.5589395761489868\n Z: -15.176371574401855\n Focal Shape Fixed Size: true\n Focal Shape Size: 0.05000000074505806\n Invert Z Axis: false\n Name: Current View\n Near Clip Distance: 0.009999999776482582\n Pitch: 0.8597971796989441\n Target Frame: hummingbird/base_link\n Yaw: 3.1526098251342773\n Saved: ~\nWindow Geometry:\n Displays:\n collapsed: true\n Height: 1163\n Hide Left Dock: true\n Hide Right Dock: true\n QMainWindow State: 000000ff00000000fd000000040000000000000156000003f2fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000005fb0000025f00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000003f2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061020000023e000001d800000280000001e0fb0000000a0049006d0061006700650000000000ffffffff0000000000000000000000010000010f000003d7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003d7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000075400000039fc0100000002fb0000000800540069006d0065010000000000000754000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000754000003f200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000\n Selection:\n collapsed: false\n Time:\n collapsed: false\n Tool Properties:\n collapsed: false\n Views:\n collapsed: true\n Width: 1876\n X: 44\n Y: 0\n" }, { "path": "rviz/follow_top_errt.rviz", "content": "Panels:\n - Class: rviz/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /Global Options1\n - /Grid1\n - /Axes1\n - /PointCloud22\n - /Marker2\n - /Marker2/Namespaces1\n - /UFOMap1\n - /UFOMap1/Voxel Rendering1\n - /UFOMap1/Voxel Rendering1/Occupied1\n - /UFOMap1/Voxel Rendering1/Free1\n - /Marker3\n - /Marker4\n - /Path1\n - /Marker5\n - /Marker5/Namespaces1\n - /Marker6\n - /Marker6/Namespaces1\n - /Path2\n - /PointCloud23\n - /Marker7\n - /Path4\n Splitter Ratio: 0.5\n Tree Height: 871\n - Class: rviz/Selection\n Name: Selection\n - Class: rviz/Tool Properties\n Expanded:\n - /2D Pose Estimate1\n - /2D Nav Goal1\n - /Publish Point1\n Name: Tool Properties\n Splitter Ratio: 0.5886790156364441\n - Class: rviz/Views\n Expanded:\n - /Current View1\n Name: Views\n Splitter Ratio: 0.5\n - Class: rviz/Time\n Name: Time\n SyncMode: 0\n SyncSource: PointCloud2\nPreferences:\n PromptSaveOnExit: true\nToolbars:\n toolButtonStyle: 2\nVisualization Manager:\n Class: \"\"\n Displays:\n - Alpha: 0.10000000149011612\n Cell Size: 1\n Class: rviz/Grid\n Color: 160; 160; 164\n Enabled: false\n Line Style:\n Line Width: 0.029999999329447746\n Value: Lines\n Name: Grid\n Normal Cell Count: 0\n Offset:\n X: 0\n Y: 0\n Z: 0\n Plane: XY\n Plane Cell Count: 100000\n Reference Frame: \n Value: false\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: Intensity\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Min Color: 0; 0; 0\n Name: PointCloud2\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.009999999776482582\n Style: Flat Squares\n Topic: /pixy/velodyne_points\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Class: rviz/Axes\n Enabled: true\n Length: 2\n Name: Axes\n Radius: 0.15000000596046448\n Reference Frame: hummingbird/base_link\n Show Trail: false\n Value: true\n - Class: rviz/MarkerArray\n Enabled: false\n Marker Topic: /frontier_cells_vis_array\n Name: MarkerArray\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /vis_next/visualization_marker\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Alpha: 0.699999988079071\n Class: rviz/Map\n Color Scheme: map\n Draw Behind: false\n Enabled: false\n Name: Map\n Topic: /projected_map\n Unreliable: false\n Use Timestamp: false\n Value: false\n - Alpha: 0.10000000149011612\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 8.72201156616211\n Min Value: -0.25360965728759766\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: AxisColor\n Decay Time: 11110000361472\n Enabled: true\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Min Color: 0; 0; 0\n Name: PointCloud2\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.014999999664723873\n Style: Boxes\n Topic: /hummingbird/velodyne_points\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Class: rviz/Marker\n Enabled: false\n Marker Topic: /PATH_TAKEN\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Class: UFOMap\n Enabled: false\n Information:\n \"# Inner Nodes\": 56 761\n \"# Leaf Nodes\": 142 152\n Resolution: 25 cm\n Size: 2,38 MiB\n Min. Depth: 0\n Name: UFOMap\n Occupancy Thresholds:\n Free (%): 50\n Occupied (%): 50\n Queue size: 10\n UFOMap Topic: /ufomap_mapping_server_node/map\n Use BBX:\n BBX Frame: \n Maximum:\n X: 1000\n Y: 1000\n Z: 1000\n Minimum:\n X: -1000\n Y: -1000\n Z: -1000\n Value: false\n Value: false\n Voxel Rendering:\n Free:\n Alpha: 0.014999999664723873\n Coloring:\n Color: 0; 255; 0\n Factor: 0.800000011920929\n Value: Fixed\n Scale: 1\n Style: Boxes\n Value: false\n Occupied:\n Alpha: 0.019999999552965164\n Coloring:\n Color: 0; 0; 255\n Factor: 0.5\n Value: Z-Axis\n Scale: 1\n Style: Boxes\n Value: true\n Unknown:\n Alpha: 0.029999999329447746\n Coloring:\n Color: 255; 255; 255\n Factor: 0.800000011920929\n Value: Fixed\n Scale: 1\n Style: Flat Squares\n Value: false\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /CHOSEN_RRT_PATH_VISUALIZATION\n Name: Marker\n Namespaces:\n points: true\n Queue Size: 100\n Value: true\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /RRT_GOALS\n Name: Marker\n Namespaces:\n points: true\n Queue Size: 100\n Value: true\n - Alpha: 1\n Buffer Length: 4\n Class: rviz/Path\n Color: 206; 76; 230\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 0.05000000074505806\n Name: Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /tracked_trajectory\n Unreliable: false\n Value: false\n - Class: rviz/Marker\n Enabled: false\n Marker Topic: /RRT_NODES\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /HITS\n Name: Marker\n Namespaces:\n points: true\n Queue Size: 100\n Value: true\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 0; 255; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 0.20000000298023224\n Name: Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 170; 255; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /command_path\n Unreliable: false\n Value: true\n - Alpha: 0.18000000715255737\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 15.449999809265137\n Min Value: -0.75\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: AxisColor\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Min Color: 0; 0; 0\n Name: PointCloud2\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 2\n Size (m): 0.20000000298023224\n Style: Points\n Topic: /octomap_point_cloud_centers\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 25; 255; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.029999999329447746\n Name: Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: \"\"\n Unreliable: false\n Value: true\n - Class: rviz/Marker\n Enabled: false\n Marker Topic: /UNKNOWN_NODES\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 204; 41; 204\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 0.10000000149011612\n Name: Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /state_history\n Unreliable: false\n Value: true\n Enabled: true\n Global Options:\n Background Color: 0; 0; 0\n Default Light: true\n Fixed Frame: world\n Frame Rate: 30\n Name: root\n Tools:\n - Class: rviz/Interact\n Hide Inactive Objects: true\n - Class: rviz/MoveCamera\n - Class: rviz/Select\n - Class: rviz/FocusCamera\n - Class: rviz/Measure\n - Class: rviz/SetInitialPose\n Theta std deviation: 0.2617993950843811\n Topic: /initialpose\n X std deviation: 0.5\n Y std deviation: 0.5\n - Class: rviz/SetGoal\n Topic: /move_base_simple/goal\n - Class: rviz/PublishPoint\n Single click: true\n Topic: /clicked_point\n Value: true\n Views:\n Current:\n Class: rviz/Orbit\n Distance: 50.55931854248047\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.05999999865889549\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Field of View: 0.7853981852531433\n Focal Point:\n X: 2.360151767730713\n Y: -4.075262546539307\n Z: -13.692914962768555\n Focal Shape Fixed Size: true\n Focal Shape Size: 0.05000000074505806\n Invert Z Axis: false\n Name: Current View\n Near Clip Distance: 0.009999999776482582\n Pitch: 1.5697963237762451\n Target Frame: hummingbird/base_link\n Yaw: 4.682620048522949\n Saved: ~\nWindow Geometry:\n Displays:\n collapsed: true\n Height: 1163\n Hide Left Dock: true\n Hide Right Dock: true\n QMainWindow State: 000000ff00000000fd000000040000000000000156000003f2fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000005fb0000025f00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000003f2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061020000023e000001d800000280000001e0fb0000000a0049006d0061006700650000000000ffffffff0000000000000000000000010000010f000003f2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003f2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000075400000039fc0100000002fb0000000800540069006d0065010000000000000754000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000754000003f200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000\n Selection:\n collapsed: false\n Time:\n collapsed: false\n Tool Properties:\n collapsed: false\n Views:\n collapsed: true\n Width: 1876\n X: 44\n Y: 0\n" }, { "path": "scripts/errt.yml", "content": "# ~/.tmuxinator/errt.yml \n\nname: errt\nroot: ~/\nwindows:\n - stage_main:\n layout: even-horizontal \n panes:\n - ufomap:\n - sleep 2 \n - mon launch errt server.launch \n - errt:\n - sleep 6 \n - mon launch errt rrt.launch\n\n - publishers:\n # layout: main-vertical \n layout: tiled \n panes:\n - rotors:\n - sleep 1 \n - roslaunch rotors_gazebo errt_mav.launch \n - takeoff:\n - sleep 2 \n - controller: \n - cd src/ExplorationRRT/controller\n - sleep 4\n - python3 errt_nmpc.py\n - roscore: \n - roscore \n" }, { "path": "scripts/nmpc_test.py", "content": "import opengen as og\nimport casadi.casadi as cs\n#import matplotlib.pyplot as plt\nimport numpy as np\nimport math \nfrom trajectory import trajectory\nmng = og.tcp.OptimizerTcpManager('MAV/rrt')\nmng.start()\nN = 50\ndt = 1.0 / 2\nx0 = [0,0,0,0,0,0,0,0]\nxref = [0,0,0]*N\n\nfor i in range(0,N):\n xref[3*i] = i*0.5\n xref[3*i+1] = i*0.5\n xref[3*i+2] = 0\n\n#obsdata = (0.0,0.0,1.0,0.0, 0.0, 0.0)\n#obsdata = [0]*N*3\nz0 = x0 + xref + [dt]\nprint(z0);\nmng.ping()\nsolution = mng.call(z0, initial_guess=[9.81,0,0]*(N), buffer_len = 4*4096)\n#print(solution['solution'])\nsolution_data = solution.get();\n#print(error_msg);\nustar = solution_data.solution\nprint(ustar)\n[p_traj, v_traj, cost] = trajectory(x0, ustar, 50, 0.5, xref)\n#print(len(p_traj))\nprint(cost)\n\npx = [0]*N\npy = [0]*N\nfor i in range(0,N):\n px[i] = p_traj[i][0]\n py[i] = p_traj[i][1]\n\npx_ref = [0]*N\t\npy_ref = [0]*N\nfor i in range(0,N):\n px_ref[i] = xref[3*i]\n py_ref[i] = xref[3*i+1]\n\nimport matplotlib.pyplot as plt\nplt.plot(px,py,px_ref,py_ref)\nplt.show()\n\nmng.kill()\n" }, { "path": "src/Trajectory.cpp", "content": "std::tuple, double, std::list> trajectory(std::list x, double* u, double N, double dt, std::list nmpc_ref){\n // Based on the initial condition and optimized trajectory u, computed the path as (x,y,z).\n // Calculate the dynamic costs based on selected weights \n double ns = 8;\n std::list p_traj{};\n std::list v_traj{};\n double cost = 0;\n // Weight matrices\n std::list Qx = {5,5,5, 0, 0,0, 5, 5};\n // P = 2*Qx; #final state weight\n std::list Ru = {15,15,15}; // input weights\n std::list Rd = {10, 10, 10}; // input rate weights\n // print(x, u, N, dt)\n std::list u_old = {9.81,0.0,0.0};\n std::list u_ref = {9.81,0.0,0.0};\n for(int i = 0; i < N; i++){\n // State costs\n // std::list x_ref = nmpc_ref[(ns*i):(ns*i+ns)];\n // #print(x_ref)\n // Setting up itterators\n std::list::iterator Qx_itterator = Qx.begin();\n //std::advance(Qx_itterator, i);\n std::list::iterator x_itterator = x.begin();\n //std::advance(x_itterator, i);\n std::list::iterator x_ref_itterator = nmpc_ref.begin();\n // std::advance(x_ref_itterator, i*ns);\n \n for(int j = 0; j < 8; j++){\n // std::cout << (*Qx_itterator) << \", \" << (*x_itterator) << \", \" << (*x_ref_itterator) << std::endl;\n if(i < 3){\n cost = cost + (*Qx_itterator) * pow((*x_itterator) - (*x_ref_itterator), 2);\n }else{\n cost = cost + (*Qx_itterator) * pow((*x_itterator), 2);\n }\n Qx_itterator++;\n x_itterator++;\n x_ref_itterator++;\n }\n //std::cout << \"\\n\" << std::endl;\n //std::cout << \"this is cost: \" << cost << std::endl;\n //cost = cost + pow((*Qx_itterator) * (*x_itterator - *x_ref_itterator), 2) + Qx[1]*(x[1]-x_ref[i + 1])**2 + Qx[2]*(x[2]-x_ref[i + 2])**2 + Qx[3]*(x[3]-x_ref[i + 3])**2 + Qx[4]*(x[4]-x_ref[i + 4])**2 + Qx[5]*(x[5]-x_ref[i + 5])**2 + Qx[6]*(x[6]-x_ref[i + 6])**2 + Qx[7]*(x[7]-x_ref[i + 7])**2; // State weights\n // Input Cost\n \n //Setting up itterators\n std::list::iterator Ru_itterator = Ru.begin();\n std::list::iterator Rd_itterator = Rd.begin();\n //std::list::iterator u_n_itterator = u.begin();\n //std::advance(u_n_itterator, 3 * i);\n std::list::iterator u_ref_itterator = u_ref.begin();\n std::list::iterator u_old_itterator = u_old.begin();\n \n for(int j = 0; j < 3; j++){\n // std::cout << (*Rd_itterator) << \", \" << u[3*i+j] << \", \" << *u_old_itterator << std::endl;\n cost = cost + *Ru_itterator * pow(u[3*i+j] - (*u_ref_itterator), 2);\n cost = cost + *Rd_itterator * pow(u[3*i+j] - *u_old_itterator, 2);\n Ru_itterator++;\n u_old_itterator++;\n Rd_itterator++;\n u_ref_itterator++;\n }\n //std::cout << \"this is cost\" << cost << std::endl;\n u_old = {u[3*i], u[3*i+1], u[3*i+2]};\n //u_n = u[(3*i):3*i+3];\n //cost += Ru[0]*(u_n[0] - u_ref[0])**2 + Ru[1]*(u_n[1] - u_ref[1])**2 + Ru[2]*(u_n[2] - u_ref[2])**2; // Input weights\n //cost += Rd[0]*(u_n[0] - u_old[0])**2 + Rd[1]*(u_n[1] - u_old[1])**2 + Rd[2]*(u_n[2] - u_old[2])**2; // Input rate weights\n //u_old = u_n;\n // x_hist = x_hist + [x];\n \n x_itterator = x.begin();\n std::list::iterator x2_itterator = x.begin();\n std::advance(x2_itterator, 3);\n //std::advance(u_n_itterator, -2);\n for(int j = 0; j < 3; j++){\n *x_itterator = *x_itterator + dt * *x2_itterator;\n x_itterator++;\n x2_itterator++;\n }\n std::list::iterator x3_itterator = x.begin();\n std::advance(x3_itterator, 7); // x[7]\n *x_itterator = *x_itterator + dt * (sin(*x3_itterator) * cos(*x2_itterator) * u[3*i] - 0.5 * (*x_itterator));\n // std::cout << \"THIS IS IMPORTANT: \" << *x_itterator << std::endl;\n x_itterator++; // x[4]\n *x_itterator = *x_itterator + dt * (-sin(*x2_itterator) * u[3*i] - 0.5 * (*x_itterator));\n x_itterator++; // x[5]\n // std::cout << \"THIS IS IMPORTANT: \"<< *x_itterator << \", \" << *x3_itterator << \", \" << *x2_itterator << \", \" << u[3*i] << std::endl;\n *x_itterator = *x_itterator + dt * (cos(*x3_itterator) * cos(*x2_itterator) * u[3*i] - 0.5 * *x_itterator - 9.81);\n x_itterator++;\n //u_n_itterator++;\n *x_itterator = *x_itterator + dt * ((1.0 / 0.8) * (u[3*i + 1] - *x_itterator));\n x_itterator++;\n //u_n_itterator++;\n *x_itterator = *x_itterator + dt * ((1.0 / 0.8) * (u[3*i + 2] - *x_itterator));\n /*\n p_hist = p_hist + [[x[0],x[1],x[2]]];*/\n x_itterator = x.begin();\n p_traj.push_back(*x_itterator);\n x_itterator++;\n p_traj.push_back(*x_itterator);\n x_itterator++; \n p_traj.push_back(*x_itterator);\n x_itterator++;\n v_traj.push_back(*x_itterator);\n x_itterator++;\n v_traj.push_back(*x_itterator);\n x_itterator++; \n v_traj.push_back(*x_itterator);\n }\n // std::cout << \"\\n\" << std::endl;\n //int schme = 0;\n // std::cout << \"This is p_hist:\" << std::endl;\n /*for(std::list::iterator x5_itterator = p_hist.begin(); x5_itterator != p_hist.end(); x5_itterator++){\n if(schme%3 == 0 and schme != 0){\n std::cout << \"\\n\" << std::endl;\n }\n std::cout << *x5_itterator << std::endl;\n schme++;\n }*\n /*schme = 0;\n std::cout << \"This is x_hist:\\n\" << std::endl;\n for(std::list::iterator x4_itterator = x_hist.begin(); x4_itterator != x_hist.end(); x4_itterator++){\n if(schme%8 == 0 and schme != 0){\n std::cout << \"\\n\" << std::endl;\n }\n std::cout << *x4_itterator << std::endl;\n schme++;\n }*/\n // std::cout << cost << std::endl;\n // std::cout << x_hist << std::endl;\n // std::cout << \"skibidibap: \" << cost << std::endl;\n return std::make_tuple(v_traj, cost, p_traj);\n}\n" }, { "path": "src/errt.cpp", "content": "#include \"std_msgs/Float64MultiArray.h\"\n#include \"std_msgs/Int64.h\"\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"std_msgs/Int64.h\"\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\nusing namespace std::chrono;\nusing namespace std;\nusing namespace std::this_thread;\ndouble planning_depth_ = 0;\n\ndouble sensor_range_;\n// helper obb function //\n\nufo::geometry::OBB makeOBB(ufo::math::Vector3 source, ufo::math::Vector3 goal,\n float radius) {\n ufo::math::Vector3 direction = goal - source;\n ufo::math::Vector3 center = source + (direction / 2.0);\n double distance = direction.norm();\n direction /= distance;\n double yaw = -atan2(direction[1], direction[0]);\n double pitch = -asin(direction[2]);\n double roll = 0; // TODO: Fix\n ufo::geometry::OBB obb(center,\n ufo::map::Point3(distance / 2.0, radius, radius),\n ufo::math::Quaternion(roll, pitch, yaw));\n\n return obb;\n}\n\n// --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------\n// // Structs\n\nstruct rrtSolverStatus rrt_solve(struct rrtCache *instance, double *u,\n const double *params, const double *y0,\n const double *c0);\n\nstruct rrtCache *rrt_new(void);\n\n// The node struct.\n// This struct represents the different kinds of points in the rrt, this\n// includes: the nodes in the rrt tree, the goals and the paths.\nstruct node {\npublic:\n ufo::math::Vector3 *point;\n node *myParent = nullptr;\n double distanceToParent = -1;\n std::vector unknowns_in_sight_;\n std::list myChilds{};\n std::list myParents{};\n std::list myPath{};\n std::list myHits{};\n\n void addParents() {\n\n myParents.clear();\n node *temp_parent = nullptr;\n\n if (myParent != nullptr) {\n\n temp_parent = myParent;\n }\n\n while (temp_parent != nullptr) {\n\n myParents.push_back(temp_parent);\n temp_parent = temp_parent->myParent;\n\n if (temp_parent == nullptr) {\n break;\n }\n }\n }\n\n double distanceToGoal;\n node(float x, float y, float z) { point = new ufo::math::Vector3(x, y, z); }\n node(ufo::math::Vector3 *givenPoint) {\n point = new ufo::math::Vector3(givenPoint->x(), givenPoint->y(),\n givenPoint->z());\n }\n ufo::math::Vector3 *getNodePointer() { return point; }\n void addChild(node *newChild) { myChilds.push_back(newChild); }\n void addParent(node *newParent) { myParent = newParent; }\n void changeDistanceToGoal(double newDistance) {\n distanceToGoal = newDistance;\n }\n void changeDistanceToParent() {\n if (myParent != nullptr and distanceToParent != -1) {\n distanceToParent = sqrt(pow(point->x() - myParent->point->x(), 2) +\n pow(point->y() - myParent->point->y(), 2) +\n pow(point->z() - myParent->point->z(), 2));\n } else {\n distanceToParent = 0;\n }\n }\n double sumDistance() {\n changeDistanceToParent();\n double myDistance = 0;\n if (myParent != nullptr) {\n myDistance = myParent->sumDistance();\n changeDistanceToParent();\n }\n if (myParent == nullptr) {\n return 0;\n }\n return (myDistance + distanceToParent);\n }\n\n void getPath(std::list *givenPath) {\n if (myParent != nullptr) {\n myParent->getPath(givenPath);\n if (myParent->myParent != nullptr) {\n if (myParent->point->x() != point->x() or\n myParent->point->y() != point->y() or\n myParent->point->z() != point->z()) {\n givenPath->push_back(new node(myParent->point->x(),\n myParent->point->y(),\n myParent->point->z()));\n }\n }\n }\n if (myParent == nullptr) {\n return;\n }\n return;\n }\n\n void\n extractUnknownVoxelsInsideFrustum(ufo::map::OccupancyMapColor const &map) {\n\n unknowns_in_sight_.clear();\n ufo::geometry::Sphere sphere(*point, 4);\n\n std::vector unknown_voxels;\n\n for (auto it = map.beginLeaves(sphere, false, false, true, false,\n planning_depth_),\n it_end = map.endLeaves();\n it != it_end; ++it) {\n if (it.isUnknown()) {\n\n ufo::math::Vector3 free_voxel(it.getX(), it.getY(), it.getZ());\n unknown_voxels.push_back(free_voxel);\n }\n }\n\n double hFOV = 2 * M_PI;\n double vFOV = M_PI / 4;\n double range = 4;\n\n // TODO @ can use OMP to parallalize the following 3 loops\n\n for (ufo::math::Vector3 voxel : unknown_voxels) {\n\n ufo::math::Vector3 toPoint = voxel - *point;\n double h_angle = std::atan2(toPoint.y(), toPoint.x());\n double v_angle = std::atan2(toPoint.z(), toPoint.norm());\n\n if (std::abs(h_angle) <= hFOV / 2 and std::abs(v_angle) <= vFOV / 2 and\n toPoint.norm() <= range) {\n\n ufo::geometry::LineSegment myLine(*point, voxel);\n if (!isInCollision(map, myLine, true, false, false, planning_depth_)) {\n unknowns_in_sight_.push_back(voxel);\n }\n }\n }\n }\n\n int findInformationGain(float v_local_, float givenHorizontal,\n float givenVertical, float givenMin, float givenMax,\n ufo::map::OccupancyMapColor const &map,\n bool excludePath, bool findAnyInfo) {\n\n if (myParents.empty()) {\n return 0;\n }\n\n double dist = 0;\n node *check_node = nullptr;\n check_node = *(myParents.begin());\n bool check_dist_pass = false;\n\n for (auto i = myParents.begin(); i != myParents.end(); i++) {\n\n if (i != myParents.begin()) {\n\n ufo::math::Vector3 v1 = *((*i)->point);\n ufo::math::Vector3 v2 = *(check_node->point);\n dist = (v1 - v2).norm();\n if (dist > 4) {\n check_node = *i;\n check_dist_pass = true;\n } else {\n check_dist_pass = false;\n }\n }\n\n if (check_dist_pass) {\n\n ufo::geometry::Sphere sphere(*((*i)->point), sensor_range_);\n\n std::list unknown_voxels;\n\n for (auto it = map.beginLeaves(sphere, false, false, true, false,\n planning_depth_),\n it_end = map.endLeaves();\n it != it_end; ++it) {\n if (it.isUnknown()) {\n\n ufo::math::Vector3 unknown_voxel(it.getX(), it.getY(), it.getZ());\n unknown_voxels.push_back(unknown_voxel);\n }\n }\n\n double hFOV = 2 * M_PI;\n double vFOV = M_PI / 4;\n double range = sensor_range_;\n\n for (ufo::math::Vector3 voxel : unknown_voxels) {\n\n ufo::math::Vector3 toPoint = voxel - *((*i)->point);\n double h_angle = std::atan2(toPoint.y(), toPoint.x());\n double v_angle = std::atan2(toPoint.z(), toPoint.norm());\n\n if (std::abs(h_angle) <= hFOV / 2 and\n std::abs(v_angle) <= vFOV / 2 and toPoint.norm() <= range) {\n\n ufo::geometry::Sphere unk_sphere(voxel, 2);\n ufo::geometry::LineSegment myLine(*((*i)->point), voxel);\n if (!isInCollision(map, unk_sphere, true, false, false,\n planning_depth_) and\n isInCollision(map, unk_sphere, false, true, false,\n planning_depth_) and\n !isInCollision(map, myLine, true, false, false,\n planning_depth_)) {\n myHits.push_back(voxel);\n }\n }\n }\n }\n }\n\n std::list myTotalHits{};\n addHits(&myTotalHits);\n int hits = myTotalHits.size();\n return hits;\n }\n\n void clearInformationGain() {\n myHits.clear();\n if (myParent != nullptr) {\n myParent->clearInformationGain();\n }\n }\n void addHits(std::list *hitList) {\n bool add = true;\n for (auto it = myHits.begin(), it_end = myHits.end(); it != it_end; ++it) {\n for (auto it2 = hitList->begin(), it_end2 = hitList->end();\n it2 != it_end2; ++it2) {\n if (it->x() == it2->x() and it->y() == it2->y() and\n it->z() == it2->z()) {\n add = false;\n break;\n }\n }\n if (add) {\n hitList->push_back(*it);\n }\n add = true;\n }\n if (myParent != nullptr) {\n myParent->addHits(hitList);\n }\n };\n\n bool findPathImprovement(struct node *targetNode,\n ufo::map::OccupancyMapColor const &map,\n float givenDistance, float givenRadious,\n auto pathImprovement_start, int givenMax) {\n bool improvementFound;\n if (targetNode == this and myParent == nullptr) {\n return true;\n }\n if (myParent != nullptr) {\n improvementFound = myParent->findPathImprovement(\n targetNode, map, givenDistance, givenRadious, pathImprovement_start,\n givenMax);\n auto pathImprovement_stop = high_resolution_clock::now();\n auto pathImprovement_total =\n duration_cast(pathImprovement_stop -\n pathImprovement_start)\n .count();\n if (pathImprovement_total > givenMax) {\n return true;\n }\n } else {\n ufo::geometry::LineSegment myLine(*(targetNode->point), *point);\n if (!isInCollision(map, myLine, true, false, true, planning_depth_)) {\n ufo::math::Vector3 newVector(targetNode->point->x() - point->x(),\n targetNode->point->y() - point->y(),\n targetNode->point->z() - point->z());\n float distance = newVector.norm();\n float itterations = (distance / givenRadious);\n float part = givenRadious / distance;\n float xStep = (targetNode->point->x() - point->x()) * part;\n float yStep = (targetNode->point->y() - point->y()) * part;\n float zStep = (targetNode->point->z() - point->z()) * part;\n for (int i = 1; i < itterations; i++) {\n auto pathImprovement_stop = high_resolution_clock::now();\n auto pathImprovement_total =\n duration_cast(pathImprovement_stop -\n pathImprovement_start)\n .count();\n if (pathImprovement_total > givenMax) {\n return true;\n }\n ufo::math::Vector3 newVector =\n ufo::math::Vector3(point->x() + i * xStep, point->y() + i * yStep,\n point->z() + i * zStep);\n ufo::geometry::Sphere new_sphere(newVector, givenRadious);\n if (isInCollision(map, new_sphere, true, false, true,\n planning_depth_)) {\n return false;\n }\n }\n targetNode->addParent(this);\n return true;\n } else {\n return false;\n };\n }\n if (!improvementFound) {\n ufo::geometry::LineSegment myLine(*(targetNode->point), *point);\n if (!isInCollision(map, myLine, true, false, true, planning_depth_)) {\n ufo::math::Vector3 newVector(targetNode->point->x() - point->x(),\n targetNode->point->y() - point->y(),\n targetNode->point->z() - point->z());\n float distance = newVector.norm();\n float itterations = (distance / givenRadious);\n float part = givenRadious / distance;\n float xStep = (targetNode->point->x() - point->x()) * part;\n float yStep = (targetNode->point->y() - point->y()) * part;\n float zStep = (targetNode->point->z() - point->z()) * part;\n for (int i = 1; i < itterations; i++) {\n auto pathImprovement_stop = high_resolution_clock::now();\n auto pathImprovement_total =\n duration_cast(pathImprovement_stop -\n pathImprovement_start)\n .count();\n if (pathImprovement_total > givenMax) {\n return true;\n }\n ufo::math::Vector3 newVector =\n ufo::math::Vector3(point->x() + i * xStep, point->y() + i * yStep,\n point->z() + i * zStep);\n ufo::geometry::Sphere new_sphere(newVector, givenRadious);\n if (isInCollision(map, new_sphere, true, false, true,\n planning_depth_)) {\n return false;\n }\n }\n targetNode->addParent(this);\n improvementFound =\n findPathImprovement(this, map, givenDistance, givenRadious,\n pathImprovement_start, givenMax);\n return true;\n } else {\n return false;\n }\n } else {\n return true;\n }\n }\n\n void readyForDeletion() { delete point; }\n\n bool isInCollision(ufo::map::OccupancyMapColor const &map,\n ufo::geometry::BoundingVar const &bounding_volume,\n bool occupied_space = true, bool free_space = false,\n bool unknown_space = false,\n ufo::map::DepthType min_depth = planning_depth_) {\n // Iterate through all leaf nodes that intersects the bounding volume\n for (auto it = map.beginLeaves(bounding_volume, occupied_space, free_space,\n unknown_space, false, min_depth),\n it_end = map.endLeaves();\n it != it_end; ++it) {\n // Is in collision since a leaf node intersects the bounding volume.\n return true;\n }\n // No leaf node intersects the bounding volume.\n return false;\n }\n};\n\n// End of structs\n// --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------\n// // Variables\n\n// Variables\n\nint n_seq_;\ndouble dt_ = 1;\n\nint number_of_nodes_;\nint number_of_goals_;\nint number_of_iterations_;\nint nmpc_horizon_;\nint itterations;\nint advance_index = 0;\nint path_improvement_max_;\nfloat dist_nodes_; // 1.0;\nfloat dist_goals_;\nfloat min_dist_to_goal_;\nfloat v_local_;\nfloat SCALER_X = 0;\nfloat SCALER_Y = 0;\nfloat SCALER_Z = 0;\nfloat position_x = 0;\nfloat position_y = 0;\nfloat position_z = 0;\nfloat velocity_x = 0;\nfloat velocity_y = 0;\nfloat velocity_z = 0;\nfloat lowest_x;\nfloat lowest_y;\nfloat lowest_z;\nfloat highest_x;\nfloat highest_y;\nfloat highest_z;\nfloat GLOBAL_STRATEGY_THRESHOLD;\nfloat GLOBAL_PATH_THRESHOLD;\nfloat initialGoalInfo = 0.0;\nbool run_by_nodes_;\nbool start_from_waypoint_;\nbool map_received = false;\nbool RRT_created = false;\nbool GOALS_generated = false;\nbool position_received = false;\nbool fetched_path = false;\nbool newPath = false;\nbool allowNewPath = true;\nbool recoveryUnderway = false;\nbool visualizeNewData = true;\ndouble min_sensor_range_;\ndouble sensor_vertical_fov_;\ndouble SENSOR_HORIZONTAL = 0.78;\ndouble k_info_;\ndouble k_dist_;\ndouble k_u_;\ndouble recalc_dist_;\ndouble path_update_dist_;\ndouble nmpc_dt_;\ndouble robot_size_;\ndouble min_info_goal_;\ndouble goal_sensor_range_;\ndouble goal_connect_dist_;\n\ndouble position_tracking_weight_x_;\ndouble position_tracking_weight_y_;\ndouble position_tracking_weight_z_;\ndouble angle_weight_roll_;\ndouble angle_weight_pitch_;\ndouble input_weight_thrust_;\ndouble input_weight_roll_;\ndouble input_weight_pitch_;\ndouble input_rate_weight_thrust_;\ndouble input_rate_weight_roll_;\ndouble input_rate_weight_pitch_;\ndouble initial_x_;\ndouble initial_y_;\ndouble initial_z_;\ndouble roll = 0;\ndouble pitch = 0;\ndouble yaw = 0;\ndouble totalCost = std::numeric_limits::max();\ndouble totalDistance = -1;\nstring map_frame_id_;\nnode *goalNode = nullptr;\nnode *reserveGoalNode = nullptr;\nnode *currentTarget;\n\n// Ufomap\nufo::map::OccupancyMapColor myMap(0.1);\n\n// Lists\nstd::list RRT_TREE{};\nstd::list CHOSEN_PATH{};\nstd::list CHOSEN_PATH_VREF{};\nstd::list::iterator vref_itterator;\nstd::list ALL_PATH{};\nstd::list myGoals{};\nstd::list myReserveGoals{};\nstd::list hits{};\nstd::list VISITED_POINTS{};\nstd::list::iterator path_itterator;\n\nros::Publisher m_trajectory_Publisher;\nros::Publisher m_command_Path_Publisher;\nnav_msgs::Path command_path;\ntrajectory_msgs::MultiDOFJointTrajectory trajectory_array_;\n\n// End of variables\n// --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------\n// Functions\n\nEigen::Quaternion Euler2Quaternion(Eigen::Vector3d v) {\n float roll = v.x();\n float pitch = v.y();\n float yaw = v.z();\n\n Eigen::Quaternion q;\n\n q = Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitX()) *\n Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) *\n Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ());\n\n return q;\n}\n\n// Fills in the space between nodes with new nodes.\n// This allows for simpler path building.\nvoid linSpace(node *givenNode, float givenDistance) {\n\n ufo::math::Vector3 newVector(\n givenNode->myParent->point->x() - givenNode->point->x(),\n givenNode->myParent->point->y() - givenNode->point->y(),\n givenNode->myParent->point->z() - givenNode->point->z());\n float distance = newVector.norm();\n float itterations = (distance / givenDistance);\n float part = givenDistance / distance;\n float xStep =\n (givenNode->myParent->point->x() - givenNode->point->x()) * part;\n float yStep =\n (givenNode->myParent->point->y() - givenNode->point->y()) * part;\n float zStep =\n (givenNode->myParent->point->z() - givenNode->point->z()) * part;\n node *parent = givenNode->myParent;\n node *nextNode = givenNode->myParent;\n for (int i = 1; i < itterations; i++) {\n node *newPoint = new node(givenNode->myParent->point->x() - i * xStep,\n givenNode->myParent->point->y() - i * yStep,\n givenNode->myParent->point->z() - i * zStep);\n newPoint->addParent(parent);\n parent = newPoint;\n RRT_TREE.push_back(newPoint);\n }\n givenNode->addParent(parent);\n if (nextNode->myParent != nullptr) {\n linSpace(nextNode, givenDistance);\n }\n}\n\nvoid segmentPath(const nav_msgs::Path &path, nav_msgs::Path &path_seg) {\n path_seg.poses.clear();\n double v_max_ = 1;\n double yaw_rate_max_ = 0.05;\n if (path.poses.size() == 0)\n return;\n if (path.poses.size() == 1)\n path_seg.poses.push_back(path.poses[0]);\n\n for (int i = 0; i < (path.poses.size() - 1); ++i) {\n Eigen::Vector3d start(path.poses[i].pose.position.x,\n path.poses[i].pose.position.y,\n path.poses[i].pose.position.z);\n Eigen::Vector3d end(path.poses[i + 1].pose.position.x,\n path.poses[i + 1].pose.position.y,\n path.poses[i + 1].pose.position.z);\n Eigen::Vector3d distance = end - start;\n double yaw_start = tf::getYaw(path.poses[i].pose.orientation);\n double yaw_end = tf::getYaw(path.poses[i + 1].pose.orientation);\n double yaw_direction = yaw_end - yaw_start;\n if (yaw_direction > M_PI) {\n yaw_direction -= 2.0 * M_PI;\n }\n if (yaw_direction < -M_PI) {\n yaw_direction += 2.0 * M_PI;\n }\n\n double dist_norm = distance.norm();\n double disc = std::min(dt_ * v_max_ / dist_norm,\n dt_ * yaw_rate_max_ / abs(yaw_direction));\n\n bool int_flag = true;\n\n if (int_flag) {\n for (double it = 0.0; it <= 1.0; it += disc) {\n tf::Vector3 origin((1.0 - it) * start[0] + it * end[0],\n (1.0 - it) * start[1] + it * end[1],\n (1.0 - it) * start[2] + it * end[2]);\n double yaw = yaw_start + yaw_direction * it;\n if (yaw > M_PI)\n yaw -= 2.0 * M_PI;\n if (yaw < -M_PI)\n yaw += 2.0 * M_PI;\n tf::Quaternion quat;\n quat.setEuler(0.0, 0.0, yaw);\n tf::Pose poseTF(quat, origin);\n geometry_msgs::Pose pose;\n tf::poseTFToMsg(poseTF, pose);\n geometry_msgs::PoseStamped pose_stamped;\n pose_stamped.pose.position = pose.position;\n pose_stamped.pose.orientation = pose.orientation;\n path_seg.poses.push_back(pose_stamped);\n }\n }\n }\n}\n\nvoid generateTrajectory() {\n\n nav_msgs::Path my_path;\n my_path.poses.clear();\n\n geometry_msgs::PoseStamped new_pose;\n\n for (auto i = std::next(CHOSEN_PATH.begin(), 0); i != CHOSEN_PATH.end();\n i++) {\n\n new_pose.pose.position.x = (*i)->point->x();\n new_pose.pose.position.y = (*i)->point->y();\n new_pose.pose.position.z = (*i)->point->z();\n\n new_pose.pose.orientation.x = 0;\n new_pose.pose.orientation.y = 0;\n new_pose.pose.orientation.z = 0;\n new_pose.pose.orientation.w = 1;\n\n my_path.poses.push_back(new_pose);\n }\n\n nav_msgs::Path seg_path;\n seg_path.poses.clear();\n\n nav_msgs::Path new_path;\n new_path.poses.clear();\n\n segmentPath(my_path, seg_path);\n\n for (int i = 0; i < seg_path.poses.size() - 1; i++) {\n\n geometry_msgs::PoseStamped p;\n float angle = atan2(seg_path.poses[i + 1].pose.position.y -\n seg_path.poses[i].pose.position.y,\n seg_path.poses[i + 1].pose.position.x -\n seg_path.poses[i].pose.position.x);\n Eigen::Vector3d v(0, 0, angle);\n Eigen::Quaternion q = Euler2Quaternion(v);\n\n p.pose.position.x = seg_path.poses[i].pose.position.x;\n p.pose.position.y = seg_path.poses[i].pose.position.y;\n p.pose.position.z = seg_path.poses[i].pose.position.z;\n p.pose.orientation.x = q.x();\n p.pose.orientation.y = q.y();\n p.pose.orientation.z = q.z();\n p.pose.orientation.w = q.w();\n\n new_path.poses.push_back(p);\n }\n n_seq_++;\n mav_msgs::EigenTrajectoryPoint trajectory_point_;\n trajectory_msgs::MultiDOFJointTrajectoryPoint trajectory_point_msg_;\n std::vector executing_path_;\n\n trajectory_array_.header.seq = n_seq_;\n trajectory_array_.header.stamp = ros::Time::now();\n trajectory_array_.header.frame_id = \"world\";\n trajectory_array_.points.clear();\n double time_sum = 0;\n\n geometry_msgs::PoseStamped pose_ref;\n command_path.poses.clear();\n pose_ref.header.frame_id = \"world\";\n command_path.header.frame_id = \"world\";\n\n for (int i = 0; i < new_path.poses.size(); i++) {\n\n double yaw = tf::getYaw(new_path.poses[i].pose.orientation);\n Eigen::Vector3d p(new_path.poses[i].pose.position.x,\n new_path.poses[i].pose.position.y,\n new_path.poses[i].pose.position.z);\n trajectory_point_.position_W.x() = p.x();\n trajectory_point_.position_W.y() = p.y();\n trajectory_point_.position_W.z() = p.z();\n trajectory_point_.setFromYaw(yaw);\n pose_ref = new_path.poses[i];\n\n mav_msgs::msgMultiDofJointTrajectoryPointFromEigen(trajectory_point_,\n &trajectory_point_msg_);\n time_sum += dt_;\n\n // uncomment bwlow for yaw tracking trajectory.\n\n /*\n trajectory_point_msg_.time_from_start = ros::Duration(time_sum);\n trajectory_array_.points.push_back(trajectory_point_msg_);\n */\n\n command_path.poses.push_back(pose_ref);\n }\n}\n\nvoid publishTrajectory() {\n\n std::pair\n traj_ref_; // position and corresponding velocity reference.\n std::vector> traj_ref_vec_;\n traj_ref_vec_.clear();\n std::vector position_vec;\n std::vector vel_vec;\n position_vec.clear();\n vel_vec.clear();\n Eigen::Vector3d position_ref;\n Eigen::Vector3d vel_ref;\n\n for (auto path_it = CHOSEN_PATH.begin(); path_it != CHOSEN_PATH.end();\n ++path_it) {\n\n position_ref[0] = (*path_it)->point->x();\n position_ref[1] = (*path_it)->point->y();\n position_ref[2] = (*path_it)->point->z();\n position_vec.push_back(position_ref);\n }\n\n for (auto vel_it = CHOSEN_PATH_VREF.begin();\n vel_it != std::prev(CHOSEN_PATH_VREF.end(), 2);) {\n\n vel_ref[0] = *vel_it;\n vel_ref[1] = *std::next(vel_it, 1);\n vel_ref[2] = *std::next(vel_it, 2);\n vel_vec.push_back(vel_ref);\n vel_it++;\n }\n\n for (int i = 0; i < position_vec.size(); ++i) {\n std::pair traj_pair =\n std::make_pair(position_vec[i], vel_vec[i]);\n traj_ref_vec_.push_back(traj_pair);\n }\n double t_sum = 0;\n geometry_msgs::Transform transform;\n geometry_msgs::Twist velocity;\n\n n_seq_++;\n trajectory_array_.header.seq = n_seq_;\n trajectory_array_.header.stamp = ros::Time::now();\n trajectory_array_.header.frame_id = \"world\";\n trajectory_array_.points.clear();\n\n for (int i = 0; i < traj_ref_vec_.size(); ++i) {\n\n trajectory_msgs::MultiDOFJointTrajectoryPoint trajectory_point;\n transform.translation.x = traj_ref_vec_[i].first[0];\n transform.translation.y = traj_ref_vec_[i].first[1];\n transform.translation.z = traj_ref_vec_[i].first[2];\n\n // velocity.linear.x = traj_ref_vec_[i].second[0];\n // velocity.linear.y = traj_ref_vec_[i].second[1];\n // velocity.linear.z = traj_ref_vec_[i].second[2];\n\n // Velocities set to zero for debugging trajectory response.\n\n velocity.linear.x = 0;\n velocity.linear.y = 0;\n velocity.linear.z = 0;\n trajectory_point.transforms.push_back(transform);\n trajectory_point.velocities.push_back(velocity);\n\n t_sum += dt_;\n trajectory_point.time_from_start = ros::Duration(t_sum);\n trajectory_array_.points.push_back(trajectory_point);\n }\n\n // Publish final trajectory. Command path is only for visualization.\n\n m_trajectory_Publisher.publish(trajectory_array_);\n m_command_Path_Publisher.publish(command_path);\n}\n\n// Evaluates the current point in the current path.\n// This includes deciding when to change the current target to the next node in\n// the path and when to calculate a new path.\nvoid evaluateCurrentPoint(ros::Publisher *chosen_path_pub) {\n\n // generateTrajectory ();\n\n // ROS_INFO_STREAM (\"Goal -- Number of parents : \" <<\n // goalNode->myParents.size() << \"\\n\");\n\n if ((sqrt(pow(position_x - goalNode->point->x(), 2) +\n pow(position_y - goalNode->point->y(), 2) +\n pow(position_z - goalNode->point->z(), 2)) < path_update_dist_)) {\n itterations = 0;\n fetched_path = false;\n RRT_created = false;\n GOALS_generated = false;\n position_received = false;\n allowNewPath = true;\n }\n\n // @NOTE : uncomment the code block below to publish REFERENCE_OUT_ on\n // nav_msgs::Odometry to use nmpc reference instead of trajectory tracking.\n\n /*\n if ((sqrt(pow(position_x - currentTarget->point->x(), 2) +\n pow(position_y - currentTarget->point->y(), 2) +\n pow(position_z - currentTarget->point->z(), 2)) < recalc_dist_) and\n path_itterator != --CHOSEN_PATH.end()) {\n advance_index++;\n path_itterator = CHOSEN_PATH.begin();\n std::advance(path_itterator, advance_index);\n currentTarget = *path_itterator;\n std::advance(vref_itterator, 3);\n if (path_itterator == CHOSEN_PATH.end()) {\n path_itterator--;\n currentTarget = *path_itterator;\n }\n }\n if (path_itterator != CHOSEN_PATH.end()) {\n nav_msgs::Odometry nextPoint;\n nextPoint.pose.pose.position.x = (currentTarget)->point->x();\n nextPoint.pose.pose.position.y = (currentTarget)->point->y();\n nextPoint.pose.pose.position.z = (currentTarget)->point->z();\n if (!recoveryUnderway) {\n nextPoint.twist.twist.linear.x = *vref_itterator;\n vref_itterator++;\n nextPoint.twist.twist.linear.y = *vref_itterator;\n vref_itterator++;\n nextPoint.twist.twist.linear.z = *vref_itterator;\n std::advance(vref_itterator, -2);\n } else {\n nextPoint.twist.twist.linear.x = 0;\n nextPoint.twist.twist.linear.y = 0;\n nextPoint.twist.twist.linear.z = 0;\n }\n nextPoint.pose.pose.orientation.x = 0;\n nextPoint.pose.pose.orientation.y = 0;\n nextPoint.pose.pose.orientation.z = 0;\n nextPoint.pose.pose.orientation.w = 0;\n nextPoint.header.stamp = ros::Time::now();\n nextPoint.header.frame_id = map_frame_id_;\n chosen_path_pub->publish(nextPoint);\n }\n\n */\n}\n\n// Builds and publishes the visualization messages.\nvoid visualize(ros::Publisher *points_pub, ros::Publisher *output_path_pub,\n ros::Publisher *all_path_pub, ros::Publisher *goal_pub,\n ros::Publisher *hits_pub, ros::Publisher *taken_path_pub,\n ros::Publisher *map_pub, ros::Publisher *position_pub,\n ros::Publisher *unknowns_pub) {\n visualization_msgs::Marker RRT_points, RRT_line_list, CHOSEN_PATH_points,\n CHOSEN_PATH_line_list, PATH_points, PATH_line_list, GOAL_points,\n HITS_points, TAKEN_PATH_points, TAKEN_PATH_line_list, POSITION_point,\n unknowns;\n // Visualize each itteration\n\n node *currentNode = new node(position_x, position_y, position_z);\n currentNode->extractUnknownVoxelsInsideFrustum(myMap);\n\n if (!currentNode->unknowns_in_sight_.empty()) {\n\n unknowns.header.frame_id = map_frame_id_;\n unknowns.ns = \"points\";\n unknowns.action = visualization_msgs::Marker::ADD;\n unknowns.pose.orientation.w = 1.0;\n unknowns.id = 0;\n unknowns.type = visualization_msgs::Marker::CUBE_LIST;\n unknowns.scale.x = 0.2;\n unknowns.scale.y = 0.2;\n unknowns.scale.z = 0.2;\n unknowns.color.r = 1.0;\n unknowns.color.g = 1.0;\n unknowns.color.b = 1.0;\n unknowns.color.a = 0.6;\n std::vector::iterator it_comeon_visualizer22;\n for (it_comeon_visualizer22 = currentNode->unknowns_in_sight_.begin();\n it_comeon_visualizer22 != currentNode->unknowns_in_sight_.end();\n it_comeon_visualizer22++) {\n geometry_msgs::Point p;\n p.x = (*it_comeon_visualizer22).x();\n p.y = (*it_comeon_visualizer22).y();\n p.z = (*it_comeon_visualizer22).z();\n unknowns.points.push_back(p);\n }\n unknowns_pub->publish(unknowns);\n }\n\n // if (position_received) {\n // POSITION_point.header.frame_id = map_frame_id_;\n // POSITION_point.ns = \"points\";\n // POSITION_point.action = visualization_msgs::Marker::ADD;\n // POSITION_point.pose.orientation.w = 1.0;\n // POSITION_point.id = 0;\n // POSITION_point.type = visualization_msgs::Marker::SPHERE;\n // POSITION_point.scale.x = 2*robot_size_;\n // POSITION_point.scale.y = 2*robot_size_;\n // POSITION_point.scale.z = 2*robot_size_;\n // POSITION_point.color.g = 1.0f;\n // POSITION_point.color.a = 0.06;\n // geometry_msgs::Point p;\n // p.x = position_x;\n // p.y = position_y;\n // p.z = position_z;\n // POSITION_point.points.push_back(p);\n // position_pub->publish(POSITION_point);\n // }\n\n // Visualize only once\n if (visualizeNewData) {\n if (RRT_created) {\n RRT_points.header.frame_id = RRT_line_list.header.frame_id =\n map_frame_id_;\n RRT_points.ns = \"points\";\n RRT_points.action = visualization_msgs::Marker::ADD;\n RRT_points.pose.orientation.w = 1.0;\n RRT_points.id = 0;\n RRT_line_list.id = 1;\n RRT_points.type = visualization_msgs::Marker::POINTS;\n RRT_line_list.type = visualization_msgs::Marker::LINE_LIST;\n RRT_points.scale.x = 0.2;\n RRT_points.scale.y = 0.2;\n RRT_line_list.scale.x = 0.1;\n RRT_points.color.g = 1.0f;\n RRT_points.color.a = 1.0;\n RRT_line_list.color.b = 1.0;\n RRT_line_list.color.a = 1.0;\n RRT_points.lifetime = ros::Duration(4.0);\n RRT_line_list.lifetime = ros::Duration(4.0);\n std::list::iterator it_comeon_visualizer;\n for (it_comeon_visualizer = RRT_TREE.begin();\n it_comeon_visualizer != RRT_TREE.end(); it_comeon_visualizer++) {\n geometry_msgs::Point p;\n p.x = (*it_comeon_visualizer)->point->x();\n p.y = (*it_comeon_visualizer)->point->y();\n p.z = (*it_comeon_visualizer)->point->z();\n RRT_points.points.push_back(p);\n if ((*it_comeon_visualizer)->myParent != nullptr) {\n RRT_line_list.points.push_back(p);\n p.x = (*it_comeon_visualizer)->myParent->point->x();\n p.y = (*it_comeon_visualizer)->myParent->point->y();\n p.z = (*it_comeon_visualizer)->myParent->point->z();\n RRT_line_list.points.push_back(p);\n }\n }\n points_pub->publish(RRT_points);\n points_pub->publish(RRT_line_list);\n }\n if (!CHOSEN_PATH.empty()) {\n nav_msgs::Path chosen_path;\n chosen_path.header.frame_id = map_frame_id_;\n chosen_path.poses.clear();\n for (auto i = CHOSEN_PATH.begin(); i != CHOSEN_PATH.end(); i++) {\n geometry_msgs::PoseStamped chosen_pose;\n chosen_pose.header.frame_id = map_frame_id_;\n chosen_pose.pose.position.x = (*i)->point->x();\n chosen_pose.pose.position.y = (*i)->point->y();\n chosen_pose.pose.position.z = (*i)->point->z();\n chosen_path.poses.push_back(chosen_pose);\n }\n output_path_pub->publish(chosen_path);\n }\n if (RRT_created) {\n\n PATH_points.header.frame_id = PATH_line_list.header.frame_id =\n map_frame_id_;\n PATH_points.ns = \"points\";\n PATH_points.action = visualization_msgs::Marker::ADD;\n PATH_points.pose.orientation.w = 1.0;\n PATH_points.id = 0;\n PATH_line_list.id = 1;\n PATH_points.type = visualization_msgs::Marker::POINTS;\n PATH_line_list.type = visualization_msgs::Marker::LINE_LIST;\n PATH_points.scale.x = 0.2;\n PATH_points.scale.y = 0.2;\n PATH_line_list.scale.x = 0.1;\n PATH_points.color.g = 1.0f;\n PATH_points.color.a = 1.0;\n PATH_line_list.color.g = 1.0;\n PATH_line_list.color.a = 1.0;\n PATH_line_list.lifetime = ros::Duration(6.0);\n PATH_points.lifetime = ros::Duration(6.0);\n std::list::iterator it_comeon_visualizer5;\n ALL_PATH.clear();\n for (it_comeon_visualizer5 = myGoals.begin();\n it_comeon_visualizer5 != myGoals.end(); it_comeon_visualizer5++) {\n std::list candidate_branch_vis;\n candidate_branch_vis.clear();\n (*it_comeon_visualizer5)->getPath(&candidate_branch_vis);\n (*it_comeon_visualizer5)->getPath(&ALL_PATH);\n ALL_PATH.push_back((*it_comeon_visualizer5));\n for (auto it = candidate_branch_vis.begin(); it != std::prev(candidate_branch_vis.end(), 1); it++) {\n\n geometry_msgs::Point p0;\n geometry_msgs::Point p1;\n p0.x = (*it)->point->x();\n p0.y = (*it)->point->y();\n p0.z = (*it)->point->z();\n auto temp_it = std::next(it, 1);\n p1.x = (*temp_it)->point->x();\n p1.y = (*temp_it)->point->y();\n p1.z = (*temp_it)->point->z();\n\n // if (temp_it != std::prev(ALL_PATH.end(), 0)) {\n PATH_line_list.points.push_back(p0);\n PATH_line_list.points.push_back(p1);\n // }\n }\n\n }\n std::list::iterator it_comeon_visualizer6;\n for (it_comeon_visualizer6 = ALL_PATH.begin();\n it_comeon_visualizer6 != std::prev(ALL_PATH.end(), 1);\n it_comeon_visualizer6++) {\n geometry_msgs::Point p;\n p.x = (*it_comeon_visualizer6)->point->x();\n p.y = (*it_comeon_visualizer6)->point->y();\n p.z = (*it_comeon_visualizer6)->point->z();\n }\n // all_path_pub->publish(PATH_points);\n all_path_pub->publish(PATH_line_list);\n }\n if (GOALS_generated) {\n GOAL_points.header.frame_id = map_frame_id_;\n GOAL_points.ns = \"points\";\n GOAL_points.action = visualization_msgs::Marker::ADD;\n GOAL_points.pose.orientation.w = 1.0;\n GOAL_points.id = 0;\n GOAL_points.type = visualization_msgs::Marker::SPHERE_LIST;\n GOAL_points.scale.x = 0.2;\n GOAL_points.scale.y = 0.2;\n GOAL_points.scale.z = 0.2;\n GOAL_points.color.g = 0.8;\n GOAL_points.color.r = 0.8;\n GOAL_points.color.a = 0.8;\n std::list::iterator it_comeon_visualizer3;\n for (it_comeon_visualizer3 = myGoals.begin();\n it_comeon_visualizer3 != myGoals.end(); it_comeon_visualizer3++) {\n geometry_msgs::Point p;\n p.x = (*it_comeon_visualizer3)->point->x();\n p.y = (*it_comeon_visualizer3)->point->y();\n p.z = (*it_comeon_visualizer3)->point->z();\n GOAL_points.points.push_back(p);\n }\n goal_pub->publish(GOAL_points);\n }\n if (goalNode != nullptr) {\n\n // hits.clear();\n HITS_points.header.frame_id = map_frame_id_;\n HITS_points.ns = \"points\";\n HITS_points.action = visualization_msgs::Marker::ADD;\n HITS_points.pose.orientation.w = 1.0;\n HITS_points.id = 0;\n HITS_points.type = visualization_msgs::Marker::POINTS;\n HITS_points.scale.x = 0.2;\n HITS_points.scale.y = 0.2;\n HITS_points.color.r = 1.0f;\n HITS_points.color.a = 1.0;\n HITS_points.lifetime = ros::Duration(8.0);\n std::list::iterator it_comeon_visualizer4;\n\n for (it_comeon_visualizer4 = goalNode->myHits.begin();\n it_comeon_visualizer4 != goalNode->myHits.end();\n it_comeon_visualizer4++) {\n geometry_msgs::Point p;\n p.x = it_comeon_visualizer4->x();\n p.y = it_comeon_visualizer4->y();\n p.z = it_comeon_visualizer4->z();\n HITS_points.points.push_back(p);\n }\n hits_pub->publish(HITS_points);\n }\n if (goalNode != nullptr) {\n TAKEN_PATH_points.header.frame_id = TAKEN_PATH_line_list.header.frame_id =\n map_frame_id_;\n TAKEN_PATH_points.ns = \"points\";\n TAKEN_PATH_line_list.ns = \"lines\";\n TAKEN_PATH_points.action = TAKEN_PATH_line_list.action =\n visualization_msgs::Marker::ADD;\n TAKEN_PATH_points.pose.orientation.w = 1.0;\n TAKEN_PATH_line_list.pose.orientation.w = 1.0;\n TAKEN_PATH_points.id = 0;\n TAKEN_PATH_line_list.id = 0;\n TAKEN_PATH_points.type = visualization_msgs::Marker::SPHERE_LIST;\n TAKEN_PATH_line_list.type = visualization_msgs::Marker::LINE_LIST;\n TAKEN_PATH_points.scale.x = 0.1;\n TAKEN_PATH_line_list.scale.x = 0.1;\n TAKEN_PATH_points.scale.y = 0.1;\n TAKEN_PATH_line_list.scale.y = 0.1;\n TAKEN_PATH_points.color.b = 0;\n TAKEN_PATH_points.color.g = 0;\n TAKEN_PATH_points.color.r = 0.7;\n TAKEN_PATH_points.color.a = 0.8;\n TAKEN_PATH_line_list.color.b = 0;\n TAKEN_PATH_line_list.color.g = 0;\n TAKEN_PATH_line_list.color.r = 0.7;\n TAKEN_PATH_line_list.color.a = 0.8;\n std::list::iterator taken_path_visualizer;\n\n // for (taken_path_visualizer = goalNode->myParents.begin();\n // taken_path_visualizer != goalNode->myParents.end();\n for (taken_path_visualizer = VISITED_POINTS.begin();\n taken_path_visualizer != VISITED_POINTS.end();\n taken_path_visualizer++) {\n geometry_msgs::Point p;\n p.x = (*taken_path_visualizer)->point->x();\n p.y = (*taken_path_visualizer)->point->y();\n p.z = (*taken_path_visualizer)->point->z();\n TAKEN_PATH_points.points.push_back(p);\n\n if ((*taken_path_visualizer)->myParent != nullptr) {\n TAKEN_PATH_line_list.points.push_back(p);\n p.x = (*taken_path_visualizer)->myParent->point->x();\n p.y = (*taken_path_visualizer)->myParent->point->y();\n p.z = (*taken_path_visualizer)->myParent->point->z();\n TAKEN_PATH_line_list.points.push_back(p);\n }\n }\n taken_path_pub->publish(TAKEN_PATH_points);\n taken_path_pub->publish(TAKEN_PATH_line_list);\n }\n ufomap_msgs::UFOMapStamped::Ptr msg(new ufomap_msgs::UFOMapStamped);\n bool compress = false;\n ufo::map::DepthType pub_depth = 0;\n if (ufomap_msgs::ufoToMsg(myMap, msg->map, compress, pub_depth)) {\n msg->header.stamp = ros::Time::now();\n msg->header.frame_id = map_frame_id_;\n map_pub->publish(msg);\n } else {\n std::cout << \"Map conversion failed!\" << std::endl;\n }\n visualizeNewData = false;\n }\n}\n\n// Evaluates and, if necessary, adds a new node to the path taken.\n// The path taken is used during visualization and in the global strategy.\nvoid updatePathTaken() {\n if (VISITED_POINTS.empty() and position_received) {\n node *myNode = new node(position_x, position_y, position_z);\n VISITED_POINTS.push_back(myNode);\n } else {\n if (position_received) {\n std::list::iterator taken_path_visualizer;\n taken_path_visualizer = --VISITED_POINTS.end();\n if (sqrt(pow((*taken_path_visualizer)->point->x() - position_x, 2) +\n pow((*taken_path_visualizer)->point->y() - position_y, 2) +\n pow((*taken_path_visualizer)->point->z() - position_z, 2)) >=\n 0.2) {\n node *myNode = new node(position_x, position_y, position_z);\n (*taken_path_visualizer)->addParent(myNode);\n VISITED_POINTS.push_back(myNode);\n }\n }\n }\n}\n\n// Tunes the generation.\n// This sets the bounding box which represents the local space.\n// The bounding box will be reduced in size as much as possible without losing\n// free space.\nvoid tuneGeneration(ufo::map::OccupancyMapColor const &map, bool occupied_space,\n bool free_space, bool unknown_space, float given_x,\n float given_y, float given_z,\n ufo::map::DepthType min_depth = planning_depth_) {\n highest_x = -9999;\n highest_y = -9999;\n highest_z = -9999;\n lowest_x = std::numeric_limits::max();\n lowest_y = std::numeric_limits::max();\n lowest_z = std::numeric_limits::max();\n ufo::math::Vector3 minPoint(given_x - 1 * (v_local_ / 2),\n given_y - 1 * (v_local_ / 2),\n given_z - 1 * (v_local_ / 2));\n ufo::math::Vector3 maxPoint(given_x + 1 * (v_local_ / 2),\n given_y + 1 * (v_local_ / 2),\n given_z + 1 * (v_local_ / 2));\n ufo::geometry::AABB aabb(minPoint, maxPoint);\n for (auto it = map.beginLeaves(aabb, occupied_space, free_space,\n unknown_space, false, min_depth),\n it_end = map.endLeaves();\n it != it_end; ++it) {\n if (it.getX() > highest_x) {\n highest_x = it.getX();\n }\n if (it.getX() < lowest_x) {\n lowest_x = it.getX();\n }\n if (it.getY() > highest_y) {\n highest_y = it.getY();\n }\n if (it.getY() < lowest_y) {\n lowest_y = it.getY();\n }\n if (it.getZ() > highest_z) {\n highest_z = it.getZ();\n }\n if (it.getZ() < lowest_z) {\n lowest_z = it.getZ();\n }\n }\n SCALER_X = highest_x - lowest_x;\n SCALER_Y = highest_y - lowest_y;\n SCALER_Z = highest_z - lowest_z;\n}\n\nbool isInCollision(ufo::map::OccupancyMapColor const &map,\n ufo::geometry::BoundingVar const &bounding_volume,\n bool occupied_space = false, bool free_space = false,\n bool unknown_space = false,\n ufo::map::DepthType min_depth = planning_depth_) {\n // Iterate through all leaf nodes that intersects the bounding volume\n for (auto it = map.beginLeaves(bounding_volume, occupied_space, free_space,\n unknown_space, false, min_depth),\n it_end = map.endLeaves();\n it != it_end; ++it) {\n // Is in collision since a leaf node intersects the bounding volume.\n return true;\n }\n // No leaf node intersects the bounding volume.\n return false;\n}\n\n// Evaluates the RRT tree for the purpose of reducing the distance in a path.\n// The node with the shortest path to the root node, which the goal in question\n// can see, will be the parent of the currently evaluated goal node. The path\n// between the goal node and its' parent has to pass a series of sphere checks,\n// which aims to guarantee a distance of robot_size_ between the path and the\n// occupied and unknown space.\nvoid findShortestPath() {\n\n for (std::list::iterator it_goals = myGoals.begin();\n it_goals != myGoals.end(); it_goals++) {\n struct node *chosenNode = nullptr;\n double distance = std::numeric_limits::max();\n for (std::list::iterator it_RRT = RRT_TREE.begin();\n it_RRT != RRT_TREE.end(); it_RRT++) {\n double distanceNodeToGoal =\n sqrt(pow((*it_RRT)->point->x() - (*it_goals)->point->x(), 2) +\n pow((*it_RRT)->point->y() - (*it_goals)->point->y(), 2) +\n pow((*it_RRT)->point->z() - (*it_goals)->point->z(), 2));\n double distanceToNode = (*it_RRT)->sumDistance();\n if (distanceNodeToGoal < goal_connect_dist_) {\n\n double totalDistance = distanceNodeToGoal + distanceToNode;\n if (totalDistance < distance) {\n\n ufo::geometry::OBB obb =\n makeOBB(*((*it_RRT)->point), *((*it_goals)->point), robot_size_);\n\n if (!isInCollision(myMap, obb, true, false, true, planning_depth_)) {\n bool add = true;\n\n if (isInCollision(myMap, obb, true, false, true, planning_depth_)) {\n add = false;\n break;\n }\n if (add) {\n\n distance = totalDistance;\n chosenNode = *it_RRT;\n }\n }\n }\n }\n }\n if (chosenNode != nullptr) {\n (*it_goals)->addParent(chosenNode);\n chosenNode->addChild(*it_goals);\n }\n }\n}\n\n// Generates goals.\n// The goals generated guarantees at least one piece of new information within\n// sensor_range_, as well as being in free space and at least robot_size_\n// distance away from both unknown and occupied space.\nvoid generateGoals(ufo::map::OccupancyMapColor const &map,\n bool evaluateOldGoals) {\n if (!myGoals.empty() and evaluateOldGoals) {\n double newCost = std::numeric_limits::max();\n double totalCost = std::numeric_limits::max();\n for (std::list::iterator it_goal = myGoals.begin();\n it_goal != myGoals.end(); it_goal++) {\n if ((*it_goal)->myParent != nullptr) {\n (*it_goal)->clearInformationGain();\n double informationGain =\n k_info_ *\n ((*it_goal)->findInformationGain(\n v_local_, SENSOR_HORIZONTAL, sensor_vertical_fov_,\n min_sensor_range_, sensor_range_, myMap, true, false));\n newCost = -informationGain;\n if (informationGain > initialGoalInfo) {\n initialGoalInfo = informationGain;\n }\n int stickyCounter = 0;\n for (std::list::iterator it_floor =\n (*it_goal)->myHits.begin();\n it_floor != (*it_goal)->myHits.end(); it_floor++) {\n if (it_floor->z() < (*it_goal)->point->z()) {\n stickyCounter++;\n }\n }\n bool infoRequirement =\n ((*it_goal)->myHits.size() > GLOBAL_PATH_THRESHOLD);\n bool stickyFloor = ((stickyCounter < 0.8 * (*it_goal)->myHits.size()));\n if ((newCost < totalCost) and\n ((*it_goal)->findInformationGain(\n v_local_, SENSOR_HORIZONTAL, sensor_vertical_fov_,\n min_sensor_range_, sensor_range_, myMap, false, false) > 0) and\n (stickyFloor and infoRequirement) and\n (*it_goal)->myParent != nullptr) {\n totalCost = newCost;\n reserveGoalNode = *it_goal;\n }\n }\n }\n if (reserveGoalNode != nullptr) {\n myReserveGoals.push_back(reserveGoalNode);\n for (std::list::iterator it_parent_finder =\n --VISITED_POINTS.end();\n it_parent_finder != VISITED_POINTS.begin(); it_parent_finder--) {\n ufo::geometry::LineSegment myLine((*(*it_parent_finder)->point),\n (*reserveGoalNode->point));\n if (!isInCollision(map, myLine, true, false, true, planning_depth_)) {\n reserveGoalNode->addParent((*it_parent_finder));\n break;\n }\n }\n }\n }\n if (goalNode != nullptr) {\n if (sqrt(pow(position_x - goalNode->point->x(), 2) +\n pow(position_y - goalNode->point->y(), 2) +\n pow(position_z - goalNode->point->z(), 2)) > path_update_dist_) {\n std::list::iterator it_goal2;\n int help_counter = 0;\n for (it_goal2 = myGoals.begin(); it_goal2 != myGoals.end(); it_goal2++) {\n help_counter++;\n if (*it_goal2 != goalNode and *it_goal2 != reserveGoalNode) {\n (*it_goal2)->readyForDeletion();\n delete (*it_goal2);\n }\n }\n myGoals.clear();\n myGoals.push_back(goalNode);\n } else {\n std::list::iterator it_goal2;\n for (it_goal2 = myGoals.begin(); it_goal2 != myGoals.end(); it_goal2++) {\n if (*it_goal2 != reserveGoalNode) {\n (*it_goal2)->readyForDeletion();\n delete (*it_goal2);\n }\n }\n goalNode = nullptr;\n reserveGoalNode = nullptr;\n myGoals.clear();\n allowNewPath = true;\n }\n }\n ufo::math::Vector3 goal;\n srand(time(0));\n itterations = 0;\n while ((myGoals.size() < number_of_goals_) and (itterations < 20000)) {\n float x = lowest_x + abs(1024 * rand() / (RAND_MAX + 1.0)) * SCALER_X;\n float y = lowest_y + abs(1024 * rand() / (RAND_MAX + 1.0)) * SCALER_Y;\n float z = lowest_z + abs(1024 * rand() / (RAND_MAX + 1.0)) * SCALER_Z;\n node goal = node(x, y, z);\n ufo::geometry::Sphere goal_sphere(*(goal.point), robot_size_);\n if (sqrt(pow(position_x - x, 2) + pow(position_y - y, 2) +\n pow(position_z - z, 2)) > min_dist_to_goal_) {\n if (!isInCollision(myMap, goal_sphere, true, false, true,\n planning_depth_) and\n isInCollision(myMap, goal_sphere, false, true, false,\n planning_depth_)) {\n bool add = true;\n for (std::list::iterator it_goal = myGoals.begin();\n it_goal != myGoals.end(); it_goal++) {\n if (sqrt(pow((*it_goal)->point->x() - x, 2) +\n pow((*it_goal)->point->y() - y, 2) +\n pow((*it_goal)->point->z() - z, 2)) < dist_goals_) {\n add = false;\n break;\n }\n }\n if (add) {\n\n int foundInfo;\n\n node *newGoal = new node(x, y, z);\n myGoals.push_back(newGoal);\n }\n };\n itterations++;\n }\n };\n if (myGoals.size() == number_of_goals_) {\n std::cout << \"Goals generated successfully\\n\" << std::endl;\n GOALS_generated = true;\n } else if (myGoals.size() == 0) {\n std::cout << \"No goals found, trying again soon\" << std::endl;\n sleep_for(microseconds(100000));\n } else {\n std::cout << \"Only \" << myGoals.size() << \" goals found\" << std::endl;\n GOALS_generated = true;\n }\n};\n\n// Evaluates and sets the current path\n// The evaluation takes the distance cost, information gain and the distance\n// cost in mind, choosing the path with the overall smallest cost. The\n// evaluation depends heavily on the values of k_dist_,\n// k_info_ and k_u_\nvoid setPath() {\n bool setDistance = false;\n if (goalNode != nullptr) {\n goalNode->clearInformationGain();\n\n if ((sqrt(pow(position_x - goalNode->point->x(), 2) +\n pow(position_y - goalNode->point->y(), 2) +\n pow(position_z - goalNode->point->z(), 2)) < path_update_dist_)) {\n allowNewPath = true;\n totalCost = std::numeric_limits::max();\n } else {\n totalCost =\n goalNode->sumDistance() * k_dist_ -\n k_info_ * (goalNode->findInformationGain(\n v_local_, SENSOR_HORIZONTAL, sensor_vertical_fov_,\n min_sensor_range_, sensor_range_, myMap, true, false));\n }\n } else {\n totalCost = std::numeric_limits::max();\n }\n double newCost = std::numeric_limits::max();\n if (allowNewPath) {\n std::list PATH_CONTAINER{};\n initialGoalInfo = 0;\n\n for (std::list::iterator it_goal = myGoals.begin();\n it_goal != myGoals.end(); it_goal++) {\n\n if (*it_goal != nullptr) {\n (*it_goal)->addParents();\n }\n\n if ((*it_goal)->myParent != nullptr) {\n\n auto pathImprovement_start = high_resolution_clock::now();\n\n (*it_goal)->findPathImprovement(*it_goal, myMap, dist_nodes_,\n robot_size_, pathImprovement_start,\n path_improvement_max_);\n\n linSpace(*it_goal, dist_nodes_);\n auto pathImprovement_start_2 = high_resolution_clock::now();\n (*it_goal)->findPathImprovement(*it_goal, myMap, dist_nodes_,\n robot_size_, pathImprovement_start_2,\n path_improvement_max_);\n\n linSpace(*it_goal, dist_nodes_);\n double informationGain =\n k_info_ *\n ((*it_goal)->findInformationGain(\n v_local_, SENSOR_HORIZONTAL, sensor_vertical_fov_,\n min_sensor_range_, sensor_range_, myMap, false, false));\n\n double distanceCost = (*it_goal)->sumDistance() * k_dist_;\n\n typedef rrtCache *(*arbitrary)();\n typedef rrtSolverStatus (*arbitrary2)(void *, double *, double *,\n double, double *);\n typedef void (*rrt_clearer)(rrtCache *);\n int i = 0;\n double p[159] = {0};\n std::list xref = {};\n std::list x0 = {position_x, position_y, position_z, velocity_x,\n velocity_y, velocity_z, roll, pitch};\n\n // Current position\n p[0] = position_x;\n p[1] = position_y;\n p[2] = position_z;\n p[3] = velocity_x;\n p[4] = velocity_y;\n p[5] = velocity_z;\n p[6] = roll;\n p[7] = pitch;\n\n // Trajectory\n std::list EVALUATE_PATH{};\n EVALUATE_PATH.clear();\n (*it_goal)->getPath(&EVALUATE_PATH);\n EVALUATE_PATH.push_back(new node((*it_goal)->point->x(),\n (*it_goal)->point->y(),\n (*it_goal)->point->z()));\n xref.push_back((*it_goal)->point->x());\n xref.push_back((*it_goal)->point->y());\n xref.push_back((*it_goal)->point->z());\n std::list::iterator it_evaluator = EVALUATE_PATH.begin();\n for (i = 1; i < nmpc_horizon_; ++i) {\n p[8 + 3 * i] = (*it_evaluator)->point->x();\n xref.push_back((*it_evaluator)->point->x());\n p[8 + 3 * i + 1] = (*it_evaluator)->point->y();\n xref.push_back((*it_evaluator)->point->y());\n p[8 + 3 * i + 2] = (*it_evaluator)->point->z();\n xref.push_back((*it_evaluator)->point->z());\n if (it_evaluator != --EVALUATE_PATH.end()) {\n it_evaluator++;\n }\n }\n p[158] = nmpc_dt_;\n\n double u[150] = {0};\n\n for (i = 0; i < nmpc_horizon_; ++i) {\n u[3 * i] = 0;\n u[3 * i + 1] = 0;\n u[3 * i + 2] = 0;\n }\n\n double init_penalty = 1;\n void *handle = dlopen((ros::package::getPath(\"errt\") +\n \"/MAV/rrt/target/release/librrt.so\")\n .c_str(),\n RTLD_LAZY);\n if (!handle) {\n fprintf(stderr, \"%s\\n\", dlerror());\n exit(EXIT_FAILURE);\n }\n arbitrary rrt_new;\n *(void **)(&rrt_new) = dlsym(handle, \"rrt_new\");\n rrtCache *cache = rrt_new();\n arbitrary2 rrt_solve;\n *(void **)(&rrt_solve) = dlsym(handle, \"rrt_solve\");\n rrt_clearer rrt_free;\n *(void **)(&rrt_free) = dlsym(handle, \"rrt_free\");\n\n ROS_INFO_STREAM(\" Calculating .. \" << init_penalty);\n\n rrtSolverStatus status = rrt_solve(cache, u, p, 0, &init_penalty);\n\n std::list uold = {9.81, 0.0, 0.0};\n std::list uref = {9.81, 0.0, 0.0};\n\n std::list x_hist;\n std::list p_hist;\n double cost;\n std::tuple, double, std::list> trajectory(\n std::list x, double *u, double N, double dt,\n std::list nmpc_ref);\n std::tie(x_hist, cost, p_hist) =\n trajectory(x0, u, nmpc_horizon_, nmpc_dt_, xref);\n xref.clear();\n rrt_free(cache);\n double actuationCost = k_u_ * cost;\n newCost = distanceCost - informationGain + actuationCost;\n ////////////////////////////////////////////////////////////////\n\n ROS_DEBUG_STREAM(\"\\n Information gain : \" << informationGain << \"\\n\");\n ROS_DEBUG_STREAM(\" Distance cost : \" << distanceCost << \"\\n\");\n ROS_DEBUG_STREAM(\" Actuation cost : \" << actuationCost << \"\\n\");\n\n ROS_DEBUG_STREAM(\"\\n ---------------- \\n\");\n\n std::list new_p_hist;\n for (auto i = p_hist.begin(); i != p_hist.end(); i++) {\n if (std::distance(p_hist.begin(), i) / 3 <=\n EVALUATE_PATH.size() + 1) {\n new_p_hist.push_back(*i);\n }\n }\n\n if (informationGain > initialGoalInfo) {\n initialGoalInfo = informationGain;\n }\n int stickyCounter = 0;\n for (std::list::iterator it_floor =\n (*it_goal)->myHits.begin();\n it_floor != (*it_goal)->myHits.end(); it_floor++) {\n if (it_floor->z() < (*it_goal)->point->z()) {\n stickyCounter++;\n }\n }\n bool infoRequirement =\n ((*it_goal)->myHits.size() > GLOBAL_STRATEGY_THRESHOLD);\n bool stickyFloor = ((stickyCounter < 0.8 * (*it_goal)->myHits.size()));\n if ((newCost < totalCost) and\n ((*it_goal)->findInformationGain(\n v_local_, SENSOR_HORIZONTAL, sensor_vertical_fov_,\n min_sensor_range_, sensor_range_, myMap, false, false) > 0) and\n allowNewPath and (stickyFloor and infoRequirement) and\n (*it_goal)->myParent != nullptr) {\n totalCost = newCost;\n goalNode = *it_goal;\n newPath = true;\n PATH_CONTAINER.clear();\n for (std::list::iterator path_itterator_helper =\n new_p_hist.begin();\n path_itterator_helper != new_p_hist.end();) {\n double x = *path_itterator_helper;\n PATH_CONTAINER.push_back(x);\n path_itterator_helper++;\n double y = *path_itterator_helper;\n PATH_CONTAINER.push_back(y);\n path_itterator_helper++;\n double z = *path_itterator_helper;\n PATH_CONTAINER.push_back(z);\n path_itterator_helper++;\n }\n if (EVALUATE_PATH.size() > nmpc_horizon_) {\n std::list::iterator path_itterator_helper2 =\n EVALUATE_PATH.begin();\n std::advance(path_itterator_helper2, nmpc_horizon_);\n while (path_itterator_helper2 != EVALUATE_PATH.end()) {\n PATH_CONTAINER.push_back((*path_itterator_helper2)->point->x());\n PATH_CONTAINER.push_back((*path_itterator_helper2)->point->y());\n PATH_CONTAINER.push_back((*path_itterator_helper2)->point->z());\n path_itterator_helper2++;\n }\n }\n CHOSEN_PATH_VREF.clear();\n for (std::list::iterator reference_itterator_helper =\n x_hist.begin();\n reference_itterator_helper != x_hist.end();\n reference_itterator_helper++) {\n CHOSEN_PATH_VREF.push_back(*reference_itterator_helper);\n }\n vref_itterator = CHOSEN_PATH_VREF.begin();\n }\n }\n }\n if (not recoveryUnderway) {\n std::list::iterator it_clear_helper;\n for (it_clear_helper = CHOSEN_PATH.begin();\n it_clear_helper != --CHOSEN_PATH.end(); it_clear_helper++) {\n (*it_clear_helper)->readyForDeletion();\n delete (*it_clear_helper);\n }\n } else {\n std::list::iterator it_clear_helper = --CHOSEN_PATH.end();\n (*it_clear_helper)->readyForDeletion();\n delete (*it_clear_helper);\n recoveryUnderway = false;\n }\n CHOSEN_PATH.clear();\n if (goalNode != nullptr and newPath) {\n setDistance = true;\n std::list::iterator it_path_helper = PATH_CONTAINER.begin();\n for (int i = 0; i < (PATH_CONTAINER.size() / 3); i++) {\n double x = *it_path_helper;\n it_path_helper++;\n double y = *it_path_helper;\n it_path_helper++;\n double z = *it_path_helper;\n it_path_helper++;\n CHOSEN_PATH.push_back(new node(x, y, z));\n }\n PATH_CONTAINER.clear();\n CHOSEN_PATH.push_back(new node(goalNode->point->x(), goalNode->point->y(),\n goalNode->point->z()));\n fetched_path = true;\n visualizeNewData = true;\n allowNewPath = false;\n newPath = false;\n path_itterator = CHOSEN_PATH.begin();\n currentTarget = *path_itterator;\n advance_index = 0;\n }\n }\n if (setDistance) {\n totalDistance = goalNode->sumDistance();\n };\n}\n\n// Generates the RRT\n// Generates the nodes in the RRT-tree and connects them to a already existing\n// parent. The root of the node exists at the current position of the plant.\nvoid generateRRT(float given_x, float given_y, float given_z) {\n // high_resolution_clock::time_point start_total =\n // high_resolution_clock::now();\n\n for (std::list::iterator it_clear_helper = RRT_TREE.begin();\n it_clear_helper != --RRT_TREE.end(); it_clear_helper++) {\n (*it_clear_helper)->readyForDeletion();\n delete (*it_clear_helper);\n }\n for (std::list::iterator it_clear_helper = myGoals.begin();\n it_clear_helper != --myGoals.end(); it_clear_helper++) {\n (*it_clear_helper)->addParent(nullptr);\n }\n RRT_TREE.clear();\n std::cout << \"Building RRT-tree\" << std::endl;\n node *origin = new node(given_x, given_y, given_z);\n origin->addParent(nullptr);\n RRT_TREE.push_back(origin);\n srand(time(0));\n itterations = 0;\n while (((RRT_TREE.size() <= number_of_nodes_ and run_by_nodes_) or\n (itterations <= number_of_iterations_ and !run_by_nodes_)) and\n itterations < 100000) {\n // Generate a random point\n float x = lowest_x + abs(1024 * rand() / (RAND_MAX + 1.0)) * SCALER_X;\n float y = lowest_y + abs(1024 * rand() / (RAND_MAX + 1.0)) * SCALER_Y;\n float z = lowest_z + abs(1024 * rand() / (RAND_MAX + 1.0)) * SCALER_Z;\n ufo::math::Vector3 random_point(x, y, z);\n ufo::geometry::Sphere point_sphere(random_point, robot_size_);\n if (!isInCollision(myMap, point_sphere, true, false, true,\n planning_depth_) and\n isInCollision(myMap, point_sphere, false, true, false,\n planning_depth_)) {\n float distance = std::numeric_limits::max();\n node *parent;\n std::list::iterator it_node;\n for (it_node = RRT_TREE.begin(); it_node != RRT_TREE.end(); it_node++) {\n ufo::math::Vector3 direction = random_point - *((*it_node)->point);\n double new_distance = abs(direction.norm());\n if (new_distance < distance) {\n distance = new_distance;\n parent = *it_node;\n }\n };\n ufo::math::Vector3 start_point(parent->point->x(), parent->point->y(),\n parent->point->z());\n ufo::geometry::OBB obb = makeOBB(start_point, random_point, robot_size_);\n if (!isInCollision(myMap, obb, true, false, true, planning_depth_)) {\n node *new_node = new node(x, y, z);\n new_node->addParent(parent);\n parent->addChild(new_node);\n RRT_TREE.push_back(new_node);\n }\n };\n itterations++;\n };\n std::cout << \"RRT-tree built successfully\" << std::endl;\n\n if (run_by_nodes_) {\n std::cout << \"Verifying tree\" << std::endl;\n int total_childs = 0;\n int total_parents = 0;\n std::list::iterator it_comeon;\n for (it_comeon = RRT_TREE.begin(); it_comeon != RRT_TREE.end();\n it_comeon++) {\n total_childs = total_childs + (*it_comeon)->myChilds.size();\n if ((*it_comeon)->myParent != nullptr) {\n total_parents++;\n }\n };\n if (total_childs != 0) {\n RRT_created = true;\n }\n if (total_childs == number_of_nodes_) {\n std::cout << \"All children accounted for\" << std::endl;\n } else {\n std::cout << \"Expected \" << number_of_nodes_ << \" children, but \"\n << total_childs << \" was found.\" << std::endl;\n };\n if (total_parents == number_of_nodes_) {\n std::cout << \"All parents accounted for\" << std::endl;\n } else {\n std::cout << \"Expected \" << number_of_nodes_ << \" parents, but \"\n << total_parents << \" was found.\" << std::endl;\n };\n } else {\n std::cout << \"Running by itterations, so the amount of nodes are unknown \"\n \"and hence can't be verified\"\n << std::endl;\n }\n};\n\n// Trajectory\nstd::tuple, double, std::list>\ntrajectory(std::list x, double *u, double N, double dt,\n std::list nmpc_ref) {\n // Calculate the dynamic costs based on selected weights\n double ns = 8;\n std::list p_traj{};\n std::list v_traj{};\n double cost = 0;\n // Weight matrices\n std::list Qx = {\n position_tracking_weight_x_,\n position_tracking_weight_y_,\n position_tracking_weight_z_,\n 0,\n 0,\n 0,\n angle_weight_roll_,\n angle_weight_pitch_}; // Position tracking weights x, y, z, 0, 0, 0, angle\n std::list Ru = {\n input_weight_thrust_, input_weight_roll_,\n input_weight_pitch_}; // input weights (Thrust, roll, pitch)\n std::list Rd = {\n input_rate_weight_thrust_, input_rate_weight_roll_,\n input_rate_weight_pitch_}; // input rate weights (thrust, roll, pitch)\n std::list u_old = {9.81, 0, 0};\n std::list u_ref = {9.81, 0.0, 0.0};\n std::list::iterator x_ref_itterator = nmpc_ref.begin();\n for (int i = 0; i < N; i++) {\n std::list::iterator Qx_itterator = Qx.begin();\n std::list::iterator x_itterator = x.begin();\n\n // Setting up itterators\n std::list::iterator Ru_itterator = Ru.begin();\n std::list::iterator Rd_itterator = Rd.begin();\n std::list::iterator u_old_itterator = u_old.begin();\n\n for (int j = 0; j < 3; j++) {\n cost = cost + *Ru_itterator * pow(u[3 * i + j] - *u_old_itterator, 2);\n cost = cost + *Rd_itterator * pow(u[3 * i + j] - *u_old_itterator, 2);\n Ru_itterator++;\n u_old_itterator++;\n Rd_itterator++;\n }\n u_old.clear();\n u_old = {u[3 * i], u[3 * i + 1], u[3 * i + 2]};\n\n x_itterator = x.begin();\n std::list::iterator x2_itterator = x.begin();\n std::advance(x2_itterator, 3);\n for (int j = 0; j < 3; j++) {\n *x_itterator = *x_itterator + dt * *x2_itterator;\n x_itterator++;\n x2_itterator++;\n }\n std::list::iterator x3_itterator = x.begin();\n std::advance(x3_itterator, 7); // x[7]\n *x_itterator = *x_itterator +\n dt * (sin(*x3_itterator) * cos(*x2_itterator) * u[3 * i] -\n 1 * (*x_itterator));\n x_itterator++; // x[4]\n *x_itterator = *x_itterator +\n dt * (-sin(*x2_itterator) * u[3 * i] - 1 * (*x_itterator));\n x_itterator++; // x[5]\n *x_itterator = *x_itterator +\n dt * (cos(*x3_itterator) * cos(*x2_itterator) * u[3 * i] -\n 1 * *x_itterator - 9.81);\n x_itterator++; // x[6]\n *x_itterator =\n *x_itterator + dt * ((1.0 / 0.5) * (u[3 * i + 1] - *x_itterator));\n x_itterator++; // x[7]\n *x_itterator =\n *x_itterator + dt * ((1.0 / 0.5) * (u[3 * i + 2] - *x_itterator));\n std::advance(x_itterator, -7);\n\n for (int j = 0; j < 8; j++) {\n if (j < 3) {\n cost = cost +\n (*Qx_itterator) * pow((*x_itterator) - (*x_ref_itterator), 2);\n x_ref_itterator++;\n } else {\n cost = cost + (*Qx_itterator) * pow((*x_itterator), 2);\n }\n Qx_itterator++;\n x_itterator++;\n }\n\n x_itterator = x.begin();\n p_traj.push_back(*x_itterator);\n x_itterator++;\n p_traj.push_back(*x_itterator);\n x_itterator++;\n p_traj.push_back(*x_itterator);\n x_itterator++;\n v_traj.push_back(*x_itterator);\n x_itterator++;\n v_traj.push_back(*x_itterator);\n x_itterator++;\n v_traj.push_back(*x_itterator);\n }\n return std::make_tuple(v_traj, cost, p_traj);\n}\n\n// End of functions\n// --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------\n// // Callback functions\n\nvoid mapCallback(ufomap_msgs::UFOMapStamped::ConstPtr const &msg) {\n if (ufomap_msgs::msgToUfo(msg->map, myMap)) {\n map_received = true;\n } else {\n std::cout << \"Conversion failed\" << std::endl;\n }\n}\n\nvoid odomCallback(const nav_msgs::Odometry::ConstPtr &msg) {\n position_received = true;\n position_x = msg->pose.pose.position.x;\n position_y = msg->pose.pose.position.y;\n position_z = msg->pose.pose.position.z;\n velocity_x = msg->twist.twist.linear.x;\n velocity_y = msg->twist.twist.linear.y;\n velocity_z = msg->twist.twist.linear.z;\n tf::Quaternion q(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y,\n msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);\n tf::Matrix3x3 m(q);\n m.getRPY(roll, pitch, yaw);\n}\n\n// End of callback functions\n// --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------\n// // Main\n\ntypedef rrtCache *(*arbitrary)();\ntypedef rrtSolverStatus (*arbitrary2)(void *, double *, double *, double,\n double *);\ntypedef void (*rrt_clearer)(rrtCache *);\n\nint main(int argc, char *argv[]) {\n // Subscribers and publishers\n ros::init(argc, argv, \"errt\");\n ros::NodeHandle nh;\n\n ros::Publisher unknowns_pub =\n nh.advertise(\"UNKNOWN_NODES\", 1);\n ros::Publisher points_pub =\n nh.advertise(\"tree_expansion\", 1);\n // ros::Publisher chosen_path_visualization_pub =\n // nh.advertise(\"selected_branch\",\n // 1);\n ros::Publisher output_path_pub =\n nh.advertise(\"selected_trajectory\", 1);\n ros::Publisher chosen_path_pub =\n nh.advertise(\"reference_out_\", 1);\n\n // change it to path msg\n ros::Publisher all_path_pub =\n nh.advertise(\"candidate_branches\", 1);\n ros::Publisher goal_pub =\n nh.advertise(\"candidate_goals\", 1);\n ros::Publisher map_pub =\n nh.advertise(\"Internal_ufo_map\", 11);\n ros::Subscriber map_sub = nh.subscribe(\"ufomap_in_\", 1, mapCallback);\n ros::Subscriber sub = nh.subscribe(\"odometry_in_\", 1, odomCallback);\n ros::Publisher hits_pub =\n nh.advertise(\"predicted_info_gain\", 1);\n ros::Publisher position_pub =\n nh.advertise(\"robot_bounding_box\", 1);\n\n // change to path msg\n ros::Publisher taken_path_pub =\n nh.advertise(\"exploration_path\", 1);\n ros::Publisher execution_time_pub =\n nh.advertise(\"errt_execution_time\", 1);\n\n m_command_Path_Publisher =\n nh.advertise(\"command_path\", 1, true);\n\n m_trajectory_Publisher =\n nh.advertise(\"path_out_\", 1,\n true);\n ros::Rate rate(10);\n\n // Initial point\n // This manually sets the first point which the drone will travel to.\n // For each point in the path CHOSEN_PATH, one needs to add the VREF vx, vy,\n // vz to CHOSEN_PATH_VREF.\n ros::param::get(ros::this_node::getName() + \"/start_from_waypoint\",\n start_from_waypoint_);\n\n ros::param::get(ros::this_node::getName() + \"/initial_x_\", initial_x_);\n ros::param::get(ros::this_node::getName() + \"/initial_y_\", initial_y_);\n ros::param::get(ros::this_node::getName() + \"/initial_z_\", initial_z_);\n\n ros::param::get(ros::this_node::getName() + \"/map_frame_id\", map_frame_id_);\n\n ros::param::get(ros::this_node::getName() + \"/run_by_nodes\", run_by_nodes_);\n ros::param::get(ros::this_node::getName() + \"/number_of_nodes\",\n number_of_nodes_);\n ros::param::get(ros::this_node::getName() + \"/number_of_goals\",\n number_of_goals_);\n ros::param::get(ros::this_node::getName() + \"/number_of_iterations\",\n number_of_iterations_);\n\n ros::param::get(ros::this_node::getName() + \"/min_info_goal\", min_info_goal_);\n ros::param::get(ros::this_node::getName() + \"/goal_sensor_range_\",\n goal_sensor_range_);\n ros::param::get(ros::this_node::getName() + \"/robot_size\", robot_size_);\n ros::param::get(ros::this_node::getName() + \"/goal_connect_dist\",\n goal_connect_dist_);\n\n ros::param::get(ros::this_node::getName() + \"/dist_nodes\", dist_nodes_);\n ros::param::get(ros::this_node::getName() + \"/dist_goals\", dist_goals_);\n ros::param::get(ros::this_node::getName() + \"/min_dist_to_goal\",\n min_dist_to_goal_);\n ros::param::get(ros::this_node::getName() + \"/planning_depth\",\n planning_depth_);\n\n ros::param::get(ros::this_node::getName() + \"/v_local\", v_local_);\n\n ros::param::get(ros::this_node::getName() + \"/k_dist\", k_dist_);\n ros::param::get(ros::this_node::getName() + \"/k_info\", k_info_);\n ros::param::get(ros::this_node::getName() + \"/k_u\", k_u_);\n ros::param::get(ros::this_node::getName() + \"/path_update_dist\",\n path_update_dist_);\n ros::param::get(ros::this_node::getName() + \"/recalc_dist\", recalc_dist_);\n ros::param::get(ros::this_node::getName() + \"/path_improvement_max\",\n path_improvement_max_);\n\n ros::param::get(ros::this_node::getName() + \"/sensor_range\", sensor_range_);\n ros::param::get(ros::this_node::getName() + \"/min_sensor_range\",\n min_sensor_range_);\n ros::param::get(ros::this_node::getName() + \"/senros_vertical_fov\",\n sensor_vertical_fov_);\n\n ros::param::get(ros::this_node::getName() + \"/nmpc_horizon\", nmpc_horizon_);\n ros::param::get(ros::this_node::getName() + \"/nmpc_dt\", nmpc_dt_);\n ros::param::get(ros::this_node::getName() + \"/position_tracking_weight_x\",\n position_tracking_weight_x_);\n ros::param::get(ros::this_node::getName() + \"/position_tracking_weight_y\",\n position_tracking_weight_y_);\n ros::param::get(ros::this_node::getName() + \"/position_tracking_weight_z\",\n position_tracking_weight_z_);\n ros::param::get(ros::this_node::getName() + \"/angle_weight_roll\",\n angle_weight_roll_);\n ros::param::get(ros::this_node::getName() + \"/angle_weight_pitch\",\n angle_weight_pitch_);\n ros::param::get(ros::this_node::getName() + \"/input_weight_thrust\",\n input_weight_thrust_);\n ros::param::get(ros::this_node::getName() + \"/input_weight_roll\",\n input_weight_roll_);\n ros::param::get(ros::this_node::getName() + \"/input_weight_pitch\",\n input_weight_pitch_);\n ros::param::get(ros::this_node::getName() + \"/input_rate_weight_thrust\",\n input_rate_weight_thrust_);\n ros::param::get(ros::this_node::getName() + \"/input_rate_weight_roll\",\n input_rate_weight_roll_);\n ros::param::get(ros::this_node::getName() + \"/input_rate_weight_pitch\",\n input_rate_weight_pitch_);\n\n if (start_from_waypoint_) {\n // start_from_waypoint_ = false;\n ros::param::get(ros::this_node::getName() + \"/initial_x\", initial_x_);\n ros::param::get(ros::this_node::getName() + \"/initial_y\", initial_y_);\n ros::param::get(ros::this_node::getName() + \"/initial_z\", initial_z_);\n std::cout << \"Init reference : \" << initial_x_ << \", \" << initial_y_ << \", \"\n << initial_z_ << std::endl;\n CHOSEN_PATH.push_back(new node(initial_x_, initial_y_, initial_z_));\n fetched_path = true;\n RRT_created = true;\n GOALS_generated = true;\n position_received = true;\n allowNewPath = false;\n currentTarget = *CHOSEN_PATH.begin();\n goalNode = *CHOSEN_PATH.begin();\n CHOSEN_PATH_VREF.push_back(0);\n CHOSEN_PATH_VREF.push_back(0);\n CHOSEN_PATH_VREF.push_back(0);\n vref_itterator = CHOSEN_PATH_VREF.begin();\n\n generateTrajectory();\n publishTrajectory();\n }\n // Main\n // When the ufomap and current position have been received through their\n // respective callback functions, the rrt executes by: 1) Tuning the\n // generation and generating goals. 2) Generating the RRT tree and attatching\n // generated goals. 3) Set the path. 4) Evaluates the current point. 5)\n // Trigger global strategy if necessary.\n while (ros::ok()) {\n\n high_resolution_clock::time_point start_total =\n high_resolution_clock::now();\n\n if (map_received and not GOALS_generated and position_received) {\n\n if (CHOSEN_PATH.empty()) {\n\n tuneGeneration(myMap, false, true, false, position_x, position_y,\n position_z, planning_depth_);\n } else {\n\n tuneGeneration(myMap, false, true, false,\n (*(--CHOSEN_PATH.end()))->point->x(),\n (*(--CHOSEN_PATH.end()))->point->y(),\n (*(--CHOSEN_PATH.end()))->point->z(), planning_depth_);\n }\n\n high_resolution_clock::time_point start_total =\n high_resolution_clock::now();\n\n generateGoals(myMap, true);\n }\n\n if (map_received and not RRT_created and GOALS_generated) {\n\n if (CHOSEN_PATH.empty()) {\n\n generateRRT(position_x, position_y, position_z);\n } else {\n\n high_resolution_clock::time_point start_total =\n high_resolution_clock::now();\n\n generateRRT((*(--CHOSEN_PATH.end()))->point->x(),\n (*(--CHOSEN_PATH.end()))->point->y(),\n (*(--CHOSEN_PATH.end()))->point->z());\n }\n\n if (RRT_created) {\n\n findShortestPath();\n }\n\n itterations = 0; // DO NOT TOUCH!\n }\n\n if (map_received and RRT_created) {\n\n if (!fetched_path) {\n\n high_resolution_clock::time_point start_total =\n high_resolution_clock::now();\n\n setPath();\n\n generateTrajectory();\n\n publishTrajectory();\n }\n\n if (fetched_path and goalNode != nullptr) {\n\n itterations++;\n\n evaluateCurrentPoint(&chosen_path_pub);\n }\n\n if ((goalNode == nullptr and GOALS_generated)) {\n if (initialGoalInfo < GLOBAL_STRATEGY_THRESHOLD and\n not recoveryUnderway) {\n tuneGeneration(myMap, false, true, false, position_x, position_y,\n position_z, planning_depth_);\n for (int i = 0; i < 3; i++) {\n generateGoals(myMap, false);\n generateRRT(position_x, position_y, position_z);\n allowNewPath = true;\n setPath();\n }\n }\n }\n }\n\n high_resolution_clock::time_point stop_total = high_resolution_clock::now();\n auto duration_total =\n duration_cast(stop_total - start_total);\n\n std_msgs::Float64MultiArray planning_time;\n\n planning_time.data = {duration_total.count(), ros::Time::now().toSec()};\n if (!duration_total.count() == 0) {\n\n execution_time_pub.publish(planning_time);\n cout << \"\\nExecution time: \" << duration_total.count() << \" ms for \"\n << myGoals.size() << \" path/s.\" << endl;\n }\n\n updatePathTaken();\n visualize(&points_pub, &output_path_pub, &all_path_pub, &goal_pub,\n &hits_pub, &taken_path_pub, &map_pub, &position_pub,\n &unknowns_pub);\n ros::spinOnce();\n rate.sleep();\n }\n return 0;\n}\n\n// --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------\n// //\n\n// ***** fix path length after NMPC calculation ** //\n\n// ***** computation time plot ** //\n" }, { "path": "trajectory.py", "content": "def trajectory(x, u, N, dt,trajectory):\n #Based on the initial condition and optimized trajectory u, computed the path as (x,y,z).\n #Calculate the dynamic costs based on selected weights \n import math\n import numpy as np\n ns = 8\n p_traj = []\n v_traj = []\n cost = 0\n ## Weight matrices\n Qx = (5,5,5, 0, 0,0, 5, 5) \n #P = 2*Qx; #final state weight\n Ru = (100,100,100); # input weights\n Rd = (100, 100, 100); # input rate weights\n #print(x, u, N, dt)\n u_old = [9.81,0,0]\n for i in range(0,N):\n####State costs\n position = trajectory[(3*i):(3*i+3)]\n #print(x_ref)\n \n #State weights\n####Input Cost\n u_n = u[(3*i):3*i+3]\n cost += Ru[0]*(u_n[0] - 9.81)**2 + Ru[1]*(u_n[1])**2 + Ru[2]*(u_n[2])**2 #Input weights\n #print(Ru[0], u_n[0], 9.81, Ru[1], u_n[1], 0, Ru[2], u_n[2], 0)\n cost += Rd[0]*(u_n[0] - u_old[0])**2 + Rd[1]*(u_n[1] - u_old[1])**2 + Rd[2]*(u_n[2] - u_old[2])**2 #Input rate weights\n #print(Rd[0], u_n[0], u_old[0], Rd[1], u_n[1], u_old[1], Rd[2], u_n[2], u_old[2]);\n u_old = u_n\n x[0] = x[0] + dt * x[3]\n x[1] = x[1] + dt * x[4] \n x[2] = x[2] + dt * x[5] \n x[3] = x[3] + dt * (math.sin(x[7]) * math.cos(x[6]) * u[3*i] - 1 * x[3]) \n x[4] = x[4] + dt * (-math.sin(x[6]) * u[3*i] - 1*x[4])\n x[5] = x[5] + dt * (math.cos(x[7]) * math.cos(x[6]) * u[3*i] - 1 * x[5] - 9.81) \n x[6] = x[6] + dt * ((1 / 0.5) * (u[3*i+1] - x[6])) \n x[7] = x[7] + dt * ((1 / 0.5) * (u[3*i+2] - x[7]))\n cost += Qx[0]*(x[0]-position[0])**2 + Qx[1]*(x[1]-position[1])**2 + Qx[2]*(x[2]-position[2])**2 + Qx[3]*(x[3])**2 + Qx[4]*(x[4])**2 + Qx[5]*(x[5])**2 + Qx[6]*(x[6])**2 + Qx[7]*(x[7])**2\n print(\"This is good\");\n print(Qx[0], x[0], position[0], Qx[1], x[1], position[1], Qx[2], x[2], position[2], Qx[3], x[3], Qx[4], x[4], Qx[5], x[5], Qx[6], x[6], Qx[7], x[7])\n p_traj = p_traj + [[x[0],x[1],x[2]]]\n v_traj = v_traj + [[x[3],x[4],x[5]]]\n #print(cost)\n #print(p_traj)\n return(p_traj, v_traj, cost)\n" } ]