Showing preview only (227K chars total). Download the full file or copy to clipboard to get everything.
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
##
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.
# 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. <br> This significantly speeds up various volumetric occupancy checks. <br> 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. <br> 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. <br> 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. <br> 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, <br> set by subsequent *x,y,z*-coordinates. |
## Test ERRT in a docker container
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 <silent> <C-F2> :if &guioptions =~# 'T' <Bar>
\set guioptions-=T <Bar>
\set guioptions-=m <Bar>
\else <Bar>
\set guioptions+=T <Bar>
\set guioptions+=m <Bar>
\endif<CR>
" set guioptions-=r
" Make the scrollbars toggable
" map <silent> <C-F3> :if &guioptions =~# 'r' <Bar>
" \set guioptions-=r <Bar>
" \else <Bar>
" \set guioptions+=r <Bar>
" \endif<CR>
" 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 <C-F11> :if &guioptions=~'m' \| set guioptions-=m \| else \| set guioptions+=m \| endif<CR>
" CTRL+F7 to toggle the right-hand scroll bar
nmap <C-F7> :if &guioptions=~'r' \| set guioptions-=r \| else \| set guioptions+=r \| endif<CR>
" CTRL+F6 to toggle the left-hand scroll bar
nmap <C-F6> :if &guioptions=~'lL' \| set guioptions-=lL \| else \| set guioptions+=lL \| endif<CR>
" CTRL+F5 to toggle the toolbar
nmap <C-F5> :if &guioptions=~'T' \| set guioptions-=T \| else \| set guioptions+=T \| endif<CR>
"""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""
" 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 <F6> :Tlist<CR>
"""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""
" Text options
"""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""
"Iabbr fori for <datum> in <data>:<CR><datum> <:system('date')><CR><CR>Cursor here:<>
vmap <C-C> "+y
nmap <C-V> "+gP
imap <C-V> <ESC
filetype off " required
" set the runtime path to include Vundle and initialize
set rtp+=~/.vim/bundle/Vundle.vim
call vundle#begin()
" let Vundle manage Vundle, required
Plugin 'VundleVim/Vundle.vim'
" Docker
Plugin 'ekalinin/Dockerfile.vim'
Bundle 'sonph/onehalf', {'rtp': 'vim/'}
Plugin 'huyvohcmc/atlas.vim'
" added nerdtree
Plugin 'scrooloose/nerdtree'
" Vim Airline
" lean & mean status/tabline for vim that's light as air
Plugin 'vim-airline/vim-airline'
Plugin 'vim-airline/vim-airline-themes'
Plugin 'bling/vim-bufferline'
Plugin 'vim-syntastic/syntastic'
Plugin 'vim-scripts/taglist.vim'
Plugin 'wincent/command-t'
" Gruvbox theme
Plugin 'morhetz/gruvbox'
" Distraction-free writing in Vim.
Plugin 'junegunn/goyo.vim'
" I'm not going to lie to you; fugitive.vim may very well be the best Git
" wrapper of all time.
Plugin 'tpope/vim-fugitive'
" Syntax highlighting, matching rules and mappings for the original Markdown
" and extensions.
Plugin 'plasticboy/vim-markdown'
" This plugin is used for displaying thin vertical lines at each indentation
" level for code indented with spaces.
Plugin 'yggdroot/indentline'
" Hyperfocus-writing in Vim.
" https://github.com/junegunn/limelight.vim
Plugin 'junegunn/limelight.vim'
Plugin 'sheerun/vim-polyglot'
Plugin 'dhruvasagar/vim-table-mode'
" When you wish to unload a file from the buffer and keep the window/split
" intact:
" :BUN
" When you wish to delete a file from the buffer and keep the window/split
" intact:
" :BD
" When you wish to wipe a file from the buffer and keep the window/split intact:
" :BW
" Notice how the key mappings are the uppercase version of the :bun :bd :bw Vim
" commands? Easy!
Plugin 'qpkorr/vim-bufkill'
Plugin 'twe4ked/vim-colorscheme-switcher'
" All of your Plugins must be added before the following line
call vundle#end() " required
filetype plugin indent on " required
let g:goyo_width = '60%'
" air-line
let g:airline_powerline_fonts = 1
if !exists('g:airline_symbols')
let g:airline_symbols = {}
endif
" unicode symbols
let g:airline_left_sep = '»'
let g:airline_left_sep = '▶'
let g:airline_right_sep = '«'
let g:airline_right_sep = '◀'
let g:airline_symbols.linenr = '␊'
let g:airline_symbols.linenr = ''
let g:airline_symbols.linenr = '¶'
let g:airline_symbols.branch = '⎇'
"let g:airline_symbols.paste = 'ρ'
"let g:airline_symbols.paste = 'Þ'
"let g:airline_symbols.paste = '∥'
let g:airline_symbols.whitespace = 'Ξ'
" airline symbols
let g:airline_left_sep = ''
let g:airline_left_alt_sep = ''
let g:airline_right_sep = ''
let g:airline_right_alt_sep = ''
let g:airline_symbols.branch = ''
let g:airline_symbols.readonly = ''
let g:airline_symbols.linenr = ''
" To ignore plugin indent changes, instead use:
"filetype plugin on
"
" Brief help
" :PluginList - lists configured plugins
" :PluginInstall - installs plugins; append `!` to update or just :PluginUpdate
" :PluginSearch foo - searches for foo; append `!` to refresh local cache
" :PluginClean - confirms removal of unused plugins; append `!` to auto-approve removal
"
" see :h vundle for more details or wiki for FAQ
" Put your non-Plugin stuff after this line
" Make Cut Copy Paste work in Unity with CtrlX, CtrlC and CtrlV
" http://superuser.com/questions/10588/how-to-make-cut-copy-paste-in-gvim-on-ubuntu-work-with-ctrlx-ctrlc-ctrlv
source $VIMRUNTIME/mswin.vim
behave mswin
"set statusline+=%#warningmsg#
"set statusline+=%{SyntasticStatuslineFlag()}
"set statusline+=%*
let g:syntastic_always_populate_loc_list = 1
let g:syntastic_auto_loc_list = 1
let g:syntastic_check_on_open = 1
let g:syntastic_check_on_wq = 0
let mapleader = ","
" nnoremap / /\v
" vnoremap / /\v
set ignorecase
set smartcase
" gdefault applies substitutions globally on lines. For example, instead of
" :%s/foo/bar/g you just type :%s/foo/bar/
set visualbell
set cursorline
set ttyfast
set ruler
set backspace=indent,eol,start
set laststatus=2
"""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""
" VIM userinterface
"""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""
set gdefault
set incsearch
set showmatch
"Highlight search things
set hlsearch
nnoremap <leader><space> :noh<cr>
nnoremap <tab> %
vnoremap <tab> %
set wrap
set textwidth=120
set formatoptions=qrn1
" Normally <F1> gives you the Help.
" When it gets hit on, you prob. wannted the <ESC>.
" Let's remap <F1> to <ESC> ;-)
inoremap <F1> <ESC>
nnoremap <F1> <ESC>
vnoremap <F1> <ESC>
" 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 <C-C> "+y
""nmap <C-V> "+gP
"imap <C-V> <ESC
command! Visual normal! v
command! VisualLine normal! V
command! VisualBlock execute "normal! \<C-v>"
"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
================================================
<?xml version="1.0" ?>
<launch> <!-- Parameter descriptions -->
<env name="ROSCONSOLE_FORMAT" value="[${severity}] : ${message}"/>
<arg name="errt_config" default="$(find errt)/config/errt.yaml"/>
<node pkg="errt" type="errt_node" name="errt_node" output="screen" required="true">
<rosparam command="load" file="$(arg errt_config)" />
<remap from="odometry_in_" to="/hummingbird/ground_truth/odometry"/> <!-- The odometry topic -->
<remap from="ufomap_in_" to="ufomap_mapping_server_node/map"/> <!-- The ufomap topic -->
<remap from="reference_out_" to="/hummingbird/reference"/> <!-- References along trajectory, updated by path_update_dist-->
<remap from="path_out_" to="/hummingbird/command/trajectory"/> <!-- Path topic as a MultiDOFJointTrajectory message - sent to trajectory tracking controller -->
</node>
<!-- <node type="visualize_navigation" name="visualize_navigation" pkg="visualization_tools" /> -->
</launch>
================================================
FILE: launch/octomap_gazebo.launch
================================================
<?xml version="1.0"?>
<!--
Example launch file for octomap_server mapping:
Listens to incoming PointCloud2 data and incrementally builds an octomap.
The data is sent out in different representations.
Copy this file into your workspace and adjust as needed, see
www.ros.org/wiki/octomap_server for details
-->
<launch>
<node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
<param name="resolution" value="0.3" />
<param name="/use_sim_time" value="false" />
<!-- fixed map frame (set to 'map' if SLAM or localization running!) -->
<param name="frame_id" type="string" value="world" />
<!-- maximum range to integrate (speedup!) -->
<param name="sensor_model/max_range" value="15.0" />
<param name="sensor_model/min_range" value="0.5" />
<!-- data source to integrate (PointCloud2) -->
<!-- <remap from="cloud_in" to="/os_cloud_node/points" /> -->
<remap from="cloud_in" to="/hummingbird/velodyne_points" />
<!-- <remap from="cloud_in" to="rmf_obelix/velodyne_points" /> -->
<param name="publish_free_space" value="true"/>
<!--param name="pointcloud_[min|max]_z" value="5.0" /-->
<!--param name="occupancy_[min|max]_z" value="5.0" /-->
</node>
<!--<node pkg="tf" type="static_transform_publisher" name="fake_loc" args="0 0 0 0 0 0 odom world 100" /-->
</launch>
<!--param name="occupancy_min_z" value="-0.5" /-->
<!--param name="occupancy_max_z" value="1.5" /-->
<!--param name="publish_free_space" value="true" /-->
================================================
FILE: launch/server.launch
================================================
<?xml version="1.0" ?>
<launch>
<arg name="resolution" default="0.2" />
<arg name="depth_levels" default="16" />
<arg name="num_workers" default="10" />
<arg name="color" default="true" />
<node pkg="ufomap_mapping" type="ufomap_mapping_server_node" name="ufomap_mapping_server_node" required="true">
<param name="cloud_in" value="/hummingbird/ouster/points" />
<!-- <param name="cloud_in" value="/velodyne_points" /> -->
<param name="frame_id" type="string" value="world" />
<remap from="cloud_in" to="/hummingbird/ouster/points" />
<!-- <remap from="cloud_in" to="/velodyne_points" /> -->
<param name="robot_frame_id" value="hummingbird/base_link" />
<!-- <param name="robot_frame_id" value="base_link" /> -->
<param name="num_workers" value="$(arg num_workers)" />
<param name="max_range" value="20" />
<param name="min_range" value="0.5" />
<param name="robot_radius" value="0.3" />
<param name="robot_height" value="0.3" />
<param name="compress" value="True" />
<param name="simple_ray_casting" value="True" />
<param name="clear_robot" value="True" />
<!-- FOR SHAFTER/PIONEER/SPOT CLEAR ROBOT SHOULD BE SET TO TRUE -->
<!-- <param name="clear_robot" value="True" /> -->
<param name="clearing_depth" value="2" />
<param name="publish_depth" value="4" />
<param name="pub_rate" value="1" />
<param name="insert_depth" value="1" />
<param name="resolution" value="$(arg resolution)" />
<param name="depth_levels" value="$(arg depth_levels)" />
<param name="color_map" value="$(arg color)"/>
</node>
<node type="rviz" name="rviz" pkg="rviz" args="-d $(find errt)/rviz/errt.rviz" />
<node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
<param name="resolution" value="0.3" />
<param name="/use_sim_time" value="false" />
<!-- fixed map frame (set to 'map' if SLAM or localization running!) -->
<param name="frame_id" type="string" value="world" />
<!-- maximum range to integrate (speedup!) -->
<param name="sensor_model/max_range" value="15.0" />
<param name="sensor_model/min_range" value="0.5" />
<!-- data source to integrate (PointCloud2) -->
<!-- <remap from="cloud_in" to="/os_cloud_node/points" /> -->
<remap from="cloud_in" to="/hummingbird/ouster/points" />
<!-- <remap from="cloud_in" to="rmf_obelix/velodyne_points" /> -->
<param name="publish_free_space" value="true"/>
<!--param name="pointcloud_[min|max]_z" value="5.0" /-->
<!--param name="occupancy_[min|max]_z" value="5.0" /-->
</node>
</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
================================================
<?xml version="1.0"?>
<package format="2">
<name>errt</name>
<version>1.0.0</version>
<description>Exploration-RRT Ros package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="akapat@ltu.se">aakapatel</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>dynamic_reconfigure</build_depend>
<build_depend>ufomap</build_depend>
<build_depend>ufomap_msgs</build_depend>
<build_depend>ufomap_ros</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>roscpp_serialization</build_depend>
<build_depend>tf2_ros</build_depend>
<build_depend>tf</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>std_srvs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>visualization_msgs</build_depend>
<build_depend>pcl_ros</build_depend>
<build_depend>eigen_catkin</build_depend>
<build_depend>move_base_msgs</build_depend>
<build_depend>actionlib</build_depend>
<build_depend>actionlib_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>catkin_simple</build_depend>
<exec_depend>ufomap</exec_depend>
<exec_depend>ufomap_msgs</exec_depend>
<exec_depend>ufomap_ros</exec_depend>
<exec_depend>roscpp_serialization</exec_depend>
<exec_depend>tf2_ros</exec_depend>
<exec_depend>tf</exec_depend>
<exec_depend>mav_msgs</exec_depend>
<exec_depend>mav_planning_msgs</exec_depend>
<exec_depend>move_base_msgs</exec_depend>
<exec_depend>visualization_msgs</exec_depend>
<exec_depend>actionlib</exec_depend>
<exec_depend>actionlib_msgs</exec_depend>
<exec_depend>eigen_catkin</exec_depend>
<exec_depend>octomap_rviz_plugins</exec_depend>
<exec_depend>octomap_ros</exec_depend>
<exec_depend>message_runtime</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
================================================
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: <Fixed 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: <Fixed 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: <Fixed 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: <Fixed 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: <Fixed 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: <Fixed 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: <Fixed 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: <Fixed 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: <Fixed 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<std::list<double>, double, std::list<double>> trajectory(std::list<double> x, double* u, double N, double dt, std::list<double> 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<double> p_traj{};
std::list<double> v_traj{};
double cost = 0;
// Weight matrices
std::list<double> Qx = {5,5,5, 0, 0,0, 5, 5};
// P = 2*Qx; #final state weight
std::list<double> Ru = {15,15,15}; // input weights
std::list<double> Rd = {10, 10, 10}; // input rate weights
// print(x, u, N, dt)
std::list<double> u_old = {9.81,0.0,0.0};
std::list<double> u_ref = {9.81,0.0,0.0};
for(int i = 0; i < N; i++){
// State costs
// std::list<float> x_ref = nmpc_ref[(ns*i):(ns*i+ns)];
// #print(x_ref)
// Setting up itterators
std::list<double>::iterator Qx_itterator = Qx.begin();
//std::advance(Qx_itterator, i);
std::list<double>::iterator x_itterator = x.begin();
//std::advance(x_itterator, i);
std::list<double>::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<double>::iterator Ru_itterator = Ru.begin();
std::list<double>::iterator Rd_itterator = Rd.begin();
//std::list<float>::iterator u_n_itterator = u.begin();
//std::advance(u_n_itterator, 3 * i);
std::list<double>::iterator u_ref_itterator = u_ref.begin();
std::list<double>::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<double>::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<double>::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<double>::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<double>::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 <chrono>
#include <dlfcn.h>
#include <dynamic_reconfigure/server.h>
#include <geometry_msgs/PoseStamped.h>
#include <iostream>
#include <list>
#include <math.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <ros/package.h>
#include <ros/ros.h>
#include <rrt/rrt_bindings.h>
#include <stdlib.h>
#include <tf/tf.h>
#include <ufo/map/occupancy_map.h>
#include <ufo/map/occupancy_map_color.h>
#include <ufomap_msgs/UFOMapStamped.h>
#include <ufomap_msgs/conversions.h>
#include <ufomap_ros/conversions.h>
#include <visualization_msgs/Marker.h>
#include "std_msgs/Int64.h"
#include <algorithm>
#include <bits/stdc++.h>
#include <chrono>
#include <climits>
#include <cmath>
#include <dlfcn.h>
#include <eigen3/Eigen/Dense>
#include <fstream>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <iostream>
#include <iterator>
#include <limits>
#include <list>
#include <math.h>
#include <mav_msgs/Status.h>
#include <mav_msgs/conversions.h>
#include <mav_msgs/default_topics.h>
#include <memory>
#include <message_filters/subscriber.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <octomap/OcTreeKey.h>
#include <octomap/octomap.h>
#include <octomap_msgs/BoundingBoxQuery.h>
#include <octomap_msgs/GetOctomap.h>
#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>
#include <octomap_ros/conversions.h>
#include <pcl/conversions.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <queue>
#include <ros/console.h>
#include <ros/message_traits.h>
#include <ros/package.h>
#include <ros/ros.h>
#include <ros/serialization.h>
#include <sensor_msgs/PointCloud2.h>
#include <set>
#include <std_msgs/ColorRGBA.h>
#include <std_msgs/String.h>
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <tf/message_filter.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <trajectory_msgs/MultiDOFJointTrajectory.h>
#include <ufo/math/vector3.h>
#include <unordered_map>
#include <utility>
#include <variant>
#include <vector>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
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<ufo::math::Vector3> unknowns_in_sight_;
std::list<struct node *> myChilds{};
std::list<struct node *> myParents{};
std::list<struct node *> myPath{};
std::list<ufo::math::Vector3> 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<struct node *> *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<ufo::math::Vector3> 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<ufo::math::Vector3> 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<ufo::math::Vector3> myTotalHits{};
addHits(&myTotalHits);
int hits = myTotalHits.size();
return hits;
}
void clearInformationGain() {
myHits.clear();
if (myParent != nullptr) {
myParent->clearInformationGain();
}
}
void addHits(std::list<ufo::math::Vector3> *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<microseconds>(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<microseconds>(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<microseconds>(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<float>::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<struct node *> RRT_TREE{};
std::list<struct node *> CHOSEN_PATH{};
std::list<double> CHOSEN_PATH_VREF{};
std::list<double>::iterator vref_itterator;
std::list<struct node *> ALL_PATH{};
std::list<struct node *> myGoals{};
std::list<struct node *> myReserveGoals{};
std::list<ufo::math::Vector3> hits{};
std::list<node *> VISITED_POINTS{};
std::list<node *>::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<float> Euler2Quaternion(Eigen::Vector3d v) {
float roll = v.x();
float pitch = v.y();
float yaw = v.z();
Eigen::Quaternion<float> 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<float> 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<geometry_msgs::Pose> 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<Eigen::Vector3d, double>
traj_ref_; // position and corresponding velocity reference.
std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> traj_ref_vec_;
traj_ref_vec_.clear();
std::vector<Eigen::Vector3d> position_vec;
std::vector<Eigen::Vector3d> 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<Eigen::Vector3d, Eigen::Vector3d> 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<ufo::math::Vector3>::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<node *>::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<node *>::iterator it_comeon_visualizer5;
ALL_PATH.clear();
for (it_comeon_visualizer5 = myGoals.begin();
it_comeon_visualizer5 != myGoals.end(); it_comeon_visualizer5++) {
std::list<node *> 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<node *>::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<node *>::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<ufo::math::Vector3>::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<node *>::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<node *>::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<float>::max();
lowest_y = std::numeric_limits<float>::max();
lowest_z = std::numeric_limits<float>::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<node *>::iterator it_goals = myGoals.begin();
it_goals != myGoals.end(); it_goals++) {
struct node *chosenNode = nullptr;
double distance = std::numeric_limits<double>::max();
for (std::list<node *>::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<float>::max();
double totalCost = std::numeric_limits<float>::max();
for (std::list<node *>::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<ufo::math::Vector3>::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<node *>::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<node *>::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<node *>::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<node *>::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<float>::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<float>::max();
}
double newCost = std::numeric_limits<float>::max();
if (allowNewPath) {
std::list<double> PATH_CONTAINER{};
initialGoalInfo = 0;
for (std::list<node *>::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<double> xref = {};
std::list<double> 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<struct node *> 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<node *>::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<double> uold = {9.81, 0.0, 0.0};
std::list<double> uref = {9.81, 0.0, 0.0};
std::list<double> x_hist;
std::list<double> p_hist;
double cost;
std::tuple<std::list<double>, double, std::list<double>> trajectory(
std::list<double> x, double *u, double N, double dt,
std::list<double> 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<double> 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<ufo::math::Vector3>::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_goa
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
SYMBOL INDEX (58 symbols across 5 files)
FILE: controller/errt_nmpc.py
function quaternion_to_euler (line 68) | def quaternion_to_euler(x, y, z, w):
function callback_pot (line 85) | def callback_pot(data):
function callback_lio (line 110) | def callback_lio(data):
function callback_imu (line 127) | def callback_imu(imu_data):
function callback_safety (line 139) | def callback_safety(data):
function callback_start (line 143) | def callback_start(data):
function callback_ref (line 155) | def callback_ref(data):
function PANOC (line 170) | def PANOC():
FILE: controller/quaternion_to_euler.py
function quaternion_to_euler (line 1) | def quaternion_to_euler(x, y, z, w):
FILE: src/Trajectory.cpp
function trajectory (line 1) | std::tuple<std::list<double>, double, std::list<double>> trajectory(std:...
FILE: src/errt.cpp
function makeOBB (line 102) | ufo::geometry::OBB makeOBB(ufo::math::Vector3 source, ufo::math::Vector3...
type rrtSolverStatus (line 121) | struct rrtSolverStatus
type rrtCache (line 121) | struct rrtCache
type rrtCache (line 125) | struct rrtCache
type node (line 130) | struct node {
type node (line 136) | struct node
type node (line 137) | struct node
type node (line 138) | struct node
method addParents (line 141) | void addParents() {
method node (line 163) | node(float x, float y, float z) { point = new ufo::math::Vector3(x, y,...
method node (line 164) | node(ufo::math::Vector3 *givenPoint) {
method addChild (line 169) | void addChild(node *newChild) { myChilds.push_back(newChild); }
method addParent (line 170) | void addParent(node *newParent) { myParent = newParent; }
method changeDistanceToGoal (line 171) | void changeDistanceToGoal(double newDistance) {
method changeDistanceToParent (line 174) | void changeDistanceToParent() {
method sumDistance (line 183) | double sumDistance() {
method getPath (line 196) | void getPath(std::list<struct node *> *givenPath) {
method extractUnknownVoxelsInsideFrustum (line 215) | void
method findInformationGain (line 257) | int findInformationGain(float v_local_, float givenHorizontal,
method clearInformationGain (line 337) | void clearInformationGain() {
method addHits (line 343) | void addHits(std::list<ufo::math::Vector3> *hitList) {
method findPathImprovement (line 364) | bool findPathImprovement(struct node *targetNode,
method readyForDeletion (line 463) | void readyForDeletion() { delete point; }
method isInCollision (line 465) | bool isInCollision(ufo::map::OccupancyMapColor const &map,
type node (line 574) | struct node
type node (line 136) | struct node
type node (line 137) | struct node
type node (line 138) | struct node
method addParents (line 141) | void addParents() {
method node (line 163) | node(float x, float y, float z) { point = new ufo::math::Vector3(x, y,...
method node (line 164) | node(ufo::math::Vector3 *givenPoint) {
method addChild (line 169) | void addChild(node *newChild) { myChilds.push_back(newChild); }
method addParent (line 170) | void addParent(node *newParent) { myParent = newParent; }
method changeDistanceToGoal (line 171) | void changeDistanceToGoal(double newDistance) {
method changeDistanceToParent (line 174) | void changeDistanceToParent() {
method sumDistance (line 183) | double sumDistance() {
method getPath (line 196) | void getPath(std::list<struct node *> *givenPath) {
method extractUnknownVoxelsInsideFrustum (line 215) | void
method findInformationGain (line 257) | int findInformationGain(float v_local_, float givenHorizontal,
method clearInformationGain (line 337) | void clearInformationGain() {
method addHits (line 343) | void addHits(std::list<ufo::math::Vector3> *hitList) {
method findPathImprovement (line 364) | bool findPathImprovement(struct node *targetNode,
method readyForDeletion (line 463) | void readyForDeletion() { delete point; }
method isInCollision (line 465) | bool isInCollision(ufo::map::OccupancyMapColor const &map,
type node (line 575) | struct node
type node (line 136) | struct node
type node (line 137) | struct node
type node (line 138) | struct node
method addParents (line 141) | void addParents() {
method node (line 163) | node(float x, float y, float z) { point = new ufo::math::Vector3(x, y,...
method node (line 164) | node(ufo::math::Vector3 *givenPoint) {
method addChild (line 169) | void addChild(node *newChild) { myChilds.push_back(newChild); }
method addParent (line 170) | void addParent(node *newParent) { myParent = newParent; }
method changeDistanceToGoal (line 171) | void changeDistanceToGoal(double newDistance) {
method changeDistanceToParent (line 174) | void changeDistanceToParent() {
method sumDistance (line 183) | double sumDistance() {
method getPath (line 196) | void getPath(std::list<struct node *> *givenPath) {
method extractUnknownVoxelsInsideFrustum (line 215) | void
method findInformationGain (line 257) | int findInformationGain(float v_local_, float givenHorizontal,
method clearInformationGain (line 337) | void clearInformationGain() {
method addHits (line 343) | void addHits(std::list<ufo::math::Vector3> *hitList) {
method findPathImprovement (line 364) | bool findPathImprovement(struct node *targetNode,
method readyForDeletion (line 463) | void readyForDeletion() { delete point; }
method isInCollision (line 465) | bool isInCollision(ufo::map::OccupancyMapColor const &map,
type node (line 578) | struct node
type node (line 136) | struct node
type node (line 137) | struct node
type node (line 138) | struct node
method addParents (line 141) | void addParents() {
method node (line 163) | node(float x, float y, float z) { point = new ufo::math::Vector3(x, y,...
method node (line 164) | node(ufo::math::Vector3 *givenPoint) {
method addChild (line 169) | void addChild(node *newChild) { myChilds.push_back(newChild); }
method addParent (line 170) | void addParent(node *newParent) { myParent = newParent; }
method changeDistanceToGoal (line 171) | void changeDistanceToGoal(double newDistance) {
method changeDistanceToParent (line 174) | void changeDistanceToParent() {
method sumDistance (line 183) | double sumDistance() {
method getPath (line 196) | void getPath(std::list<struct node *> *givenPath) {
method extractUnknownVoxelsInsideFrustum (line 215) | void
method findInformationGain (line 257) | int findInformationGain(float v_local_, float givenHorizontal,
method clearInformationGain (line 337) | void clearInformationGain() {
method addHits (line 343) | void addHits(std::list<ufo::math::Vector3> *hitList) {
method findPathImprovement (line 364) | bool findPathImprovement(struct node *targetNode,
method readyForDeletion (line 463) | void readyForDeletion() { delete point; }
method isInCollision (line 465) | bool isInCollision(ufo::map::OccupancyMapColor const &map,
type node (line 579) | struct node
type node (line 136) | struct node
type node (line 137) | struct node
type node (line 138) | struct node
method addParents (line 141) | void addParents() {
method node (line 163) | node(float x, float y, float z) { point = new ufo::math::Vector3(x, y,...
method node (line 164) | node(ufo::math::Vector3 *givenPoint) {
method addChild (line 169) | void addChild(node *newChild) { myChilds.push_back(newChild); }
method addParent (line 170) | void addParent(node *newParent) { myParent = newParent; }
method changeDistanceToGoal (line 171) | void changeDistanceToGoal(double newDistance) {
method changeDistanceToParent (line 174) | void changeDistanceToParent() {
method sumDistance (line 183) | double sumDistance() {
method getPath (line 196) | void getPath(std::list<struct node *> *givenPath) {
method extractUnknownVoxelsInsideFrustum (line 215) | void
method findInformationGain (line 257) | int findInformationGain(float v_local_, float givenHorizontal,
method clearInformationGain (line 337) | void clearInformationGain() {
method addHits (line 343) | void addHits(std::list<ufo::math::Vector3> *hitList) {
method findPathImprovement (line 364) | bool findPathImprovement(struct node *targetNode,
method readyForDeletion (line 463) | void readyForDeletion() { delete point; }
method isInCollision (line 465) | bool isInCollision(ufo::map::OccupancyMapColor const &map,
type node (line 580) | struct node
type node (line 136) | struct node
type node (line 137) | struct node
type node (line 138) | struct node
method addParents (line 141) | void addParents() {
method node (line 163) | node(float x, float y, float z) { point = new ufo::math::Vector3(x, y,...
method node (line 164) | node(ufo::math::Vector3 *givenPoint) {
method addChild (line 169) | void addChild(node *newChild) { myChilds.push_back(newChild); }
method addParent (line 170) | void addParent(node *newParent) { myParent = newParent; }
method changeDistanceToGoal (line 171) | void changeDistanceToGoal(double newDistance) {
method changeDistanceToParent (line 174) | void changeDistanceToParent() {
method sumDistance (line 183) | double sumDistance() {
method getPath (line 196) | void getPath(std::list<struct node *> *givenPath) {
method extractUnknownVoxelsInsideFrustum (line 215) | void
method findInformationGain (line 257) | int findInformationGain(float v_local_, float givenHorizontal,
method clearInformationGain (line 337) | void clearInformationGain() {
method addHits (line 343) | void addHits(std::list<ufo::math::Vector3> *hitList) {
method findPathImprovement (line 364) | bool findPathImprovement(struct node *targetNode,
method readyForDeletion (line 463) | void readyForDeletion() { delete point; }
method isInCollision (line 465) | bool isInCollision(ufo::map::OccupancyMapColor const &map,
function Euler2Quaternion (line 594) | Eigen::Quaternion<float> Euler2Quaternion(Eigen::Vector3d v) {
function linSpace (line 610) | void linSpace(node *givenNode, float givenDistance) {
function segmentPath (line 641) | void segmentPath(const nav_msgs::Path &path, nav_msgs::Path &path_seg) {
function generateTrajectory (line 698) | void generateTrajectory() {
function publishTrajectory (line 791) | void publishTrajectory() {
function evaluateCurrentPoint (line 871) | void evaluateCurrentPoint(ros::Publisher *chosen_path_pub) {
function visualize (line 937) | void visualize(ros::Publisher *points_pub, ros::Publisher *output_path_pub,
function updatePathTaken (line 1229) | void updatePathTaken() {
function tuneGeneration (line 1253) | void tuneGeneration(ufo::map::OccupancyMapColor const &map, bool occupie...
function isInCollision (line 1298) | bool isInCollision(ufo::map::OccupancyMapColor const &map,
function findShortestPath (line 1321) | void findShortestPath() {
function generateGoals (line 1369) | void generateGoals(ufo::map::OccupancyMapColor const &map,
function setPath (line 1505) | void setPath() {
function generateRRT (line 1769) | void generateRRT(float given_x, float given_y, float given_z) {
function trajectory (line 1862) | std::tuple<std::list<double>, double, std::list<double>>
function mapCallback (line 1968) | void mapCallback(ufomap_msgs::UFOMapStamped::ConstPtr const &msg) {
function odomCallback (line 1976) | void odomCallback(const nav_msgs::Odometry::ConstPtr &msg) {
function main (line 1999) | int main(int argc, char *argv[]) {
FILE: trajectory.py
function trajectory (line 1) | def trajectory(x, u, N, dt,trajectory):
Condensed preview — 37 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (231K chars).
[
{
"path": ".gitignore",
"chars": 66,
"preview": "docker/ignition_models\ndocker/models\ncontroller/MAV/\n__pycache__/\n"
},
{
"path": "CMakeLists.txt",
"chars": 8824,
"preview": "cmake_minimum_required(VERSION 3.0.2)\nproject(errt)\n\nset(CMAKE_CXX_STANDARD 14)\nset(CMAKE_CXX_STANDARD_REQUIRED ON)\nset("
},
{
"path": "LICENSE",
"chars": 1526,
"preview": "BSD 3-Clause License\n\nCopyright (c) 2024, LTU Robotics & AI \nAll rights reserved.\n\nRedistribution and use in source and "
},
{
"path": "README.md",
"chars": 9696,
"preview": "# ExplorationRRT\n\nA Tree-based Next-best-trajectory Method for 3D UAV Exploration\n\n![image(14)](https://github.com/LTU-R"
},
{
"path": "config/errt.yaml",
"chars": 5041,
"preview": "######## ERRT Tuning Parameters #########\n#########################################\n\nmap_frame_id: \"worl"
},
{
"path": "controller/errt_OpEn.py",
"chars": 4336,
"preview": "import opengen as og\nimport casadi.casadi as cs\nimport numpy as np\n\n## Problem size\n\nN = 15\ndt = 1.0/20\nnMAV = 1 #Numbe"
},
{
"path": "controller/errt_nmpc.py",
"chars": 8567,
"preview": " #!/usr/bin/env python\n # license removed for brevity\nimport rospkg\nimport rospy\nimport opengen as og\nimport numpy \nfrom"
},
{
"path": "controller/quaternion_to_euler.py",
"chars": 459,
"preview": "def quaternion_to_euler(x, y, z, w):\n\n import math\n t0 = +2.0 * (w * x + y * z)\n t1 = +1.0 - 2.0 * "
},
{
"path": "docker/.powerline.sh",
"chars": 183,
"preview": "function _update_ps1() {\n PS1=\" 🐳 $(powerline-shell $?)\"\n}\n\nif [[ $TERM != linux && ! $PROMPT_COMMAND =~ _update_ps1"
},
{
"path": "docker/.tmux.conf",
"chars": 2283,
"preview": "#set -g default-terminal \"tmux-256color\"\nset -g default-terminal \"xterm-256color\"\n#set -ga terminal-overrides \",*256col*"
},
{
"path": "docker/.vimrc",
"chars": 11843,
"preview": "\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\n\" General\n\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\"\""
},
{
"path": "docker/Dockerfile",
"chars": 7414,
"preview": "FROM osrf/ros:noetic-desktop-full-focal\n\nENV LANG C.UTF-8\n\nRUN echo 'debconf debconf/frontend select Noninteractive' | d"
},
{
"path": "docker/errt.yml",
"chars": 799,
"preview": "# ~/.tmuxinator/errt.yml \ntmux_options: '-2'\ndefine: &takeoff \"rosservice call /hummingbird/takeoff \\\"{}\\\" \" \nname: errt"
},
{
"path": "docker/stage_planning.yml",
"chars": 972,
"preview": "# ~/.tmuxinator/stage_planning.yml \ntmux_options: '-2'\ndefine: &initialize_stage \"rosservice call /initialize_planner \\\""
},
{
"path": "docker/start_errt_gpu.sh",
"chars": 936,
"preview": "#!/bin/bash\nsudo xhost +si:localuser:root\nXSOCK=/tmp/.X11-unix\n\nXAUTH=/tmp/.docker.xauth\nxauth_list=$(xauth nlist :0 | s"
},
{
"path": "docker/start_errt_no_gpu.sh",
"chars": 298,
"preview": "#!/bin/bash\n\nXAUTH=/path/to/Xauthority\n# Allow local Docker containers to access the X server\nxhost +local:docker\ndocker"
},
{
"path": "docker/statusline.conf",
"chars": 1450,
"preview": "# vim: ft=tmux\nset -g mode-style \"fg=#eee8d5,bg=#073642\"\n\nset -g message-style \"fg=#eee8d5,bg=#073642\"\nset -g message-co"
},
{
"path": "docker/tmux/macos.conf",
"chars": 374,
"preview": "# osx clipboard\nset-option -g default-command \"which reattach-to-user-namespace > /dev/null && reattach-to-user-namespac"
},
{
"path": "docker/tmux/statusline.conf",
"chars": 1450,
"preview": "# vim: ft=tmux\nset -g mode-style \"fg=#eee8d5,bg=#073642\"\n\nset -g message-style \"fg=#eee8d5,bg=#073642\"\nset -g message-co"
},
{
"path": "docker/tmux/tmux.conf",
"chars": 2319,
"preview": "#set -g default-terminal \"tmux-256color\"\nset -g default-terminal \"xterm-256color\"\n#set -ga terminal-overrides \",*256col*"
},
{
"path": "docker/tmux/utility.conf",
"chars": 92,
"preview": "# Display lazygit\nbind -r g display-popup -d '#{pane_current_path}' -w80% -h80% -E lazygit\n\n"
},
{
"path": "docker/utility.conf",
"chars": 92,
"preview": "# Display lazygit\nbind -r g display-popup -d '#{pane_current_path}' -w80% -h80% -E lazygit\n\n"
},
{
"path": "launch/errt.launch",
"chars": 994,
"preview": "<?xml version=\"1.0\" ?>\n<launch>\t\t\t\t\t\t\t\t\t<!-- Parameter descriptions -->\n \n <env name=\"ROSCONSOLE_FORMAT\" value=\"[${sev"
},
{
"path": "launch/octomap_gazebo.launch",
"chars": 1558,
"preview": "<?xml version=\"1.0\"?>\n<!-- \n Example launch file for octomap_server mapping: \n Listens to incoming PointCloud2 data an"
},
{
"path": "launch/server.launch",
"chars": 2605,
"preview": "<?xml version=\"1.0\" ?>\n<launch>\n\n\t\t<arg name=\"resolution\" default=\"0.2\" />\n\t\t<arg name=\"depth_levels\" default=\"16\" />\n\t\t"
},
{
"path": "nmpc_test.py",
"chars": 1036,
"preview": "import opengen as og\nimport casadi.casadi as cs\n#import matplotlib.pyplot as plt\nimport numpy as np\nimport math \nfrom tr"
},
{
"path": "package.xml",
"chars": 2325,
"preview": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n <name>errt</name>\n <version>1.0.0</version>\n <description>Exploration-RRT"
},
{
"path": "rrt_costgen.py",
"chars": 3788,
"preview": "import opengen as og\nimport casadi.casadi as cs\n#import matplotlib.pyplot as plt\nimport numpy as np\nfrom trajectory impo"
},
{
"path": "rviz/errt.rviz",
"chars": 9362,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "rviz/errt_field.rviz",
"chars": 11271,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "rviz/follow_errt.rviz",
"chars": 12730,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "rviz/follow_top_errt.rviz",
"chars": 12726,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "scripts/errt.yml",
"chars": 654,
"preview": "# ~/.tmuxinator/errt.yml \n\nname: errt\nroot: ~/\nwindows:\n - stage_main:\n layout: even-horizontal \n panes:\n "
},
{
"path": "scripts/nmpc_test.py",
"chars": 1036,
"preview": "import opengen as og\nimport casadi.casadi as cs\n#import matplotlib.pyplot as plt\nimport numpy as np\nimport math \nfrom tr"
},
{
"path": "src/Trajectory.cpp",
"chars": 6003,
"preview": "std::tuple<std::list<double>, double, std::list<double>> trajectory(std::list<double> x, double* u, double N, double dt,"
},
{
"path": "src/errt.cpp",
"chars": 83094,
"preview": "#include \"std_msgs/Float64MultiArray.h\"\n#include \"std_msgs/Int64.h\"\n#include <chrono>\n#include <dlfcn.h>\n#include <dynam"
},
{
"path": "trajectory.py",
"chars": 2088,
"preview": "def trajectory(x, u, N, dt,trajectory):\n #Based on the initial condition and optimized trajectory u, computed the pat"
}
]
About this extraction
This page contains the full source code of the LTU-RAI/ExplorationRRT GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 37 files (215.1 KB), approximately 65.4k tokens, and a symbol index with 58 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.