├── .DS_Store ├── LICENSE ├── README.md ├── dependencies.rosinstall ├── docs ├── aero.png ├── forest.png ├── maze.png └── rviz.png ├── ootp ├── CMakeLists.txt ├── include │ ├── ootp │ │ ├── actions.hpp │ │ ├── config.hpp │ │ └── global.hpp │ └── timer │ │ └── tictoc_timer.h ├── package.xml └── src │ ├── actions.cpp │ ├── config.cpp │ └── timer │ └── tictoc_timer.cpp ├── ootp_ros ├── CMakeLists.txt ├── config │ └── aero.yaml ├── include │ └── ootp_ros │ │ └── initializer.hpp ├── launch │ └── aero.launch ├── package.xml └── src │ ├── initializer.cpp │ └── main.cpp ├── ootp_simulator ├── CMakeLists.txt ├── config │ └── simulation_maze.yaml ├── include │ └── ootp_simulator │ │ └── simulation_tools.hpp ├── launch │ ├── maze.launch │ └── maze_node.launch ├── models │ └── maze │ │ ├── model.config │ │ └── model.sdf ├── package.xml ├── src │ ├── gazebo_main.cpp │ └── simulation_tools.cpp ├── urdf │ └── mav_with_rs_sensor.gazebo └── worlds │ └── maze.world ├── path_contour ├── CMakeLists.txt ├── config │ └── aero.yaml ├── include │ └── path_contour │ │ └── path_contour.h ├── launch │ └── aero.launch ├── package.xml └── src │ ├── path_contour │ └── path_contour.cpp │ └── path_contourROSnode.cpp ├── rlmap ├── CMakeLists.txt ├── include │ └── rlmap │ │ ├── config.hpp │ │ ├── global.hpp │ │ ├── hash_functions.hpp │ │ ├── map_storage.hpp │ │ ├── ootp_map.hpp │ │ ├── probability_manager.hpp │ │ ├── rlmap.hpp │ │ ├── tictoc.hpp │ │ └── voxel.hpp ├── package.xml └── src │ ├── config.cpp │ ├── map_storage.cpp │ ├── ootp_map.cpp │ ├── probability_manager.cpp │ ├── rlmap.cpp │ ├── tictoc.cpp │ └── voxel.cpp ├── rlmap_ros ├── CMakeLists.txt ├── include │ └── rlmap_ros │ │ ├── initializer.hpp │ │ └── visualizer.hpp ├── package.xml └── src │ ├── initializer.cpp │ └── visualizer.cpp └── rrt ├── CMakeLists.txt ├── include └── rrt │ ├── config.hpp │ ├── global.hpp │ ├── node.hpp │ ├── rrt.hpp │ └── rrt_connect.hpp ├── package.xml └── src ├── config.cpp ├── node.cpp ├── rrt.cpp └── rrt_connect.cpp /.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelLabs/autonomousmavs/d3a2dee9e28768dd4f4f398501de8953d258dede/.DS_Store -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2020, Intel Labs 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # DISCONTINUATION OF PROJECT # 2 | This project will no longer be maintained by Intel. 3 | Intel has ceased development and contributions including, but not limited to, maintenance, bug fixes, new releases, or updates, to this project. 4 | Intel no longer accepts patches to this project. 5 | If you have an ongoing need to use this project, are interested in independently developing it, or would like to maintain patches for the open source software community, please create your own fork of this project. 6 | 7 | Framework for Autonomous Navigation of Micro Aerial Vehicles 8 | =============== 9 | 10 | | Authors | Email | 11 | |:---------------------:|:----------------------------------:| 12 | | Leo Campos | leobardo.e.campos.macias@intel.com | 13 | | Rafael de la Guardia | rafael.de.la.guardia@intel.com | 14 | | **Lead Architect** | **Email** | 15 | | Leo Campos | leobardo.e.campos.macias@intel.com | 16 | 17 | This package provides the implementation of a framework for autonomous drone navigation towards a given goal within an unknown cluttered environment while fulfilling dynamical constraints. 18 | 19 | It can be simulated using [RotorS](https://github.com/ethz-asl/rotors_simulator) or implemented in actual drones. It has been fully tested on [Aero Ready to Fly Drone](https://www.intel.com/content/www/us/en/drones/aero-ready-to-fly-brief.html). 20 | 21 | We provide the instructions necessary for getting started. 22 | 23 | If you are using this framework implementation within the research for your publication, please cite: 24 | ```bibtex 25 | @article{Campos2020, 26 | author = {Campos-Macías, Leobardo and Aldana-López, Rodrigo and de la Guardia, 27 | Rafael and Parra-Vilchis, José I. and Gómez-Gutiérrez, David}, 28 | title = {Autonomous navigation of MAVs in unknown cluttered environments}, 29 | journal = {Journal of Field Robotics}, 30 | volume = {n/a}, 31 | number = {n/a}, 32 | pages = {}, 33 | doi = {10.1002/rob.21959}, 34 | url = {https://onlinelibrary.wiley.com/doi/abs/10.1002/rob.21959}, 35 | eprint = {https://onlinelibrary.wiley.com/doi/pdf/10.1002/rob.21959} 36 | } 37 | ``` 38 | A video of some real-world experiments is available at: [https://youtu.be/79IFfQfvXLE](https://youtu.be/79IFfQfvXLE) 39 | 40 | Installation Instructions - Ubuntu 18.04 with ROS Melodic 41 | --------------------------------------------------------- 42 | 1. Install ROS Melodic with dependencies: 43 | ``` 44 | $ sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' 45 | $ wget http://packages.ros.org/ros.key -O - | sudo apt-key add - 46 | $ sudo apt update 47 | $ sudo apt install ros-melodic-desktop-full ros-melodic-joy ros-melodic-octomap-ros ros-melodic-mavlink python-wstool python-catkin-tools protobuf-compiler libgoogle-glog-dev ros-melodic-control-toolbox python-rosdep python-rosinstall python-rosinstall-generator python-wstool build-essential ros-melodic-gps-common ros-melodic-gps-umd ros-melodic-gpsd-client 48 | $ sudo rosdep init 49 | $ rosdep update 50 | $ echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc 51 | $ source ~/.bashrc 52 | ``` 53 | 54 | 2. Init the ros workspace and clone necessary packages 55 | 56 | ``` 57 | $ mkdir -p ~/catkin_ws/src 58 | $ cd ~/catkin_ws/src 59 | $ catkin_init_workspace # initialize your catkin workspace 60 | $ wstool init 61 | $ wget https://raw.githubusercontent.com/IntelLabs/autonomousmavs/master/dependencies.rosinstall 62 | $ wstool merge dependencies.rosinstall 63 | $ wstool update 64 | ``` 65 | 66 | 3. The workspace is built with catkin tools 67 | ``` 68 | $ cd ~/catkin_ws/ 69 | $ catkin build 70 | ``` 71 | 72 | 4. Add sourcing 73 | 74 | ``` 75 | $ source devel/setup.bash 76 | ``` 77 | 78 | Maze Simulation 79 | ----------- 80 | 81 | Launch the maze simulation environment and the quad-copter [AscTec Hummingbird](http://www.asctec.de/en/uav-uas-drone-products/asctec-hummingbird/) model with a [RealSense R200](https://ark.intel.com/content/www/us/en/ark/products/92256/intel-realsense-camera-r200.html) attached 82 | 83 | ``` 84 | $ roslaunch ootp_simulator maze.launch 85 | ``` 86 | 87 | The simulator starts by default in paused mode. To start the navigation algorithm, open another terminal and type: 88 | 89 | ``` 90 | $ source ~/catkin_ws/devel/setup.bash 91 | $ roslaunch ootp_simulator maze_node.launch 92 | ``` 93 | 94 | You should see the drone navigating towards the exit of the maze. 95 | 96 | ![Gazebo Maze Environments](docs/maze.png) 97 | 98 | The following topics display the navigation process: 99 | - **/map** [visualization_msgs::MarkerArray]: Displays the currently occupied map 100 | - **/path** [nav_msgs::Path]: Displays the proposed path to the goal 101 | - **/path_consumed** [nav_msgs::Path]: Displays the path followed by the drone 102 | 103 | ![Rviz topics](docs/rviz.png) 104 | *You have to change the frame world to "world" in Rviz to see the topics.* 105 | 106 | The navigation framework only needs as input the odometry, given by RotorS plugin, and the depth image, provided by Real Sense plugin, with the two topics: 107 | - **/rs0r200/camera/depth/image_raw** [sensor_msgs::ImageConstPtr] 108 | - **/hummingbird/odometry_sensor1/odometry** [nav_msgs::Odometry] 109 | 110 | and outputs the next state that, for simulation purposes, consists only on the next position and yaw: 111 | - **/hummingbird/command/trajectory** [trajectory_msgs::MultiDOFJointTrajectory] 112 | 113 | Finally, the file `ootp_simulator/config/simulation_maze.yaml` is used to configure the parameters used by the algorithm, such as the camera calibration or the initial and final given poses. 114 | 115 | Real World Experiments 116 | --------------------- 117 | Real world experiments were performed using the [Aero Ready to Fly Drone](https://www.intel.com/content/www/us/en/drones/aero-ready-to-fly-brief.html). A custom fly controller was implemented requiring the position and two derivatives. 118 | 119 | ![Aero Ready to Fly Drone Configuration](docs/aero.png) 120 | 121 | Depth image was provided by the [Intel RealSense D435](https://www.intelrealsense.com/depth-camera-d435/), position was provided by the [Intel RealSense Tracking Camera T265](https://www.intelrealsense.com/tracking-camera-t265/), and orientation with the internal BMI160-IMU: 122 | - **/depth/image_raw** [sensor_msgs::ImageConstPtr] 123 | - **/odom_control/pose** [geometry_msgs::PoseStamped] 124 | - **/euler** [geometry_msgs::Vector3Stamped] 125 | 126 | Position, velocity, and acceleration for x, y, z, and yaw states are given by: 127 | - **/aero/ref_pos** [geometry_msgs::PoseStamped] 128 | - **/aero/ref_vel** [geometry_msgs::PoseStamped] 129 | - **/aero/ref_acc** [geometry_msgs::PoseStamped] 130 | 131 | Finally, the file `ootp_ros/config/aero.yaml` is used to configure the parameters used by the algorithm, such as the camera calibration or the initial and final given poses. 132 | 133 | ![Reaching a goal in a forest environment](docs/forest.png) 134 | 135 | License 136 | --------------------- 137 | This project is licensed under the BSD-3-Clause see the [LICENSE](LICENSE) file for details 138 | -------------------------------------------------------------------------------- /dependencies.rosinstall: -------------------------------------------------------------------------------- 1 | - git: {local-name: mav_comm, uri: 'https://github.com/ethz-asl/mav_comm.git'} 2 | - git: {local-name: mavros, uri: 'https://github.com/ethz-asl/mavros.git', version: 0880a97575f8c6b705096aca17ca04c6c6dcfc02} 3 | - git: {local-name: glog_catkin, uri: 'https://github.com/ethz-asl/glog_catkin.git'} 4 | - git: {local-name: mav_tools, uri: 'https://github.com/ethz-asl/mav_tools_public.git'} 5 | - git: {local-name: rotors_simulator, uri: 'https://github.com/ethz-asl/rotors_simulator.git', version: feature/gazebo9} 6 | - git: {local-name: catkin_simple, uri: 'https://github.com/catkin/catkin_simple.git'} 7 | - git: {local-name: realsense_gazebo_plugin, uri: 'https://github.com/SyrianSpock/realsense_gazebo_plugin.git'} 8 | - git: {local-name: autonomousmavs, uri: 'https://github.com/IntelLabs/autonomousmavs.git'} -------------------------------------------------------------------------------- /docs/aero.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelLabs/autonomousmavs/d3a2dee9e28768dd4f4f398501de8953d258dede/docs/aero.png -------------------------------------------------------------------------------- /docs/forest.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelLabs/autonomousmavs/d3a2dee9e28768dd4f4f398501de8953d258dede/docs/forest.png -------------------------------------------------------------------------------- /docs/maze.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelLabs/autonomousmavs/d3a2dee9e28768dd4f4f398501de8953d258dede/docs/maze.png -------------------------------------------------------------------------------- /docs/rviz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelLabs/autonomousmavs/d3a2dee9e28768dd4f4f398501de8953d258dede/docs/rviz.png -------------------------------------------------------------------------------- /ootp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | SET(PROJECT_NAME ootp) 2 | PROJECT(${PROJECT_NAME}) 3 | CMAKE_MINIMUM_REQUIRED (VERSION 2.8.3) 4 | #SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo 5 | SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/CMakeModules/") 6 | 7 | # Set build flags. Set IS_ARM on odroid board as environment variable 8 | SET(CMAKE_CXX_FLAGS "-Wall -D_LINUX -D_REENTRANT -march=native -Wno-unused-variable -Wno-unused-but-set-variable -Wno-unknown-pragmas") 9 | IF(DEFINED ENV{ARM_ARCHITECTURE}) 10 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -march=armv7-a") 11 | ELSE() 12 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse -msse2 -msse3 -mssse3") 13 | ENDIF() 14 | IF(CMAKE_COMPILER_IS_GNUCC) 15 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 16 | ELSE() 17 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 18 | ENDIF() 19 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") 20 | 21 | # Add catkin and required ROS packages 22 | FIND_PACKAGE(catkin REQUIRED COMPONENTS 23 | cmake_modules 24 | roscpp 25 | rlmap 26 | path_contour 27 | rrt_lib 28 | ) 29 | 30 | # Add plain cmake packages 31 | FIND_PACKAGE(Eigen REQUIRED) 32 | 33 | # Describe catkin Project 34 | catkin_package( 35 | DEPENDS Eigen 36 | CATKIN_DEPENDS roscpp rlmap path_contour 37 | INCLUDE_DIRS include 38 | LIBRARIES ootp 39 | ) 40 | 41 | # Include dirs 42 | INCLUDE_DIRECTORIES( 43 | include 44 | ${Eigen_INCLUDE_DIRS} 45 | ${catkin_INCLUDE_DIRS} 46 | ) 47 | 48 | # Set link libraries 49 | LIST(APPEND LINK_LIBS 50 | ${catkin_LIBRARIES} 51 | ) 52 | ADD_LIBRARY(ootp SHARED 53 | src/config.cpp 54 | src/actions.cpp 55 | src/timer/tictoc_timer.cpp 56 | ) 57 | TARGET_LINK_LIBRARIES(ootp ${LINK_LIBS}) 58 | -------------------------------------------------------------------------------- /ootp/include/ootp/actions.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file actions.hpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date Jun 27, 2019 5 | * @brief State machine interface 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | 38 | #ifndef ACTIONS_HPP_ 39 | #define ACTIONS_HPP_ 40 | 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include 48 | #include 49 | #include 50 | 51 | namespace ootp 52 | { 53 | 54 | static const uint8_t k_dimension = 3; 55 | 56 | // kNStates = number of states 57 | typedef enum {kIdle =0, kAngleFollowing, kWalkFollowing, kLost, kNStates}eStates; 58 | typedef enum {kPos =0, kVel, kAcc, kJerk, kNDynbamics}eDynbamics; 59 | 60 | class Actions : boost::noncopyable 61 | { 62 | private: 63 | // external handlers 64 | rlmap::OOTP_Map *map_handler_; 65 | contour::PathDynamics *traj_handler_; 66 | 67 | // state machine 68 | eStates current_state_; 69 | bool valid_goal_; //goal_lost_; 70 | uint32_t lost_turns_counter_; 71 | 72 | // Perception 73 | Transform current_pose_; 74 | rlmap::FrameObj current_frame_; 75 | bool new_scene_; 76 | 77 | // new_states variables 78 | MatrixXD states_; 79 | double yaw_; 80 | Point yaw_states_; 81 | Point goal_; 82 | rlmap::TicTocPtr sampling_time_; 83 | 84 | Point position_waypoint_; 85 | double yaw_waypoint_; 86 | 87 | //threads! 88 | std::condition_variable thread_update_scene_; 89 | std::mutex mtx_actions_; 90 | std::mutex mutex_map_; 91 | std::thread *actions_thread_; 92 | bool new_action_; 93 | 94 | // Debugging members 95 | Point local_point_; 96 | Point reference_to_check_; 97 | Point reference_; 98 | PointList path_consumed_; 99 | // helper variables for WalkFollowing 100 | PointList path_waypoints_; 101 | std::mutex mtx_path_waypoints_; 102 | 103 | Point last_position_rrt_; 104 | Point last_direction_rrt_; 105 | 106 | 107 | // State machine functions 108 | void (Actions::*CurrentState[kNStates])(void); 109 | 110 | void Idle(void); 111 | void AngleFollowing(void); 112 | void WalkFollowing(void); 113 | void Lost(void); 114 | 115 | void RenewState(void); 116 | 117 | // general helper functions 118 | finline void UpdateStates(void); 119 | finline void PushWaypoint2Filter(const Point &position_ref, const double &yaw_ref); 120 | 121 | // helper functions for WalkFollowing 122 | finline bool IsPathOccupied(void); 123 | finline void ClearPath(void); 124 | finline void GenerateNewPath(void); 125 | bool GetWaypoint2Filter(Point &point, double &yaw); 126 | bool IsPointInsideFoV(const Point &start, const Point &point, const Point &unitary); 127 | 128 | // helper functions for AngleFollowing 129 | finline double GetSpinDirection(void); 130 | 131 | // threads 132 | void RunActions(void); 133 | 134 | // prevent copy of class 135 | Actions(const Actions&); 136 | Actions& operator=(const Actions&){ return *this;} 137 | 138 | public: 139 | Actions(); 140 | Actions(const Point &initial_position); 141 | virtual ~Actions(void); 142 | 143 | 144 | void AddScene( const rlmap::FrameObj& scene, const Transform &pose, double time_stamp ); 145 | void UpdateGoal( const Point& point, bool new_data ); 146 | void GetMovement( MatrixXD &states, double &yaw ); 147 | void GetMovement( MatrixXD &state, VectorX &yaw ); 148 | 149 | void StartTimer(void); 150 | 151 | // debugging methods, map visualization 152 | void GetState(eStates &state); 153 | void GetMovement( MatrixXD &states, Point &debug ); 154 | 155 | // debugging methods 156 | void get_occupied_map(rlmap::stdVoxelVector &points); 157 | void get_local_path(PointList &result); 158 | void get_local_goal(Point &goal); 159 | void get_raw_points(PointList &raw_points); 160 | void get_reference_to_check(Point &reference_to_check, Point &reference); 161 | void get_path_consumed(PointList &result); 162 | 163 | }; 164 | 165 | 166 | } /* namespace ootp*/ 167 | 168 | 169 | #endif /* ACTIONS_HPP_ */ 170 | -------------------------------------------------------------------------------- /ootp/include/ootp/config.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file config.hpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date June, 2018 5 | * @brief constant values for exploration algorithm 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | 38 | #ifndef CONFIG_HPP_ 39 | #define CONFIG_HPP_ 40 | #include 41 | 42 | namespace ootp 43 | { 44 | class Config 45 | { 46 | public: 47 | static Config& getInstance(); 48 | static VectorX& path_gains() { return getInstance().path_gains_; } 49 | static VectorX& yaw_gains() { return getInstance().yaw_gains_; } 50 | static double& path_error() { return getInstance().path_error_; } 51 | static double& arc_velocity() { return getInstance().arc_velocity_; } 52 | static double& waypoint_lengh() { return getInstance().waypoint_lengh_; } 53 | static double& agent_radius() { return getInstance().agent_radius_; } 54 | static double& rho() { return getInstance().rho_; } 55 | static double& y_rho() { return getInstance().y_rho_; } 56 | 57 | static double& vmax() { return getInstance().vmax_; } 58 | static double& amax() { return getInstance().amax_; } 59 | static double& gain() { return getInstance().gain_; } 60 | 61 | 62 | static double& safe_radius_person() { return getInstance().safe_radius_person_; } 63 | static double& yaw_velocity() { return getInstance().yaw_velocity_; } 64 | static int& max_lost_turns() { return getInstance().max_lost_turns_; } 65 | static int& max_lost_turns_counter() { return getInstance().max_lost_turns_counter_; } 66 | static double& yaw_error() { return getInstance().yaw_error_; } 67 | static double& dtime() { return getInstance().dtime_; } 68 | 69 | static double& hfov() { return getInstance().hfov_; } 70 | static double& tvfov() { return getInstance().tvfov_; } 71 | static double& max_depth_distance() { return getInstance().max_depth_distance_; } 72 | 73 | static std::string& log_files() { return getInstance().log_files_; } 74 | 75 | 76 | static void init_params_externally( 77 | VectorX path_gains, 78 | VectorX yaw_gains, 79 | double path_error, 80 | double arc_velocity, 81 | double waypoint_lengh, 82 | double agent_radius, 83 | 84 | double safe_radius_person, 85 | int max_lost_turns, 86 | double yaw_velocity, 87 | double yaw_error, 88 | 89 | double rho, 90 | double y_rho, 91 | double dtime, 92 | 93 | double vmax, 94 | double amax, 95 | double gain, 96 | 97 | double hfov, 98 | double vfov, 99 | double max_depth_distance, 100 | std::string log_files ); 101 | 102 | private: 103 | Config(void); 104 | Config(Config const&); 105 | void operator=(Config const&); 106 | 107 | VectorX path_gains_; 108 | VectorX yaw_gains_; 109 | double path_error_; 110 | double arc_velocity_; 111 | double waypoint_lengh_; 112 | double agent_radius_; 113 | 114 | double dtime_; 115 | double rho_; 116 | double y_rho_; 117 | 118 | double vmax_; 119 | double amax_; 120 | double gain_; 121 | 122 | 123 | double safe_radius_person_; 124 | // TODO :: yaw_velocity_ and number of lost turns give this : 125 | int max_lost_turns_; 126 | int max_lost_turns_counter_; 127 | double yaw_velocity_; 128 | double yaw_error_; 129 | 130 | // added for FoV point 131 | double hfov_; 132 | double vfov_; 133 | double tvfov_; 134 | double max_depth_distance_; 135 | 136 | // log file 137 | std::string log_files_; 138 | }; 139 | 140 | 141 | }/* ootp */ 142 | 143 | 144 | #endif /* CONFIG_HPP_ */ 145 | -------------------------------------------------------------------------------- /ootp/include/ootp/global.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file global.hpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date May, 2018 5 | * @brief global definitions for exploration algorithm 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | #ifndef OOTP_GLOBAL_HPP_ 38 | #define OOTP_GLOBAL_HPP_ 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | namespace ootp 48 | { 49 | 50 | #ifndef finline 51 | #ifdef WIN32 52 | #define finline __forceinline 53 | #else 54 | #define finline inline __attribute__((always_inline)) 55 | #endif 56 | #endif 57 | 58 | typedef enum {VALID_GOAL=0, ZERO_GOAL, NSTATES} image_state_t; 59 | typedef enum {FROM_VELOCITY=0, TURNING, YAW_TRANS, NANGLE_STATES} angle_sate_t; 60 | 61 | 62 | typedef Eigen::Vector2i Pixel; 63 | typedef std::vector PixelList; 64 | typedef Eigen::Vector3d Point; 65 | typedef Eigen::Vector2d Point2D; 66 | typedef std::vector PointList; 67 | typedef Eigen::Matrix3d Rotation3D; 68 | typedef Eigen::Quaterniond quat; 69 | typedef Eigen::VectorXd VectorX; 70 | typedef Eigen::MatrixXd MatrixXD; 71 | 72 | 73 | template 74 | finline T sign(T value) 75 | { 76 | return value < 0 ? -1. : 1.; 77 | } 78 | 79 | Rotation3D finline GetYawRotation(double yaw) 80 | { 81 | Rotation3D rot; rot<< cos(yaw), -sin(yaw), 0.0, sin(yaw), cos(yaw), 0.0, 0.0, 0.0, 1.0; 82 | return rot; 83 | } 84 | Rotation3D finline GetRollRotation(double roll) 85 | { 86 | Rotation3D rot; rot<< 1.0, 0.0, 0.0, 0.0, cos(roll), -sin(roll), 0.0, sin(roll), cos(roll); 87 | return rot; 88 | } 89 | Rotation3D finline GetPitchRotation(double pitch) 90 | { 91 | Rotation3D rot; rot<< cos(pitch), 0.0, sin(pitch), 0.0, 1.0, 0.0, -sin(pitch), 0.0, cos(pitch); 92 | return rot; 93 | } 94 | Rotation3D finline GetZYXRotation(double roll, double pitch, double yaw) 95 | { 96 | Rotation3D rot; rot=GetYawRotation(yaw)*GetPitchRotation(pitch)*GetRollRotation(roll); 97 | return rot; 98 | } 99 | 100 | struct Transform 101 | { 102 | Point position_; 103 | Rotation3D rotation_; 104 | double yaw_; 105 | Transform() 106 | { 107 | 108 | } 109 | Transform(Point position, Point rotation) : position_(position) 110 | { 111 | rotation_ = GetZYXRotation(rotation(0), rotation(1), rotation(2)); 112 | yaw_ = rotation(2); 113 | } 114 | Transform(Point position, double yaw) : position_(position) 115 | { 116 | rotation_ = GetYawRotation(yaw); 117 | yaw_ = yaw; 118 | } 119 | Transform(Point position, Rotation3D rotation) : position_(position), rotation_(rotation) 120 | { 121 | yaw_ = 0; 122 | } 123 | }; 124 | 125 | 126 | } /* ootp */ 127 | 128 | 129 | 130 | 131 | #endif /* OOTP_GLOBAL_HPP_ */ 132 | -------------------------------------------------------------------------------- /ootp/include/timer/tictoc_timer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file tictoc_timer.h 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date May, 2018 5 | * @brief library to measure the time 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | 38 | typedef unsigned long long tictoc_timer_t; 39 | 40 | void timer_tic(tictoc_timer_t * timer); 41 | 42 | float timer_toc(tictoc_timer_t timer); 43 | 44 | -------------------------------------------------------------------------------- /ootp/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ootp 4 | 0.1.0 5 | 6 | A library for autonomous micro-mav navigation. 7 | 8 | Leo Campos 9 | Leo Campos 10 | BSD 11 | 12 | 13 | catkin 14 | 15 | 16 | roscpp 17 | cmake_modules 18 | roslib 19 | rlmap 20 | path_contour 21 | rrt_lib 22 | 23 | 24 | roscpp 25 | roslib 26 | rlmap 27 | path_contour 28 | rrt_lib 29 | -------------------------------------------------------------------------------- /ootp/src/config.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file config.cpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date June, 2018 5 | * @brief constant values for exploration algorithm 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | #include 38 | 39 | namespace ootp 40 | { 41 | 42 | Config::Config() : 43 | path_gains_(), 44 | yaw_gains_(), 45 | path_error_(0.1), 46 | arc_velocity_(1.0), 47 | waypoint_lengh_(0.2), 48 | agent_radius_(0.3), 49 | safe_radius_person_(1.0), 50 | max_lost_turns_(3), 51 | yaw_velocity_(0.1), 52 | yaw_error_(0.005), 53 | rho_(2.0), 54 | y_rho_(2.0), 55 | dtime_(0.01), 56 | vmax_(0.8), 57 | amax_(0.1), 58 | gain_(0.5), 59 | 60 | log_files_("/home/leointel/") 61 | { 62 | max_lost_turns_counter_ = (int)( ((double)max_lost_turns_)*( 2.0*M_PI / yaw_velocity_) ) ; 63 | path_gains_.resize(5); 64 | path_gains_ << -384.0, -400.0, -140.0, -20.0, 1.0; 65 | yaw_gains_.resize(2); 66 | yaw_gains_ << -1.0,0.0; 67 | 68 | }; 69 | 70 | Config& Config::getInstance() 71 | { 72 | static Config instance; // Instantiated on first use and guaranteed to be destroyed 73 | return instance; 74 | } 75 | 76 | void Config::init_params_externally( 77 | VectorX path_gains, 78 | VectorX yaw_gains, 79 | double path_error, 80 | double arc_velocity, 81 | double waypoint_lengh, 82 | double agent_radius, 83 | double safe_radius_person, 84 | int max_lost_turns, 85 | double yaw_velocity, 86 | double yaw_error, 87 | double rho, 88 | double y_rho, 89 | double dtime, 90 | 91 | double vmax, 92 | double amax, 93 | double gain, 94 | 95 | double hfov, 96 | double vfov, 97 | double max_depth_distance, 98 | std::string log_files) 99 | { 100 | getInstance().path_gains_ = path_gains; 101 | getInstance().yaw_gains_ = yaw_gains; 102 | getInstance().path_error_ = path_error; 103 | getInstance().arc_velocity_ = arc_velocity; 104 | getInstance().waypoint_lengh_ = waypoint_lengh; 105 | getInstance().agent_radius_ = agent_radius; 106 | getInstance().safe_radius_person_ = safe_radius_person; 107 | getInstance().max_lost_turns_ = max_lost_turns; 108 | getInstance().yaw_velocity_ = yaw_velocity; 109 | getInstance().yaw_error_ = yaw_error; 110 | getInstance().rho_ = rho; 111 | getInstance().y_rho_ = y_rho; 112 | getInstance().dtime_ = dtime; 113 | 114 | getInstance().vmax_ = vmax; 115 | getInstance().amax_ = amax; 116 | getInstance().gain_ = gain; 117 | 118 | getInstance().max_depth_distance_ = max_depth_distance; 119 | getInstance().hfov_ = hfov; 120 | getInstance().vfov_ = vfov; 121 | getInstance().tvfov_ = tan(vfov / 2.0); 122 | getInstance().log_files_ = log_files; 123 | } 124 | 125 | }/* ootp */ 126 | -------------------------------------------------------------------------------- /ootp/src/timer/tictoc_timer.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file tictoc_timer.cpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date May, 2018 5 | * @brief library to measure the time 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | #include 38 | #include 39 | 40 | void timer_tic(tictoc_timer_t * timer) 41 | { 42 | const unsigned long long nano = 1000000000; 43 | struct timespec tm; 44 | clock_gettime( CLOCK_REALTIME, &tm ); 45 | *timer = tm.tv_nsec + tm.tv_sec * nano; 46 | } 47 | 48 | float timer_toc(tictoc_timer_t timer) 49 | { 50 | const unsigned long long nano = 1000000000; 51 | struct timespec tm; 52 | clock_gettime( CLOCK_REALTIME, &tm ); 53 | tictoc_timer_t t_now; 54 | t_now = tm.tv_nsec + tm.tv_sec * nano; 55 | return (float)(t_now-timer)/(float)nano; 56 | } 57 | -------------------------------------------------------------------------------- /ootp_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | SET(PROJECT_NAME ootp_ros) 2 | PROJECT(${PROJECT_NAME}) 3 | CMAKE_MINIMUM_REQUIRED (VERSION 2.8.3) 4 | #SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo 5 | SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/CMakeModules/") 6 | 7 | # Set build flags. Set IS_ARM on odroid board as environment variable 8 | SET(CMAKE_CXX_FLAGS "-Wall -D_LINUX -D_REENTRANT -march=native -Wno-unused-variable -Wno-unused-but-set-variable -Wno-unknown-pragmas") 9 | IF(DEFINED ENV{ARM_ARCHITECTURE}) 10 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -march=armv7-a") 11 | ELSE() 12 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse -msse2 -msse3 -mssse3") 13 | ENDIF() 14 | IF(CMAKE_COMPILER_IS_GNUCC) 15 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 16 | ELSE() 17 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 18 | ENDIF() 19 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") 20 | 21 | 22 | # Add catkin and required ROS packages 23 | FIND_PACKAGE(catkin REQUIRED COMPONENTS 24 | cmake_modules 25 | roscpp 26 | image_transport 27 | cv_bridge 28 | sensor_msgs 29 | ootp 30 | rlmap_ros 31 | rrt_lib 32 | ) 33 | 34 | # Add plain cmake packages 35 | FIND_PACKAGE(Eigen3 REQUIRED) 36 | 37 | # Describe catkin Project 38 | catkin_package( 39 | DEPENDS Eigen3 40 | CATKIN_DEPENDS roscpp image_transport cv_bridge sensor_msgs ootp 41 | rlmap_ros rrt_lib 42 | INCLUDE_DIRS include ${Eigen3_INCLUDE_DIRS} 43 | LIBRARIES ootp_ros 44 | ) 45 | 46 | # Include dirs 47 | INCLUDE_DIRECTORIES( 48 | include 49 | ${Eigen3_INCLUDE_DIRS} 50 | ${catkin_INCLUDE_DIRS} 51 | ) 52 | 53 | # Set link libraries 54 | LIST(APPEND LINK_LIBS 55 | ${catkin_LIBRARIES} 56 | ) 57 | ADD_LIBRARY(ootp_ros SHARED 58 | src/initializer.cpp 59 | ) 60 | TARGET_LINK_LIBRARIES(ootp_ros ${LINK_LIBS}) 61 | 62 | # # Create Executables 63 | ADD_EXECUTABLE(ootp_node src/main.cpp) 64 | target_link_libraries(ootp_node ${catkin_LIBRARIES} ootp_ros) 65 | 66 | -------------------------------------------------------------------------------- /ootp_ros/config/aero.yaml: -------------------------------------------------------------------------------- 1 | ## this file correspond to aero parameters in real time experiments with D435 realsense camera 2 | 3 | ## Parameters for OOTP 4 | # filter parameters 5 | path_gains: [ -3.0104536742267722,-9.257906132819208,-10.574352237875011,-5.326766977660398, 1.5 ] 6 | yaw_gains: [-3.0, -2.0] 7 | path_error: 0.1 8 | arc_velocity: 10.0 9 | waypoint_lengh: 0.2 10 | yaw_velocity: 2.0 11 | yaw_error: 0.05 12 | dynamics_iterations: 5 13 | dynamics_step_size: 0.005 #0.1 14 | 15 | # main params 16 | agent_radius: 0.18 17 | safe_radius_person: 0.1 18 | max_lost_turns: 3 19 | goal: [6.0, 0.5, 0.7] 20 | 21 | ## Parameters for RLMAP 22 | 23 | ## sensor params 24 | frame_width: 640 25 | frame_height: 480 26 | scale_factor: 1 27 | 28 | # correspond to typical fx and fy factors in projection matrix 29 | calibration_rfx: 387.1847839355469 30 | calibration_rfy: 387.1847839355469 31 | 32 | # correspond to typical cx and cy factors in projection matrix 33 | calibration_rpx: 323.66632080078125 34 | calibration_rpy: 244.58934020996094 35 | 36 | # transformation between camera and drone frame 37 | camera_translation: [0.08950079, 0.03418742, -0.05983145] 38 | #camera_rotation: [-0.033135, 0.149826, 0.988157, 39 | # -0.998240, 0.043690, -0.040098, 40 | # -0.049181, -0.987747, 0.148115] 41 | 42 | camera_rotation: [ -0.037295, 0.063168, 0.997306, 43 | -0.998240, 0.043690, -0.040098, 44 | -0.046106, -0.997047, 0.061428] 45 | 46 | # depth camera parameters 47 | min_depth_distance: 0.3 48 | max_depth_distance: 3.0 49 | depth_factor: 0.001 50 | 51 | # field of view of the sensor 52 | horizontal_fov: 2.22 53 | vertical_fov: 0.75 54 | 55 | # map parameters 56 | simple_probability: 0.0000001 ## gain 'g' of probability p{k+1} = p{k} + g(1-p{k}) 57 | occupancy_threshold: 0.99 58 | #occupancy_threshold: 0.5 59 | voxel_leaf_size: 0.14 60 | map_size: 200 61 | image_percent: 0.0 62 | 63 | # rrt params 64 | rrt_informed_iteration: 1 65 | rrt_wall_time: 0.20 66 | rrt_angle: 2.2 67 | rrt_max: [6.5, 3.0, 0.75] 68 | rrt_min: [-0.1, -3.0, 0.69] 69 | 70 | ## Debug Data 71 | log_files: /home/aero/ 72 | debugging_msg: True 73 | -------------------------------------------------------------------------------- /ootp_ros/include/ootp_ros/initializer.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file initializer.hpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date June, 2018 5 | * @brief a yaml wrapper 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | #ifndef OOTP_INITIALIZER_HPP_ 38 | #define OOTP_INITIALIZER_HPP_ 39 | 40 | #include 41 | 42 | 43 | namespace ootp 44 | { 45 | 46 | void InitializeParams(ros::NodeHandle nh); 47 | 48 | } /* namespace ootp */ 49 | 50 | #endif /* INITIALIZER_HPP_ */ 51 | -------------------------------------------------------------------------------- /ootp_ros/launch/aero.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /ootp_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ootp_ros 4 | 0.1.0 5 | 6 | A ros wrapper for autonomous micro-mav navigation. 7 | 8 | Leo Campos 9 | Leo Campos 10 | BSD 11 | 12 | 13 | catkin 14 | 15 | 16 | cmake_modules 17 | roscpp 18 | ootp 19 | image_transport 20 | cv_bridge 21 | sensor_msgs 22 | rlmap_ros 23 | rrt_lib 24 | 25 | 26 | roscpp 27 | ootp 28 | image_transport 29 | cv_bridge 30 | sensor_msgs 31 | rlmap_ros 32 | rrt_lib 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /ootp_ros/src/initializer.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file initializer.cpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date June, 2018 5 | * @brief a yaml wrapper 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | #include 38 | #include 39 | #include 40 | 41 | namespace ootp 42 | { 43 | 44 | void InitializeParams(ros::NodeHandle nh) 45 | { 46 | std::vector path_gains; 47 | std::vector yaw_gains; 48 | ootp::VectorX Vpath_gains; 49 | ootp::VectorX Vyaw_gains; 50 | 51 | double path_error; 52 | double arc_velocity; 53 | double waypoint_lengh; 54 | double agent_radius; 55 | double safe_radius_person; 56 | int max_lost_turns; 57 | double yaw_velocity; 58 | double yaw_error; 59 | double rho; 60 | double y_rho; 61 | double dtime; 62 | 63 | double amax; 64 | double vmax; 65 | double gain; 66 | 67 | std::string log_files; 68 | 69 | // trajectory 70 | nh.getParam("path_gains", path_gains); 71 | nh.getParam("yaw_gains", yaw_gains); 72 | nh.getParam("path_error", path_error); 73 | nh.getParam("arc_velocity", arc_velocity); 74 | nh.getParam("waypoint_lengh", waypoint_lengh); 75 | nh.getParam("agent_radius", agent_radius); 76 | 77 | nh.getParam("safe_radius_person", safe_radius_person); 78 | nh.getParam("max_lost_turns", max_lost_turns); 79 | nh.getParam("yaw_velocity", yaw_velocity); 80 | nh.getParam("yaw_error", yaw_error); 81 | 82 | nh.getParam("rho", rho); 83 | nh.getParam("y_rho", y_rho); 84 | nh.getParam("dtime", dtime); 85 | 86 | nh.getParam("amax", amax); 87 | nh.getParam("vmax", vmax); 88 | nh.getParam("gain", gain); 89 | 90 | nh.getParam("log_files", log_files); 91 | 92 | 93 | Vpath_gains.resize( path_gains.size() ); 94 | Vpath_gains << path_gains[0], path_gains[1], path_gains[2], path_gains[3], path_gains[4]; 95 | 96 | Vyaw_gains.resize( yaw_gains.size() ); 97 | Vyaw_gains << yaw_gains[0], yaw_gains[1]; 98 | 99 | 100 | 101 | /* rrt parameters! */ 102 | 103 | std::vector vrrt_max; 104 | std::vector vrrt_min; 105 | double rrt_wall_time; 106 | double rrt_informed_iteration; 107 | Point rrt_max; 108 | Point rrt_min; 109 | double horizontal_fov; 110 | double vertical_fov; 111 | double max_depth_distance; 112 | 113 | nh.getParam("rrt_max", vrrt_max); 114 | nh.getParam("rrt_min", vrrt_min); 115 | nh.getParam("rrt_wall_time", rrt_wall_time); 116 | nh.getParam("rrt_informed_iteration", rrt_informed_iteration); 117 | 118 | nh.getParam("horizontal_fov", horizontal_fov); 119 | nh.getParam("vertical_fov", vertical_fov); 120 | nh.getParam("max_depth_distance", max_depth_distance); 121 | 122 | rrt_max << vrrt_max[0], vrrt_max[1], vrrt_max[2]; 123 | rrt_min << vrrt_min[0], vrrt_min[1], vrrt_min[2]; 124 | 125 | rrt::Config::init_params_externally(3, rrt_max, rrt_min, rrt_wall_time, rrt_informed_iteration, agent_radius, 126 | horizontal_fov, vertical_fov, max_depth_distance, waypoint_lengh); 127 | 128 | 129 | Config::init_params_externally( 130 | Vpath_gains, 131 | Vyaw_gains, 132 | path_error, 133 | arc_velocity, 134 | waypoint_lengh, 135 | agent_radius, 136 | safe_radius_person, 137 | max_lost_turns, 138 | yaw_velocity, 139 | yaw_error, 140 | rho, 141 | y_rho, 142 | dtime, 143 | vmax, 144 | amax, 145 | gain, 146 | horizontal_fov, 147 | vertical_fov, 148 | max_depth_distance, 149 | log_files 150 | ); 151 | } 152 | 153 | } /* namespace ootp */ 154 | -------------------------------------------------------------------------------- /ootp_ros/src/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file gazebo_main.hpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date May, 2018 5 | * @brief gazebo simulation 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | #include // std::thread 45 | #include 46 | 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | namespace ootp 53 | { 54 | 55 | class OOTPNode 56 | { 57 | private: 58 | rlmap::Visualizer map_visualizer_; 59 | std::thread *spin_drone_thread_; 60 | 61 | Point position_; 62 | Point orientation_; 63 | 64 | bool initialized_pos_; 65 | bool initialized_ori_; 66 | 67 | void publish_pose(void); 68 | 69 | public: 70 | OOTPNode(); 71 | virtual~OOTPNode(); 72 | void depthCb(const sensor_msgs::ImageConstPtr& msg); 73 | void EulerAnglesCb(const geometry_msgs::Vector3Stamped& msg ); 74 | void PosCb(const geometry_msgs::PoseStamped& msg); 75 | void start_drone_pose_thread(void); 76 | 77 | void set_goal(const Point &goal); 78 | void UpZRoutine(void); 79 | 80 | ros::Publisher reference_; 81 | ros::Publisher dreference_; 82 | ros::Publisher ddreference_; 83 | 84 | Point position_goal_; 85 | Point orientation_goal_; 86 | double rate_; 87 | 88 | Actions *actions_hand; 89 | }; 90 | 91 | OOTPNode::OOTPNode(): orientation_goal_(0.0,0.0,0.0) 92 | { 93 | initialized_pos_ = false; 94 | initialized_ori_ = false; 95 | rate_ = 0.01; 96 | spin_drone_thread_ = NULL; 97 | } 98 | OOTPNode::~OOTPNode() 99 | { 100 | if(spin_drone_thread_ != NULL) 101 | { 102 | spin_drone_thread_->join(); 103 | } 104 | 105 | } 106 | 107 | void OOTPNode::set_goal(const Point &goal) 108 | { 109 | position_goal_ = goal; 110 | } 111 | void OOTPNode::PosCb(const geometry_msgs::PoseStamped& msg) 112 | { 113 | position_(0) = msg.pose.position.x; 114 | position_(1) = msg.pose.position.y; 115 | position_(2) = msg.pose.position.z; 116 | initialized_pos_ = true; 117 | } 118 | 119 | void OOTPNode::EulerAnglesCb(const geometry_msgs::Vector3Stamped& msg ) 120 | { 121 | orientation_(0) = msg.vector.x; 122 | orientation_(1) = msg.vector.y; 123 | orientation_(2) = msg.vector.z; 124 | initialized_ori_ = true; 125 | } 126 | 127 | void OOTPNode::start_drone_pose_thread(void) 128 | { 129 | spin_drone_thread_ = new std::thread(&OOTPNode::publish_pose, this); 130 | } 131 | void OOTPNode::UpZRoutine(void) 132 | { 133 | double point = 0.1; 134 | while(true) 135 | { 136 | geometry_msgs::PoseStamped msg_control; 137 | msg_control.header.stamp = ros::Time::now(); 138 | if(reference_.getNumSubscribers() > 0) 139 | { 140 | msg_control.pose.position.x = 0.0; 141 | msg_control.pose.position.y = 0.0; 142 | msg_control.pose.position.z = point; 143 | point += 0.05; 144 | reference_.publish(msg_control); 145 | ROS_DEBUG("Send : %f mts", point); 146 | if(point >= 0.7) 147 | { 148 | ROS_INFO("UpZ routine end ..."); 149 | return; 150 | } 151 | } 152 | 153 | ros::Duration(0.2).sleep(); // sleep for 10 ms 154 | } 155 | } 156 | void OOTPNode::publish_pose(void) 157 | { 158 | MatrixXD states; 159 | double yaw; 160 | 161 | geometry_msgs::PoseStamped msg; 162 | geometry_msgs::PoseStamped msg_pub; 163 | ootp::eStates state_value; 164 | Point state_yaw; 165 | 166 | actions_hand->StartTimer(); 167 | 168 | while(ros::ok()) 169 | { 170 | actions_hand->GetMovement(states, state_yaw); 171 | 172 | geometry_msgs::PoseStamped msg_control; 173 | msg_control.header.stamp = ros::Time::now(); 174 | if(reference_.getNumSubscribers() > 0) 175 | { 176 | Point hand = states.col(kPos); 177 | msg_control.pose.position.x = hand(0); 178 | msg_control.pose.position.y = hand(1); 179 | msg_control.pose.position.z = hand(2); 180 | 181 | msg_control.pose.orientation.z = state_yaw(0); 182 | reference_.publish(msg_control); 183 | } 184 | if(dreference_.getNumSubscribers() > 0) 185 | { 186 | Point hand = states.col(kVel); 187 | msg_control.pose.position.x = hand(0); 188 | msg_control.pose.position.y = hand(1); 189 | msg_control.pose.position.z = hand(2); 190 | 191 | msg_control.pose.orientation.z = state_yaw(1); 192 | dreference_.publish(msg_control); 193 | } 194 | if(ddreference_.getNumSubscribers() > 0) 195 | { 196 | Point hand = states.col(kAcc); 197 | msg_control.pose.position.x = hand(0); 198 | msg_control.pose.position.y = hand(1); 199 | msg_control.pose.position.z = hand(2); 200 | 201 | msg_control.pose.orientation.z = state_yaw(2); 202 | ddreference_.publish(msg_control); 203 | } 204 | 205 | ros::Duration(rate_).sleep(); 206 | } 207 | } 208 | 209 | void OOTPNode::depthCb(const sensor_msgs::ImageConstPtr& msg) 210 | { 211 | try 212 | { 213 | 214 | if(initialized_pos_ && initialized_ori_) 215 | { 216 | //ROS_INFO("Received new frame with timestamp %f", msg->header.stamp.toSec()); 217 | 218 | initialized_pos_ = false; 219 | initialized_ori_ = false; 220 | 221 | rlmap::FrameObj dst = (cv_bridge::toCvShare(msg, "16UC1")->image).clone(); 222 | 223 | Transform pose(position_, orientation_ ); 224 | 225 | actions_hand->AddScene(dst, pose, msg->header.stamp.toSec()); 226 | 227 | rlmap::stdVoxelVector map_occup; 228 | actions_hand->get_occupied_map(map_occup); 229 | map_visualizer_.PublishOccupiedMap(map_occup); 230 | 231 | PointList points; 232 | actions_hand->get_raw_points(points); 233 | map_visualizer_.PublishRawPoints(points); 234 | 235 | //PointList points; 236 | actions_hand->get_local_path(points); 237 | map_visualizer_.PublishPath(points); 238 | 239 | actions_hand->get_path_consumed(points); 240 | map_visualizer_.PublishPathConsumed(points); 241 | } 242 | 243 | }catch (cv_bridge::Exception& e) 244 | { 245 | ROS_ERROR("cv_bridge exception: %s", e.what()); 246 | } 247 | } 248 | 249 | } /* namespace ootp */ 250 | 251 | int main(int argc, char **argv) 252 | { 253 | ros::init(argc, argv, "ootp_node"); 254 | ROS_INFO( "created ootp_node \n"); 255 | ros::NodeHandle node_nh("~"); 256 | rlmap::InitializeParams(node_nh); 257 | ootp::InitializeParams(node_nh); 258 | ROS_INFO( "params initiated \n"); 259 | ootp::OOTPNode ootp_node; 260 | node_nh.getParam("dtime", ootp_node.rate_); 261 | 262 | //check for ros log status... 263 | bool is_debug; 264 | node_nh.getParam("debugging_msg", is_debug); 265 | if(is_debug) 266 | { 267 | ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug); 268 | }else 269 | { 270 | ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); 271 | } 272 | ros::NodeHandle node_n; 273 | ros::Subscriber sub = node_n.subscribe("/depth/image_raw", 1, &ootp::OOTPNode::depthCb, &ootp_node); 274 | 275 | ros::Subscriber sub_euler = node_n.subscribe("/euler", 1, &ootp::OOTPNode::EulerAnglesCb, &ootp_node); 276 | 277 | ros::Subscriber sub_pos = node_n.subscribe("/odom_control/pose", 1, &ootp::OOTPNode::PosCb, &ootp_node); 278 | 279 | ootp_node.reference_ = node_n.advertise("/aero/ref_pos", 1, true); 280 | ootp_node.dreference_ = node_n.advertise("/aero/ref_vel", 1, true); 281 | ootp_node.ddreference_ = node_n.advertise("/aero/ref_acc", 1, true); 282 | 283 | ROS_DEBUG("publishing goals in : %s", "/aero/ref_pos"); 284 | 285 | std::cout<<"starting node ..."< goal_vector; 292 | node_nh.getParam("goal", goal_vector); 293 | // starting new goal ootp algo 294 | ootp::Point goal(goal_vector[0], goal_vector[1], goal_vector[2]); 295 | ROS_INFO( "starting exploration algorithm with goal at : %f, %f, %f \n", goal(0), goal(1), goal(2)); 296 | ootp_node.actions_hand->UpdateGoal(goal, true); 297 | 298 | ootp_node.start_drone_pose_thread(); 299 | 300 | ros::spin(); 301 | 302 | ROS_INFO("ootp_node terminated.\n"); 303 | return 0; 304 | } 305 | -------------------------------------------------------------------------------- /ootp_simulator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | SET(PROJECT_NAME ootp_simulator) 2 | PROJECT(${PROJECT_NAME}) 3 | CMAKE_MINIMUM_REQUIRED (VERSION 2.8.3) 4 | #SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo 5 | SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/CMakeModules/") 6 | 7 | # Set build flags. Set IS_ARM on odroid board as environment variable 8 | SET(CMAKE_CXX_FLAGS "-Wall -D_LINUX -D_REENTRANT -march=native -Wno-unused-variable -Wno-unused-but-set-variable -Wno-unknown-pragmas") 9 | IF(DEFINED ENV{ARM_ARCHITECTURE}) 10 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -march=armv7-a") 11 | ELSE() 12 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse -msse2 -msse3 -mssse3") 13 | ENDIF() 14 | IF(CMAKE_COMPILER_IS_GNUCC) 15 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 16 | ELSE() 17 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 18 | ENDIF() 19 | SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") 20 | 21 | 22 | # Add catkin and required ROS packages 23 | FIND_PACKAGE(catkin REQUIRED COMPONENTS 24 | cmake_modules 25 | roscpp 26 | image_transport 27 | cv_bridge 28 | sensor_msgs 29 | pcl_conversions 30 | pcl_ros 31 | ootp 32 | ootp_ros 33 | mav_msgs 34 | trajectory_msgs 35 | nav_msgs 36 | ) 37 | 38 | # Add plain cmake packages 39 | FIND_PACKAGE(OpenCV REQUIRED) 40 | FIND_PACKAGE(Eigen REQUIRED) 41 | 42 | # Describe catkin Project 43 | catkin_package( 44 | DEPENDS Eigen OpenCV 45 | CATKIN_DEPENDS roscpp image_transport cv_bridge sensor_msgs pcl_conversions pcl_ros 46 | ootp mav_msgs trajectory_msgs ootp_ros nav_msgs 47 | INCLUDE_DIRS include 48 | LIBRARIES ootp_simulator 49 | ) 50 | 51 | # Include dirs 52 | INCLUDE_DIRECTORIES( 53 | include 54 | ${Eigen_INCLUDE_DIRS} 55 | ${OpenCV_INCLUDE_DIRS} 56 | ${catkin_INCLUDE_DIRS} 57 | ) 58 | # Set link libraries 59 | LIST(APPEND LINK_LIBS 60 | ${OpenCV_LIBS} 61 | ${catkin_LIBRARIES} 62 | ) 63 | ADD_LIBRARY(ootp_simulator SHARED 64 | src/simulation_tools.cpp 65 | ) 66 | TARGET_LINK_LIBRARIES(ootp_simulator ${LINK_LIBS}) 67 | 68 | ADD_EXECUTABLE(ootp_gazebo 69 | src/gazebo_main.cpp) 70 | target_link_libraries(ootp_gazebo ${catkin_LIBRARIES} ootp_simulator) 71 | 72 | -------------------------------------------------------------------------------- /ootp_simulator/config/simulation_maze.yaml: -------------------------------------------------------------------------------- 1 | ## this file correspond to simualtion parameters in RotorS gazebo world with R200 realsense camera plugin 2 | 3 | ## Parameters for OOTP 4 | # filter parameters 5 | path_gains: [ -384.0, -400.0, -140.0, -20.0, 1.5 ] 6 | yaw_gains: [-1000.0, -250.0] 7 | path_error: 0.1 8 | arc_velocity: 10.0 9 | waypoint_lengh: 0.2 10 | yaw_velocity: 2.0 11 | yaw_error: 0.05 12 | dynamics_iterations: 5 13 | dynamics_step_size: 0.005 #0.1 14 | 15 | # main params 16 | agent_radius: 0.4 17 | agent_height: 0.1 18 | safe_radius_person: 0.5 19 | max_lost_turns: 3 20 | goal: [50.0, 50.0, 1.0] 21 | 22 | ## Parameters for RLMAP 23 | 24 | ## sensor params 25 | frame_width: 640 26 | frame_height: 480 27 | scale_factor: 1 28 | 29 | # correspond to typical fx and fy factors in projection matrix 30 | calibration_rfx: 463.8890075683594 31 | calibration_rfy: 463.8890075683594 32 | 33 | # correspond to typical cx and cy factors in projection matrix 34 | calibration_rpx: 320.0 35 | calibration_rpy: 240.0 36 | 37 | # transformation between camera and drone frame 38 | camera_translation: [0.0, -0.030, -0.061] 39 | camera_rotation: [0.00000,-0.19867,0.98007, 40 | -1.00000,-0.00000,0.00000, 41 | 0.00000,-0.98007,-0.19867] 42 | 43 | # depth camera parameters 44 | min_depth_distance: 0.3 45 | max_depth_distance: 3.0 46 | depth_factor: 0.001 47 | 48 | # field of view of the sensor 49 | horizontal_fov: 2.22 50 | vertical_fov: 0.75 51 | 52 | # map parameters 53 | simple_probability: 0.0001 ## gain 'g' of probability p{k+1} = p{k} + g(1-p{k}) 54 | occupancy_threshold: 0.99 55 | voxel_leaf_size: 0.2 56 | map_size: 200 57 | image_percent: 0.0 58 | 59 | # rrt params 60 | rrt_informed_iteration: 1 61 | rrt_wall_time: 0.20 62 | rrt_angle: 2.2 63 | rrt_max: [50.0, 50.0, 1.3] 64 | rrt_min: [-50.0, -50.0, 0.5] 65 | 66 | ## Parameters for Simulation 67 | trajectory_rate: 0.01 #0.001 68 | debugging_msg: True 69 | -------------------------------------------------------------------------------- /ootp_simulator/include/ootp_simulator/simulation_tools.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file simulation_tools.hpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date May, 2018 5 | * @brief gazebo simulation tools 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | #ifndef SIMULATION_TOOLS_HPP_ 38 | #define SIMULATION_TOOLS_HPP_ 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | 45 | 46 | namespace ootp 47 | { 48 | char getch(); 49 | bool init_gazebo_engine(void); 50 | void publish_goal(const Point &point, double yaw, ros::Publisher traj_pub); 51 | Point get_euler_from_quat(double x, double y, double z, double w); 52 | void toEulerAngle(const quat& q, double& roll, double& pitch, double& yaw); 53 | 54 | 55 | }/* namespace ootp */ 56 | 57 | 58 | #endif /* SIMULATION_TOOLS_HPP_ */ 59 | -------------------------------------------------------------------------------- /ootp_simulator/launch/maze.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /ootp_simulator/launch/maze_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /ootp_simulator/models/maze/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | maze 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /ootp_simulator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ootp_simulator 4 | 0.1.0 5 | 6 | A ros wrapper for autonomous micro-mav navigation simulation in gazebo physics simulator 7 | 8 | Leo Campos 9 | Leo Campos 10 | BSD 11 | 12 | 13 | catkin 14 | 15 | 16 | cmake_modules 17 | roscpp 18 | ootp 19 | image_transport 20 | cv_bridge 21 | sensor_msgs 22 | pcl_conversions 23 | pcl_ros 24 | mav_msgs 25 | trajectory_msgs 26 | ootp_ros 27 | nav_msgs 28 | 29 | 30 | roscpp 31 | ootp 32 | image_transport 33 | cv_bridge 34 | sensor_msgs 35 | pcl_conversions 36 | pcl_ros 37 | mav_msgs 38 | trajectory_msgs 39 | ootp_ros 40 | nav_msgs 41 | 42 | 43 | -------------------------------------------------------------------------------- /ootp_simulator/src/gazebo_main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file gazebo_main.hpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date May, 2018 5 | * @brief gazebo simulation 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include // std::thread 48 | 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | 55 | namespace ootp 56 | { 57 | 58 | void toEulerAngle(const quat& q, double& roll, double& pitch, double& yaw); 59 | 60 | class OOTPNode 61 | { 62 | private: 63 | rlmap::Visualizer map_visualizer_; 64 | 65 | nav_msgs::Odometry current_pose_; 66 | std::thread *spin_drone_thread_; 67 | 68 | bool initialized_pos_; 69 | bool initialized_ori_; 70 | void publish_pose(void); 71 | 72 | public: 73 | OOTPNode(); 74 | virtual~OOTPNode(); 75 | void depthCb(const sensor_msgs::ImageConstPtr& msg); 76 | void poseCb(const nav_msgs::Odometry& msg); 77 | void start_drone_pose_thread(void); 78 | 79 | void set_goal(const Point &goal); 80 | 81 | ros::Publisher traj_pub_; 82 | ros::Publisher state_machine_; 83 | Point position_goal_; 84 | Point orientation_goal_; 85 | double rate_; 86 | 87 | Actions *actions_hand; 88 | }; 89 | 90 | OOTPNode::OOTPNode(): orientation_goal_(0.0,0.0,0.0) 91 | { 92 | initialized_pos_ = false; 93 | initialized_ori_ = false; 94 | rate_ = 0.01; 95 | spin_drone_thread_ = NULL; 96 | } 97 | OOTPNode::~OOTPNode() 98 | { 99 | if(spin_drone_thread_ != NULL) 100 | { 101 | spin_drone_thread_->join(); 102 | } 103 | 104 | } 105 | 106 | void OOTPNode::set_goal(const Point &goal) 107 | { 108 | position_goal_ = goal; 109 | } 110 | 111 | void OOTPNode::poseCb(const nav_msgs::Odometry& msg) 112 | { 113 | current_pose_ = msg; 114 | initialized_pos_ = true; 115 | initialized_ori_ = true; 116 | } 117 | void OOTPNode::start_drone_pose_thread(void) 118 | { 119 | spin_drone_thread_ = new std::thread(&OOTPNode::publish_pose, this); 120 | } 121 | void OOTPNode::publish_pose(void) 122 | { 123 | MatrixXD states; 124 | double yaw; 125 | 126 | geometry_msgs::PoseStamped msg; 127 | geometry_msgs::PoseStamped msg_pub; 128 | ootp::eStates state_value; 129 | 130 | while(ros::ok()) 131 | { 132 | actions_hand->GetMovement(states, yaw); 133 | Point pose = states.col(0); 134 | 135 | ootp::publish_goal( pose, yaw, traj_pub_); 136 | actions_hand->GetState(state_value); 137 | 138 | msg_pub.header.stamp = ros::Time::now(); 139 | msg_pub.pose.position.x = (int)state_value; 140 | msg_pub.pose.orientation.x = pose(0); 141 | msg_pub.pose.orientation.y = pose(1); 142 | msg_pub.pose.orientation.z = pose(2); 143 | msg_pub.pose.orientation.w = yaw; 144 | 145 | state_machine_.publish(msg_pub); 146 | 147 | PointList path; 148 | actions_hand->get_local_path(path); 149 | map_visualizer_.PublishPath(path); 150 | 151 | actions_hand->get_path_consumed(path); 152 | map_visualizer_.PublishPathConsumed(path); 153 | 154 | PointList points; 155 | actions_hand->get_raw_points(points); 156 | map_visualizer_.PublishRawPoints(points); 157 | 158 | 159 | ros::Duration(rate_).sleep(); // sleep for 10 ms 160 | } 161 | } 162 | 163 | 164 | void OOTPNode::depthCb(const sensor_msgs::ImageConstPtr& msg) 165 | { 166 | try 167 | { 168 | if(initialized_pos_ && initialized_ori_) 169 | { 170 | initialized_pos_ = false; 171 | initialized_ori_ = false; 172 | 173 | rlmap::FrameObj dst = (cv_bridge::toCvShare(msg, "16UC1")->image).clone(); 174 | 175 | Point position; 176 | position(0) = current_pose_.pose.pose.position.x; 177 | position(1) = current_pose_.pose.pose.position.y; 178 | position(2) = current_pose_.pose.pose.position.z; 179 | quat orientation(current_pose_.pose.pose.orientation.w, current_pose_.pose.pose.orientation.x,current_pose_.pose.pose.orientation.y, 180 | current_pose_.pose.pose.orientation.z); 181 | 182 | double roll, pitch, yaw; 183 | toEulerAngle(orientation, roll, pitch, yaw); 184 | 185 | Transform pose(position, Point(roll, pitch, yaw) ); 186 | 187 | actions_hand->AddScene(dst, pose, msg->header.stamp.toSec()); 188 | 189 | rlmap::stdVoxelVector map_occup; 190 | actions_hand->get_occupied_map(map_occup); 191 | map_visualizer_.PublishOccupiedMap(map_occup); 192 | 193 | } 194 | 195 | }catch (cv_bridge::Exception& e) 196 | { 197 | ROS_ERROR("cv_bridge exception: %s", e.what()); 198 | } 199 | } 200 | 201 | } /* namespace ootp */ 202 | 203 | int main(int argc, char **argv) 204 | { 205 | 206 | ros::init(argc, argv, "ootp_gazebo"); 207 | ROS_INFO( "created ootp_gazebo \n"); 208 | 209 | ros::NodeHandle node_nh("~"); 210 | 211 | 212 | rlmap::InitializeParams(node_nh); 213 | ootp::InitializeParams(node_nh); 214 | 215 | ROS_INFO( "params initiated \n"); 216 | 217 | ootp::OOTPNode ootp_node; 218 | 219 | node_nh.getParam("dtime", ootp_node.rate_); 220 | 221 | //check for ros log status... 222 | bool is_debug; 223 | node_nh.getParam("debugging_msg", is_debug); 224 | if(is_debug) 225 | { 226 | ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug); 227 | }else 228 | { 229 | ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); 230 | } 231 | 232 | ros::NodeHandle node_n; 233 | ros::Subscriber sub = node_n.subscribe("/rs0r200/camera/depth/image_raw", 1, &ootp::OOTPNode::depthCb, &ootp_node); 234 | ros::Subscriber sub_pose = node_n.subscribe("/hummingbird/odometry_sensor1/odometry", 1, &ootp::OOTPNode::poseCb, &ootp_node); 235 | ootp_node.traj_pub_ = node_n.advertise("/hummingbird/command/trajectory", 10); 236 | 237 | ootp_node.state_machine_ = node_n.advertise("/state_machine", 1); 238 | 239 | ROS_DEBUG("publishing goal in : %s", "/hummingbird/command/trajectory"); 240 | 241 | // initializing gazebo physics engine 242 | if ( !ootp::init_gazebo_engine() ) 243 | { 244 | ROS_FATAL("Could not wake up Gazebo."); 245 | return 0; 246 | } 247 | // sending initial position 248 | ootp::Point position(0.0,0.0,0.7); 249 | ootp::publish_goal( position, 0.0, ootp_node.traj_pub_); 250 | 251 | ootp_node.actions_hand = new ootp::Actions( position ); 252 | 253 | 254 | ROS_INFO(" Starting drone's initial position at origin"); 255 | // Wait for drone to start 256 | ros::Duration(2).sleep(); 257 | 258 | // starting new goal ootp algo 259 | ootp::Point goal(25.0, 25.0, 1.0); 260 | ROS_INFO( "starting exploration algorithm with goal at : %f, %f, %f \n", goal(0), goal(1), goal(2)); 261 | 262 | ootp_node.set_goal(goal); 263 | ootp_node.actions_hand->UpdateGoal(goal, true); 264 | 265 | ootp_node.start_drone_pose_thread(); 266 | 267 | ros::spin(); 268 | 269 | ROS_INFO("ootp_gazebo terminated.\n"); 270 | return 0; 271 | } 272 | -------------------------------------------------------------------------------- /ootp_simulator/src/simulation_tools.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file simulation_tools.cpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date May, 2018 5 | * @brief gazebo simulation tools 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | #include 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | #include 53 | 54 | namespace ootp 55 | { 56 | 57 | char getch() 58 | { 59 | fd_set set; 60 | struct timeval timeout; 61 | int rv; 62 | char buff = 0; 63 | int len = 1; 64 | int filedesc = 0; 65 | FD_ZERO(&set); 66 | FD_SET(filedesc, &set); 67 | 68 | timeout.tv_sec = 0; 69 | timeout.tv_usec = 1000; 70 | 71 | rv = select(filedesc + 1, &set, NULL, NULL, &timeout); 72 | 73 | struct termios old = {0}; 74 | if (tcgetattr(filedesc, &old) < 0) 75 | ROS_ERROR("tcsetattr()"); 76 | old.c_lflag &= ~ICANON; 77 | old.c_lflag &= ~ECHO; 78 | old.c_cc[VMIN] = 1; 79 | old.c_cc[VTIME] = 0; 80 | if (tcsetattr(filedesc, TCSANOW, &old) < 0) 81 | ROS_ERROR("tcsetattr ICANON"); 82 | 83 | if(rv == -1) 84 | ROS_ERROR("select"); 85 | else if(rv != 0) 86 | //ROS_INFO("no_key_pressed"); 87 | 88 | //else 89 | read(filedesc, &buff, len ); 90 | 91 | old.c_lflag |= ICANON; 92 | old.c_lflag |= ECHO; 93 | if (tcsetattr(filedesc, TCSADRAIN, &old) < 0) 94 | ROS_ERROR ("tcsetattr ~ICANON"); 95 | return (buff); 96 | } 97 | 98 | bool init_gazebo_engine(void) 99 | { 100 | std_srvs::Empty srv; 101 | bool unpaused = ros::service::call("/gazebo/unpause_physics", srv); 102 | unsigned int i = 0; 103 | // Trying to unpause Gazebo for 10 seconds. 104 | while (i <= 10 && !unpaused) 105 | { 106 | ROS_INFO("Wait for 1 second before trying to unpause Gazebo again."); 107 | std::this_thread::sleep_for(std::chrono::seconds(1)); 108 | unpaused = ros::service::call("/gazebo/unpause_physics", srv); 109 | ++i; 110 | } 111 | if (!unpaused) 112 | { 113 | ROS_FATAL("Could not wake up Gazebo."); 114 | return false; 115 | } 116 | ROS_INFO("Unpaused the Gazebo simulation."); 117 | // Wait for Gazebo GUI show up. 118 | ros::Duration(10).sleep(); 119 | return true; 120 | } 121 | 122 | void publish_goal(const Point &point, double yaw, ros::Publisher traj_pub) 123 | { 124 | trajectory_msgs::MultiDOFJointTrajectory trajectory_msg; 125 | trajectory_msg.header.stamp = ros::Time::now(); 126 | mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(point, yaw, &trajectory_msg); 127 | traj_pub.publish(trajectory_msg); 128 | } 129 | 130 | Point toEuler(double x, double y, double z, double w) 131 | { 132 | Point orientation; 133 | 134 | // roll (x-axis rotation) 135 | double sinr_cosp = +2.0 * (w * x + y * z); 136 | double cosr_cosp = +1.0 - 2.0 * (x * x + y * y); 137 | orientation(0) = atan2(sinr_cosp, cosr_cosp); 138 | 139 | // pitch (y-axis rotation) 140 | double sinp = +2.0 * (w * y - z * x); 141 | if (fabs(sinp) >= 1) 142 | orientation(1) = copysign(M_PI / 2, sinp); // use 90 degrees if out of range 143 | else 144 | orientation(1) = asin(sinp); 145 | 146 | // yaw (z-axis rotation) 147 | double siny_cosp = +2.0 * (w * z + x * y); 148 | double cosy_cosp = +1.0 - 2.0 * (y * y + z * z); 149 | orientation(2) = atan2(siny_cosp, cosy_cosp); 150 | 151 | return orientation; 152 | } 153 | 154 | void toEulerAngle(const quat& q, double& roll, double& pitch, double& yaw) 155 | { 156 | // roll (x-axis rotation) 157 | double sinr_cosp = +2.0 * (q.w() * q.x() + q.y() * q.z()); 158 | double cosr_cosp = +1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y()); 159 | roll = atan2(sinr_cosp, cosr_cosp); 160 | 161 | // pitch (y-axis rotation) 162 | double sinp = +2.0 * (q.w() * q.y() - q.z() * q.x()); 163 | if (fabs(sinp) >= 1) 164 | pitch = copysign(M_PI / 2, sinp); // use 90 degrees if out of range 165 | else 166 | pitch = asin(sinp); 167 | 168 | // yaw (z-axis rotation) 169 | double siny_cosp = +2.0 * (q.w() * q.z() + q.x() * q.y()); 170 | double cosy_cosp = +1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z()); 171 | yaw = atan2(siny_cosp, cosy_cosp); 172 | } 173 | 174 | }/* namespace ootp */ 175 | -------------------------------------------------------------------------------- /ootp_simulator/urdf/mav_with_rs_sensor.gazebo: -------------------------------------------------------------------------------- 1 | 2 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /ootp_simulator/worlds/maze.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://ground_plane 6 | 7 | 8 | model://sun 9 | 10 | 11 | model://maze 12 | 1.0 12.0 0 0 0 0 13 | 14 | 15 | 16 | 17 | 19 | 20 | 21 | 22 | EARTH_WGS84 23 | 47.3667 24 | 8.5500 25 | 500.0 26 | 0 27 | 28 | 29 | 30 | 31 | quick 32 | 1000 33 | 1.3 34 | 35 | 36 | 0 37 | 0.2 38 | 100 39 | 0.001 40 | 41 | 42 | 0.01 43 | 1 44 | 100 45 | 0 0 -9.8 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /path_contour/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(path_contour) 3 | 4 | set(CMAKE_CXX_FLAGS "-std=c++11 -O3 ${CMAKE_CXX_FLAGS}") 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | std_msgs 8 | geometry_msgs 9 | roscpp 10 | ) 11 | 12 | include_directories( 13 | ${catkin_INCLUDE_DIRS} 14 | ${EIGEN3_INCLUDE_DIR} 15 | include 16 | ) 17 | 18 | catkin_package( 19 | INCLUDE_DIRS include 20 | CATKIN_DEPENDS std_msgs geometry_msgs 21 | LIBRARIES path_contour 22 | ) 23 | 24 | ADD_LIBRARY(path_contour SHARED 25 | src/path_contour/path_contour.cpp 26 | ) 27 | 28 | target_link_libraries(path_contour 29 | ${catkin_LIBRARIES} 30 | ) 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /path_contour/config/aero.yaml: -------------------------------------------------------------------------------- 1 | # filter parameters 2 | path_gains: [ -3.0104536742267722,-9.257906132819208,-10.574352237875011,-5.326766977660398, 1.5 ] 3 | #path_gains: [ -1.0,-8.0,-28.0,-56.0, -70.0, -56.0, -28.0, -8.0 ] 4 | yaw_gains: [-3.0, -2.0] 5 | path_error: 0.4 6 | arc_velocity: 10.0 7 | waypoint_lengh: 0.4 8 | yaw_velocity: 0.001 9 | yaw_error: 0.05 10 | 11 | dynamics_iterations: 1 #15 12 | 13 | position: [0.0, 0.0, 0.1] 14 | yaw: 0.0 15 | 16 | rho: 5.5 17 | y_rho: 1.0 18 | 19 | dtime: 0.001 20 | 21 | vmax: 0.5 22 | amax: 0.3 23 | gain: 0.9 24 | 25 | -------------------------------------------------------------------------------- /path_contour/include/path_contour/path_contour.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file path_contour.h 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date May, 2018 5 | * @brief Trajectory generation algorithm implementation 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | #ifndef PATH_CONTOUR_H 38 | #define PATH_CONTOUR_H 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | namespace contour 45 | { 46 | 47 | typedef enum{use_ellipse,use_error}invariant_set_mode_t; 48 | 49 | struct State 50 | { 51 | Eigen::Vector3d position; 52 | Eigen::Vector3d velocity; 53 | Eigen::Vector3d acceleration; 54 | Eigen::Vector3d jerk; 55 | Eigen::Vector3d snap; 56 | double yaw; 57 | double omega; 58 | double alpha; 59 | }; 60 | 61 | struct Waypoint 62 | { 63 | Eigen::VectorXd position; 64 | double error; 65 | double yaw; 66 | double yaw_error; 67 | 68 | }; 69 | 70 | struct InvariantSet 71 | { 72 | Eigen::MatrixXd ellipse; 73 | Eigen::VectorXd gains; 74 | Eigen::VectorXd yaw_gains; 75 | invariant_set_mode_t mode; 76 | }; 77 | 78 | class PathDynamics 79 | { 80 | 81 | public: 82 | PathDynamics(); 83 | PathDynamics(uint8_t space_dimension, double arc_velocity); 84 | ~PathDynamics(); 85 | void SetGains(Eigen::VectorXd gains, Eigen::VectorXd yaw_gains, double error); 86 | void SetEllipse(Eigen::MatrixXd ellipse, Eigen::VectorXd gains, double error); 87 | void PushWaypoint(Eigen::VectorXd waypoint, double error, double yaw_ref, double yaw_error); 88 | State UpdateDynamics(double dt); 89 | State GetState(); 90 | void SetInitialPosition(const Eigen::VectorXd &position); 91 | void SetInitialPose(const Eigen::VectorXd &waypoint, const double &path_error, const double &yaw, const double &yaw_error); 92 | bool addWaypoint(uint32_t size); 93 | 94 | Eigen::Vector3d getLastWaypoint(void); 95 | Eigen::Vector3d getReference(void); 96 | 97 | double getLastYawWaypoint(void); 98 | double getYawReference(void); 99 | 100 | uint32_t waypointListSize(void); 101 | Eigen::Vector3d getLastVel(void); 102 | 103 | private: 104 | Eigen::Vector3d last_direction_; 105 | Eigen::Vector3d reference_; 106 | double yaw_ref_; 107 | std::vector waypoint_list_; 108 | State State_; 109 | double arc_; 110 | double arc_velocity_; 111 | InvariantSet gains_; 112 | }; 113 | 114 | }//contour 115 | 116 | #endif 117 | -------------------------------------------------------------------------------- /path_contour/launch/aero.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /path_contour/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | path_contour 4 | 0.0.0 5 | Path to trajectory 6 | Leo Campos 7 | Leo Campos 8 | 9 | BSD 10 | 11 | catkin 12 | roscpp 13 | std_msgs 14 | geometry_msgs 15 | 16 | 17 | std_msgs 18 | nav_msgs 19 | geometry_msgs 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /path_contour/src/path_contour/path_contour.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file path_contour.cpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date May, 2018 5 | * @brief Trajectory generation algorithm implementation 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | #include "path_contour/path_contour.h" 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #define maxf(x,y) (x>y?x:y) 44 | 45 | namespace contour 46 | { 47 | 48 | PathDynamics::PathDynamics() 49 | { 50 | yaw_ref_=0.0; 51 | last_direction_= Eigen::Vector3d::Zero(); 52 | reference_= Eigen::Vector3d::Zero(); 53 | State_.position = Eigen::Vector3d::Zero(); 54 | State_.velocity = Eigen::Vector3d::Zero(); 55 | State_.acceleration = Eigen::Vector3d::Zero(); 56 | State_.jerk = Eigen::Vector3d::Zero(); 57 | State_.snap = Eigen::Vector3d::Zero(); 58 | State_.yaw = 0.0; 59 | State_.omega = 0.0; 60 | State_.alpha = 0.0; 61 | arc_ = 0; 62 | arc_velocity_ = 1; 63 | waypoint_list_.resize(0); 64 | } 65 | 66 | PathDynamics::PathDynamics(uint8_t space_dimension, double arc_velocity) 67 | { 68 | yaw_ref_=0.0; 69 | last_direction_= Eigen::Vector3d::Zero(); 70 | reference_= Eigen::VectorXd::Zero(space_dimension); 71 | State_.position = Eigen::VectorXd::Zero(space_dimension); 72 | State_.velocity = Eigen::VectorXd::Zero(space_dimension); 73 | State_.acceleration = Eigen::VectorXd::Zero(space_dimension); 74 | State_.jerk = Eigen::VectorXd::Zero(space_dimension); 75 | State_.snap = Eigen::VectorXd::Zero(space_dimension); 76 | State_.yaw = 0.0; 77 | State_.omega = 0.0; 78 | State_.alpha = 0.0; 79 | arc_ = 0; 80 | arc_velocity_ = arc_velocity; 81 | waypoint_list_.resize(0); 82 | } 83 | bool PathDynamics::addWaypoint(uint32_t size) 84 | { 85 | if(waypoint_list_.size() < size) 86 | { 87 | return true; 88 | } 89 | return false; 90 | } 91 | 92 | Eigen::Vector3d PathDynamics::getReference(void) 93 | { 94 | return reference_; 95 | } 96 | 97 | Eigen::Vector3d PathDynamics::getLastWaypoint(void) 98 | { 99 | return waypoint_list_.back().position; 100 | } 101 | 102 | double PathDynamics::getLastYawWaypoint(void) 103 | { 104 | return waypoint_list_.back().yaw; 105 | } 106 | 107 | double PathDynamics::getYawReference(void) 108 | { 109 | return yaw_ref_; 110 | } 111 | 112 | Eigen::Vector3d PathDynamics::getLastVel(void) 113 | { 114 | Eigen::Vector3d direction; 115 | direction << cos(State_.yaw), sin(State_.yaw), 0.0f; 116 | return direction; 117 | } 118 | 119 | void PathDynamics::SetInitialPosition(const Eigen::VectorXd &position) 120 | { 121 | State_.position = position; 122 | PushWaypoint(position, 0.1, 0.0, 0.1); 123 | } 124 | 125 | void PathDynamics::SetInitialPose(const Eigen::VectorXd &waypoint, const double &path_error, const double &yaw, const double &yaw_error) 126 | { 127 | State_.position = waypoint; 128 | State_.yaw = yaw; 129 | yaw_ref_=yaw; 130 | PushWaypoint(waypoint, path_error, yaw, yaw_error); 131 | } 132 | 133 | 134 | void PathDynamics::SetGains(Eigen::VectorXd gains, Eigen::VectorXd yaw_gains, double error) 135 | { 136 | gains_.yaw_gains = yaw_gains; 137 | gains_.gains = gains; 138 | gains_.mode = use_error; 139 | } 140 | 141 | void PathDynamics::SetEllipse(Eigen::MatrixXd ellipse, Eigen::VectorXd gains, double error) 142 | { 143 | gains_.ellipse = ellipse; 144 | gains_.mode = use_ellipse; 145 | gains_.gains = gains; 146 | } 147 | 148 | PathDynamics::~PathDynamics() 149 | { 150 | } 151 | 152 | void PathDynamics::PushWaypoint(Eigen::VectorXd waypoint, double error, double yaw_ref, double yaw_error) 153 | { 154 | if(waypoint_list_.size()) 155 | { 156 | if( (waypoint - waypoint_list_[0].position).norm() < error && fabs(yaw_ref - waypoint_list_[0].yaw) < yaw_error) 157 | { 158 | return; 159 | } 160 | } 161 | Waypoint wp; 162 | wp.position = waypoint; 163 | wp.error = error; 164 | wp.yaw = yaw_ref; 165 | wp.yaw_error = yaw_error; 166 | waypoint_list_.push_back(wp); 167 | } 168 | uint32_t PathDynamics::waypointListSize(void) 169 | { 170 | return waypoint_list_.size(); 171 | } 172 | 173 | State PathDynamics::UpdateDynamics(double dt) 174 | { 175 | double distance; 176 | double yaw_error = 0.0; 177 | if(waypoint_list_.size()) 178 | { 179 | double control = arc_velocity_; 180 | double ellipse = 0.0f; 181 | for(int index = 0; index 0.0 ) 233 | { 234 | arc_-=distance; 235 | waypoint_list_.erase(waypoint_list_.begin()); 236 | } 237 | } 238 | } 239 | 240 | Eigen::VectorXd gains = gains_.gains; 241 | Eigen::VectorXd yaw_gains = gains_.yaw_gains; 242 | State_.snap = gains(0)*(State_.position - reference_) + gains(1)*State_.velocity + gains(2)*State_.acceleration + 243 | gains(3)*State_.jerk; 244 | State_.alpha = yaw_gains(0)*(yaw_error) + yaw_gains(1)*State_.omega; 245 | State_.position+=dt*State_.velocity + dt*dt*State_.acceleration/2.0 + pow(dt,3)*State_.jerk/6.0 + pow(dt,4)*State_.snap/24.0; 246 | State_.velocity+=dt*State_.acceleration + dt*dt*State_.jerk/2.0 + pow(dt,3)*State_.snap/6.0; 247 | State_.acceleration+=dt*State_.jerk + dt*dt*State_.snap/2.0; 248 | State_.jerk+=dt*State_.snap; 249 | 250 | State_.yaw+=dt*State_.omega + dt*dt*State_.alpha/2.0; 251 | State_.omega+=dt*State_.alpha; 252 | 253 | } 254 | return State_; 255 | } 256 | 257 | State PathDynamics::GetState() 258 | { 259 | return State_; 260 | } 261 | 262 | } 263 | -------------------------------------------------------------------------------- /rlmap/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | SET(PROJECT_NAME rlmap) 2 | PROJECT(${PROJECT_NAME}) 3 | CMAKE_MINIMUM_REQUIRED (VERSION 2.8.3) 4 | #SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo 5 | SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/CMakeModules/") 6 | 7 | # Set build flags. Set IS_ARM on odroid board as environment variable 8 | SET(CMAKE_CXX_FLAGS "-Wall -D_LINUX -D_REENTRANT -march=native -Wno-unused-variable -Wno-unused-but-set-variable -Wno-unknown-pragmas") 9 | IF(DEFINED ENV{ARM_ARCHITECTURE}) 10 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -march=armv7-a") 11 | ELSE() 12 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse -msse2 -msse3 -mssse3") 13 | ENDIF() 14 | IF(CMAKE_COMPILER_IS_GNUCC) 15 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 16 | ELSE() 17 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 18 | ENDIF() 19 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") 20 | 21 | # Add catkin and required ROS packages 22 | FIND_PACKAGE(catkin REQUIRED COMPONENTS 23 | cmake_modules 24 | roscpp 25 | ) 26 | 27 | # Add plain cmake packages 28 | FIND_PACKAGE(Eigen REQUIRED) 29 | FIND_PACKAGE(OpenCV REQUIRED) 30 | find_package(TBB REQUIRED) 31 | 32 | # Describe catkin Project 33 | catkin_package( 34 | DEPENDS Eigen OpenCV 35 | CATKIN_DEPENDS roscpp 36 | INCLUDE_DIRS include 37 | LIBRARIES rlmap 38 | ) 39 | 40 | # Include dirs 41 | INCLUDE_DIRECTORIES( 42 | include 43 | ${Eigen_INCLUDE_DIRS} 44 | ${OpenCV_INCLUDE_DIRS} 45 | ${TBB_INCLUDE_DIRS} 46 | ${catkin_INCLUDE_DIRS} 47 | ) 48 | 49 | # Set link libraries 50 | LIST(APPEND LINK_LIBS 51 | ${OpenCV_LIBS} 52 | ${TBB_LIBS} 53 | ${catkin_LIBRARIES} 54 | ) 55 | ADD_LIBRARY(rlmap SHARED 56 | src/config.cpp 57 | src/voxel.cpp 58 | src/map_storage.cpp 59 | src/rlmap.cpp 60 | src/ootp_map.cpp 61 | src/probability_manager.cpp 62 | src/tictoc.cpp 63 | 64 | ) 65 | TARGET_LINK_LIBRARIES(rlmap ${LINK_LIBS}) 66 | -------------------------------------------------------------------------------- /rlmap/include/rlmap/config.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file config.hpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date June, 2018 5 | * @brief This is the implementation of constant values of the algorithm 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | 38 | #ifndef RLCONFIG_HPP_ 39 | #define RLCONFIG_HPP_ 40 | 41 | #include 42 | 43 | namespace rlmap 44 | { 45 | 46 | class Config 47 | { 48 | public: 49 | static Config& getInstance(); 50 | 51 | // map storage constants 52 | static uint8_t& dimensions() { return getInstance().dimensions_; } 53 | static double& map_size() { return getInstance().map_size_; } 54 | static double& voxel_leaf_size() { return getInstance().voxel_leaf_size_; } 55 | static double& occupancy_threshold() { return getInstance().occupancy_threshold_; } 56 | static double& simple_probability() { return getInstance().simple_probability_; } 57 | // depth image processing constants 58 | static uint32_t& frame_width() { return getInstance().frame_width_; } 59 | static uint32_t& frame_height() { return getInstance().frame_height_; } 60 | static double& min_depth_distance() { return getInstance().min_depth_distance_; } 61 | static double& max_depth_distance() { return getInstance().max_depth_distance_; } 62 | static double& depth_factor() { return getInstance().depth_factor_; } 63 | static double& calibration_rfx() { return getInstance().calibration_rfx_; } 64 | static double& calibration_rfy() { return getInstance().calibration_rfy_; } 65 | static double& calibration_rpx() { return getInstance().calibration_rpx_; } 66 | static double& calibration_rpy() { return getInstance().calibration_rpy_; } 67 | static double& image_percent() { return getInstance().image_percent_; } 68 | static Point& camera_translation_wrt_drone() { return getInstance().camera_translation_wrt_drone_; } 69 | static Rotation3D& camera_rotation_wrt_drone() { return getInstance().camera_rotation_wrt_drone_; } 70 | 71 | 72 | static void map_params(double map_size, double voxel_leaf_size); 73 | static void init_params_externally( 74 | // map storage constants 75 | uint8_t dimensions, 76 | double map_size, 77 | double voxel_leaf_size, 78 | double occupancy_threshold, 79 | double simple_probability, 80 | // depth image processing constants 81 | uint32_t frame_width, 82 | uint32_t frame_height, 83 | double min_depth_distance, 84 | double max_depth_distance, 85 | double depth_factor, 86 | double calibration_rfx, 87 | double calibration_rfy, 88 | double calibration_rpx, 89 | double calibration_rpy, 90 | double image_percent, 91 | 92 | Point camera_translation_wrt_drone, 93 | Rotation3D camera_rotation_wrt_drone 94 | ); 95 | 96 | private: 97 | Config(void); 98 | Config(Config const&); 99 | void operator=(Config const&); 100 | // map storage constants 101 | uint8_t dimensions_; 102 | double map_size_; 103 | double voxel_leaf_size_; 104 | double occupancy_threshold_; 105 | double simple_probability_; 106 | 107 | // depth image processing constants 108 | uint32_t frame_width_; 109 | uint32_t frame_height_; 110 | double min_depth_distance_; 111 | double max_depth_distance_; 112 | double depth_factor_; 113 | double calibration_rfx_; 114 | double calibration_rfy_; 115 | double calibration_rpx_; 116 | double calibration_rpy_; 117 | double image_percent_; 118 | 119 | Point camera_translation_wrt_drone_; 120 | Rotation3D camera_rotation_wrt_drone_; 121 | 122 | }; 123 | 124 | }/* namespace rlmap */ 125 | 126 | #endif /* RLCONFIG_HPP_ */ 127 | -------------------------------------------------------------------------------- /rlmap/include/rlmap/global.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file global.hpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date June, 2018 5 | * @brief Definitions and common headers 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | 38 | 39 | #ifndef RLGLOBAL_HPP_ 40 | #define RLGLOBAL_HPP_ 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | namespace rlmap 52 | { 53 | 54 | #ifndef finline 55 | #ifdef WIN32 56 | #define finline __forceinline 57 | #else 58 | #define finline inline __attribute__((always_inline)) 59 | #endif 60 | #endif 61 | 62 | class Voxel; 63 | typedef unsigned __int128 uint128_t; 64 | typedef uint64_t type_key; 65 | typedef uint16_t data_key_size; 66 | typedef int16_t s_data_key_size; 67 | typedef std::set keySet; 68 | typedef std::vector keyVector; 69 | typedef std::map VoxelMap; 70 | 71 | typedef Eigen::Vector3d Point; 72 | typedef Eigen::MatrixXd Matrix; 73 | typedef std::vector PointList; 74 | typedef std::vector stdVoxelVector; 75 | typedef VoxelMap::iterator VoxelMap_it; 76 | typedef Eigen::Vector2i Pixel; 77 | typedef Eigen::Vector2d Point2D; 78 | typedef std::vector PixelList; 79 | typedef cv::Mat FrameObj; 80 | typedef Eigen::Matrix3d Rotation3D; 81 | typedef Eigen::Quaterniond Quaternion; 82 | 83 | 84 | template 85 | T clamp(T value, T min, T max) 86 | { 87 | const T tmp = value < min ? min : value; 88 | return tmp > max ? max : tmp; 89 | } 90 | 91 | Rotation3D finline GetYawRotation(double yaw) 92 | { 93 | Rotation3D rot; rot<< cos(yaw), -sin(yaw), 0.0, sin(yaw), cos(yaw), 0.0, 0.0, 0.0, 1.0; 94 | return rot; 95 | } 96 | Rotation3D finline GetRollRotation(double roll) 97 | { 98 | Rotation3D rot; rot<< 1.0, 0.0, 0.0, 0.0, cos(roll), -sin(roll), 0.0, sin(roll), cos(roll); 99 | return rot; 100 | } 101 | Rotation3D finline GetPitchRotation(double pitch) 102 | { 103 | Rotation3D rot; rot<< cos(pitch), 0.0, sin(pitch), 0.0, 1.0, 0.0, -sin(pitch), 0.0, cos(pitch); 104 | return rot; 105 | } 106 | Rotation3D finline GetZYXRotation(double roll, double pitch, double yaw) 107 | { 108 | Rotation3D rot; rot=GetYawRotation(yaw)*GetPitchRotation(pitch)*GetRollRotation(roll); 109 | return rot; 110 | } 111 | 112 | struct Transform 113 | { 114 | Point position_; 115 | Rotation3D rotation_; 116 | double yaw_; 117 | 118 | Transform(Point position, Quaternion rotation) : position_(position) 119 | { 120 | rotation_ = rotation.normalized().toRotationMatrix(); 121 | } 122 | 123 | Transform(Point position, Point rotation) : position_(position) 124 | { 125 | rotation_ = GetZYXRotation(rotation(0), rotation(1), rotation(2)); 126 | yaw_ = rotation(2); 127 | } 128 | Transform(Point position, double yaw) : position_(position) 129 | { 130 | rotation_ = GetYawRotation(yaw); 131 | yaw_ = yaw; 132 | } 133 | Transform(Point position, Rotation3D rotation, double yaw) : position_(position), rotation_(rotation), yaw_(yaw) 134 | { 135 | } 136 | }; 137 | 138 | }/* namespace rlmap */ 139 | 140 | 141 | 142 | 143 | #endif /* GLOBAL_HPP_ */ 144 | -------------------------------------------------------------------------------- /rlmap/include/rlmap/hash_functions.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file hash_functions.hpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date June, 2018 5 | * @brief This is the implementation of the hash function for map a point to a key 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | 38 | #ifndef HASH_FUNCTIONS_HPP_ 39 | #define HASH_FUNCTIONS_HPP_ 40 | 41 | #include 42 | #include 43 | 44 | namespace rlmap 45 | { 46 | 47 | class HashFunct 48 | { 49 | public: 50 | static const uint8_t ksize = 2*sizeof(type_key); 51 | static const data_key_size kmask = pow(2, ksize) - 1; 52 | 53 | finline static type_key Hashf(data_key_size level, const Point& point) 54 | { 55 | type_key key = level; 56 | double div_level = 1.0 / (((double)(1< 41 | #include 42 | #include 43 | #include 44 | 45 | namespace rlmap 46 | { 47 | const double invlog2 = (1.0/std::log(2.0)); 48 | const int32_t kchildren = 1 << 3; 49 | 50 | class MapStorage 51 | { 52 | public: 53 | // general methods 54 | MapStorage(); 55 | virtual ~MapStorage(); 56 | void InsertOccupiedPoints(PointList& points); 57 | void UpdateFreeSpace(keyVector &free_voxels); 58 | 59 | // get helper functions 60 | Voxel* GetVoxel(const Point &point); 61 | Voxel* GetVoxel(const type_key &key); 62 | bool GetKey(const Point &point, type_key &key); 63 | void GetCurrentOccupiedNodes(keySet ¤t_occupied_nodes); 64 | 65 | // collision methods 66 | bool IsLineCollisionFree(const Point& start, const Point& end, double radius); 67 | bool IsLineCollisionFreeOLD(const Point& start, const Point& end, double radius); 68 | bool IsPointCollisionFree(const Point& point, double radius); 69 | bool IsPointCollisionFree(Voxel *in_voxel, double radius); 70 | // TODO:: check that saving is performed correctly 71 | void SaveMap(const std::string &file_directory); 72 | void LoadMap(const std::string &file_directory); 73 | 74 | // debbug 75 | void GetCompleteMap(stdVoxelVector &map); 76 | void GetOccupiedMap(stdVoxelVector &map); 77 | 78 | private: 79 | VoxelMap map_; 80 | data_key_size levels_; 81 | keySet deleted_nodes_; 82 | CProbabilityMan probability_; 83 | keySet current_occupied_nodes_; 84 | type_key key_root_; 85 | Matrix children_; 86 | // general methods 87 | void InitMap(void); 88 | void Initialize3dChildren(void); 89 | void FillLevels(PointList& points); 90 | void DeleteOldNodes(void); 91 | 92 | // get functions 93 | void GetChildren(PointList& points, uint32_t level, const Point& center); 94 | void GetChildren3d(PointList& points, uint32_t level, const Point& center); 95 | finline uint32_t GetHashLevel(uint32_t map_level); 96 | finline bool GetKeyFromPoint(const Point &point, VoxelMap_it &vox_it); 97 | finline Point GetPointFromKey(const type_key &key); 98 | 99 | // for voxel collision method 100 | bool DetectCollision(const type_key &key, const Point& start, const Point& end, 101 | const Point &inv_direction, const Point &sign, double radius); 102 | finline bool IsVoxelInLine(const type_key &key, const Point& start, const Point& end, 103 | const Point &inv_direction, const Point &sign); 104 | 105 | }; 106 | 107 | } /* namespace rlmap */ 108 | 109 | 110 | 111 | #endif /* MAP_STORAGE_HPP_ */ 112 | -------------------------------------------------------------------------------- /rlmap/include/rlmap/ootp_map.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ootp_map.hpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date July, 2018 5 | * @brief This is the implementation for our customized version of map for the ootp application 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | 38 | 39 | #ifndef OOTP_MAP_HPP_ 40 | #define OOTP_MAP_HPP_ 41 | 42 | #include 43 | 44 | namespace rlmap 45 | { 46 | 47 | class OOTP_Map : public RLMap 48 | { 49 | public: 50 | OOTP_Map(); 51 | OOTP_Map(double map_size, double min_step); 52 | virtual ~OOTP_Map(); 53 | 54 | virtual void AddDepthImage(const FrameObj &image, const Transform &pose) override; 55 | 56 | //debugging methods 57 | void GetAllOccupiedMap(stdVoxelVector &points); 58 | void GetRawPoints(PointList &raw_points); 59 | 60 | private: 61 | uint32_t size_; 62 | PointList raw_points_; 63 | 64 | void Depht2Points(const FrameObj &image, const Transform &pose); 65 | bool Calculate3DPointFromDepth(const Pixel &pixel, uint16_t depth_raw, Point &projected_point); 66 | finline static bool isMinDepthValid(uint16_t depthMm, uint16_t minDepthMm) { return depthMm >= minDepthMm; } 67 | finline static bool isMaxDepthValid(uint16_t depthMm, uint16_t maxDepthMm) { return depthMm <= maxDepthMm; } 68 | 69 | OOTP_Map(const OOTP_Map&); 70 | OOTP_Map& operator=(const OOTP_Map&){ return *this;} 71 | 72 | }; 73 | 74 | } /* namespace rlmap */ 75 | 76 | 77 | 78 | #endif /* OOTP_MAP_HPP_ */ 79 | -------------------------------------------------------------------------------- /rlmap/include/rlmap/probability_manager.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file probability_manager.hpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date August, 2018 5 | * @brief Implementation for the probability of a voxel 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | 38 | #ifndef RLMAP_INCLUDE_RLMAP_PROBABILITY_MANAGER_HPP_ 39 | #define RLMAP_INCLUDE_RLMAP_PROBABILITY_MANAGER_HPP_ 40 | 41 | #include 42 | 43 | namespace rlmap 44 | { 45 | 46 | 47 | class CProbabilityMan 48 | { 49 | public: 50 | CProbabilityMan(); 51 | virtual ~CProbabilityMan(); 52 | double OccupancyThreshold(void){return occupancy_threshold_;} 53 | double ComputeVoxelProbability(double current_probability); 54 | private: 55 | double occupancy_threshold_; 56 | }; 57 | 58 | 59 | } /* namespace rlmap */ 60 | 61 | 62 | 63 | 64 | 65 | #endif /* RLMAP_INCLUDE_RLMAP_PROBABILITY_MANAGER_HPP_ */ 66 | -------------------------------------------------------------------------------- /rlmap/include/rlmap/rlmap.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file rlmap.hpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date June, 2018 5 | * @brief This is the implementation for our customized version of map 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | 38 | #ifndef RLMAP_HPP_ 39 | #define RLMAP_HPP_ 40 | 41 | 42 | 43 | #include 44 | #include 45 | 46 | namespace rlmap 47 | { 48 | 49 | class RLMap 50 | { 51 | public: 52 | RLMap(); 53 | virtual ~RLMap(); 54 | 55 | virtual void AddDepthImage(const FrameObj &image, const Transform &pose){}; 56 | void AddOccupiedPoints(PointList& points); 57 | 58 | // collision methods 59 | bool IsLineCollisionFree(const Point& start, const Point& end, double radius); 60 | bool IsLineCollisionFreeOLD(const Point& start, const Point& end, double radius); 61 | 62 | // Disk interacting map 63 | void SaveMap(const std::string &file_directory); 64 | void LoadMap(const std::string &file_directory); 65 | 66 | //debugging methods 67 | void GetCompleteMap(stdVoxelVector &map); 68 | 69 | 70 | protected: 71 | 72 | 73 | uint16_t min_distance_; 74 | uint16_t max_distance_; 75 | Point2D inverse_focal_length_; 76 | Point2D principal_point_; 77 | uint32_t step_size_; 78 | uint32_t free_space_image_percent_; 79 | 80 | void UpdateFreeSpace( const Transform &pose ); 81 | 82 | //debugging methods 83 | void GetOccupiedPoints(stdVoxelVector &points); 84 | 85 | 86 | 87 | 88 | private: 89 | MapStorage *map_; 90 | PointList ray_points_; 91 | 92 | void PrecalculateRays(void); 93 | 94 | // prevent copy of class 95 | RLMap(const RLMap&); 96 | RLMap& operator=(const RLMap&){ return *this;} 97 | }; 98 | 99 | } /* namespace rlmap */ 100 | 101 | #endif /* RLMAP_HPP_ */ 102 | -------------------------------------------------------------------------------- /rlmap/include/rlmap/tictoc.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file tictoc.hpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date Sep 19, 2018 5 | * @section LICENSE 6 | * 7 | * BSD-3-Clause License 8 | * 9 | * @copyright Copyright (C) 2020 Intel Corporation 10 | * 11 | * Redistribution and use in source and binary forms, with or without modification, 12 | * are permitted provided that the following conditions are met: 13 | *  14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | *    this list of conditions and the following disclaimer. 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | *    this list of conditions and the following disclaimer in the documentation 18 | *    and/or other materials provided with the distribution. 19 | * 3. Neither the name of the copyright holder nor the names of its contributors 20 | *    may be used to endorse or promote products derived from this software 21 | *    without specific prior written permission. 22 | *   23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 25 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 27 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 28 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 29 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 30 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 31 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 32 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 33 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | **/ 36 | 37 | #ifndef STP_INCLUDE_RLMAP_TICTOC_HPP_ 38 | #define STP_INCLUDE_RLMAP_TICTOC_HPP_ 39 | 40 | #include 41 | 42 | namespace rlmap 43 | { 44 | 45 | typedef unsigned long long TicToc_t; 46 | 47 | 48 | class TicToc 49 | { 50 | private: 51 | const unsigned long long kNano_ = 1000000000; 52 | 53 | TicToc_t timer_; 54 | 55 | public: 56 | TicToc(); 57 | virtual ~TicToc(); 58 | double Toc(void); 59 | }; 60 | 61 | typedef std::shared_ptr TicTocPtr; 62 | 63 | }/* namespace rlmap */ 64 | 65 | 66 | 67 | 68 | #endif /* STP_INCLUDE_RLMAP_TICTOC_HPP_ */ 69 | -------------------------------------------------------------------------------- /rlmap/include/rlmap/voxel.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file voxel.hpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date June, 2018 5 | * @brief This is the implementation of the voxel properties 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | 38 | #ifndef VOXEL_HPP_ 39 | #define VOXEL_HPP_ 40 | 41 | #include 42 | 43 | namespace rlmap 44 | { 45 | 46 | class Voxel 47 | { 48 | public: 49 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 50 | Voxel(void); 51 | Voxel(const Point ¢er, uint32_t level, int32_t probability); 52 | virtual ~Voxel(void); 53 | 54 | // Returning methods 55 | Point GetCenter(void) {return center_; } 56 | uint32_t GetLevel(void) {return level_; } 57 | double GetProbability(void) {return probability_; } 58 | 59 | // Assigning methods 60 | void AddProbability(const double probability); 61 | void ResetProbability(void); 62 | 63 | private: 64 | Point center_; 65 | double probability_; 66 | uint32_t level_; 67 | 68 | }; 69 | 70 | } /* namespace rlmap */ 71 | 72 | 73 | 74 | #endif /* VOXEL_HPP_ */ 75 | -------------------------------------------------------------------------------- /rlmap/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rlmap 4 | 0.1.0 5 | 6 | A library for a custom map 7 | 8 | Leo Campos 9 | Leo Campos 10 | BSD 11 | 12 | 13 | catkin 14 | 15 | 16 | roscpp 17 | cmake_modules 18 | roslib 19 | 20 | 21 | roscpp 22 | roslib 23 | 24 | 25 | -------------------------------------------------------------------------------- /rlmap/src/config.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file config.cpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date June, 2018 5 | * @brief This is the implementation of constant values of the algorithm 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | #include 38 | 39 | 40 | 41 | namespace rlmap 42 | { 43 | Config::Config() : 44 | dimensions_(3), 45 | map_size_(51.2), // must be voxel_leaf_size_ multiple 46 | voxel_leaf_size_(0.2), 47 | occupancy_threshold_(0.8), 48 | simple_probability_(0.001), 49 | frame_width_(640), 50 | frame_height_(480), 51 | min_depth_distance_(0.001), //(0.3), 52 | max_depth_distance_(3.0), //(25.0), 53 | depth_factor_(0.001), 54 | calibration_rfx_( (double)307.215 ), 55 | calibration_rfy_( calibration_rfx_ ), 56 | calibration_rpx_( (double)frame_width_ / (double)2.0), 57 | calibration_rpy_( (double)frame_height_ / (double)2.0 ), 58 | image_percent_(0.9), 59 | camera_translation_wrt_drone_(0.0,0.0,0.0), 60 | camera_rotation_wrt_drone_( GetZYXRotation(0.0, 0.0, 0.0) ) 61 | { 62 | } 63 | Config& Config::getInstance() 64 | { 65 | static Config instance; // Instantiated on first use and guaranteed to be destroyed 66 | return instance; 67 | } 68 | 69 | void Config::map_params(double map_size, double voxel_leaf_size) 70 | { 71 | getInstance().map_size_ = map_size; 72 | getInstance().voxel_leaf_size_ = voxel_leaf_size; 73 | } 74 | void Config::init_params_externally( 75 | // map storage constants 76 | uint8_t dimensions, 77 | double map_size, 78 | double voxel_leaf_size, 79 | double occupancy_threshold, 80 | double simple_probability, 81 | // depth image processing constants 82 | uint32_t frame_width, 83 | uint32_t frame_height, 84 | double min_depth_distance, 85 | double max_depth_distance, 86 | double depth_factor, 87 | double calibration_rfx, 88 | double calibration_rfy, 89 | double calibration_rpx, 90 | double calibration_rpy, 91 | double image_percent, 92 | 93 | Point camera_translation_wrt_drone, 94 | Rotation3D camera_rotation_wrt_drone) 95 | { 96 | 97 | getInstance().dimensions_ = dimensions; 98 | getInstance().map_size_ = map_size; 99 | getInstance().voxel_leaf_size_ = voxel_leaf_size; 100 | getInstance().occupancy_threshold_ = occupancy_threshold; 101 | getInstance().simple_probability_ = simple_probability; 102 | getInstance().frame_width_ = frame_width; 103 | getInstance().frame_height_ = frame_height; 104 | getInstance().min_depth_distance_ = min_depth_distance; 105 | getInstance().max_depth_distance_ = max_depth_distance; 106 | getInstance().depth_factor_ = depth_factor; 107 | getInstance().calibration_rfx_ = calibration_rfx; 108 | getInstance().calibration_rfy_ = calibration_rfy; 109 | getInstance().calibration_rpx_ = calibration_rpx; 110 | getInstance().calibration_rpy_ = calibration_rpy; 111 | getInstance().image_percent_ = image_percent; 112 | getInstance().camera_translation_wrt_drone_ = camera_translation_wrt_drone; 113 | getInstance().camera_rotation_wrt_drone_ = camera_rotation_wrt_drone; 114 | 115 | } 116 | 117 | }/* namespace rlmap */ 118 | -------------------------------------------------------------------------------- /rlmap/src/ootp_map.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ootp_map.cpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date July, 2018 5 | * @brief This is the implementation for our customized version of map for the ootp application 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | 38 | #include 39 | #include 40 | 41 | #include 42 | #include 43 | #include 44 | 45 | 46 | namespace rlmap 47 | { 48 | OOTP_Map::OOTP_Map() : RLMap() 49 | { 50 | Point step_dummy_one; 51 | Point step_dummy_two; 52 | Point step(1, 1, 1); 53 | if( Calculate3DPointFromDepth(Pixel(1,1), Config::max_depth_distance()/Config::depth_factor(), step_dummy_one) 54 | && Calculate3DPointFromDepth(Pixel(0,0), Config::max_depth_distance()/Config::depth_factor(), step_dummy_two)) 55 | { 56 | step = step_dummy_one - step_dummy_two; 57 | } 58 | size_ = std::floor(Config::voxel_leaf_size() / (2.0*fabs(step(0)))); 59 | } 60 | OOTP_Map::OOTP_Map(double map_size, double min_step) 61 | { 62 | Config::map_params(map_size, min_step); 63 | RLMap(); 64 | Point step_dummy_one; 65 | Point step_dummy_two; 66 | Point step(1, 1, 1); 67 | if( Calculate3DPointFromDepth(Pixel(1,1), Config::max_depth_distance()/Config::depth_factor(), step_dummy_one) 68 | && Calculate3DPointFromDepth(Pixel(0,0), Config::max_depth_distance()/Config::depth_factor(), step_dummy_two)) 69 | { 70 | step = step_dummy_one - step_dummy_two; 71 | } 72 | size_ = std::floor(Config::voxel_leaf_size() / (2.0*fabs(step(0)))); 73 | } 74 | OOTP_Map::~OOTP_Map() 75 | { 76 | 77 | } 78 | void OOTP_Map::AddDepthImage(const FrameObj &image, const Transform &pose) 79 | { 80 | Depht2Points( image, pose); 81 | UpdateFreeSpace(pose); 82 | 83 | } 84 | 85 | void OOTP_Map::Depht2Points(const FrameObj &image, const Transform &pose) 86 | { 87 | PointList points; 88 | std::vector< std::tuple > pixels; 89 | std::tuple pixel; 90 | 91 | for(int32_t rows = 0; rows < image.rows; ) 92 | { 93 | for(int32_t cols=0; cols < image.cols; ) 94 | { 95 | // cols -> x rows -> y, to match opencv with rs 96 | pixels.push_back( std::make_tuple( Pixel(cols, rows), ((uint16_t*)image.data)[image.cols*rows + cols]) ); 97 | cols+=size_; 98 | } 99 | rows+=size_; 100 | } 101 | for(size_t ind =0; ind < pixels.size() ;ind++) 102 | //tbb::parallel_for(size_t(0), (const size_t)pixels.size(), [&](size_t ind) 103 | { 104 | pixel = pixels[ind]; 105 | Point local; 106 | if(Calculate3DPointFromDepth( std::get<0>(pixel), std::get<1>(pixel), local )) 107 | { 108 | local = Config::camera_rotation_wrt_drone()*local + Config::camera_translation_wrt_drone(); 109 | Point global = pose.rotation_*local + pose.position_; 110 | points.push_back(global); 111 | } 112 | }//); 113 | raw_points_ = points; 114 | AddOccupiedPoints(points); 115 | } 116 | 117 | bool OOTP_Map::Calculate3DPointFromDepth(const Pixel &pixel, uint16_t depth_raw, Point &projected_point) 118 | { 119 | if( isMinDepthValid(depth_raw, min_distance_) && isMaxDepthValid(depth_raw, max_distance_) ) 120 | { 121 | const double depth = static_cast(depth_raw) * Config::depth_factor(); 122 | projected_point << (pixel.cast() - principal_point_).cwiseProduct(inverse_focal_length_) * depth , depth; 123 | return true; 124 | } 125 | return false; 126 | } 127 | 128 | // debugging methods 129 | void OOTP_Map::GetRawPoints(PointList &raw_points) 130 | { 131 | raw_points = raw_points_; 132 | } 133 | void OOTP_Map::GetAllOccupiedMap(stdVoxelVector &points) 134 | { 135 | GetOccupiedPoints(points); 136 | } 137 | 138 | } /* namespace rlmap */ 139 | 140 | -------------------------------------------------------------------------------- /rlmap/src/probability_manager.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file probability_manager.cpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date August, 2018 5 | * @brief Implementation for the probability of a voxel 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | 38 | 39 | #include 40 | #include 41 | 42 | namespace rlmap 43 | { 44 | CProbabilityMan::CProbabilityMan() 45 | { 46 | occupancy_threshold_ = Config::occupancy_threshold(); 47 | } 48 | CProbabilityMan::~CProbabilityMan() 49 | { 50 | 51 | } 52 | double CProbabilityMan::ComputeVoxelProbability(double current_probability) 53 | { 54 | return current_probability + Config::simple_probability()*(1.0 - current_probability); 55 | } 56 | 57 | 58 | }/* namespace rlmap */ 59 | 60 | 61 | -------------------------------------------------------------------------------- /rlmap/src/rlmap.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file rlmap.cpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date June, 2018 5 | * @brief This is the implementation for our customized version of map 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | namespace rlmap 45 | { 46 | 47 | RLMap::RLMap() 48 | { 49 | map_ = new MapStorage(); 50 | 51 | min_distance_ = (uint16_t)(Config::min_depth_distance() / Config::depth_factor()); 52 | max_distance_ = (uint16_t)(Config::max_depth_distance() / Config::depth_factor()); 53 | 54 | inverse_focal_length_ << 1.0 / Config::calibration_rfx(), 1.0 / Config::calibration_rfy(); 55 | principal_point_ << Config::calibration_rpx(), Config::calibration_rpy(); 56 | 57 | Point projected_point_min; 58 | Point projected_point_max; 59 | projected_point_max << (Pixel(1,1).cast() - principal_point_).cwiseProduct(inverse_focal_length_) * Config::max_depth_distance() , Config::max_depth_distance(); 60 | projected_point_min << (Pixel(0,0).cast() - principal_point_).cwiseProduct(inverse_focal_length_) * Config::max_depth_distance() , Config::max_depth_distance(); 61 | 62 | Point step = projected_point_max - projected_point_min; 63 | step_size_ = std::floor(Config::voxel_leaf_size() / (2.0*fabs(step(0)))); 64 | 65 | PrecalculateRays(); 66 | free_space_image_percent_ = (uint32_t)( (double)ray_points_.size()*Config::image_percent() ); 67 | 68 | } 69 | 70 | RLMap::~RLMap() 71 | { 72 | delete map_; 73 | } 74 | 75 | void RLMap::PrecalculateRays(void) 76 | { 77 | double depth = Config::max_depth_distance(); 78 | // TODO:: calculate this parameter depending on drone size 79 | int32_t safe_region = 50; 80 | 81 | for(int32_t rows = safe_region; rows < Config::frame_height() - safe_region; ) 82 | { 83 | for(int32_t cols = safe_region; cols < Config::frame_width() - safe_region; ) 84 | { 85 | Point projected_point; 86 | projected_point << (Pixel(cols, rows).cast() - principal_point_).cwiseProduct(inverse_focal_length_) * depth , depth; 87 | 88 | ray_points_.push_back( Config::camera_rotation_wrt_drone()*(projected_point) + Config::camera_translation_wrt_drone() ); 89 | 90 | cols+=step_size_; 91 | } 92 | rows+=step_size_; 93 | } 94 | } 95 | 96 | void RLMap::UpdateFreeSpace( const Transform &pose ) 97 | { 98 | 99 | //generate index 100 | std::set index_taken; 101 | keySet occupied_voxels; 102 | keyVector free_voxels; 103 | tbb::mutex mutex; 104 | 105 | uint32_t rays = ray_points_.size(); 106 | map_->GetCurrentOccupiedNodes(occupied_voxels); 107 | 108 | while( free_space_image_percent_ > index_taken.size() ) 109 | { 110 | uint32_t random = ((double)rand()) / ((double)RAND_MAX)*( rays - 1); 111 | index_taken.insert(random); 112 | } 113 | keyVector indexs; 114 | indexs.assign(index_taken.begin(), index_taken.end()); 115 | 116 | tbb::parallel_for(size_t(0), (const size_t)indexs.size(), [&](size_t idx) 117 | { 118 | Point pivot = pose.rotation_*( ray_points_[indexs[idx]] ); 119 | uint32_t line_interpolation = 1.0 + pivot.norm() / Config::voxel_leaf_size() ; 120 | pivot /= double(line_interpolation); 121 | Point point = pose.position_; 122 | 123 | for(uint32_t ind = 1; ind < line_interpolation; ind++ ) 124 | { 125 | type_key key; 126 | if ( map_->GetKey(point, key) ) 127 | { 128 | if( occupied_voxels.find(key) != occupied_voxels.end() ) 129 | { 130 | break; 131 | } 132 | 133 | mutex.lock(); 134 | free_voxels.push_back(key); 135 | mutex.unlock(); 136 | }else 137 | { 138 | break; 139 | } 140 | point+=pivot; 141 | } 142 | } ); 143 | map_->UpdateFreeSpace(free_voxels); 144 | } 145 | 146 | void RLMap::AddOccupiedPoints(PointList& points) 147 | { 148 | map_->InsertOccupiedPoints(points); 149 | 150 | } 151 | // collision methods 152 | bool RLMap::IsLineCollisionFree(const Point& start, const Point& end, double radius) 153 | { 154 | return map_->IsLineCollisionFree(start, end, radius); 155 | } 156 | bool RLMap::IsLineCollisionFreeOLD(const Point& start, const Point& end, double radius) 157 | { 158 | return map_->IsLineCollisionFreeOLD(start, end, radius); 159 | } 160 | 161 | // Disk interacting map 162 | void RLMap::SaveMap(const std::string &file_directory) 163 | { 164 | map_->SaveMap(file_directory); 165 | } 166 | void RLMap::LoadMap(const std::string &file_directory) 167 | { 168 | map_->LoadMap(file_directory); 169 | } 170 | // Debugging Methods 171 | void RLMap::GetOccupiedPoints(stdVoxelVector &points) 172 | { 173 | map_->GetOccupiedMap(points); 174 | } 175 | void RLMap::GetCompleteMap(stdVoxelVector &map) 176 | { 177 | map_->GetCompleteMap(map); 178 | } 179 | } /* namespace rlmap */ 180 | -------------------------------------------------------------------------------- /rlmap/src/tictoc.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file tictoc.cpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date Sep 19, 2018 5 | * @section LICENSE 6 | * 7 | * BSD-3-Clause License 8 | * 9 | * @copyright Copyright (C) 2020 Intel Corporation 10 | * 11 | * Redistribution and use in source and binary forms, with or without modification, 12 | * are permitted provided that the following conditions are met: 13 | *  14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | *    this list of conditions and the following disclaimer. 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | *    this list of conditions and the following disclaimer in the documentation 18 | *    and/or other materials provided with the distribution. 19 | * 3. Neither the name of the copyright holder nor the names of its contributors 20 | *    may be used to endorse or promote products derived from this software 21 | *    without specific prior written permission. 22 | *   23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 25 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 27 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 28 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 29 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 30 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 31 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 32 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 33 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | **/ 36 | 37 | #include 38 | #include 39 | 40 | namespace rlmap 41 | { 42 | 43 | TicToc::TicToc() 44 | { 45 | struct timespec tm; 46 | clock_gettime( CLOCK_REALTIME, &tm ); 47 | timer_ = tm.tv_nsec + tm.tv_sec * kNano_; 48 | 49 | } 50 | TicToc::~TicToc() 51 | { 52 | 53 | } 54 | double TicToc::Toc(void) 55 | { 56 | struct timespec tm; 57 | clock_gettime( CLOCK_REALTIME, &tm ); 58 | TicToc_t t_now; 59 | t_now = tm.tv_nsec + tm.tv_sec * kNano_; 60 | return (float)(t_now-timer_)/(float)kNano_; 61 | } 62 | 63 | 64 | }/* namespace rlmap */ 65 | -------------------------------------------------------------------------------- /rlmap/src/voxel.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file voxel.cpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date June, 2018 5 | * @brief This is the implementation of the voxel properties 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | 38 | #include 39 | 40 | 41 | namespace rlmap 42 | { 43 | 44 | Voxel::Voxel(void) 45 | { 46 | } 47 | Voxel::Voxel(const Point ¢er, uint32_t level, int32_t probability) : 48 | center_(center), 49 | probability_(probability), 50 | level_(level) 51 | { 52 | } 53 | Voxel::~Voxel(void) 54 | { 55 | 56 | } 57 | void Voxel::AddProbability(const double probability) 58 | { 59 | probability_ = clamp( (probability_ + probability), 0.0, 1.0); 60 | } 61 | void Voxel::ResetProbability(void) 62 | { 63 | probability_ = 0.0; 64 | } 65 | 66 | } /* namespace rlmap */ 67 | 68 | -------------------------------------------------------------------------------- /rlmap_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | SET(PROJECT_NAME rlmap_ros) 2 | PROJECT(${PROJECT_NAME}) 3 | CMAKE_MINIMUM_REQUIRED (VERSION 2.8.3) 4 | #SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo 5 | SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/CMakeModules/") 6 | 7 | # Set build flags. Set IS_ARM on odroid board as environment variable 8 | SET(CMAKE_CXX_FLAGS "-Wall -D_LINUX -D_REENTRANT -march=native -Wno-unused-variable -Wno-unused-but-set-variable -Wno-unknown-pragmas") 9 | IF(DEFINED ENV{ARM_ARCHITECTURE}) 10 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -march=armv7-a") 11 | ELSE() 12 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse -msse2 -msse3 -mssse3") 13 | ENDIF() 14 | IF(CMAKE_COMPILER_IS_GNUCC) 15 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 16 | ELSE() 17 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 18 | ENDIF() 19 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") 20 | 21 | 22 | # Add catkin and required ROS packages 23 | FIND_PACKAGE(catkin REQUIRED COMPONENTS 24 | cmake_modules 25 | roscpp 26 | rlmap 27 | visualization_msgs 28 | geometry_msgs 29 | image_transport 30 | cv_bridge 31 | sensor_msgs 32 | pcl_conversions 33 | pcl_ros 34 | ) 35 | 36 | # Add plain cmake packages 37 | FIND_PACKAGE(Eigen REQUIRED) 38 | 39 | # Describe catkin Project 40 | catkin_package( 41 | DEPENDS Eigen 42 | CATKIN_DEPENDS roscpp rlmap visualization_msgs geometry_msgs image_transport cv_bridge sensor_msgs pcl_conversions pcl_ros 43 | INCLUDE_DIRS include 44 | LIBRARIES rlmap_ros 45 | ) 46 | 47 | # Include dirs 48 | INCLUDE_DIRECTORIES( 49 | include 50 | ${Eigen_INCLUDE_DIRS} 51 | ${catkin_INCLUDE_DIRS} 52 | ) 53 | 54 | # Set link libraries 55 | LIST(APPEND LINK_LIBS 56 | ${catkin_LIBRARIES} 57 | ) 58 | ADD_LIBRARY(rlmap_ros SHARED 59 | src/visualizer.cpp 60 | src/initializer.cpp 61 | ) 62 | TARGET_LINK_LIBRARIES(rlmap_ros ${LINK_LIBS}) -------------------------------------------------------------------------------- /rlmap_ros/include/rlmap_ros/initializer.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file initializer.hpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date June, 2018 5 | * @brief A yaml parser 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | #ifndef RLMAP_INITIALIZER_HPP_ 38 | #define RLMAP_INITIALIZER_HPP_ 39 | 40 | #include 41 | 42 | namespace rlmap 43 | { 44 | 45 | void InitializeParams(ros::NodeHandle nh); 46 | 47 | } /* namespace rlmap */ 48 | 49 | #endif /* RLMAP_INITIALIZER_HPP_ */ 50 | -------------------------------------------------------------------------------- /rlmap_ros/include/rlmap_ros/visualizer.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file visualizer.hpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date June, 2018 5 | * @brief A visualizer for rviz 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | 38 | #ifndef RLVISUALIZER_HPP_ 39 | #define RLVISUALIZER_HPP_ 40 | 41 | #include 42 | #include 43 | 44 | namespace rlmap 45 | { 46 | 47 | 48 | class Visualizer 49 | { 50 | public: 51 | ros::NodeHandle nh_; 52 | 53 | ros::Publisher map_; 54 | ros::Publisher path_; 55 | ros::Publisher path_consumed_; 56 | ros::Publisher publisher_pointCloud_; 57 | 58 | 59 | 60 | Visualizer(); 61 | virtual ~Visualizer(); 62 | 63 | void PublishOccupiedMap(const stdVoxelVector &map ); 64 | void PublishPath(PointList &result); 65 | void PublishPathConsumed(PointList &result); 66 | void PublishRawPoints(const PointList &points); 67 | }; 68 | 69 | void GetColor(double max_value, double value, double &color_r, double &color_g, double &color_b); 70 | 71 | }/* namespace rlmap */ 72 | 73 | 74 | 75 | 76 | #endif /* RLVISUALIZER_HPP_ */ 77 | -------------------------------------------------------------------------------- /rlmap_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rlmap_ros 4 | 0.1.0 5 | 6 | A ros wrapper for a custom map 7 | 8 | Leo Campos 9 | Leo Campos 10 | BSD 11 | 12 | 13 | catkin 14 | 15 | 16 | cmake_modules 17 | roscpp 18 | rlmap 19 | visualization_msgs 20 | geometry_msgs 21 | image_transport 22 | cv_bridge 23 | sensor_msgs 24 | pcl_conversions 25 | pcl_ros 26 | 27 | 28 | 29 | roscpp 30 | rlmap 31 | visualization_msgs 32 | geometry_msgs 33 | image_transport 34 | cv_bridge 35 | sensor_msgs 36 | pcl_conversions 37 | pcl_ros 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /rlmap_ros/src/initializer.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file initializer.cpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date June, 2018 5 | * @brief A yaml parser 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | 38 | #include 39 | #include 40 | 41 | namespace rlmap 42 | { 43 | 44 | void InitializeParams(ros::NodeHandle nh) 45 | { 46 | uint8_t dimensions = 3; 47 | 48 | double voxel_leaf_size; 49 | nh.getParam("voxel_leaf_size", voxel_leaf_size); 50 | 51 | double map_size; 52 | double virtual_map_size; 53 | nh.getParam("map_size", virtual_map_size); 54 | double map_count_ = 0.; 55 | do 56 | { 57 | map_size = voxel_leaf_size*pow(2., map_count_); 58 | map_count_ += 1.; 59 | }while(map_size < virtual_map_size); 60 | std::cout<<"Map size : "< camera_translation; 95 | std::vector camera_rotation; 96 | nh.getParam("camera_translation", camera_translation); 97 | nh.getParam("camera_rotation", camera_rotation); 98 | 99 | camera_translation_wrt_drone << camera_translation[0], camera_translation[1], camera_translation[2]; 100 | camera_rotation_wrt_drone << camera_rotation[0], camera_rotation[1], camera_rotation[2], 101 | camera_rotation[3], camera_rotation[4], camera_rotation[5], 102 | camera_rotation[6], camera_rotation[7], camera_rotation[8]; 103 | 104 | 105 | Config::init_params_externally( 106 | dimensions, 107 | map_size, 108 | voxel_leaf_size, 109 | occupancy_threshold, 110 | simple_probability, 111 | uint32_t(frame_width), 112 | uint32_t(frame_height), 113 | min_depth_distance, 114 | max_depth_distance, 115 | depth_factor, 116 | calibration_rfx, 117 | calibration_rfy, 118 | calibration_rpx, 119 | calibration_rpy, 120 | image_percent, 121 | camera_translation_wrt_drone, 122 | camera_rotation_wrt_drone 123 | ); 124 | } 125 | 126 | } /* namespace rlmap */ 127 | -------------------------------------------------------------------------------- /rlmap_ros/src/visualizer.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file visualizer.cpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date June, 2018 5 | * @brief A visualizer for rviz 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | namespace rlmap 54 | { 55 | Visualizer::Visualizer(): 56 | nh_("~") 57 | { 58 | // init ROS publishers 59 | map_ = nh_.advertise("/map", 1, true); 60 | path_ = nh_.advertise("/path", 1, true); 61 | path_consumed_ = nh_.advertise("/path_consumed", 1, true); 62 | publisher_pointCloud_ = nh_.advertise("/raw_image", 1, true); 63 | } 64 | Visualizer::~Visualizer() 65 | { 66 | 67 | } 68 | void Visualizer::PublishRawPoints(const PointList &points) 69 | { 70 | 71 | sensor_msgs::PointCloud2 pointCloud; 72 | 73 | pcl::PointCloud cloud; 74 | uint32_t auxiliaryCount; 75 | 76 | cloud.width = points.size(); 77 | cloud.height = 1; 78 | cloud.points.resize(cloud.width * cloud.height); 79 | for (auxiliaryCount = 0; auxiliaryCount < points.size(); auxiliaryCount++) 80 | { 81 | cloud.points[auxiliaryCount].x = ((float)points.at(auxiliaryCount)(0)); 82 | cloud.points[auxiliaryCount].y = ((float)points.at(auxiliaryCount)(1)); 83 | cloud.points[auxiliaryCount].z = ((float)points.at(auxiliaryCount)(2)); 84 | } 85 | pcl::toROSMsg(cloud, pointCloud); 86 | 87 | pointCloud.header.frame_id = "world"; 88 | publisher_pointCloud_.publish(pointCloud); 89 | } 90 | void Visualizer::PublishPathConsumed(PointList &result) 91 | { 92 | if(result.size()) 93 | { 94 | 95 | nav_msgs::Path path; 96 | geometry_msgs::PoseStamped point; 97 | 98 | 99 | path.header.frame_id = "world"; 100 | path.header.stamp = ros::Time::now(); 101 | 102 | point.header.frame_id = "world"; 103 | point.header.stamp = ros::Time::now(); 104 | 105 | for(int32_t ind = 0; ind < result.size(); ind ++) 106 | { 107 | point.pose.position.x = result[ind](0); 108 | point.pose.position.y = result[ind](1); 109 | point.pose.position.z = result[ind](2); 110 | 111 | 112 | path.poses.push_back(point); 113 | } 114 | path_consumed_.publish(path); 115 | } 116 | 117 | } 118 | void Visualizer::PublishPath(PointList &result) 119 | { 120 | if(result.size()) 121 | { 122 | 123 | nav_msgs::Path path; 124 | geometry_msgs::PoseStamped point; 125 | 126 | 127 | path.header.frame_id = "world"; 128 | path.header.stamp = ros::Time::now(); 129 | 130 | point.header.frame_id = "world"; 131 | point.header.stamp = ros::Time::now(); 132 | 133 | for(int32_t ind = 0; ind < result.size(); ind ++) 134 | { 135 | point.pose.position.x = result[ind](0); 136 | point.pose.position.y = result[ind](1); 137 | point.pose.position.z = result[ind](2); 138 | 139 | 140 | path.poses.push_back(point); 141 | } 142 | path_.publish(path); 143 | } 144 | } 145 | 146 | 147 | void Visualizer::PublishOccupiedMap(const stdVoxelVector &map ) 148 | { 149 | visualization_msgs::MarkerArray marker_array; 150 | visualization_msgs::Marker marker; 151 | marker.header.frame_id = "world"; 152 | marker.header.stamp = ros::Time::now(); 153 | marker.pose.orientation.x = 0.0; 154 | marker.pose.orientation.y = 0.0; 155 | marker.pose.orientation.z = 0.0; 156 | marker.pose.orientation.w = 1.0; 157 | marker.action = visualization_msgs::Marker::ADD; 158 | marker.type = visualization_msgs::Marker::CUBE; 159 | 160 | for(int32_t ind =0; ind < map.size();ind++) 161 | { 162 | marker.id = ind; 163 | Voxel vox = map[ind]; 164 | Point center = vox.GetCenter(); 165 | uint32_t level= vox.GetLevel(); 166 | double step = pow(2, (double)level)*Config::voxel_leaf_size(); 167 | 168 | marker.scale.x = step; 169 | marker.scale.y = step; 170 | marker.scale.z = step; 171 | marker.pose.position.x = center(0); 172 | marker.pose.position.y =center(1); 173 | 174 | double color_r; 175 | double color_g; 176 | double color_b; 177 | 178 | GetColor(2.0, center(2), color_r, color_g, color_b); 179 | 180 | marker.color.g = color_g; 181 | marker.color.b = color_b; 182 | marker.color.r = color_r; 183 | marker.color.a = 1.0; 184 | 185 | if(center.rows() > 2) 186 | { 187 | marker.pose.position.z = center(2); 188 | }else 189 | { 190 | marker.pose.position.z = 1.0; 191 | } 192 | marker_array.markers.push_back(marker); 193 | } 194 | map_.publish(marker_array); 195 | 196 | } 197 | 198 | void GetColor(double max_value, double value, double &color_r, double &color_g, double &color_b) 199 | { 200 | value > max_value ? value = max_value : value; 201 | value < 0 ? value = 0 : value; 202 | double H = ( ( value )/(max_value)) * 315.0 + 225.0; 203 | double S = 1.0; 204 | double V = 1.0; 205 | 206 | if (S == 0) 207 | { 208 | color_r = V; 209 | color_g = V; 210 | color_b = V; 211 | } 212 | else 213 | { 214 | int i; 215 | double f, p, q, t; 216 | if (H == 360) 217 | H = 0; 218 | else 219 | H = H / 60; 220 | 221 | i = (int)trunc(H); 222 | f = H - i; 223 | 224 | p = V * (1.0 - S); 225 | q = V * (1.0 - (S * f)); 226 | t = V * (1.0 - (S * (1.0 - f))); 227 | 228 | switch (i) 229 | { 230 | case 0: 231 | color_r = V; 232 | color_g = t; 233 | color_b = p; 234 | break; 235 | 236 | case 1: 237 | color_r = q; 238 | color_g = V; 239 | color_b = p; 240 | break; 241 | 242 | case 2: 243 | color_r = p; 244 | color_g = V; 245 | color_b = t; 246 | break; 247 | 248 | case 3: 249 | color_r = p; 250 | color_g = q; 251 | color_b = V; 252 | break; 253 | 254 | case 4: 255 | color_r = t; 256 | color_g = p; 257 | color_b = V; 258 | break; 259 | 260 | default: 261 | color_r = V; 262 | color_g = p; 263 | color_b = q; 264 | break; 265 | } 266 | 267 | } 268 | 269 | (color_r > 1.0)?color_r=1.0:color_r; 270 | (color_g > 1.0)?color_g=1.0:color_g; 271 | (color_b > 1.0)?color_b=1.0:color_b; 272 | } 273 | 274 | 275 | 276 | 277 | }/* namespace rlmap */ 278 | -------------------------------------------------------------------------------- /rrt/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | SET(PROJECT_NAME rrt_lib) 2 | PROJECT(${PROJECT_NAME}) 3 | CMAKE_MINIMUM_REQUIRED (VERSION 2.8.3) 4 | #SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo 5 | SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/CMakeModules/") 6 | 7 | # Set build flags. Set IS_ARM on odroid board as environment variable 8 | SET(CMAKE_CXX_FLAGS "-Wall -D_LINUX -D_REENTRANT -march=native -Wno-unused-variable -Wno-unused-but-set-variable -Wno-unknown-pragmas") 9 | IF(DEFINED ENV{ARM_ARCHITECTURE}) 10 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -march=armv7-a") 11 | ELSE() 12 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse -msse2 -msse3 -mssse3") 13 | ENDIF() 14 | IF(CMAKE_COMPILER_IS_GNUCC) 15 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 16 | ELSE() 17 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 18 | ENDIF() 19 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") 20 | 21 | 22 | # Add catkin and required ROS packages 23 | FIND_PACKAGE(catkin REQUIRED COMPONENTS 24 | cmake_modules 25 | rlmap 26 | roscpp 27 | ) 28 | 29 | # Add plain cmake packages 30 | FIND_PACKAGE(Eigen REQUIRED) 31 | find_package(TBB REQUIRED) 32 | 33 | # Describe catkin Project 34 | catkin_package( 35 | DEPENDS Eigen 36 | CATKIN_DEPENDS roscpp rlmap 37 | INCLUDE_DIRS include 38 | LIBRARIES rrt_lib 39 | ) 40 | 41 | # Include dirs 42 | INCLUDE_DIRECTORIES( 43 | include 44 | ${Eigen_INCLUDE_DIRS} 45 | ${TBB_INCLUDE_DIRS} 46 | ${catkin_INCLUDE_DIRS} 47 | ) 48 | 49 | # Set link libraries 50 | LIST(APPEND LINK_LIBS 51 | ${TBB_LIBS} 52 | ${catkin_LIBRARIES} 53 | ) 54 | ADD_LIBRARY(rrt_lib SHARED 55 | src/rrt.cpp 56 | src/rrt_connect.cpp 57 | src/node.cpp 58 | src/config.cpp 59 | ) 60 | TARGET_LINK_LIBRARIES(rrt_lib ${LINK_LIBS}) -------------------------------------------------------------------------------- /rrt/include/rrt/config.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file config.hpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date May 22, 2019 5 | * @brief 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | #ifndef RRT_CONFIG_HPP_ 38 | #define RRT_CONFIG_HPP_ 39 | 40 | #include 41 | 42 | namespace rrt 43 | { 44 | 45 | 46 | class Config 47 | { 48 | public: 49 | static Config& getInstance(); 50 | 51 | static uint8_t& dimensions() { return getInstance().dimensions_; } 52 | 53 | static Point& rrt_max() { return getInstance().rrt_max_; } 54 | 55 | static Point& rrt_min() { return getInstance().rrt_min_; } 56 | 57 | static double& rrt_wall_time() { return getInstance().rrt_wall_time_; } 58 | 59 | static double& agent_radius() { return getInstance().agent_radius_; } 60 | 61 | static double& waypoint_lengh() { return getInstance().waypoint_lengh_; } 62 | 63 | static uint32_t& rrt_informed_iterations() { return getInstance().rrt_informed_iterations_; } 64 | 65 | static double& hfov() { return getInstance().hfov_; } 66 | 67 | static double& vfov() { return getInstance().vfov_; } 68 | 69 | static double& tvfov() { return getInstance().tvfov_; } 70 | 71 | static double& max_depth_distance() { return getInstance().max_depth_distance_; } 72 | 73 | 74 | static void init_params_externally(uint8_t dimensions, 75 | Point rrt_max, 76 | Point rrt_min, 77 | double rrt_wall_time, 78 | uint32_t rrt_informed_iterations, 79 | double agent_radius, 80 | double hfov, 81 | double vfov, 82 | double max_depth_distance, 83 | double waypoint_lengh); 84 | 85 | private: 86 | Config(void); 87 | Config(Config const&); 88 | void operator=(Config const&); 89 | 90 | uint8_t dimensions_; 91 | Point rrt_max_; 92 | Point rrt_min_; 93 | double rrt_wall_time_; 94 | uint32_t rrt_informed_iterations_; 95 | double agent_radius_; 96 | 97 | 98 | 99 | // connect FoV variables 100 | double hfov_; 101 | double vfov_; 102 | double tvfov_; 103 | double max_depth_distance_; 104 | 105 | double waypoint_lengh_; 106 | }; 107 | 108 | } /* namespace rrt */ 109 | 110 | 111 | #endif /* RRT_CONFIG_HPP_ */ 112 | -------------------------------------------------------------------------------- /rrt/include/rrt/global.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file global.hpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date May 22, 2019 5 | * @brief 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | #ifndef RRT_GLOBAL_HPP_ 38 | #define RRT_GLOBAL_HPP_ 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | namespace rrt 45 | { 46 | 47 | typedef enum {kMax=0, kMin, kNIndex}eIndex; 48 | typedef enum {kFov=0, kMap, kNTypes}eDirection; 49 | 50 | const double k_rrt_solution_error = 0.1; 51 | 52 | #ifndef finline 53 | #ifdef WIN32 54 | #define finline __forceinline 55 | #else 56 | #define finline inline __attribute__((always_inline)) 57 | #endif 58 | #endif 59 | 60 | class Node; 61 | 62 | typedef Eigen::Vector3d Point; 63 | typedef std::vector PointList; 64 | typedef Eigen::MatrixXd Matrix; 65 | typedef std::vector Nodes; 66 | 67 | 68 | static Matrix CalculateRotationMatrix(const double &roll, const double &pitch, const double &yaw) 69 | { 70 | Matrix Rz(3,3); 71 | Rz<< cos(yaw), -sin(yaw), 0.0, 72 | sin(yaw), cos(yaw), 0.0, 73 | 0.0, 0.0, 1.0; 74 | 75 | Matrix Rx(3,3); 76 | Rx<< 1.0, 0.0, 0.0, 77 | 0.0, cos(roll), -sin(roll), 78 | 0.0, sin(roll), cos(roll); 79 | 80 | Matrix Ry(3,3); 81 | Ry<< cos(pitch), 0.0, sin(pitch), 82 | 0.0, 1.0, 0.0, 83 | -sin(pitch), 0.0, cos(pitch); 84 | return Rz*Ry*Rx; 85 | } 86 | static Matrix CalculateRotationMatrix(const double &theta) 87 | { 88 | Matrix Rz(2,2); 89 | Rz<< cos(theta), -sin(theta), 90 | sin(theta), cos(theta); 91 | return Rz; 92 | } 93 | 94 | } /* namespace rrt */ 95 | 96 | #endif /* RRT_GLOBAL_HPP_ */ 97 | -------------------------------------------------------------------------------- /rrt/include/rrt/node.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file node.hpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date May 22, 2019 5 | * @brief 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | #ifndef RRT_NODE_HPP_ 38 | #define RRT_NODE_HPP_ 39 | 40 | #include 41 | 42 | namespace rrt 43 | { 44 | 45 | class Node 46 | { 47 | public: 48 | Node(); 49 | Node(const Point &coordinates, double carried_distance, uint32_t parent, const Point &parent_vector); 50 | virtual ~Node(); 51 | 52 | Point coordinates(void){ return coordinates_; } 53 | double carried_distance(void){ return carried_distance_; } 54 | uint32_t parent(void){ return parent_; } 55 | Point parent_vector(void){ return parent_vector_; } 56 | 57 | void carried_distance(double carried_distance); 58 | void parent(uint32_t parent); 59 | void coordinates(const Point &coordinates); 60 | void parent_vector(const Point &parent_vector); 61 | 62 | private: 63 | Point coordinates_; 64 | double carried_distance_; 65 | uint32_t parent_; 66 | Point parent_vector_; 67 | }; 68 | 69 | } /* namespace rrt */ 70 | 71 | 72 | #endif /* RRT_NODE_HPP_ */ 73 | -------------------------------------------------------------------------------- /rrt/include/rrt/rrt.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file rrt.hpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date May 22, 2019 5 | * @brief 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | #ifndef RRT_RRT_HPP_ 38 | #define RRT_RRT_HPP_ 39 | 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | namespace rrt 47 | { 48 | 49 | class RRT 50 | { 51 | 52 | private: 53 | rlmap::OOTP_Map *map_; 54 | rlmap::TicTocPtr rrt_time_; 55 | 56 | Point start_; 57 | Point goal_; 58 | Matrix rrt_world_boundaries_; 59 | Nodes tree_; 60 | Nodes solution_; 61 | bool goal_reached_; 62 | Matrix waypoints_; 63 | PointList pl_waypoints_; 64 | bool time_out_; 65 | 66 | // rrt star members 67 | double connection_radius_; 68 | double max_distance_; 69 | 70 | // inf rrt star members 71 | bool from_ellipse_; 72 | double c_min_; 73 | Point vX_centre_; 74 | Matrix rotation_matrix_; 75 | 76 | // normal rrt methods 77 | void SolveRRTStar(void); 78 | Point GetRandomPoint(void); 79 | void RefreshConnectionRadius(void); 80 | uint32_t FindNodesInCircle(const Point &random_point, std::vector< uint32_t > &nodes); 81 | uint32_t ClosestVertex(const Point &random_point, double &distance); 82 | bool IsCollisionFree(const Point &random_point, const Point &parent_point); 83 | void IsNearOfGoal(const Point &random_point); 84 | void CreateSolution(void); 85 | void SolveInformedRrtStar(void); 86 | 87 | void CalculateWaypoints(void); 88 | 89 | // rrt informed methods 90 | void CreateEllipse(void); 91 | 92 | public: 93 | 94 | RRT(rlmap::OOTP_Map *map, const Point &start, const Point &goal); 95 | RRT( const Point &start, const Point &goal); 96 | virtual ~RRT(); 97 | void Run(void); 98 | void GetWaypoints(Matrix &points); 99 | void GetWaypoints(PointList &points); 100 | }; 101 | 102 | } /* namespace rrt */ 103 | 104 | 105 | 106 | #endif /* RRT_RRT_HPP_ */ 107 | -------------------------------------------------------------------------------- /rrt/include/rrt/rrt_connect.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file rrt_connect.hpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date May 22, 2019 5 | * @brief 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #ifndef RRT_RRT_CONNECT_HPP_ 44 | #define RRT_RRT_CONNECT_HPP_ 45 | 46 | namespace rrt 47 | { 48 | 49 | class RRTConnect 50 | { 51 | 52 | private: 53 | rlmap::OOTP_Map *map_; 54 | rlmap::TicTocPtr rrt_time_; 55 | 56 | Point start_; 57 | Point goal_; 58 | Point unitary_; 59 | Matrix rrt_world_boundaries_; 60 | 61 | Nodes tree_[kNTypes]; 62 | Nodes solution_; 63 | 64 | bool goal_reached_; 65 | uint32_t other_parent_; 66 | Matrix waypoints_; 67 | 68 | PointList pl_waypoints_; 69 | bool time_out_; 70 | 71 | // rrt star members 72 | double connection_radius_; 73 | double max_distance_; 74 | 75 | // inf rrt star members 76 | bool from_ellipse_; 77 | double c_min_; 78 | Point vX_centre_; 79 | Matrix rotation_matrix_; 80 | 81 | // rrt connect methods 82 | void SolveRrtConnect(void); 83 | void SolveRrt(eDirection direction); 84 | 85 | Point GetRandomPoint(eDirection direction); 86 | void RefreshConnectionRadius(eDirection direction); 87 | uint32_t FindNodesInCircle(eDirection direction, const Point &random_point, std::vector< uint32_t > &nodes); 88 | uint32_t ClosestVertex(eDirection direction, const Point &random_point, double &distance); 89 | bool IsCollisionFree(const Point &random_point, const Point &parent_point); 90 | 91 | bool IsPointInsideFoV(const Point &point); 92 | 93 | void IsNearOfGoal(eDirection direction, const Point &random_point); 94 | void CreateSolution(eDirection direction); 95 | 96 | 97 | void CalculateWaypoints(void); 98 | 99 | // rrt informed methods 100 | void CreateEllipse(void); 101 | void SolveInformedRrtStar(void); 102 | 103 | public: 104 | 105 | RRTConnect(rlmap::OOTP_Map *map, const Point &start, const Point &goal, double yaw); 106 | RRTConnect( const Point &start, const Point &goal, double yaw); 107 | virtual ~RRTConnect(); 108 | void Run(void); 109 | void GetWaypoints(Matrix &points); 110 | void GetWaypoints(PointList &points); 111 | }; 112 | 113 | 114 | } /* namespace rrt */ 115 | 116 | 117 | 118 | 119 | #endif /* RRT_RRT_CONNECT_HPP_ */ 120 | -------------------------------------------------------------------------------- /rrt/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rrt_lib 4 | 0.1.0 5 | 6 | An RRT library 7 | 8 | Leo Campos 9 | Leo Campos 10 | BSD 11 | 12 | 13 | catkin 14 | 15 | 16 | cmake_modules 17 | roscpp 18 | rlmap 19 | 20 | 21 | roscpp 22 | rlmap 23 | 24 | 25 | -------------------------------------------------------------------------------- /rrt/src/config.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file config.cpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date May 22, 2019 5 | * @brief 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | #include 38 | 39 | namespace rrt 40 | { 41 | 42 | Config::Config() : 43 | dimensions_(3), 44 | rrt_max_(3.0,3.0,0.95), 45 | rrt_min_(-3.0,-3.0,0.85), 46 | rrt_wall_time_(0.1), 47 | rrt_informed_iterations_(9), 48 | agent_radius_(0.5), 49 | hfov_(3.1416/2.), 50 | vfov_(3.1416/2.), 51 | tvfov_(tan(vfov_ / 2.0 )), 52 | max_depth_distance_(3.0), 53 | waypoint_lengh_(0.2) 54 | { 55 | 56 | } 57 | Config& Config::getInstance() 58 | { 59 | static Config instance; // Instantiated on first use and guaranteed to be destroyed 60 | return instance; 61 | } 62 | 63 | void Config::init_params_externally(uint8_t dimensions, 64 | Point rrt_max, 65 | Point rrt_min, 66 | double rrt_wall_time, 67 | uint32_t rrt_informed_iterations, 68 | double agent_radius, 69 | double hfov, 70 | double vfov, 71 | double max_depth_distance, 72 | double waypoint_lengh) 73 | { 74 | getInstance().dimensions_ = dimensions; 75 | getInstance().rrt_max_ = rrt_max; 76 | getInstance().rrt_min_ = rrt_min; 77 | getInstance().rrt_wall_time_ = rrt_wall_time; 78 | getInstance().rrt_informed_iterations_ = rrt_informed_iterations; 79 | getInstance().agent_radius_ = agent_radius; 80 | getInstance().hfov_ = hfov; 81 | getInstance().vfov_ = vfov; 82 | getInstance().tvfov_ = tan(vfov / 2.0); 83 | getInstance().max_depth_distance_ = max_depth_distance; 84 | getInstance().waypoint_lengh_ = waypoint_lengh; 85 | } 86 | 87 | } /* namespace rrt */ 88 | -------------------------------------------------------------------------------- /rrt/src/node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file node.cpp 3 | * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com) 4 | * @date May 22, 2019 5 | * @brief 6 | * @section LICENSE 7 | * 8 | * BSD-3-Clause License 9 | * 10 | * @copyright Copyright (C) 2020 Intel Corporation 11 | * 12 | * Redistribution and use in source and binary forms, with or without modification, 13 | * are permitted provided that the following conditions are met: 14 | *  15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | *    this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | *    this list of conditions and the following disclaimer in the documentation 19 | *    and/or other materials provided with the distribution. 20 | * 3. Neither the name of the copyright holder nor the names of its contributors 21 | *    may be used to endorse or promote products derived from this software 22 | *    without specific prior written permission. 23 | *   24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 26 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 28 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 29 | * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 30 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 31 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 33 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 34 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | **/ 37 | 38 | #include 39 | 40 | namespace rrt 41 | { 42 | 43 | Node::Node() 44 | { 45 | 46 | } 47 | 48 | Node::Node(const Point &coordinates, double carried_distance, uint32_t parent, const Point &parent_vector) : 49 | coordinates_(coordinates), 50 | carried_distance_(carried_distance), 51 | parent_(parent), 52 | parent_vector_(parent_vector) 53 | { 54 | 55 | } 56 | 57 | Node::~Node() 58 | { 59 | 60 | } 61 | void Node::coordinates(const Point &coordinates) 62 | { 63 | coordinates_ = coordinates; 64 | } 65 | void Node::carried_distance(double carried_distance) 66 | { 67 | carried_distance_ = carried_distance; 68 | } 69 | void Node::parent(uint32_t parent) 70 | { 71 | parent_ = parent; 72 | } 73 | void Node::parent_vector(const Point &parent_vector) 74 | { 75 | parent_vector_ = parent_vector; 76 | } 77 | 78 | } /* namespace rrt */ 79 | --------------------------------------------------------------------------------