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