├── .gitignore ├── LICENSE ├── README.md ├── aeplanner.rviz ├── aeplanner ├── CMakeLists.txt ├── action │ └── aeplanner.action ├── include │ └── aeplanner │ │ ├── aeplanner.h │ │ ├── aeplanner_nodelet.h │ │ ├── aeplanner_viz.h │ │ ├── data_structures.h │ │ └── param.h ├── launch │ ├── aeplanner_nodelet.launch │ └── aeplanner_standalone.launch ├── msg │ └── Node.msg ├── nodelet_plugins.xml ├── package.xml ├── src │ ├── aeplanner.cpp │ ├── aeplanner_node.cpp │ ├── aeplanner_nodelet.cpp │ ├── aeplanner_viz.cpp │ └── param.cpp └── srv │ └── Reevaluate.srv ├── aeplanner_evaluation ├── CMakeLists.txt ├── config │ └── evaluation.yaml ├── launch │ └── coverage_evaluation.launch ├── package.xml ├── src │ └── evaluation.cpp └── srv │ └── Coverage.srv ├── kdtree ├── CMakeLists.txt ├── include │ └── kdtree │ │ └── kdtree.h ├── package.xml └── src │ └── kdtree.c ├── pigain ├── CMakeLists.txt ├── config │ └── config.yaml ├── launch │ └── pig.launch ├── msg │ └── Node.msg ├── nodes │ ├── gp.py │ ├── pig.py │ └── test_gp.py ├── package.xml └── srv │ ├── BestNode.srv │ └── Query.srv ├── rpl_exploration ├── CMakeLists.txt ├── action │ └── FlyTo.action ├── config │ └── exploration.yaml ├── launch │ ├── exploration.launch │ ├── fly_to.launch │ ├── laser_tf.launch │ ├── laserscan_to_pointcloud.launch │ ├── octomap_server.launch │ ├── primesense_tf.launch │ ├── rpl_exploration.launch │ ├── rpl_inspector.launch │ └── teleop.launch ├── nodes │ ├── image_maxing.py │ └── teleop.py ├── package.xml ├── rpl_exploration_nodelets.xml └── src │ ├── fly_to.cpp │ ├── laserscan_to_pointcloud.cpp │ └── rpl_exploration.cpp └── rrtplanner ├── CMakeLists.txt ├── action └── rrt.action ├── include └── rrtplanner │ └── rrt.h ├── launch └── rrtplanner.launch ├── package.xml └── src ├── rrt.cpp └── rrtplanner_node.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | *.csv 3 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2019, Magnus Selin 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 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * 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 | * 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 | # Autonomous Exploration Planner - aeplanner 2 | 3 | aeplanner is an exploration planning package for 3d environments. It subscribes to an [OctoMap](https://octomap.github.io/) and will propose waypoints that maximizes the information gain while minimizing the traversed distance. For a more detailed description see our paper. 4 | 5 |
6 | 7 | Efficient Autonomous Exploration Planning of Large Scale 3D-Environments 11 | 12 |
13 | 14 | Use aeplanner? 15 | If you are using aeplanner, please cite our paper **Efficient Autonomous Exploration Planning of Large-Scale 3-D Environments** in IEEE Robotics and Automation Letters, vol. 4, no. 2, pp. 1699-1706, April 2019. 16 | 17 | BibTeX: 18 | ``` 19 | @ARTICLE{8633925, 20 | author={M. {Selin} and M. {Tiger} and D. {Duberg} and F. {Heintz} and P. {Jensfelt}}, 21 | journal={IEEE Robotics and Automation Letters}, 22 | title={Efficient Autonomous Exploration Planning of Large-Scale 3-D Environments}, 23 | year={2019}, 24 | volume={4}, 25 | number={2}, 26 | pages={1699-1706}, 27 | keywords={Planning;Sensors;Uncertainty;Drones;Mobile robots;Path planning;Search and rescue robots;motion and path planning;mapping}, 28 | doi={10.1109/LRA.2019.2897343}, 29 | ISSN={2377-3766}, 30 | month={April},} 31 | ``` 32 | 33 | 34 | ## System Requirements 35 | This repository has been tested with: 36 | ``` 37 | Ubuntu 16.04 38 | ROS Kinetic (Desktop full installation) 39 | catkin tools 40 | catkin simple 41 | OctoMap 42 | ``` 43 | it might work with other setups, but no guarantees are given. 44 | 45 | ## Assumptions 46 | 1. You are running `Ubuntu 16.04` 47 | 2. You are using bash, zsh is also possible with some [modifications](https://github.com/mseln/aeplanner/wiki/Use-zsh-instead-of-bash). 48 | 3. You have `ros-kinetic-desktop-installed`, otherwise follow installation instructions [here](http://wiki.ros.org/kinetic/Installation/Ubuntu) 49 | 4. You have created a workspace in `~/catkin_ws`, without running `catkin_make`. If in another location [these](https://github.com/mseln/aeplanner/wiki/Other-location-than-~-catkin_ws) things need to be modified. 50 | 5. You have installed [`catkin tools`](https://catkin-tools.readthedocs.io/en/latest/installing.html) 51 | 52 | 53 | ## Installation of dependencies 54 | 55 | 1. You have installed [`octomap`](http://wiki.ros.org/octomap) if not install it by running `sudo apt-get install "ros-kinetic-octomap-*"` or install from source. 56 | 57 | 2. Catkin simple is needed to build the rtree package 58 | ``` 59 | cd ~/catkin_ws/src 60 | git clone https://github.com/catkin/catkin_simple.git 61 | ``` 62 | 63 | 3. PIGain depends on rtree which in turn needs libspatialindex (note this rtree is different from the one above). 64 | ``` 65 | pip install rtree 66 | sudo apt-get install libspatialindex-dev 67 | ``` 68 | 69 | ## Setup workspace 70 | If you do not already have a workspace, set up a new one. 71 | ``` 72 | mkdir -p ~/catkin_ws/src 73 | cd ~/catkin_ws 74 | catkin init 75 | catkin build 76 | ``` 77 | 78 | ## Setup aeplanner 79 | 80 | Clone this package (make sure that you are in catkin_ws/src) 81 | ``` 82 | cd ~/catkin_ws/src/ 83 | git clone https://github.com/mseln/aeplanner.git 84 | cd .. 85 | catkin build 86 | ``` 87 | 88 | ## Simulation environment 89 | 90 | To run the example directly without having to integrate the interface, the kth uav simulation package can be installed, see: 91 | 92 | ``` 93 | https://github.com/danielduberg/kth_uav 94 | ``` 95 | 96 | Read the Assumptions and follow the install instructions on that page. 97 | 98 | #### Troubleshooting installation of kth_uav 99 | 100 | If you get `couldn't find python module jinja2:` install jinja2 by running: 101 | ``` 102 | sudo apt-get install python-jinja2 103 | ``` 104 | 105 | If you get `No rule to make target '/home/rpl/catkin_ws/src/kth_uav/Firmware/Tools/sitl_gazebo/PROTOBUF_PROTOC_EXECUTABLE-NOTFOUND', needed by 'Groundtruth.pb.cc'. Stop.`, install libprotobuf by running: 106 | 107 | ``` 108 | sudo apt-get install protobuf-compiler 109 | ``` 110 | 111 | If you get `ImportError: No module named future` install it by running: 112 | ``` 113 | pip install future 114 | ``` 115 | 116 | If you get `Could not find a package configuration file provided by "geographic_msgs" with any of the following names:` install it by running: 117 | ``` 118 | sudo apt-get install ros-kinetic-geographic-msgs 119 | ``` 120 | 121 | If you get `Could NOT find GeographicLib (missing: GeographicLib_LIBRARIES GeographicLib_INCLUDE_DIRS)` fix it by installing: 122 | ``` 123 | sudo apt-get install libgeographic-dev 124 | ``` 125 | 126 | If gazebo is complaining about `GeographicLib exception: File not readable /usr/share/GeographicLib/geoids/egm96-5.pgm`, install `geographiclib-tools` and run the install script again. 127 | ``` 128 | sudo apt-get install geographiclib-tools 129 | sudo ./kth_uav/mavros/mavros/scripts/install_geographiclib_datasets.sh 130 | ``` 131 | 132 | 133 | When running `roslaunch simulation simulation.launch` if you get `Arg xml is ` fix it by changing line 14 and 15 in `kth_uav/Firmware/launch/mavros_posix_sitl.launch` to 134 | ``` 135 | 136 | 137 | ``` 138 | do the same with line in file 12 and 13 `kth_uav/Firmware/launch/posix_sitl.launch`. 139 | 140 | -------------------------------------------------------------------------------- /aeplanner.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /TF1 8 | Splitter Ratio: 0.5 9 | Tree Height: 848 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.588679016 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: "" 29 | Visualization Manager: 30 | Class: "" 31 | Displays: 32 | - Class: rviz/TF 33 | Enabled: true 34 | Frame Timeout: 15 35 | Frames: 36 | All Enabled: false 37 | base_link: 38 | Value: false 39 | camera_link: 40 | Value: true 41 | fcu: 42 | Value: false 43 | fcu_frd: 44 | Value: false 45 | local_origin: 46 | Value: false 47 | local_origin_ned: 48 | Value: false 49 | map: 50 | Value: true 51 | Marker Scale: 10 52 | Name: TF 53 | Show Arrows: false 54 | Show Axes: true 55 | Show Names: false 56 | Tree: 57 | map: 58 | base_link: 59 | camera_link: 60 | {} 61 | Update Interval: 0 62 | Value: true 63 | - Class: rviz/MarkerArray 64 | Enabled: true 65 | Marker Topic: /aeplanner/rrtree 66 | Name: AEP-RRT 67 | Namespaces: 68 | {} 69 | Queue Size: 100 70 | Value: true 71 | - Class: rviz/Marker 72 | Enabled: true 73 | Marker Topic: /aeplanner/rrt_path 74 | Name: RRT 75 | Namespaces: 76 | {} 77 | Queue Size: 100 78 | Value: true 79 | - Class: rviz/MarkerArray 80 | Enabled: true 81 | Marker Topic: /aeplanner/pig_markers 82 | Name: Cached Points 83 | Namespaces: 84 | "": true 85 | Queue Size: 100 86 | Value: true 87 | - Class: rviz/MarkerArray 88 | Enabled: true 89 | Marker Topic: /aeplanner/mean_markers 90 | Name: GaussianProcess (g) 91 | Namespaces: 92 | "": true 93 | Queue Size: 100 94 | Value: true 95 | - Alpha: 0.699999988 96 | Class: rviz/Map 97 | Color Scheme: map 98 | Draw Behind: false 99 | Enabled: true 100 | Name: Map 101 | Topic: /aeplanner/projected_map 102 | Unreliable: false 103 | Value: true 104 | - Class: rviz/MarkerArray 105 | Enabled: false 106 | Marker Topic: /aeplanner/occupied_cells_vis_array 107 | Name: 3D-Map 108 | Namespaces: 109 | {} 110 | Queue Size: 100 111 | Value: false 112 | Enabled: true 113 | Global Options: 114 | Background Color: 255; 255; 255 115 | Fixed Frame: map 116 | Frame Rate: 30 117 | Name: root 118 | Tools: 119 | - Class: rviz/Interact 120 | Hide Inactive Objects: true 121 | - Class: rviz/MoveCamera 122 | - Class: rviz/Select 123 | - Class: rviz/FocusCamera 124 | - Class: rviz/Measure 125 | - Class: rviz/SetInitialPose 126 | Topic: /initialpose 127 | - Class: rviz/SetGoal 128 | Topic: /move_base_simple/goal 129 | - Class: rviz/PublishPoint 130 | Single click: true 131 | Topic: /clicked_point 132 | Value: true 133 | Views: 134 | Current: 135 | Angle: 0 136 | Class: rviz/TopDownOrtho 137 | Enable Stereo Rendering: 138 | Stereo Eye Separation: 0.0599999987 139 | Stereo Focal Distance: 1 140 | Swap Stereo Eyes: false 141 | Value: false 142 | Name: Current View 143 | Near Clip Distance: 0.00999999978 144 | Scale: 20 145 | Target Frame: 146 | Value: TopDownOrtho (rviz) 147 | X: 0 148 | Y: 0 149 | Saved: ~ 150 | Window Geometry: 151 | Displays: 152 | collapsed: false 153 | Height: 1136 154 | Hide Left Dock: false 155 | Hide Right Dock: true 156 | QMainWindow State: 000000ff00000000fd000000040000000000000156000003d8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003a000003d8000000c600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003fcfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003a000003fc0000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004fc0000003efc0100000002fb0000000800540069006d00650100000000000004fc0000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000003a0000003d800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 157 | Selection: 158 | collapsed: false 159 | Time: 160 | collapsed: false 161 | Tool Properties: 162 | collapsed: false 163 | Views: 164 | collapsed: true 165 | Width: 1276 166 | X: 0 167 | Y: 27 168 | -------------------------------------------------------------------------------- /aeplanner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(aeplanner) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | roscpp 8 | geometry_msgs 9 | visualization_msgs 10 | message_generation 11 | tf 12 | tf2 13 | genmsg 14 | actionlib_msgs 15 | actionlib 16 | 17 | kdtree 18 | pigain 19 | dynamic_reconfigure 20 | ) 21 | find_package(cmake_modules REQUIRED) 22 | find_package(Eigen3 REQUIRED) 23 | find_package(octomap REQUIRED) 24 | 25 | add_message_files( 26 | DIRECTORY msg 27 | FILES Node.msg 28 | ) 29 | add_action_files( 30 | DIRECTORY action 31 | FILES aeplanner.action 32 | ) 33 | add_service_files( 34 | DIRECTORY srv 35 | FILES Reevaluate.srv 36 | ) 37 | 38 | generate_messages( 39 | DEPENDENCIES 40 | geometry_msgs 41 | visualization_msgs 42 | std_msgs 43 | actionlib_msgs 44 | ) 45 | 46 | catkin_package( 47 | INCLUDE_DIRS include ${Eigen_INCLUDE_DIRS} ${OCTOMAP_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} 48 | LIBRARIES aeplanner ${catkin_LIBRARIES} ${OCTOMAP_LIBRARIES} 49 | CATKIN_DEPENDS message_runtime roscpp geometry_msgs visualization_msgs tf kdtree 50 | ) 51 | 52 | include_directories( 53 | include 54 | ${catkin_INCLUDE_DIRS} 55 | ${Eigen_INCLUDE_DIRS} 56 | ${OCTOMAP_INCLUDE_DIRS} 57 | ) 58 | 59 | add_library(aeplanner src/aeplanner_nodelet.cpp src/aeplanner.cpp src/aeplanner_viz.cpp src/param.cpp) 60 | target_link_libraries(aeplanner 61 | ${catkin_LIBRARIES} 62 | ${OCTOMAP_LIBRARIES} 63 | ) 64 | add_dependencies(aeplanner 65 | ${catkin_EXPORTED_TARGETS} 66 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 67 | aeplanner_ref_generate_messages_cpp 68 | ${PROJECT_NAME}_gencfg 69 | ) 70 | 71 | add_executable(aeplanner_node src/aeplanner_node.cpp src/aeplanner.cpp src/aeplanner_viz.cpp src/param.cpp) 72 | target_link_libraries(aeplanner_node 73 | ${catkin_LIBRARIES} 74 | ${OCTOMAP_LIBRARIES} 75 | ) 76 | add_dependencies(aeplanner_node 77 | ${catkin_EXPORTED_TARGETS} 78 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 79 | aeplanner_ref_generate_messages_cpp 80 | ${PROJECT_NAME}_gencfg 81 | ) 82 | -------------------------------------------------------------------------------- /aeplanner/action/aeplanner.action: -------------------------------------------------------------------------------- 1 | # Define the goal 2 | Header header 3 | int32 actions_taken 4 | ------- 5 | # Define the result 6 | geometry_msgs/PoseStamped pose 7 | geometry_msgs/PoseArray frontiers 8 | bool is_clear 9 | std_msgs/Duration sampling_time 10 | std_msgs/Duration planning_time 11 | std_msgs/Duration collision_check_time 12 | int32 tree_size 13 | ------- 14 | # Define a feedback message 15 | -------------------------------------------------------------------------------- /aeplanner/include/aeplanner/aeplanner.h: -------------------------------------------------------------------------------- 1 | #ifndef AEPLANNER_H 2 | #define AEPLANNER_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | namespace aeplanner 36 | { 37 | class AEPlanner 38 | { 39 | private: 40 | ros::NodeHandle nh_; 41 | actionlib::SimpleActionServer as_; 42 | 43 | Params params_; 44 | 45 | // Current state of agent (x, y, z, yaw) 46 | Eigen::Vector4d current_state_; 47 | bool current_state_initialized_; 48 | 49 | // Keep track of the best node and its score 50 | RRTNode* best_node_; 51 | RRTNode* best_branch_root_; 52 | 53 | std::shared_ptr ot_; 54 | 55 | // kd tree for finding nearest neighbours 56 | kdtree* kd_tree_; 57 | 58 | // Subscribers 59 | ros::Subscriber octomap_sub_; 60 | ros::Subscriber agent_pose_sub_; 61 | 62 | // Publishers 63 | ros::Publisher rrt_marker_pub_; 64 | ros::Publisher gain_pub_; 65 | 66 | // Services 67 | ros::ServiceClient best_node_client_; 68 | ros::ServiceClient gp_query_client_; 69 | ros::ServiceServer reevaluate_server_; 70 | 71 | // Service server callback 72 | bool reevaluate(aeplanner::Reevaluate::Request& req, 73 | aeplanner::Reevaluate::Response& res); 74 | 75 | // ---------------- Initialization ---------------- 76 | RRTNode* initialize(); 77 | void initializeKDTreeWithPreviousBestBranch(RRTNode* root); 78 | void reevaluatePotentialInformationGainRecursive(RRTNode* node); 79 | 80 | // ---------------- Expand RRT Tree ---------------- 81 | void expandRRT(); 82 | 83 | Eigen::Vector4d sampleNewPoint(); 84 | bool isInsideBoundaries(Eigen::Vector4d point); 85 | bool collisionLine(Eigen::Vector4d p1, Eigen::Vector4d p2, double r); 86 | RRTNode* chooseParent(RRTNode* node, double l); 87 | void rewire(kdtree* kd_tree, RRTNode* new_node, double l, double r, double r_os); 88 | Eigen::Vector4d restrictDistance(Eigen::Vector4d nearest, Eigen::Vector4d new_pos); 89 | 90 | std::pair getGain(RRTNode* node); 91 | std::pair gainCubature(Eigen::Vector4d state); 92 | 93 | // ---------------- Helpers ---------------- 94 | // 95 | void publishEvaluatedNodesRecursive(RRTNode* node); 96 | 97 | geometry_msgs::Pose vecToPose(Eigen::Vector4d state); 98 | 99 | float CylTest_CapsFirst(const octomap::point3d& pt1, const octomap::point3d& pt2, 100 | float lsq, float rsq, const octomap::point3d& pt); 101 | 102 | // ---------------- Frontier ---------------- 103 | geometry_msgs::PoseArray getFrontiers(); 104 | 105 | public: 106 | AEPlanner(const ros::NodeHandle& nh); 107 | 108 | void execute(const aeplanner::aeplannerGoalConstPtr& goal); 109 | 110 | void octomapCallback(const octomap_msgs::Octomap& msg); 111 | void agentPoseCallback(const geometry_msgs::PoseStamped& msg); 112 | }; 113 | 114 | } // namespace aeplanner 115 | 116 | #endif 117 | -------------------------------------------------------------------------------- /aeplanner/include/aeplanner/aeplanner_nodelet.h: -------------------------------------------------------------------------------- 1 | #ifndef AEPLANNER_NODELET_H 2 | #define AEPLANNER_NODELET_H 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace aeplanner 9 | { 10 | class AEPlannerNodelet : public nodelet::Nodelet 11 | { 12 | private: 13 | AEPlanner * aeplanner_; 14 | 15 | public: 16 | virtual void onInit(); 17 | ~AEPlannerNodelet(); 18 | }; 19 | } 20 | 21 | # endif 22 | -------------------------------------------------------------------------------- /aeplanner/include/aeplanner/aeplanner_viz.h: -------------------------------------------------------------------------------- 1 | #ifndef _AEPVIZ_H_ 2 | #define _AEPVIZ_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | namespace aeplanner { 14 | visualization_msgs::MarkerArray createRRTMarkerArray(RRTNode * root, double lambda); 15 | void recurse(RRTNode * node, visualization_msgs::MarkerArray * marker_array, int * id, double lambda); 16 | 17 | visualization_msgs::Marker createNodeMarker(RRTNode * node, int id, std::string frame_id); 18 | visualization_msgs::Marker createEdgeMarker(RRTNode * node, int id, std::string frame_id, double lambda); 19 | } 20 | #endif 21 | -------------------------------------------------------------------------------- /aeplanner/include/aeplanner/data_structures.h: -------------------------------------------------------------------------------- 1 | #ifndef DATA_STRUCTURES_H 2 | #define DATA_STRUCTURES_H 3 | 4 | #include 5 | #include 6 | 7 | namespace aeplanner 8 | { 9 | class RRTNode 10 | { 11 | public: 12 | Eigen::Vector4d state_; 13 | RRTNode* parent_; 14 | std::vector children_; 15 | double gain_; 16 | bool gain_explicitly_calculated_; 17 | 18 | RRTNode() : parent_(NULL), gain_(0.0), gain_explicitly_calculated_(false) 19 | { 20 | } 21 | ~RRTNode() 22 | { 23 | for (typename std::vector::iterator node_it = children_.begin(); 24 | node_it != children_.end(); ++node_it) 25 | { 26 | delete (*node_it); 27 | (*node_it) = NULL; 28 | } 29 | } 30 | 31 | RRTNode* getCopyOfParentBranch() 32 | { 33 | RRTNode* current_node = this; 34 | RRTNode* current_child_node = NULL; 35 | RRTNode* new_node; 36 | RRTNode* new_child_node = NULL; 37 | 38 | while (current_node) 39 | { 40 | new_node = new RRTNode(); 41 | new_node->state_ = current_node->state_; 42 | new_node->gain_ = current_node->gain_; 43 | new_node->gain_explicitly_calculated_ = current_node->gain_explicitly_calculated_; 44 | new_node->parent_ = NULL; 45 | 46 | if (new_child_node) 47 | { 48 | new_node->children_.push_back(new_child_node); 49 | new_child_node->parent_ = new_node; 50 | } 51 | 52 | current_child_node = current_node; 53 | current_node = current_node->parent_; 54 | new_child_node = new_node; 55 | } 56 | 57 | return new_node; 58 | } 59 | 60 | double score(double lambda) 61 | { 62 | if (this->parent_) 63 | return this->parent_->score(lambda) + 64 | this->gain_ * exp(-lambda * this->distance(this->parent_)); 65 | else 66 | return this->gain_; 67 | } 68 | 69 | double cost() 70 | { 71 | if (this->parent_) 72 | return this->distance(this->parent_) + this->parent_->cost(); 73 | else 74 | return 0; 75 | } 76 | 77 | double distance(RRTNode* other) 78 | { 79 | Eigen::Vector3d p3(this->state_[0], this->state_[1], this->state_[2]); 80 | Eigen::Vector3d q3(other->state_[0], other->state_[1], other->state_[2]); 81 | return (p3 - q3).norm(); 82 | } 83 | }; 84 | } // namespace aeplanner 85 | 86 | #endif 87 | -------------------------------------------------------------------------------- /aeplanner/include/aeplanner/param.h: -------------------------------------------------------------------------------- 1 | #ifndef READ_PARAMS_H 2 | #define READ_PARAMS_H 3 | 4 | namespace aeplanner 5 | { 6 | struct Params 7 | { 8 | double hfov; 9 | double vfov; 10 | double r_max; 11 | double r_min; 12 | 13 | double dr; 14 | double dphi; 15 | double dtheta; 16 | 17 | double lambda; 18 | double zero_gain; 19 | double extension_range; 20 | double max_sampling_radius; 21 | double sigma_thresh; 22 | 23 | double d_overshoot_; 24 | double bounding_radius; 25 | 26 | int init_iterations; 27 | int cutoff_iterations; 28 | 29 | std::vector boundary_min; 30 | std::vector boundary_max; 31 | 32 | std::string robot_frame; 33 | std::string world_frame; 34 | 35 | bool visualize_tree; 36 | bool visualize_rays; 37 | bool visualize_exploration_area; 38 | }; 39 | 40 | Params readParams(); 41 | } 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /aeplanner/launch/aeplanner_nodelet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /aeplanner/launch/aeplanner_standalone.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /aeplanner/msg/Node.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Pose pose 3 | float64 gain 4 | -------------------------------------------------------------------------------- /aeplanner/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | This is the Autonomous Planner Nodelet. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /aeplanner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | aeplanner 4 | 1.0.0 5 | The Autonomous Exploration Planner package 6 | 7 | Magnus Selin 8 | 9 | BSD-3-Clause 10 | 11 | Magnus Selin 12 | 13 | catkin 14 | 15 | roscpp 16 | nodelet 17 | tf 18 | tf2 19 | message_generation 20 | geometry_msgs 21 | visualization_msgs 22 | kdtree 23 | pigain 24 | dynamic_reconfigure 25 | 26 | roscpp 27 | nodelet 28 | tf 29 | tf2 30 | message_runtime 31 | geometry_msgs 32 | visualization_msgs 33 | kdtree 34 | pigain 35 | dynamic_reconfigure 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /aeplanner/src/aeplanner.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace aeplanner 5 | { 6 | AEPlanner::AEPlanner(const ros::NodeHandle& nh) 7 | : nh_(nh) 8 | , as_(nh_, "make_plan", boost::bind(&AEPlanner::execute, this, _1), false) 9 | , octomap_sub_(nh_.subscribe("octomap", 1, &AEPlanner::octomapCallback, this)) 10 | , agent_pose_sub_(nh_.subscribe("agent_pose", 1, &AEPlanner::agentPoseCallback, this)) 11 | , rrt_marker_pub_(nh_.advertise("rrtree", 1000)) 12 | , gain_pub_(nh_.advertise("gain_node", 1000)) 13 | , gp_query_client_(nh_.serviceClient("gp_query_server")) 14 | , reevaluate_server_(nh_.advertiseService("reevaluate", &AEPlanner::reevaluate, this)) 15 | , best_node_client_(nh_.serviceClient("best_node_server")) 16 | , current_state_initialized_(false) 17 | , ot_(NULL) 18 | , best_node_(NULL) 19 | , best_branch_root_(NULL) 20 | { 21 | params_ = readParams(); 22 | as_.start(); 23 | 24 | // Initialize kd-tree 25 | kd_tree_ = kd_create(3); 26 | } 27 | 28 | void AEPlanner::execute(const aeplanner::aeplannerGoalConstPtr& goal) 29 | { 30 | aeplanner::aeplannerResult result; 31 | 32 | // Check if aeplanner has recieved agent's pose yet 33 | if (!current_state_initialized_) 34 | { 35 | ROS_WARN("Agent's pose not yet received"); 36 | ROS_WARN("Make sure it is being published and correctly mapped"); 37 | as_.setSucceeded(result); 38 | return; 39 | } 40 | if (!ot_) 41 | { 42 | ROS_WARN("No octomap received"); 43 | as_.setSucceeded(result); 44 | return; 45 | } 46 | 47 | ROS_DEBUG("Init"); 48 | RRTNode* root = initialize(); 49 | ROS_DEBUG("expandRRT"); 50 | if (root->gain_ > 0.25 or !root->children_.size() or // FIXME parameterize 51 | root->score(params_.lambda) < params_.zero_gain) 52 | expandRRT(); 53 | else 54 | best_node_ = root->children_[0]; 55 | 56 | ROS_DEBUG("getCopyOfParent"); 57 | best_branch_root_ = best_node_->getCopyOfParentBranch(); 58 | 59 | ROS_DEBUG("createRRTMarker"); 60 | rrt_marker_pub_.publish(createRRTMarkerArray(root, params_.lambda)); 61 | ROS_DEBUG("publishRecursive"); 62 | publishEvaluatedNodesRecursive(root); 63 | 64 | ROS_DEBUG("extractPose"); 65 | result.pose.pose = vecToPose(best_branch_root_->children_[0]->state_); 66 | ROS_INFO_STREAM("Best node score: " << best_node_->score(params_.lambda)); 67 | if (best_node_->score(params_.lambda) > params_.zero_gain) 68 | result.is_clear = true; 69 | else 70 | { 71 | result.frontiers = getFrontiers(); 72 | result.is_clear = false; 73 | delete best_branch_root_; 74 | best_branch_root_ = NULL; 75 | } 76 | as_.setSucceeded(result); 77 | 78 | ROS_DEBUG("Deleting/Freeing!"); 79 | delete root; 80 | kd_free(kd_tree_); 81 | ROS_DEBUG("Done!"); 82 | } 83 | 84 | RRTNode* AEPlanner::initialize() 85 | { 86 | // Initialize kd-tree 87 | kd_tree_ = kd_create(3); 88 | best_node_ = NULL; 89 | RRTNode* root = NULL; 90 | 91 | if (best_branch_root_) 92 | { 93 | // Initialize with previous best branch 94 | 95 | // Discard root node from tree since we just were there... 96 | root = best_branch_root_->children_[0]; 97 | root->parent_ = NULL; 98 | best_branch_root_->children_.clear(); 99 | delete best_branch_root_; 100 | 101 | initializeKDTreeWithPreviousBestBranch(root); 102 | reevaluatePotentialInformationGainRecursive(root); 103 | } 104 | else 105 | { 106 | // Initialize without any previous branch 107 | root = new RRTNode(); 108 | root->state_[0] = current_state_[0]; 109 | root->state_[1] = current_state_[1]; 110 | root->state_[2] = current_state_[2]; 111 | kd_insert3(kd_tree_, root->state_[0], root->state_[1], root->state_[2], root); 112 | } 113 | 114 | return root; 115 | } 116 | 117 | void AEPlanner::initializeKDTreeWithPreviousBestBranch(RRTNode* root) 118 | { 119 | RRTNode* current_node = root; 120 | do 121 | { 122 | kd_insert3(kd_tree_, current_node->state_[0], current_node->state_[1], 123 | current_node->state_[2], current_node); 124 | if (current_node->children_.size()) 125 | current_node = current_node->children_[0]; 126 | } while (current_node->children_.size()); 127 | } 128 | 129 | void AEPlanner::reevaluatePotentialInformationGainRecursive(RRTNode* node) 130 | { 131 | std::pair ret = gainCubature(node->state_); // FIXME use gp? 132 | node->state_[3] = ret.second; // Assign yaw angle that maximizes g 133 | node->gain_ = ret.first; 134 | for (typename std::vector::iterator node_it = node->children_.begin(); 135 | node_it != node->children_.end(); ++node_it) 136 | reevaluatePotentialInformationGainRecursive(*node_it); 137 | } 138 | 139 | void AEPlanner::expandRRT() 140 | { 141 | std::shared_ptr ot = ot_; 142 | 143 | // Expand an RRT tree and calculate information gain in every node 144 | ROS_DEBUG_STREAM("Entering expanding RRT"); 145 | for (int n = 0; (n < params_.init_iterations or 146 | (n < params_.cutoff_iterations and 147 | best_node_->score(params_.lambda) < params_.zero_gain)) and 148 | ros::ok(); 149 | ++n) 150 | { 151 | ROS_DEBUG_STREAM("In expand RRT iteration: " << n); 152 | RRTNode* new_node = new RRTNode(); 153 | RRTNode* nearest; 154 | octomap::OcTreeNode* ot_result; 155 | 156 | // Sample new point around agent and check that 157 | // (1) it is within the boundaries 158 | // (2) it is in known space 159 | // (3) the path between the new node and it's parent does not contain any 160 | // obstacles 161 | 162 | do 163 | { 164 | Eigen::Vector4d offset = sampleNewPoint(); 165 | new_node->state_ = current_state_ + offset; 166 | 167 | nearest = chooseParent(new_node, params_.extension_range); 168 | 169 | new_node->state_ = restrictDistance(nearest->state_, new_node->state_); 170 | 171 | ROS_DEBUG_STREAM("Trying node (" << new_node->state_[0] << ", " 172 | << new_node->state_[1] << ", " 173 | << new_node->state_[2] << ")"); 174 | ROS_DEBUG_STREAM(" nearest (" << nearest->state_[0] << ", " << nearest->state_[1] 175 | << ", " << nearest->state_[2] << ")"); 176 | ot_result = ot->search(octomap::point3d(new_node->state_[0], new_node->state_[1], 177 | new_node->state_[2])); 178 | if (ot_result == NULL) 179 | continue; 180 | ROS_DEBUG_STREAM("ot check done!"); 181 | 182 | ROS_DEBUG_STREAM("Inside boundaries? " << isInsideBoundaries(new_node->state_)); 183 | ROS_DEBUG_STREAM("In known space? " << ot_result); 184 | ROS_DEBUG_STREAM("Collision? " << collisionLine( 185 | nearest->state_, new_node->state_, params_.bounding_radius)); 186 | } while (!isInsideBoundaries(new_node->state_) or !ot_result or 187 | collisionLine(nearest->state_, new_node->state_, params_.bounding_radius)); 188 | 189 | ROS_DEBUG_STREAM("New node (" << new_node->state_[0] << ", " << new_node->state_[1] 190 | << ", " << new_node->state_[2] << ")"); 191 | // new_node is now ready to be added to tree 192 | new_node->parent_ = nearest; 193 | nearest->children_.push_back(new_node); 194 | 195 | // rewire tree with new node 196 | rewire(kd_tree_, nearest, params_.extension_range, params_.bounding_radius, 197 | params_.d_overshoot_); 198 | 199 | // Calculate potential information gain for new_node 200 | ROS_DEBUG_STREAM("Get gain"); 201 | std::pair ret = getGain(new_node); 202 | new_node->state_[3] = ret.second; // Assign yaw angle that maximizes g 203 | new_node->gain_ = ret.first; 204 | ROS_DEBUG_STREAM("Insert into KDTREE"); 205 | kd_insert3(kd_tree_, new_node->state_[0], new_node->state_[1], new_node->state_[2], 206 | new_node); 207 | 208 | // Update best node 209 | 210 | ROS_DEBUG_STREAM("Update best node"); 211 | if (!best_node_ or 212 | new_node->score(params_.lambda) > best_node_->score(params_.lambda)) 213 | best_node_ = new_node; 214 | 215 | ROS_DEBUG_STREAM("iteration Done!"); 216 | } 217 | 218 | ROS_DEBUG_STREAM("expandRRT Done!"); 219 | } 220 | 221 | Eigen::Vector4d AEPlanner::sampleNewPoint() 222 | { 223 | // Samples one point uniformly over a sphere with a radius of 224 | // param_.max_sampling_radius 225 | Eigen::Vector4d point; 226 | do 227 | { 228 | for (int i = 0; i < 3; i++) 229 | point[i] = params_.max_sampling_radius * 2.0 * 230 | (((double)rand()) / ((double)RAND_MAX) - 0.5); 231 | } while (pow(point[0], 2.0) + pow(point[1], 2.0) + pow(point[2], 2.0) > 232 | pow(params_.max_sampling_radius, 2.0)); 233 | 234 | return point; 235 | } 236 | 237 | RRTNode* AEPlanner::chooseParent(RRTNode* node, double l) 238 | { 239 | std::shared_ptr ot = ot_; 240 | Eigen::Vector4d current_state = current_state_; 241 | 242 | // Find nearest neighbour 243 | kdres* nearest = kd_nearest_range3(kd_tree_, node->state_[0], node->state_[1], 244 | node->state_[2], l + 0.5); // FIXME why +0.5? 245 | 246 | if (kd_res_size(nearest) <= 0) 247 | nearest = kd_nearest3(kd_tree_, node->state_[0], node->state_[1], node->state_[2]); 248 | if (kd_res_size(nearest) <= 0) 249 | { 250 | kd_res_free(nearest); 251 | return NULL; 252 | } 253 | 254 | RRTNode* node_nn = (RRTNode*)kd_res_item_data(nearest); 255 | 256 | RRTNode* best_node = node_nn; 257 | double best_node_cost = best_node->cost(); 258 | while (!kd_res_end(nearest)) 259 | { 260 | node_nn = (RRTNode*)kd_res_item_data(nearest); 261 | double node_cost = node_nn->cost(); 262 | if (best_node and node_cost < best_node_cost) 263 | { 264 | best_node = node_nn; 265 | best_node_cost = node_cost; 266 | } 267 | 268 | kd_res_next(nearest); 269 | } 270 | 271 | kd_res_free(nearest); 272 | return best_node; 273 | } 274 | 275 | void AEPlanner::rewire(kdtree* kd_tree, RRTNode* new_node, double l, double r, 276 | double r_os) 277 | { 278 | std::shared_ptr ot = ot_; 279 | Eigen::Vector4d current_state = current_state_; 280 | 281 | RRTNode* node_nn; 282 | kdres* nearest = kd_nearest_range3(kd_tree, new_node->state_[0], new_node->state_[1], 283 | new_node->state_[2], l + 0.5); // FIXME why +0.5? 284 | while (!kd_res_end(nearest)) 285 | { 286 | node_nn = (RRTNode*)kd_res_item_data(nearest); 287 | Eigen::Vector3d p1(new_node->state_[0], new_node->state_[1], new_node->state_[2]); 288 | Eigen::Vector3d p2(node_nn->state_[0], node_nn->state_[1], node_nn->state_[2]); 289 | if (node_nn->cost() > new_node->cost() + (p1 - p2).norm()) 290 | { 291 | if (!collisionLine(new_node->state_, node_nn->state_, r)) 292 | node_nn->parent_ = new_node; 293 | } 294 | kd_res_next(nearest); 295 | } 296 | } 297 | 298 | Eigen::Vector4d AEPlanner::restrictDistance(Eigen::Vector4d nearest, 299 | Eigen::Vector4d new_pos) 300 | { 301 | // Check for collision 302 | Eigen::Vector3d origin(nearest[0], nearest[1], nearest[2]); 303 | Eigen::Vector3d direction(new_pos[0] - origin[0], new_pos[1] - origin[1], 304 | new_pos[2] - origin[2]); 305 | if (direction.norm() > params_.extension_range) 306 | direction = params_.extension_range * direction.normalized(); 307 | 308 | new_pos[0] = origin[0] + direction[0]; 309 | new_pos[1] = origin[1] + direction[1]; 310 | new_pos[2] = origin[2] + direction[2]; 311 | 312 | return new_pos; 313 | } 314 | 315 | std::pair AEPlanner::getGain(RRTNode* node) 316 | { 317 | pigain::Query srv; 318 | srv.request.point.x = node->state_[0]; 319 | srv.request.point.y = node->state_[1]; 320 | srv.request.point.z = node->state_[2]; 321 | 322 | if (gp_query_client_.call(srv)) 323 | { 324 | if (srv.response.sigma < params_.sigma_thresh) 325 | { 326 | double gain = srv.response.mu; 327 | double yaw = srv.response.yaw; 328 | 329 | ROS_INFO_STREAM("gain impl: " << gain); 330 | return std::make_pair(gain, yaw); 331 | } 332 | } 333 | 334 | node->gain_explicitly_calculated_ = true; 335 | std::pair ret = gainCubature(node->state_); 336 | ROS_INFO_STREAM("gain expl: " << ret.first); 337 | return ret; 338 | } 339 | 340 | bool AEPlanner::reevaluate(aeplanner::Reevaluate::Request& req, 341 | aeplanner::Reevaluate::Response& res) 342 | { 343 | ROS_DEBUG_STREAM("Reevaluate start!"); 344 | for (std::vector::iterator it = req.point.begin(); 345 | it != req.point.end(); ++it) 346 | { 347 | Eigen::Vector4d pos(it->x, it->y, it->z, 0); 348 | std::pair gain_response = gainCubature(pos); 349 | res.gain.push_back(gain_response.first); 350 | res.yaw.push_back(gain_response.second); 351 | } 352 | ROS_DEBUG_STREAM("Reevaluate done!"); 353 | 354 | return true; 355 | } 356 | 357 | std::pair AEPlanner::gainCubature(Eigen::Vector4d state) 358 | { 359 | std::shared_ptr ot = ot_; 360 | double gain = 0.0; 361 | 362 | // This function computes the gain 363 | double fov_y = params_.hfov, fov_p = params_.vfov; 364 | 365 | double dr = params_.dr, dphi = params_.dphi, dtheta = params_.dtheta; 366 | double dphi_rad = M_PI * dphi / 180.0f, dtheta_rad = M_PI * dtheta / 180.0f; 367 | double r; 368 | int phi, theta; 369 | double phi_rad, theta_rad; 370 | 371 | std::map gain_per_yaw; 372 | 373 | Eigen::Vector3d origin(state[0], state[1], state[2]); 374 | Eigen::Vector3d vec, dir; 375 | 376 | int id = 0; 377 | for (theta = -180; theta < 180; theta += dtheta) 378 | { 379 | theta_rad = M_PI * theta / 180.0f; 380 | for (phi = 90 - fov_p / 2; phi < 90 + fov_p / 2; phi += dphi) 381 | { 382 | phi_rad = M_PI * phi / 180.0f; 383 | 384 | double g = 0; 385 | for (r = params_.r_min; r < params_.r_max; r += dr) 386 | { 387 | vec[0] = state[0] + r * cos(theta_rad) * sin(phi_rad); 388 | vec[1] = state[1] + r * sin(theta_rad) * sin(phi_rad); 389 | vec[2] = state[2] + r * cos(phi_rad); 390 | dir = vec - origin; 391 | 392 | octomap::point3d query(vec[0], vec[1], vec[2]); 393 | octomap::OcTreeNode* result = ot->search(query); 394 | 395 | Eigen::Vector4d v(vec[0], vec[1], vec[2], 0); 396 | if (!isInsideBoundaries(v)) 397 | break; 398 | if (result) 399 | { 400 | // Break if occupied so we don't count any information gain behind a wall. 401 | if (result->getLogOdds() > 0) 402 | break; 403 | } 404 | else 405 | g += (2 * r * r * dr + 1 / 6 * dr * dr * dr) * dtheta_rad * sin(phi_rad) * 406 | sin(dphi_rad / 2); 407 | } 408 | 409 | gain += g; 410 | gain_per_yaw[theta] += g; 411 | } 412 | } 413 | 414 | int best_yaw = 0; 415 | double best_yaw_score = 0; 416 | for (int yaw = -180; yaw < 180; yaw++) 417 | { 418 | double yaw_score = 0; 419 | for (int fov = -fov_y / 2; fov < fov_y / 2; fov++) 420 | { 421 | int theta = yaw + fov; 422 | if (theta < -180) 423 | theta += 360; 424 | if (theta > 180) 425 | theta -= 360; 426 | yaw_score += gain_per_yaw[theta]; 427 | } 428 | 429 | if (best_yaw_score < yaw_score) 430 | { 431 | best_yaw_score = yaw_score; 432 | best_yaw = yaw; 433 | } 434 | } 435 | 436 | double r_max = params_.r_max; 437 | double h_max = params_.hfov / M_PI * 180; 438 | double v_max = params_.vfov / M_PI * 180; 439 | 440 | gain = best_yaw_score; // / ((r_max*r_max*r_max/3) * h_max * (1-cos(v_max))) ; 441 | 442 | double yaw = M_PI * best_yaw / 180.f; 443 | 444 | state[3] = yaw; 445 | return std::make_pair(gain, yaw); 446 | } 447 | 448 | geometry_msgs::PoseArray AEPlanner::getFrontiers() 449 | { 450 | geometry_msgs::PoseArray frontiers; 451 | 452 | pigain::BestNode srv; 453 | srv.request.threshold = 16; // FIXME parameterize 454 | if (best_node_client_.call(srv)) 455 | { 456 | for (int i = 0; i < srv.response.best_node.size(); ++i) 457 | { 458 | geometry_msgs::Pose frontier; 459 | frontier.position = srv.response.best_node[i]; 460 | frontiers.poses.push_back(frontier); 461 | } 462 | } 463 | else 464 | { 465 | } 466 | 467 | return frontiers; 468 | } 469 | 470 | bool AEPlanner::isInsideBoundaries(Eigen::Vector4d point) 471 | { 472 | return point[0] > params_.boundary_min[0] and point[0] < params_.boundary_max[0] and 473 | point[1] > params_.boundary_min[1] and point[1] < params_.boundary_max[1] and 474 | point[2] > params_.boundary_min[2] and point[2] < params_.boundary_max[2]; 475 | } 476 | 477 | bool AEPlanner::collisionLine(Eigen::Vector4d p1, Eigen::Vector4d p2, double r) 478 | { 479 | std::shared_ptr ot = ot_; 480 | ROS_DEBUG_STREAM("In collision"); 481 | octomap::point3d start(p1[0], p1[1], p1[2]); 482 | octomap::point3d end(p2[0], p2[1], p2[2]); 483 | octomap::point3d min(std::min(p1[0], p2[0]) - r, std::min(p1[1], p2[1]) - r, 484 | std::min(p1[2], p2[2]) - r); 485 | octomap::point3d max(std::max(p1[0], p2[0]) + r, std::max(p1[1], p2[1]) + r, 486 | std::max(p1[2], p2[2]) + r); 487 | double lsq = (end - start).norm_sq(); 488 | double rsq = r * r; 489 | 490 | for (octomap::OcTree::leaf_bbx_iterator it = ot->begin_leafs_bbx(min, max), 491 | it_end = ot->end_leafs_bbx(); 492 | it != it_end; ++it) 493 | { 494 | octomap::point3d pt(it.getX(), it.getY(), it.getZ()); 495 | 496 | if (it->getLogOdds() > 0) // Node is occupied 497 | { 498 | if (CylTest_CapsFirst(start, end, lsq, rsq, pt) > 0 or (end - pt).norm() < r) 499 | { 500 | return true; 501 | } 502 | } 503 | } 504 | ROS_DEBUG_STREAM("In collision (exiting)"); 505 | 506 | return false; 507 | } 508 | 509 | void AEPlanner::octomapCallback(const octomap_msgs::Octomap& msg) 510 | { 511 | ROS_DEBUG_STREAM("Freeing ot_"); 512 | octomap::AbstractOcTree* aot = octomap_msgs::msgToMap(msg); 513 | octomap::OcTree* ot = (octomap::OcTree*)aot; 514 | ot_ = std::make_shared(*ot); 515 | 516 | delete ot; 517 | ROS_DEBUG_STREAM("Freeing ot_ done:"); 518 | } 519 | 520 | void AEPlanner::publishEvaluatedNodesRecursive(RRTNode* node) 521 | { 522 | if (!node) 523 | return; 524 | for (typename std::vector::iterator node_it = node->children_.begin(); 525 | node_it != node->children_.end(); ++node_it) 526 | { 527 | if ((*node_it)->gain_explicitly_calculated_) 528 | { 529 | pigain::Node pig_node; 530 | pig_node.gain = (*node_it)->gain_; 531 | pig_node.position.x = (*node_it)->state_[0]; 532 | pig_node.position.y = (*node_it)->state_[1]; 533 | pig_node.position.z = (*node_it)->state_[2]; 534 | pig_node.yaw = (*node_it)->state_[3]; 535 | gain_pub_.publish(pig_node); 536 | } 537 | 538 | publishEvaluatedNodesRecursive(*node_it); 539 | } 540 | } 541 | 542 | void AEPlanner::agentPoseCallback(const geometry_msgs::PoseStamped& msg) 543 | { 544 | current_state_[0] = msg.pose.position.x; 545 | current_state_[1] = msg.pose.position.y; 546 | current_state_[2] = msg.pose.position.z; 547 | current_state_[3] = tf2::getYaw(msg.pose.orientation); 548 | 549 | current_state_initialized_ = true; 550 | } 551 | 552 | geometry_msgs::Pose AEPlanner::vecToPose(Eigen::Vector4d state) 553 | { 554 | tf::Vector3 origin(state[0], state[1], state[2]); 555 | double yaw = state[3]; 556 | 557 | tf::Quaternion quat; 558 | quat.setEuler(0.0, 0.0, yaw); 559 | tf::Pose pose_tf(quat, origin); 560 | 561 | geometry_msgs::Pose pose; 562 | tf::poseTFToMsg(pose_tf, pose); 563 | 564 | return pose; 565 | } 566 | 567 | //----------------------------------------------------------------------------- 568 | // Name: CylTest_CapsFirst 569 | // Orig: Greg James - gjames@NVIDIA.com 570 | // Lisc: Free code - no warranty & no money back. Use it all you want 571 | // Desc: 572 | // This function tests if the 3D point 'pt' lies within an arbitrarily 573 | // oriented cylinder. The cylinder is defined by an axis from 'pt1' to 'pt2', 574 | // the axis having a length squared of 'lsq' (pre-compute for each cylinder 575 | // to avoid repeated work!), and radius squared of 'rsq'. 576 | // The function tests against the end caps first, which is cheap -> only 577 | // a single dot product to test against the parallel cylinder caps. If the 578 | // point is within these, more work is done to find the distance of the point 579 | // from the cylinder axis. 580 | // Fancy Math (TM) makes the whole test possible with only two dot-products 581 | // a subtract, and two multiplies. For clarity, the 2nd mult is kept as a 582 | // divide. It might be faster to change this to a mult by also passing in 583 | // 1/lengthsq and using that instead. 584 | // Elminiate the first 3 subtracts by specifying the cylinder as a base 585 | // point on one end cap and a vector to the other end cap (pass in {dx,dy,dz} 586 | // instead of 'pt2' ). 587 | // 588 | // The dot product is constant along a plane perpendicular to a vector. 589 | // The magnitude of the cross product divided by one vector length is 590 | // constant along a cylinder surface defined by the other vector as axis. 591 | // 592 | // Return: -1.0 if point is outside the cylinder 593 | // Return: distance squared from cylinder axis if point is inside. 594 | // 595 | //----------------------------------------------------------------------------- 596 | float AEPlanner::CylTest_CapsFirst(const octomap::point3d& pt1, 597 | const octomap::point3d& pt2, float lsq, float rsq, 598 | const octomap::point3d& pt) 599 | { 600 | float dx, dy, dz; // vector d from line segment point 1 to point 2 601 | float pdx, pdy, pdz; // vector pd from point 1 to test point 602 | float dot, dsq; 603 | 604 | dx = pt2.x() - pt1.x(); // translate so pt1 is origin. Make vector from 605 | dy = pt2.y() - pt1.y(); // pt1 to pt2. Need for this is easily eliminated 606 | dz = pt2.z() - pt1.z(); 607 | 608 | pdx = pt.x() - pt1.x(); // vector from pt1 to test point. 609 | pdy = pt.y() - pt1.y(); 610 | pdz = pt.z() - pt1.z(); 611 | 612 | // Dot the d and pd vectors to see if point lies behind the 613 | // cylinder cap at pt1.x, pt1.y, pt1.z 614 | 615 | dot = pdx * dx + pdy * dy + pdz * dz; 616 | 617 | // If dot is less than zero the point is behind the pt1 cap. 618 | // If greater than the cylinder axis line segment length squared 619 | // then the point is outside the other end cap at pt2. 620 | 621 | if (dot < 0.0f || dot > lsq) 622 | return (-1.0f); 623 | else 624 | { 625 | // Point lies within the parallel caps, so find 626 | // distance squared from point to line, using the fact that sin^2 + cos^2 = 1 627 | // the dot = cos() * |d||pd|, and cross*cross = sin^2 * |d|^2 * |pd|^2 628 | // Carefull: '*' means mult for scalars and dotproduct for vectors 629 | // In short, where dist is pt distance to cyl axis: 630 | // dist = sin( pd to d ) * |pd| 631 | // distsq = dsq = (1 - cos^2( pd to d)) * |pd|^2 632 | // dsq = ( 1 - (pd * d)^2 / (|pd|^2 * |d|^2) ) * |pd|^2 633 | // dsq = pd * pd - dot * dot / lengthsq 634 | // where lengthsq is d*d or |d|^2 that is passed into this function 635 | 636 | // distance squared to the cylinder axis: 637 | 638 | dsq = (pdx * pdx + pdy * pdy + pdz * pdz) - dot * dot / lsq; 639 | 640 | if (dsq > rsq) 641 | return (-1.0f); 642 | else 643 | return (dsq); // return distance squared to axis 644 | } 645 | } 646 | 647 | } // namespace aeplanner 648 | -------------------------------------------------------------------------------- /aeplanner/src/aeplanner_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | int main(int argc, char** argv) 5 | { 6 | ros::init(argc, argv, "aeplanner"); 7 | ros::NodeHandle nh; 8 | 9 | aeplanner::AEPlanner aeplanner(nh); 10 | 11 | ros::spin(); 12 | return 0; 13 | } 14 | -------------------------------------------------------------------------------- /aeplanner/src/aeplanner_nodelet.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | PLUGINLIB_EXPORT_CLASS(aeplanner::AEPlannerNodelet, nodelet::Nodelet) 5 | 6 | namespace aeplanner 7 | { 8 | void AEPlannerNodelet::onInit() 9 | { 10 | NODELET_DEBUG("Initializing nodelet AEPlanner..."); 11 | 12 | ros::NodeHandle& nh = getMTNodeHandle(); // getNodeHandle(); 13 | aeplanner_ = new AEPlanner(nh); 14 | } 15 | AEPlannerNodelet::~AEPlannerNodelet() 16 | { 17 | delete aeplanner_; 18 | } 19 | } // namespace aeplanner 20 | -------------------------------------------------------------------------------- /aeplanner/src/aeplanner_viz.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace aeplanner { 4 | visualization_msgs::MarkerArray createRRTMarkerArray(RRTNode * root, double lambda){ 5 | int id = 0; 6 | visualization_msgs::MarkerArray marker_array; 7 | recurse(root, &marker_array, &id, lambda); 8 | 9 | return marker_array; 10 | } 11 | void recurse(RRTNode * node, visualization_msgs::MarkerArray * marker_array, int * id, double lambda){ 12 | for(std::vector::iterator child_it = node->children_.begin(); 13 | child_it != node->children_.end(); ++child_it){ 14 | RRTNode * child = (*child_it); 15 | if(child) recurse(child, marker_array, id, lambda); 16 | marker_array->markers.push_back(createEdgeMarker(child, (*id), "map", lambda)); // FIXME read frame id from config 17 | marker_array->markers.push_back(createNodeMarker(child, (*id)++, "map")); 18 | } 19 | } 20 | 21 | visualization_msgs::Marker createNodeMarker(RRTNode * node, int id, std::string frame_id){ 22 | visualization_msgs::Marker marker; 23 | marker.header.stamp = ros::Time::now(); 24 | marker.header.seq = id; 25 | marker.header.frame_id = frame_id; 26 | marker.id = id; 27 | marker.ns = "nodes"; 28 | marker.type = visualization_msgs::Marker::ARROW; 29 | marker.action = visualization_msgs::Marker::ADD; 30 | marker.pose.position.x = node->state_[0]; 31 | marker.pose.position.y = node->state_[1]; 32 | marker.pose.position.z = node->state_[2]; 33 | tf::Quaternion quat; 34 | quat.setEuler(0.0, 0.0, node->state_[3]); 35 | marker.pose.orientation.x = quat.x(); 36 | marker.pose.orientation.y = quat.y(); 37 | marker.pose.orientation.z = quat.z(); 38 | marker.pose.orientation.w = quat.w(); 39 | marker.scale.x = std::max(node->gain_ / 72.0, 0.05); 40 | marker.scale.y = 0.1; 41 | marker.scale.z = 0.1; 42 | marker.color.r = 167.0 / 255.0; 43 | marker.color.g = 167.0 / 255.0; 44 | marker.color.b = 0.0; 45 | marker.color.a = 1.0; 46 | marker.lifetime = ros::Duration(10.0); 47 | marker.frame_locked = false; 48 | 49 | return marker; 50 | } 51 | 52 | visualization_msgs::Marker createEdgeMarker(RRTNode * node, int id, std::string frame_id, double lambda){ 53 | visualization_msgs::Marker marker; 54 | marker.header.stamp = ros::Time::now(); 55 | marker.header.seq = id; 56 | marker.header.frame_id = frame_id; 57 | marker.id = id; 58 | marker.ns = "edges"; 59 | marker.type = visualization_msgs::Marker::ARROW; 60 | marker.action = visualization_msgs::Marker::ADD; 61 | marker.pose.position.x = node->parent_->state_[0]; 62 | marker.pose.position.y = node->parent_->state_[1]; 63 | marker.pose.position.z = node->parent_->state_[2]; 64 | Eigen::Quaternion q; 65 | Eigen::Vector3d init(1.0, 0.0, 0.0); 66 | Eigen::Vector3d dir(node->state_[0] - node->parent_->state_[0], 67 | node->state_[1] - node->parent_->state_[1], 68 | node->state_[2] - node->parent_->state_[2]); 69 | q.setFromTwoVectors(init, dir); 70 | q.normalize(); 71 | marker.pose.orientation.x = q.x(); 72 | marker.pose.orientation.y = q.y(); 73 | marker.pose.orientation.z = q.z(); 74 | marker.pose.orientation.w = q.w(); 75 | marker.scale.x = dir.norm(); 76 | marker.scale.y = 0.03; 77 | marker.scale.z = 0.03; 78 | marker.color.r = node->score(lambda) / 60.0; 79 | marker.color.g = 0.0; 80 | marker.color.b = 1.0; 81 | marker.color.a = 1.0; 82 | marker.lifetime = ros::Duration(10.0); 83 | marker.frame_locked = false; 84 | 85 | return marker; 86 | } 87 | 88 | } 89 | -------------------------------------------------------------------------------- /aeplanner/src/param.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | namespace aeplanner 6 | { 7 | Params readParams() 8 | { 9 | // FIXME namespaces 10 | Params params; 11 | std::string ns = ros::this_node::getNamespace(); 12 | params.hfov = 60; 13 | if (!ros::param::get(ns + "/camera/horizontal_fov", params.hfov)) { 14 | ROS_WARN_STREAM("No horizontal fov specified. Default: " << params.hfov); 15 | } 16 | params.vfov = 45; 17 | if (!ros::param::get(ns + "/camera/vertical_fov", params.vfov)) { 18 | ROS_WARN_STREAM("No vertical fov specified. Default: " << params.vfov); 19 | } 20 | params.dr = 0.1; 21 | if (!ros::param::get("/octomap_server/resolution", params.dr)) { 22 | ROS_WARN_STREAM("Could not read octomap resolution. Looking for /octomap_server/resolution."); 23 | ROS_WARN_STREAM("Using resolution specified by param file instead"); 24 | } 25 | else if (!ros::param::get(ns + "/raycast/dr", params.dr)) { 26 | ROS_WARN_STREAM("No dr specified. Default: " << params.dr); 27 | } 28 | params.dphi = 10; 29 | if (!ros::param::get(ns + "/raycast/dphi", params.dphi)) { 30 | ROS_WARN_STREAM("No dphi specified. Default: " << params.dphi); 31 | } 32 | params.dtheta = 10; 33 | if (!ros::param::get(ns + "/raycast/dtheta", params.dtheta)) { 34 | ROS_WARN_STREAM("No dtheta specified. Default: " << params.dtheta); 35 | } 36 | params.lambda = 0.5; 37 | if (!ros::param::get(ns + "/aep/gain/lambda", params.lambda)) { 38 | ROS_WARN_STREAM("No lambda specified. Default: " << params.lambda); 39 | } 40 | params.extension_range = 1.0; 41 | if (!ros::param::get(ns + "/aep/tree/extension_range", params.extension_range)) { 42 | ROS_WARN_STREAM("No extension_range specified. Default: " << params.extension_range); 43 | } 44 | params.max_sampling_radius = 10.0; 45 | if (!ros::param::get(ns + "/aep/tree/max_sampling_radius", params.max_sampling_radius)) { 46 | ROS_WARN_STREAM("No max_sampling_radius specified. Default: " << params.max_sampling_radius); 47 | } 48 | params.sigma_thresh = 0.2; 49 | if (!ros::param::get(ns + "/aep/gain/sigma_thresh", params.sigma_thresh)) { 50 | ROS_WARN_STREAM("No sigma_thresh specified. Default: " << params.sigma_thresh); 51 | } 52 | params.init_iterations = 15; 53 | if (!ros::param::get(ns + "/aep/tree/initial_iterations", params.init_iterations)) { 54 | ROS_WARN_STREAM("No init_iterations specified. Default: " << params.init_iterations); 55 | } 56 | params.r_max = 4.0; 57 | if (!ros::param::get(ns + "/aep/gain/r_max", params.r_max)) { 58 | ROS_WARN_STREAM("No r_max specified. Default: " << params.r_max); 59 | } 60 | params.r_min = 0.5; 61 | if (!ros::param::get(ns + "/aep/gain/r_min", params.r_min)) { 62 | ROS_WARN_STREAM("No r_min specified. Default: " << params.r_min); 63 | } 64 | if (!ros::param::get(ns + "/boundary/min", params.boundary_min)) { 65 | ROS_WARN_STREAM("No boundary/min specified."); 66 | } 67 | if (!ros::param::get(ns + "/boundary/max", params.boundary_max)) { 68 | ROS_WARN_STREAM("No boundary/max specified."); 69 | } 70 | params.bounding_radius; 71 | if (!ros::param::get(ns + "/system/bbx/r", params.bounding_radius)) { 72 | ROS_WARN_STREAM("No /system/bbx/r specified. Default: " << params.bounding_radius); 73 | } 74 | params.cutoff_iterations = 200; 75 | if (!ros::param::get(ns + "/aep/tree/cutoff_iterations", params.cutoff_iterations)) { 76 | ROS_WARN_STREAM("No /aep/tree/cutoff_iterations specified. Default: " << params.cutoff_iterations); 77 | } 78 | params.zero_gain = 0.0; 79 | if (!ros::param::get(ns + "/aep/gain/zero", params.zero_gain)) { 80 | ROS_WARN_STREAM("No /aep/gain/zero specified. Default: " << params.zero_gain); 81 | } 82 | params.d_overshoot_ = 0.5; 83 | if (!ros::param::get(ns + "/system/bbx/overshoot", params.d_overshoot_)) { 84 | ROS_WARN_STREAM("No /system/bbx/overshoot specified. Default: " << params.d_overshoot_); 85 | } 86 | params.world_frame = "world"; 87 | if (!ros::param::get(ns + "/world_frame", params.world_frame)) { 88 | ROS_WARN_STREAM("No /world_frame specified. Default: " << params.world_frame); 89 | } 90 | params.robot_frame = "base_link"; 91 | if (!ros::param::get(ns + "/robot_frame", params.robot_frame)) { 92 | ROS_WARN_STREAM("No /robot_frame specified. Default: " << params.robot_frame); 93 | } 94 | params.visualize_tree = false; 95 | if (!ros::param::get(ns + "/visualize_tree", params.visualize_tree)) { 96 | ROS_WARN_STREAM("No /visualize_tree specified. Default: " << params.visualize_tree); 97 | } 98 | params.visualize_rays = false; 99 | if (!ros::param::get(ns + "/visualize_rays", params.visualize_rays)) { 100 | ROS_WARN_STREAM("No /visualize_rays specified. Default: " << params.visualize_rays); 101 | } 102 | params.visualize_exploration_area = false; 103 | if (!ros::param::get(ns + "/visualize_exploration_area", params.visualize_exploration_area)) { 104 | ROS_WARN_STREAM("No /visualize_exploration_area specified. Default: " << params.visualize_exploration_area); 105 | } 106 | 107 | return params; 108 | } 109 | } 110 | -------------------------------------------------------------------------------- /aeplanner/srv/Reevaluate.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Point[] point 2 | --- 3 | float64[] gain 4 | float64[] yaw 5 | -------------------------------------------------------------------------------- /aeplanner_evaluation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(aeplanner_evaluation) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | std_msgs 13 | octomap_msgs 14 | message_generation 15 | ) 16 | find_package(octomap REQUIRED) 17 | 18 | ## System dependencies are found with CMake's conventions 19 | # find_package(Boost REQUIRED COMPONENTS system) 20 | 21 | 22 | ## Uncomment this if the package has a setup.py. This macro ensures 23 | ## modules and global scripts declared therein get installed 24 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 25 | # catkin_python_setup() 26 | 27 | ################################################ 28 | ## Declare ROS messages, services and actions ## 29 | ################################################ 30 | 31 | ## To declare and build messages, services or actions from within this 32 | ## package, follow these steps: 33 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 34 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 35 | ## * In the file package.xml: 36 | ## * add a build_depend tag for "message_generation" 37 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 38 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 39 | ## but can be declared for certainty nonetheless: 40 | ## * add a run_depend tag for "message_runtime" 41 | ## * In this file (CMakeLists.txt): 42 | ## * add "message_generation" and every package in MSG_DEP_SET to 43 | ## find_package(catkin REQUIRED COMPONENTS ...) 44 | ## * add "message_runtime" and every package in MSG_DEP_SET to 45 | ## catkin_package(CATKIN_DEPENDS ...) 46 | ## * uncomment the add_*_files sections below as needed 47 | ## and list every .msg/.srv/.action file to be processed 48 | ## * uncomment the generate_messages entry below 49 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 50 | 51 | ## Generate messages in the 'msg' folder 52 | # add_message_files( 53 | # FILES 54 | # Message1.msg 55 | # Message2.msg 56 | # ) 57 | 58 | ## Generate services in the 'srv' folder 59 | add_service_files( 60 | FILES 61 | Coverage.srv 62 | ) 63 | 64 | ## Generate actions in the 'action' folder 65 | # add_action_files( 66 | # FILES 67 | # Action1.action 68 | # Action2.action 69 | # ) 70 | 71 | ## Generate added messages and services with any dependencies listed here 72 | generate_messages( 73 | DEPENDENCIES 74 | std_msgs # Or other packages containing msgs 75 | ) 76 | 77 | ################################################ 78 | ## Declare ROS dynamic reconfigure parameters ## 79 | ################################################ 80 | 81 | ## To declare and build dynamic reconfigure parameters within this 82 | ## package, follow these steps: 83 | ## * In the file package.xml: 84 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 85 | ## * In this file (CMakeLists.txt): 86 | ## * add "dynamic_reconfigure" to 87 | ## find_package(catkin REQUIRED COMPONENTS ...) 88 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 89 | ## and list every .cfg file to be processed 90 | 91 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 92 | # generate_dynamic_reconfigure_options( 93 | # cfg/DynReconf1.cfg 94 | # cfg/DynReconf2.cfg 95 | # ) 96 | 97 | ################################### 98 | ## catkin specific configuration ## 99 | ################################### 100 | ## The catkin_package macro generates cmake config files for your package 101 | ## Declare things to be passed to dependent projects 102 | ## INCLUDE_DIRS: uncomment this if your package contains header files 103 | ## LIBRARIES: libraries you create in this project that dependent projects also need 104 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 105 | ## DEPENDS: system dependencies of this project that dependent projects also need 106 | catkin_package( 107 | # INCLUDE_DIRS include 108 | # LIBRARIES aeplanner_evaluation 109 | CATKIN_DEPENDS message_runtime 110 | # DEPENDS system_lib 111 | ) 112 | 113 | ########### 114 | ## Build ## 115 | ########### 116 | 117 | ## Specify additional locations of header files 118 | ## Your package locations should be listed before other locations 119 | include_directories( 120 | # include 121 | ${catkin_INCLUDE_DIRS} 122 | ) 123 | 124 | ## Declare a C++ library 125 | # add_library(${PROJECT_NAME} 126 | # src/${PROJECT_NAME}/aeplanner_evaluation.cpp 127 | # ) 128 | 129 | ## Add cmake target dependencies of the library 130 | ## as an example, code may need to be generated before libraries 131 | ## either from message generation or dynamic reconfigure 132 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 133 | 134 | ## Declare a C++ executable 135 | ## With catkin_make all packages are built within a single CMake context 136 | ## The recommended prefix ensures that target names across packages don't collide 137 | add_executable(coverage_evaluation src/evaluation.cpp) 138 | 139 | ## Rename C++ executable without prefix 140 | ## The above recommended prefix causes long target names, the following renames the 141 | ## target back to the shorter version for ease of user use 142 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 143 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 144 | 145 | ## Add cmake target dependencies of the executable 146 | ## same as for the library above 147 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 148 | 149 | target_link_libraries(coverage_evaluation ${catkin_LIBRARIES} ${OCTOMAP_LIBRARIES}) 150 | add_dependencies(coverage_evaluation ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 151 | 152 | ## Specify libraries to link a library or executable target against 153 | # target_link_libraries(${PROJECT_NAME}_node 154 | # ${catkin_LIBRARIES} 155 | # ) 156 | 157 | ############# 158 | ## Install ## 159 | ############# 160 | 161 | # all install targets should use catkin DESTINATION variables 162 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 163 | 164 | ## Mark executable scripts (Python etc.) for installation 165 | ## in contrast to setup.py, you can choose the destination 166 | # install(PROGRAMS 167 | # scripts/my_python_script 168 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 169 | # ) 170 | 171 | ## Mark executables and/or libraries for installation 172 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 173 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 175 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 176 | # ) 177 | 178 | ## Mark cpp header files for installation 179 | # install(DIRECTORY include/${PROJECT_NAME}/ 180 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 181 | # FILES_MATCHING PATTERN "*.h" 182 | # PATTERN ".svn" EXCLUDE 183 | # ) 184 | 185 | ## Mark other files for installation (e.g. launch and bag files, etc.) 186 | # install(FILES 187 | # # myfile1 188 | # # myfile2 189 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 190 | # ) 191 | 192 | ############# 193 | ## Testing ## 194 | ############# 195 | 196 | ## Add gtest based cpp test target and link libraries 197 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_aeplanner_evaluation.cpp) 198 | # if(TARGET ${PROJECT_NAME}-test) 199 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 200 | # endif() 201 | 202 | ## Add folders to be run by python nosetests 203 | # catkin_add_nosetests(test) 204 | -------------------------------------------------------------------------------- /aeplanner_evaluation/config/evaluation.yaml: -------------------------------------------------------------------------------- 1 | # volume: 600 2 | volume: 450 3 | # volume: 2080 4 | -------------------------------------------------------------------------------- /aeplanner_evaluation/launch/coverage_evaluation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /aeplanner_evaluation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | aeplanner_evaluation 4 | 0.0.0 5 | The aeplanner_evaluation package 6 | 7 | 8 | 9 | 10 | mseln 11 | 12 | 13 | 14 | 15 | 16 | TODO 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 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | octomap 53 | octomap_msgs 54 | roscpp 55 | message_generation 56 | 57 | octomap 58 | octomap_msgs 59 | roscpp 60 | message_runtime 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /aeplanner_evaluation/src/evaluation.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | 7 | void octomapCallback(const octomap_msgs::Octomap& msg); 8 | 9 | class CoverageEvaluator{ 10 | private: 11 | ros::NodeHandle nh_; 12 | ros::Subscriber octomap_sub_; 13 | ros::ServiceServer coverage_srv_; 14 | 15 | double total_volume_; 16 | double coverage_, free_, occupied_, unmapped_; 17 | public: 18 | CoverageEvaluator(ros::NodeHandle& nh) : nh_(nh), 19 | octomap_sub_(nh_.subscribe("octomap", 10, &CoverageEvaluator::octomapCallback2, this)), 20 | coverage_srv_(nh.advertiseService("get_coverage", &CoverageEvaluator::coverageSrvCallback, this)){ 21 | if (!nh.getParam("volume", total_volume_)) { 22 | ROS_ERROR("No volume found..."); 23 | } 24 | } 25 | 26 | void octomapCallback(const octomap_msgs::Octomap& msg); 27 | void octomapCallback2(const octomap_msgs::Octomap& msg); 28 | bool coverageSrvCallback(aeplanner_evaluation::Coverage::Request& request, aeplanner_evaluation::Coverage::Response& response); 29 | 30 | }; 31 | 32 | int main(int argc, char** argv) { 33 | ros::init(argc, argv, "coverage_evaluation"); 34 | ros::NodeHandle nh; 35 | 36 | CoverageEvaluator ce_(nh); 37 | 38 | ros::spin(); 39 | } 40 | 41 | void CoverageEvaluator::octomapCallback(const octomap_msgs::Octomap& msg){ 42 | octomap::AbstractOcTree* aot = octomap_msgs::msgToMap(msg); 43 | octomap::OcTree * ot = (octomap::OcTree*)aot; 44 | double volume = 0; 45 | 46 | for(octomap::OcTree::leaf_iterator it = ot->begin_leafs(); 47 | it != ot->end_leafs(); ++it){ 48 | // if(!ot->isNodeOccupied(*it)){ 49 | double l = it.getSize(); 50 | volume += l*l*l; 51 | // } 52 | } 53 | 54 | coverage_ = volume; 55 | } 56 | 57 | void CoverageEvaluator::octomapCallback2(const octomap_msgs::Octomap& msg){ 58 | octomap::AbstractOcTree* aot = octomap_msgs::msgToMap(msg); 59 | octomap::OcTree * ot = (octomap::OcTree*)aot; 60 | 61 | double occupied = 0; 62 | double free = 0; 63 | double unmapped = 0; 64 | 65 | double min[3] = {-10 , -15, 0}; 66 | double max[3] = { 10, 15, 3}; 67 | double res = 0.2; 68 | double dV = res*res*res; 69 | 70 | for(double x = min[0]; x < max[0]-res/2; x+=res){ 71 | for(double y = min[1]; y < max[1]-res/2; y+=res){ 72 | for(double z = min[2]; z < max[2]-res/2; z+=res){ 73 | octomap::OcTreeNode* result = ot->search(x+res/2,y+res/2,z+res/2); 74 | if (!result) 75 | unmapped+=dV; 76 | else if(result->getLogOdds() > 0) 77 | occupied+=dV; 78 | else 79 | free+=dV; 80 | } 81 | } 82 | } 83 | 84 | coverage_ = free+occupied; 85 | free_ = free; 86 | occupied_ = occupied; 87 | unmapped_ = unmapped; 88 | } 89 | 90 | bool CoverageEvaluator::coverageSrvCallback(aeplanner_evaluation::Coverage::Request& request, aeplanner_evaluation::Coverage::Response& response){ 91 | response.coverage = coverage_; 92 | response.free = free_; 93 | response.occupied = occupied_; 94 | response.unmapped = unmapped_; 95 | return true; 96 | } 97 | -------------------------------------------------------------------------------- /aeplanner_evaluation/srv/Coverage.srv: -------------------------------------------------------------------------------- 1 | --- 2 | float64 coverage 3 | float64 free 4 | float64 occupied 5 | float64 unmapped 6 | -------------------------------------------------------------------------------- /kdtree/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(kdtree) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | ############# 8 | # LIBRARIES # 9 | ############# 10 | cs_add_library(${PROJECT_NAME} 11 | src/kdtree.c 12 | ) 13 | 14 | include_directories(include ${catkin_INCLUDE_DIRS}) 15 | 16 | catkin_package( 17 | INCLUDE_DIRS include ${catkin_INCLUDE_DIRS} 18 | ) 19 | 20 | ########## 21 | # EXPORT # 22 | ########## 23 | cs_install() 24 | cs_export() 25 | -------------------------------------------------------------------------------- /kdtree/include/kdtree/kdtree.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of ``kdtree'', a library for working with kd-trees. 3 | Copyright (C) 2007-2009 John Tsiombikas 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 3. The name of the author may not be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED 17 | WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 18 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO 19 | EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 21 | OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING 24 | IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 25 | OF SUCH DAMAGE. 26 | */ 27 | #ifndef _KDTREE_H_ 28 | #define _KDTREE_H_ 29 | 30 | #ifdef __cplusplus 31 | extern "C" { 32 | #endif 33 | 34 | struct kdtree; 35 | struct kdres; 36 | 37 | 38 | /* create a kd-tree for "k"-dimensional data */ 39 | struct kdtree *kd_create(int k); 40 | 41 | /* free the struct kdtree */ 42 | void kd_free(struct kdtree *tree); 43 | 44 | /* remove all the elements from the tree */ 45 | void kd_clear(struct kdtree *tree); 46 | 47 | /* if called with non-null 2nd argument, the function provided 48 | * will be called on data pointers (see kd_insert) when nodes 49 | * are to be removed from the tree. 50 | */ 51 | void kd_data_destructor(struct kdtree *tree, void (*destr)(void*)); 52 | 53 | /* insert a node, specifying its position, and optional data */ 54 | int kd_insert(struct kdtree *tree, const double *pos, void *data); 55 | int kd_insertf(struct kdtree *tree, const float *pos, void *data); 56 | int kd_insert3(struct kdtree *tree, double x, double y, double z, void *data); 57 | int kd_insert3f(struct kdtree *tree, float x, float y, float z, void *data); 58 | 59 | /* Find one of the nearest nodes from the specified point. 60 | * 61 | * This function returns a pointer to a result set with at most one element. 62 | */ 63 | struct kdres *kd_nearest(struct kdtree *tree, const double *pos); 64 | struct kdres *kd_nearestf(struct kdtree *tree, const float *pos); 65 | struct kdres *kd_nearest3(struct kdtree *tree, double x, double y, double z); 66 | struct kdres *kd_nearest3f(struct kdtree *tree, float x, float y, float z); 67 | 68 | /* Find any nearest nodes from the specified point within a range. 69 | * 70 | * This function returns a pointer to a result set, which can be manipulated 71 | * by the kd_res_* functions. 72 | * The returned pointer can be null as an indication of an error. Otherwise 73 | * a valid result set is always returned which may contain 0 or more elements. 74 | * The result set must be deallocated with kd_res_free, after use. 75 | */ 76 | struct kdres *kd_nearest_range(struct kdtree *tree, const double *pos, double range); 77 | struct kdres *kd_nearest_rangef(struct kdtree *tree, const float *pos, float range); 78 | struct kdres *kd_nearest_range3(struct kdtree *tree, double x, double y, double z, double range); 79 | struct kdres *kd_nearest_range3f(struct kdtree *tree, float x, float y, float z, float range); 80 | 81 | /* frees a result set returned by kd_nearest_range() */ 82 | void kd_res_free(struct kdres *set); 83 | 84 | /* returns the size of the result set (in elements) */ 85 | int kd_res_size(struct kdres *set); 86 | 87 | /* rewinds the result set iterator */ 88 | void kd_res_rewind(struct kdres *set); 89 | 90 | /* returns non-zero if the set iterator reached the end after the last element */ 91 | int kd_res_end(struct kdres *set); 92 | 93 | /* advances the result set iterator, returns non-zero on success, zero if 94 | * there are no more elements in the result set. 95 | */ 96 | int kd_res_next(struct kdres *set); 97 | 98 | /* returns the data pointer (can be null) of the current result set item 99 | * and optionally sets its position to the pointers(s) if not null. 100 | */ 101 | void *kd_res_item(struct kdres *set, double *pos); 102 | void *kd_res_itemf(struct kdres *set, float *pos); 103 | void *kd_res_item3(struct kdres *set, double *x, double *y, double *z); 104 | void *kd_res_item3f(struct kdres *set, float *x, float *y, float *z); 105 | 106 | /* equivalent to kd_res_item(set, 0) */ 107 | void *kd_res_item_data(struct kdres *set); 108 | 109 | 110 | #ifdef __cplusplus 111 | } 112 | #endif 113 | 114 | #endif /* _KDTREE_H_ */ 115 | -------------------------------------------------------------------------------- /kdtree/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kdtree 4 | 0.0.0 5 | 6 | kdtree functionality 7 | 8 | Andreas Bircher 9 | 10 | TBD 11 | 12 | Andreas Bircher 13 | John Tsiombikas 14 | Tamas Nepusz 15 | 16 | catkin 17 | catkin_simple 18 | 19 | 20 | -------------------------------------------------------------------------------- /kdtree/src/kdtree.c: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of ``kdtree'', a library for working with kd-trees. 3 | Copyright (C) 2007-2009 John Tsiombikas 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 3. The name of the author may not be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED 17 | WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 18 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO 19 | EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 21 | OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING 24 | IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 25 | OF SUCH DAMAGE. 26 | */ 27 | /* single nearest neighbor search written by Tamas Nepusz */ 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include "kdtree/kdtree.h" 33 | 34 | #if defined(WIN32) || defined(__WIN32__) 35 | #include 36 | #endif 37 | 38 | #ifdef USE_LIST_NODE_ALLOCATOR 39 | 40 | #ifndef NO_PTHREADS 41 | #include 42 | #else 43 | 44 | #ifndef I_WANT_THREAD_BUGS 45 | #error "You are compiling with the fast list node allocator, with pthreads disabled! This WILL break if used from multiple threads." 46 | #endif /* I want thread bugs */ 47 | 48 | #endif /* pthread support */ 49 | #endif /* use list node allocator */ 50 | 51 | struct kdhyperrect { 52 | int dim; 53 | double *min, *max; /* minimum/maximum coords */ 54 | }; 55 | 56 | struct kdnode { 57 | double *pos; 58 | int dir; 59 | void *data; 60 | 61 | struct kdnode *left, *right; /* negative/positive side */ 62 | }; 63 | 64 | struct res_node { 65 | struct kdnode *item; 66 | double dist_sq; 67 | struct res_node *next; 68 | }; 69 | 70 | struct kdtree { 71 | int dim; 72 | struct kdnode *root; 73 | struct kdhyperrect *rect; 74 | void (*destr)(void*); 75 | }; 76 | 77 | struct kdres { 78 | struct kdtree *tree; 79 | struct res_node *rlist, *riter; 80 | int size; 81 | }; 82 | 83 | #define SQ(x) ((x) * (x)) 84 | 85 | 86 | static void clear_rec(struct kdnode *node, void (*destr)(void*)); 87 | static int insert_rec(struct kdnode **node, const double *pos, void *data, int dir, int dim); 88 | static int rlist_insert(struct res_node *list, struct kdnode *item, double dist_sq); 89 | static void clear_results(struct kdres *set); 90 | 91 | static struct kdhyperrect* hyperrect_create(int dim, const double *min, const double *max); 92 | static void hyperrect_free(struct kdhyperrect *rect); 93 | static struct kdhyperrect* hyperrect_duplicate(const struct kdhyperrect *rect); 94 | static void hyperrect_extend(struct kdhyperrect *rect, const double *pos); 95 | static double hyperrect_dist_sq(struct kdhyperrect *rect, const double *pos); 96 | 97 | #ifdef USE_LIST_NODE_ALLOCATOR 98 | static struct res_node *alloc_resnode(void); 99 | static void free_resnode(struct res_node*); 100 | #else 101 | #define alloc_resnode() malloc(sizeof(struct res_node)) 102 | #define free_resnode(n) free(n) 103 | #endif 104 | 105 | 106 | 107 | struct kdtree *kd_create(int k) 108 | { 109 | struct kdtree *tree; 110 | 111 | if(!(tree = malloc(sizeof *tree))) { 112 | return 0; 113 | } 114 | 115 | tree->dim = k; 116 | tree->root = 0; 117 | tree->destr = 0; 118 | tree->rect = 0; 119 | 120 | return tree; 121 | } 122 | 123 | void kd_free(struct kdtree *tree) 124 | { 125 | if(tree) { 126 | kd_clear(tree); 127 | free(tree); 128 | } 129 | } 130 | 131 | static void clear_rec(struct kdnode *node, void (*destr)(void*)) 132 | { 133 | if(!node) return; 134 | 135 | clear_rec(node->left, destr); 136 | clear_rec(node->right, destr); 137 | 138 | if(destr) { 139 | destr(node->data); 140 | } 141 | free(node->pos); 142 | free(node); 143 | } 144 | 145 | void kd_clear(struct kdtree *tree) 146 | { 147 | clear_rec(tree->root, tree->destr); 148 | tree->root = 0; 149 | 150 | if (tree->rect) { 151 | hyperrect_free(tree->rect); 152 | tree->rect = 0; 153 | } 154 | } 155 | 156 | void kd_data_destructor(struct kdtree *tree, void (*destr)(void*)) 157 | { 158 | tree->destr = destr; 159 | } 160 | 161 | 162 | static int insert_rec(struct kdnode **nptr, const double *pos, void *data, int dir, int dim) 163 | { 164 | int new_dir; 165 | struct kdnode *node; 166 | 167 | if(!*nptr) { 168 | if(!(node = malloc(sizeof *node))) { 169 | return -1; 170 | } 171 | if(!(node->pos = malloc(dim * sizeof *node->pos))) { 172 | free(node); 173 | return -1; 174 | } 175 | memcpy(node->pos, pos, dim * sizeof *node->pos); 176 | node->data = data; 177 | node->dir = dir; 178 | node->left = node->right = 0; 179 | *nptr = node; 180 | return 0; 181 | } 182 | 183 | node = *nptr; 184 | new_dir = (node->dir + 1) % dim; 185 | if(pos[node->dir] < node->pos[node->dir]) { 186 | return insert_rec(&(*nptr)->left, pos, data, new_dir, dim); 187 | } 188 | return insert_rec(&(*nptr)->right, pos, data, new_dir, dim); 189 | } 190 | 191 | int kd_insert(struct kdtree *tree, const double *pos, void *data) 192 | { 193 | if (insert_rec(&tree->root, pos, data, 0, tree->dim)) { 194 | return -1; 195 | } 196 | 197 | if (tree->rect == 0) { 198 | tree->rect = hyperrect_create(tree->dim, pos, pos); 199 | } else { 200 | hyperrect_extend(tree->rect, pos); 201 | } 202 | 203 | return 0; 204 | } 205 | 206 | int kd_insertf(struct kdtree *tree, const float *pos, void *data) 207 | { 208 | static double sbuf[16]; 209 | double *bptr, *buf = 0; 210 | int res, dim = tree->dim; 211 | 212 | if(dim > 16) { 213 | #ifndef NO_ALLOCA 214 | if(dim <= 256) 215 | bptr = buf = alloca(dim * sizeof *bptr); 216 | else 217 | #endif 218 | if(!(bptr = buf = malloc(dim * sizeof *bptr))) { 219 | return -1; 220 | } 221 | } else { 222 | bptr = sbuf; 223 | } 224 | 225 | while(dim-- > 0) { 226 | *bptr++ = *pos++; 227 | } 228 | 229 | res = kd_insert(tree, buf, data); 230 | #ifndef NO_ALLOCA 231 | if(tree->dim > 256) 232 | #else 233 | if(tree->dim > 16) 234 | #endif 235 | free(buf); 236 | return res; 237 | } 238 | 239 | int kd_insert3(struct kdtree *tree, double x, double y, double z, void *data) 240 | { 241 | double buf[3]; 242 | buf[0] = x; 243 | buf[1] = y; 244 | buf[2] = z; 245 | return kd_insert(tree, buf, data); 246 | } 247 | 248 | int kd_insert3f(struct kdtree *tree, float x, float y, float z, void *data) 249 | { 250 | double buf[3]; 251 | buf[0] = x; 252 | buf[1] = y; 253 | buf[2] = z; 254 | return kd_insert(tree, buf, data); 255 | } 256 | 257 | static int find_nearest(struct kdnode *node, const double *pos, double range, struct res_node *list, int ordered, int dim) 258 | { 259 | double dist_sq, dx; 260 | int i, ret, added_res = 0; 261 | 262 | if(!node) return 0; 263 | 264 | dist_sq = 0; 265 | for(i=0; ipos[i] - pos[i]); 267 | } 268 | if(dist_sq <= SQ(range)) { 269 | if(rlist_insert(list, node, ordered ? dist_sq : -1.0) == -1) { 270 | return -1; 271 | } 272 | added_res = 1; 273 | } 274 | 275 | dx = pos[node->dir] - node->pos[node->dir]; 276 | 277 | ret = find_nearest(dx <= 0.0 ? node->left : node->right, pos, range, list, ordered, dim); 278 | if(ret >= 0 && fabs(dx) < range) { 279 | added_res += ret; 280 | ret = find_nearest(dx <= 0.0 ? node->right : node->left, pos, range, list, ordered, dim); 281 | } 282 | if(ret == -1) { 283 | return -1; 284 | } 285 | added_res += ret; 286 | 287 | return added_res; 288 | } 289 | 290 | static void kd_nearest_i(struct kdnode *node, const double *pos, struct kdnode **result, double *result_dist_sq, struct kdhyperrect* rect) 291 | { 292 | int dir = node->dir; 293 | int i, side; 294 | double dummy, dist_sq; 295 | struct kdnode *nearer_subtree, *farther_subtree; 296 | double *nearer_hyperrect_coord, *farther_hyperrect_coord; 297 | 298 | /* Decide whether to go left or right in the tree */ 299 | dummy = pos[dir] - node->pos[dir]; 300 | if (dummy <= 0) { 301 | nearer_subtree = node->left; 302 | farther_subtree = node->right; 303 | nearer_hyperrect_coord = rect->max + dir; 304 | farther_hyperrect_coord = rect->min + dir; 305 | side = 0; 306 | } else { 307 | nearer_subtree = node->right; 308 | farther_subtree = node->left; 309 | nearer_hyperrect_coord = rect->min + dir; 310 | farther_hyperrect_coord = rect->max + dir; 311 | side = 1; 312 | } 313 | 314 | if (nearer_subtree) { 315 | /* Slice the hyperrect to get the hyperrect of the nearer subtree */ 316 | dummy = *nearer_hyperrect_coord; 317 | *nearer_hyperrect_coord = node->pos[dir]; 318 | /* Recurse down into nearer subtree */ 319 | kd_nearest_i(nearer_subtree, pos, result, result_dist_sq, rect); 320 | /* Undo the slice */ 321 | *nearer_hyperrect_coord = dummy; 322 | } 323 | 324 | /* Check the distance of the point at the current node, compare it 325 | * with our best so far */ 326 | dist_sq = 0; 327 | for(i=0; i < rect->dim; i++) { 328 | dist_sq += SQ(node->pos[i] - pos[i]); 329 | } 330 | if (dist_sq < *result_dist_sq) { 331 | *result = node; 332 | *result_dist_sq = dist_sq; 333 | } 334 | 335 | if (farther_subtree) { 336 | /* Get the hyperrect of the farther subtree */ 337 | dummy = *farther_hyperrect_coord; 338 | *farther_hyperrect_coord = node->pos[dir]; 339 | /* Check if we have to recurse down by calculating the closest 340 | * point of the hyperrect and see if it's closer than our 341 | * minimum distance in result_dist_sq. */ 342 | if (hyperrect_dist_sq(rect, pos) < *result_dist_sq) { 343 | /* Recurse down into farther subtree */ 344 | kd_nearest_i(farther_subtree, pos, result, result_dist_sq, rect); 345 | } 346 | /* Undo the slice on the hyperrect */ 347 | *farther_hyperrect_coord = dummy; 348 | } 349 | } 350 | 351 | struct kdres *kd_nearest(struct kdtree *kd, const double *pos) 352 | { 353 | struct kdhyperrect *rect; 354 | struct kdnode *result; 355 | struct kdres *rset; 356 | double dist_sq; 357 | int i; 358 | 359 | if (!kd) return 0; 360 | if (!kd->rect) return 0; 361 | 362 | /* Allocate result set */ 363 | if(!(rset = malloc(sizeof *rset))) { 364 | return 0; 365 | } 366 | if(!(rset->rlist = alloc_resnode())) { 367 | free(rset); 368 | return 0; 369 | } 370 | rset->rlist->next = 0; 371 | rset->tree = kd; 372 | 373 | /* Duplicate the bounding hyperrectangle, we will work on the copy */ 374 | if (!(rect = hyperrect_duplicate(kd->rect))) { 375 | kd_res_free(rset); 376 | return 0; 377 | } 378 | 379 | /* Our first guesstimate is the root node */ 380 | result = kd->root; 381 | dist_sq = 0; 382 | for (i = 0; i < kd->dim; i++) 383 | dist_sq += SQ(result->pos[i] - pos[i]); 384 | 385 | /* Search for the nearest neighbour recursively */ 386 | kd_nearest_i(kd->root, pos, &result, &dist_sq, rect); 387 | 388 | /* Free the copy of the hyperrect */ 389 | hyperrect_free(rect); 390 | 391 | /* Store the result */ 392 | if (result) { 393 | if (rlist_insert(rset->rlist, result, -1.0) == -1) { 394 | kd_res_free(rset); 395 | return 0; 396 | } 397 | rset->size = 1; 398 | kd_res_rewind(rset); 399 | return rset; 400 | } else { 401 | kd_res_free(rset); 402 | return 0; 403 | } 404 | } 405 | 406 | struct kdres *kd_nearestf(struct kdtree *tree, const float *pos) 407 | { 408 | static double sbuf[16]; 409 | double *bptr, *buf = 0; 410 | int dim = tree->dim; 411 | struct kdres *res; 412 | 413 | if(dim > 16) { 414 | #ifndef NO_ALLOCA 415 | if(dim <= 256) 416 | bptr = buf = alloca(dim * sizeof *bptr); 417 | else 418 | #endif 419 | if(!(bptr = buf = malloc(dim * sizeof *bptr))) { 420 | return 0; 421 | } 422 | } else { 423 | bptr = sbuf; 424 | } 425 | 426 | while(dim-- > 0) { 427 | *bptr++ = *pos++; 428 | } 429 | 430 | res = kd_nearest(tree, buf); 431 | #ifndef NO_ALLOCA 432 | if(tree->dim > 256) 433 | #else 434 | if(tree->dim > 16) 435 | #endif 436 | free(buf); 437 | return res; 438 | } 439 | 440 | struct kdres *kd_nearest3(struct kdtree *tree, double x, double y, double z) 441 | { 442 | double pos[3]; 443 | pos[0] = x; 444 | pos[1] = y; 445 | pos[2] = z; 446 | return kd_nearest(tree, pos); 447 | } 448 | 449 | struct kdres *kd_nearest3f(struct kdtree *tree, float x, float y, float z) 450 | { 451 | double pos[3]; 452 | pos[0] = x; 453 | pos[1] = y; 454 | pos[2] = z; 455 | return kd_nearest(tree, pos); 456 | } 457 | 458 | struct kdres *kd_nearest_range(struct kdtree *kd, const double *pos, double range) 459 | { 460 | int ret; 461 | struct kdres *rset; 462 | 463 | if(!(rset = malloc(sizeof *rset))) { 464 | return 0; 465 | } 466 | if(!(rset->rlist = alloc_resnode())) { 467 | free(rset); 468 | return 0; 469 | } 470 | rset->rlist->next = 0; 471 | rset->tree = kd; 472 | 473 | if((ret = find_nearest(kd->root, pos, range, rset->rlist, 0, kd->dim)) == -1) { 474 | kd_res_free(rset); 475 | return 0; 476 | } 477 | rset->size = ret; 478 | kd_res_rewind(rset); 479 | return rset; 480 | } 481 | 482 | struct kdres *kd_nearest_rangef(struct kdtree *kd, const float *pos, float range) 483 | { 484 | static double sbuf[16]; 485 | double *bptr, *buf = 0; 486 | int dim = kd->dim; 487 | struct kdres *res; 488 | 489 | if(dim > 16) { 490 | #ifndef NO_ALLOCA 491 | if(dim <= 256) 492 | bptr = buf = alloca(dim * sizeof *bptr); 493 | else 494 | #endif 495 | if(!(bptr = buf = malloc(dim * sizeof *bptr))) { 496 | return 0; 497 | } 498 | } else { 499 | bptr = sbuf; 500 | } 501 | 502 | while(dim-- > 0) { 503 | *bptr++ = *pos++; 504 | } 505 | 506 | res = kd_nearest_range(kd, buf, range); 507 | #ifndef NO_ALLOCA 508 | if(kd->dim > 256) 509 | #else 510 | if(kd->dim > 16) 511 | #endif 512 | free(buf); 513 | return res; 514 | } 515 | 516 | struct kdres *kd_nearest_range3(struct kdtree *tree, double x, double y, double z, double range) 517 | { 518 | double buf[3]; 519 | buf[0] = x; 520 | buf[1] = y; 521 | buf[2] = z; 522 | return kd_nearest_range(tree, buf, range); 523 | } 524 | 525 | struct kdres *kd_nearest_range3f(struct kdtree *tree, float x, float y, float z, float range) 526 | { 527 | double buf[3]; 528 | buf[0] = x; 529 | buf[1] = y; 530 | buf[2] = z; 531 | return kd_nearest_range(tree, buf, range); 532 | } 533 | 534 | void kd_res_free(struct kdres *rset) 535 | { 536 | clear_results(rset); 537 | free_resnode(rset->rlist); 538 | free(rset); 539 | } 540 | 541 | int kd_res_size(struct kdres *set) 542 | { 543 | return (set->size); 544 | } 545 | 546 | void kd_res_rewind(struct kdres *rset) 547 | { 548 | rset->riter = rset->rlist->next; 549 | } 550 | 551 | int kd_res_end(struct kdres *rset) 552 | { 553 | return rset->riter == 0; 554 | } 555 | 556 | int kd_res_next(struct kdres *rset) 557 | { 558 | rset->riter = rset->riter->next; 559 | return rset->riter != 0; 560 | } 561 | 562 | void *kd_res_item(struct kdres *rset, double *pos) 563 | { 564 | if(rset->riter) { 565 | if(pos) { 566 | memcpy(pos, rset->riter->item->pos, rset->tree->dim * sizeof *pos); 567 | } 568 | return rset->riter->item->data; 569 | } 570 | return 0; 571 | } 572 | 573 | void *kd_res_itemf(struct kdres *rset, float *pos) 574 | { 575 | if(rset->riter) { 576 | if(pos) { 577 | int i; 578 | for(i=0; itree->dim; i++) { 579 | pos[i] = rset->riter->item->pos[i]; 580 | } 581 | } 582 | return rset->riter->item->data; 583 | } 584 | return 0; 585 | } 586 | 587 | void *kd_res_item3(struct kdres *rset, double *x, double *y, double *z) 588 | { 589 | if(rset->riter) { 590 | if(*x) *x = rset->riter->item->pos[0]; 591 | if(*y) *y = rset->riter->item->pos[1]; 592 | if(*z) *z = rset->riter->item->pos[2]; 593 | } 594 | return 0; 595 | } 596 | 597 | void *kd_res_item3f(struct kdres *rset, float *x, float *y, float *z) 598 | { 599 | if(rset->riter) { 600 | if(*x) *x = rset->riter->item->pos[0]; 601 | if(*y) *y = rset->riter->item->pos[1]; 602 | if(*z) *z = rset->riter->item->pos[2]; 603 | } 604 | return 0; 605 | } 606 | 607 | void *kd_res_item_data(struct kdres *set) 608 | { 609 | return kd_res_item(set, 0); 610 | } 611 | 612 | /* ---- hyperrectangle helpers ---- */ 613 | static struct kdhyperrect* hyperrect_create(int dim, const double *min, const double *max) 614 | { 615 | size_t size = dim * sizeof(double); 616 | struct kdhyperrect* rect = 0; 617 | 618 | if (!(rect = malloc(sizeof(struct kdhyperrect)))) { 619 | return 0; 620 | } 621 | 622 | rect->dim = dim; 623 | if (!(rect->min = malloc(size))) { 624 | free(rect); 625 | return 0; 626 | } 627 | if (!(rect->max = malloc(size))) { 628 | free(rect->min); 629 | free(rect); 630 | return 0; 631 | } 632 | memcpy(rect->min, min, size); 633 | memcpy(rect->max, max, size); 634 | 635 | return rect; 636 | } 637 | 638 | static void hyperrect_free(struct kdhyperrect *rect) 639 | { 640 | free(rect->min); 641 | free(rect->max); 642 | free(rect); 643 | } 644 | 645 | static struct kdhyperrect* hyperrect_duplicate(const struct kdhyperrect *rect) 646 | { 647 | return hyperrect_create(rect->dim, rect->min, rect->max); 648 | } 649 | 650 | static void hyperrect_extend(struct kdhyperrect *rect, const double *pos) 651 | { 652 | int i; 653 | 654 | for (i=0; i < rect->dim; i++) { 655 | if (pos[i] < rect->min[i]) { 656 | rect->min[i] = pos[i]; 657 | } 658 | if (pos[i] > rect->max[i]) { 659 | rect->max[i] = pos[i]; 660 | } 661 | } 662 | } 663 | 664 | static double hyperrect_dist_sq(struct kdhyperrect *rect, const double *pos) 665 | { 666 | int i; 667 | double result = 0; 668 | 669 | for (i=0; i < rect->dim; i++) { 670 | if (pos[i] < rect->min[i]) { 671 | result += SQ(rect->min[i] - pos[i]); 672 | } else if (pos[i] > rect->max[i]) { 673 | result += SQ(rect->max[i] - pos[i]); 674 | } 675 | } 676 | 677 | return result; 678 | } 679 | 680 | /* ---- static helpers ---- */ 681 | 682 | #ifdef USE_LIST_NODE_ALLOCATOR 683 | /* special list node allocators. */ 684 | static struct res_node *free_nodes; 685 | 686 | #ifndef NO_PTHREADS 687 | static pthread_mutex_t alloc_mutex = PTHREAD_MUTEX_INITIALIZER; 688 | #endif 689 | 690 | static struct res_node *alloc_resnode(void) 691 | { 692 | struct res_node *node; 693 | 694 | #ifndef NO_PTHREADS 695 | pthread_mutex_lock(&alloc_mutex); 696 | #endif 697 | 698 | if(!free_nodes) { 699 | node = malloc(sizeof *node); 700 | } else { 701 | node = free_nodes; 702 | free_nodes = free_nodes->next; 703 | node->next = 0; 704 | } 705 | 706 | #ifndef NO_PTHREADS 707 | pthread_mutex_unlock(&alloc_mutex); 708 | #endif 709 | 710 | return node; 711 | } 712 | 713 | static void free_resnode(struct res_node *node) 714 | { 715 | #ifndef NO_PTHREADS 716 | pthread_mutex_lock(&alloc_mutex); 717 | #endif 718 | 719 | node->next = free_nodes; 720 | free_nodes = node; 721 | 722 | #ifndef NO_PTHREADS 723 | pthread_mutex_unlock(&alloc_mutex); 724 | #endif 725 | } 726 | #endif /* list node allocator or not */ 727 | 728 | 729 | /* inserts the item. if dist_sq is >= 0, then do an ordered insert */ 730 | static int rlist_insert(struct res_node *list, struct kdnode *item, double dist_sq) 731 | { 732 | struct res_node *rnode; 733 | 734 | if(!(rnode = alloc_resnode())) { 735 | return -1; 736 | } 737 | rnode->item = item; 738 | rnode->dist_sq = dist_sq; 739 | 740 | if(dist_sq >= 0.0) { 741 | while(list->next && list->next->dist_sq < dist_sq) { 742 | list = list->next; 743 | } 744 | } 745 | rnode->next = list->next; 746 | list->next = rnode; 747 | return 0; 748 | } 749 | 750 | static void clear_results(struct kdres *rset) 751 | { 752 | struct res_node *tmp, *node = rset->rlist->next; 753 | 754 | while(node) { 755 | tmp = node; 756 | node = node->next; 757 | free_resnode(tmp); 758 | } 759 | 760 | rset->rlist->next = 0; 761 | } 762 | -------------------------------------------------------------------------------- /pigain/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(pigain) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | rospy 12 | std_msgs 13 | geometry_msgs 14 | message_generation 15 | ) 16 | 17 | ## System dependencies are found with CMake's conventions 18 | # find_package(Boost REQUIRED COMPONENTS system) 19 | 20 | 21 | ## Uncomment this if the package has a setup.py. This macro ensures 22 | ## modules and global scripts declared therein get installed 23 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 24 | # catkin_python_setup() 25 | 26 | ################################################ 27 | ## Declare ROS messages, services and actions ## 28 | ################################################ 29 | 30 | ## To declare and build messages, services or actions from within this 31 | ## package, follow these steps: 32 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 33 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 34 | ## * In the file package.xml: 35 | ## * add a build_depend tag for "message_generation" 36 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 37 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 38 | ## but can be declared for certainty nonetheless: 39 | ## * add a run_depend tag for "message_runtime" 40 | ## * In this file (CMakeLists.txt): 41 | ## * add "message_generation" and every package in MSG_DEP_SET to 42 | ## find_package(catkin REQUIRED COMPONENTS ...) 43 | ## * add "message_runtime" and every package in MSG_DEP_SET to 44 | ## catkin_package(CATKIN_DEPENDS ...) 45 | ## * uncomment the add_*_files sections below as needed 46 | ## and list every .msg/.srv/.action file to be processed 47 | ## * uncomment the generate_messages entry below 48 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 49 | 50 | ## Generate messages in the 'msg' folder 51 | add_message_files( 52 | DIRECTORY 53 | msg 54 | FILES 55 | Node.msg 56 | ) 57 | 58 | ## Generate services in the 'srv' folder 59 | add_service_files( 60 | DIRECTORY 61 | srv 62 | FILES 63 | Query.srv 64 | BestNode.srv 65 | ) 66 | 67 | ## Generate actions in the 'action' folder 68 | # add_action_files( 69 | # FILES 70 | # Action1.action 71 | # Action2.action 72 | # ) 73 | 74 | ## Generate added messages and services with any dependencies listed here 75 | generate_messages( 76 | DEPENDENCIES 77 | std_msgs 78 | geometry_msgs 79 | ) 80 | 81 | ################################################ 82 | ## Declare ROS dynamic reconfigure parameters ## 83 | ################################################ 84 | 85 | ## To declare and build dynamic reconfigure parameters within this 86 | ## package, follow these steps: 87 | ## * In the file package.xml: 88 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 89 | ## * In this file (CMakeLists.txt): 90 | ## * add "dynamic_reconfigure" to 91 | ## find_package(catkin REQUIRED COMPONENTS ...) 92 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 93 | ## and list every .cfg file to be processed 94 | 95 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 96 | # generate_dynamic_reconfigure_options( 97 | # cfg/DynReconf1.cfg 98 | # cfg/DynReconf2.cfg 99 | # ) 100 | 101 | ################################### 102 | ## catkin specific configuration ## 103 | ################################### 104 | ## The catkin_package macro generates cmake config files for your package 105 | ## Declare things to be passed to dependent projects 106 | ## INCLUDE_DIRS: uncomment this if your package contains header files 107 | ## LIBRARIES: libraries you create in this project that dependent projects also need 108 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 109 | ## DEPENDS: system dependencies of this project that dependent projects also need 110 | catkin_package( 111 | # INCLUDE_DIRS include 112 | # LIBRARIES pigain 113 | CATKIN_DEPENDS rospy std_msgs geometry_msgs message_runtime 114 | # DEPENDS system_lib 115 | ) 116 | 117 | ########### 118 | ## Build ## 119 | ########### 120 | 121 | ## Specify additional locations of header files 122 | ## Your package locations should be listed before other locations 123 | include_directories( 124 | # include 125 | ${catkin_INCLUDE_DIRS} 126 | ) 127 | 128 | ## Declare a C++ library 129 | # add_library(${PROJECT_NAME} 130 | # src/${PROJECT_NAME}/pigain.cpp 131 | # ) 132 | 133 | ## Add cmake target dependencies of the library 134 | ## as an example, code may need to be generated before libraries 135 | ## either from message generation or dynamic reconfigure 136 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 137 | 138 | ## Declare a C++ executable 139 | ## With catkin_make all packages are built within a single CMake context 140 | ## The recommended prefix ensures that target names across packages don't collide 141 | # add_executable(${PROJECT_NAME}_node src/pigain_node.cpp) 142 | 143 | ## Rename C++ executable without prefix 144 | ## The above recommended prefix causes long target names, the following renames the 145 | ## target back to the shorter version for ease of user use 146 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 147 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 148 | 149 | ## Add cmake target dependencies of the executable 150 | ## same as for the library above 151 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 152 | 153 | ## Specify libraries to link a library or executable target against 154 | # target_link_libraries(${PROJECT_NAME}_node 155 | # ${catkin_LIBRARIES} 156 | # ) 157 | 158 | ############# 159 | ## Install ## 160 | ############# 161 | 162 | # all install targets should use catkin DESTINATION variables 163 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 164 | 165 | ## Mark executable scripts (Python etc.) for installation 166 | ## in contrast to setup.py, you can choose the destination 167 | # install(PROGRAMS 168 | # scripts/my_python_script 169 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 170 | # ) 171 | 172 | ## Mark executables and/or libraries for installation 173 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 174 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 175 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 176 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 177 | # ) 178 | 179 | ## Mark cpp header files for installation 180 | # install(DIRECTORY include/${PROJECT_NAME}/ 181 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 182 | # FILES_MATCHING PATTERN "*.h" 183 | # PATTERN ".svn" EXCLUDE 184 | # ) 185 | 186 | ## Mark other files for installation (e.g. launch and bag files, etc.) 187 | # install(FILES 188 | # # myfile1 189 | # # myfile2 190 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 191 | # ) 192 | 193 | ############# 194 | ## Testing ## 195 | ############# 196 | 197 | ## Add gtest based cpp test target and link libraries 198 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_pigain.cpp) 199 | # if(TARGET ${PROJECT_NAME}-test) 200 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 201 | # endif() 202 | 203 | ## Add folders to be run by python nosetests 204 | # catkin_add_nosetests(test) 205 | -------------------------------------------------------------------------------- /pigain/config/config.yaml: -------------------------------------------------------------------------------- 1 | visualize/mean: false 2 | visualize/sigma: false 3 | visualize/pts: true 4 | 5 | visualize/resolution: 0.5 6 | -------------------------------------------------------------------------------- /pigain/launch/pig.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /pigain/msg/Node.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Point position 2 | float64 yaw 3 | float64 gain 4 | -------------------------------------------------------------------------------- /pigain/nodes/gp.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from collections import namedtuple 4 | HyperParam = namedtuple("HyperParam", "l sigma_f sigma_n") 5 | 6 | def sqexpkernel(x1, x2, hyperparam): 7 | n1 = x1.shape[0] 8 | n2 = x2.shape[0] 9 | K = np.empty([n1, n2]) 10 | print(n1, n2) 11 | 12 | for i in range(0,n2): 13 | #print(x1) 14 | #print(x2[i,:]) 15 | #print(x1 - x2[i,:]) 16 | l = np.linalg.norm(x1 - x2[i,:], axis = 1) 17 | #print(l) 18 | K[:,i] = hyperparam.sigma_f**2 * np.exp(-0.5 * (l / hyperparam.l)**2) 19 | 20 | return K 21 | 22 | def gp(y, x, xstar, hyperparam, kernel): 23 | if(y.shape[0] is 0 or x.shape[0] is 0): 24 | return (np.empty((0)), np.empty((0))) 25 | 26 | K = kernel(x, x, hyperparam) # K(x,x) 27 | kstar = kernel(x, xstar, hyperparam) # K(x,x*) 28 | kss = kernel(xstar, xstar, hyperparam) # K(x*,x*) 29 | 30 | # Algorithm 2.1 from Rasmussen & Williams 31 | L = np.linalg.cholesky(K + hyperparam.sigma_n**2*np.identity(K.shape[0])) 32 | alpha = np.linalg.solve( np.transpose(L), np.linalg.solve(L, y)) 33 | posterior_mean = np.matmul(np.transpose(kstar), alpha) 34 | 35 | v = np.linalg.solve(L, kstar) 36 | posterior_variance = np.diag(kss - np.matmul(np.transpose(v), v)) 37 | 38 | return posterior_mean, posterior_variance 39 | -------------------------------------------------------------------------------- /pigain/nodes/pig.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | from visualization_msgs.msg import Marker 4 | from visualization_msgs.msg import MarkerArray 5 | from geometry_msgs.msg import PoseStamped 6 | 7 | from pigain.msg import Node 8 | from pigain.srv import Query, QueryResponse 9 | from pigain.srv import BestNode, BestNodeResponse 10 | 11 | from aeplanner.srv import Reevaluate 12 | 13 | import numpy as np 14 | from rtree import index 15 | 16 | import gp 17 | 18 | class PIGain: 19 | def __init__(self): 20 | self.s = rospy.Service('gp_query_server', Query, self.query_server) 21 | self.best_node_srv = rospy.Service('best_node_server', BestNode, self.best_node_srv_callback) 22 | self.reevaluate_client = rospy.ServiceProxy('reevaluate', Reevaluate) 23 | 24 | self.visualize_mean = rospy.get_param('~visualize/mean', False) 25 | self.visualize_sigma = rospy.get_param('~visualize/sigma', False) 26 | self.visualize_pts = rospy.get_param('~visualize/pts', False) 27 | self.resolution = rospy.get_param('~visualize/resolution', 1) 28 | 29 | self.gain_sub = rospy.Subscriber('gain_node', Node, self.gain_callback) 30 | self.pose_sub = rospy.Subscriber('pose', PoseStamped, self.pose_callback) 31 | self.marker_pub = rospy.Publisher('pig_markers', MarkerArray, queue_size=10) 32 | self.mean_pub = rospy.Publisher('mean_markers', MarkerArray, queue_size=10) 33 | self.sigma_pub = rospy.Publisher('sigma_markers', MarkerArray, queue_size=10) 34 | 35 | if self.visualize_pts: 36 | rospy.Timer(rospy.Duration(1), self.rviz_callback) 37 | if self.visualize_mean or self.visualize_sigma: 38 | rospy.Timer(rospy.Duration(5), self.evaluate) 39 | 40 | # Get environment boundaries 41 | try: 42 | self.min = rospy.get_param('boundary/min') 43 | self.max = rospy.get_param('boundary/max') 44 | except KeyError: 45 | rospy.logwarn("Boundary parameters not specified") 46 | rospy.logwarn("Defaulting to (-100, -100, 0), (100, 100, 3)...") 47 | self.min = [-100, -100, 0] 48 | self.max = [ 100, 100, 3] 49 | 50 | try: 51 | self.range = rospy.get_param('aep/gain/r_max') * 2 52 | except KeyError: 53 | rospy.logwarn("Range max parameter not specified") 54 | rospy.logwarn("Defaulting to 8 m...") 55 | 56 | self.bbx = (self.min[0], self.min[1], self.min[2], self.max[0], self.max[1], self.max[2]) 57 | 58 | self.x = None 59 | self.y = None 60 | self.z = None 61 | 62 | self.hyperparam = gp.HyperParam(l = 1, sigma_f = 1, sigma_n = 0.1) 63 | 64 | # Create r-tree 65 | p = index.Property() 66 | p.dimension = 3 67 | self.idx = index.Index(properties = p) 68 | self.id = 0 69 | 70 | rospy.Timer(rospy.Duration(5), self.reevaluate_timer_callback) 71 | 72 | """ Save current pose of agent """ 73 | def pose_callback(self, msg): 74 | self.x = msg.pose.position.x 75 | self.y = msg.pose.position.y 76 | self.z = msg.pose.position.z 77 | 78 | """ Reevaluate gain in all cached nodes that are closer to agent than self.range """ 79 | def reevaluate_timer_callback(self, event): 80 | rospy.loginfo("reevaluate start") 81 | if self.x is None or self.y is None or self.z is None: 82 | rospy.logwarn("No position received yet...") 83 | rospy.logwarn("Make sure that 'pose' has been correctly mapped and that it is being published") 84 | return 85 | 86 | bbx = (self.x-self.range, self.y-self.range, self.z-self.range, 87 | self.x+self.range, self.y+self.range, self.z+self.range) 88 | 89 | hits = self.idx.intersection(bbx, objects=True) 90 | 91 | reevaluate_list = [] 92 | reevaluate_position_list = [] 93 | for item in hits: 94 | if(item.object.gain > 2): 95 | reevaluate_position_list.append(item.object.position) 96 | reevaluate_list.append(item) 97 | try: 98 | res = self.reevaluate_client(reevaluate_position_list) 99 | except rospy.ServiceException, e: 100 | rospy.logerr("Calling reevaluate service failed") 101 | return 102 | 103 | for i, item in enumerate(reevaluate_list): 104 | item.object.gain = res.gain[i] 105 | item.object.yaw = res.yaw[i] 106 | 107 | self.idx.delete(item.id, (item.object.position.x, item.object.position.y, item.object.position.z)) 108 | self.idx.insert(item.id, (item.object.position.x, item.object.position.y, item.object.position.z), obj=item.object) 109 | 110 | rospy.loginfo("reevaluate done") 111 | 112 | """ Insert node with estimated gain in rtree """ 113 | def gain_callback(self, msg): 114 | self.idx.insert(self.id, (msg.position.x, msg.position.y, msg.position.z), obj=msg) 115 | self.id += 1 116 | 117 | """ Handle query to Gaussian Process """ 118 | def query_server(self, req): 119 | bbx = (req.point.x-2, req.point.y-2, req.point.z-2, 120 | req.point.x+2, req.point.y+2, req.point.z+2) 121 | y = np.empty((0)) 122 | x = np.empty((0,3)) 123 | hits = self.idx.intersection(bbx, objects=True) 124 | nearest = self.idx.nearest(bbx, 1, objects=True) 125 | 126 | for item in hits: 127 | y = np.append(y, [item.object.gain], axis=0) 128 | x = np.append(x, [[item.object.position.x, item.object.position.y, item.object.position.z]], axis = 0) 129 | 130 | yaw = 0 131 | for item in nearest: 132 | yaw = item.object.yaw 133 | 134 | if y.shape[0] == 0: 135 | response = QueryResponse() 136 | response.mu = 0 137 | response.sigma = 1 138 | return response 139 | 140 | xstar = np.array([[req.point.x, req.point.y, req.point.z]]) 141 | 142 | mean, sigma = gp.gp(y, x, xstar, self.hyperparam, gp.sqexpkernel) 143 | 144 | response = QueryResponse() 145 | response.mu = mean 146 | response.sigma = sigma 147 | response.yaw = yaw 148 | 149 | return response 150 | 151 | """ Return all nodes with gain higher than req.threshold """ 152 | def best_node_srv_callback(self, req): 153 | hits = self.idx.intersection(self.bbx, objects=True) 154 | 155 | best_gain = -1 156 | best_pose = None 157 | response = BestNodeResponse() 158 | for item in hits: 159 | if item.object.gain > req.threshold: 160 | response.best_node.append(item.object.position) 161 | if item.object.gain > best_gain: 162 | best_gain = item.object.gain 163 | 164 | response.gain = best_gain 165 | return response 166 | 167 | """ Evaluate potential information gain function over grid and publish it in rviz """ 168 | def evaluate(self, event): 169 | y = np.empty((0)) 170 | x = np.empty((0,3)) 171 | xstar = np.empty((0,3)) 172 | hits = self.idx.intersection(self.bbx, objects=True) 173 | for item in hits: 174 | y = np.append(y, [item.object.gain], axis=0) 175 | x = np.append(x, [[item.object.position.x, item.object.position.y, item.object.position.z]], axis = 0) 176 | 177 | xt = np.arange(self.min[0], self.max[0], self.resolution) 178 | yt = np.arange(self.min[1], self.max[1], self.resolution) 179 | zt = [1] 180 | 181 | for xx in xt: 182 | for yy in yt: 183 | for zz in zt: 184 | xstar = np.append(xstar, [[xx, yy, zz]], axis = 0) 185 | 186 | mean, sigma = gp.gp(y, x, xstar, self.hyperparam, gp.sqexpkernel) 187 | mean_markers = MarkerArray() 188 | sigma_markers = MarkerArray() 189 | for id, pts in enumerate(zip(xstar, mean, sigma)): 190 | mean_markers.markers.append(self.np_array_to_marker(id, pts[0], pts[1], max(1-pts[2], 0))) 191 | # sigma_markers.markers.append(self.np_array_to_marker(id, pts[0], pts[2] * 2)) 192 | 193 | self.mean_pub.publish(mean_markers) 194 | self.sigma_pub.publish(sigma_markers) 195 | 196 | """ Publish all cached nodes in rviz """ 197 | def rviz_callback(self, event): 198 | markers = MarkerArray() 199 | hits = self.idx.intersection(self.bbx, objects=True) 200 | for item in hits: 201 | markers.markers.append(self.node_to_marker(item.id, item.object)) 202 | 203 | self.marker_pub.publish(markers) 204 | 205 | 206 | def np_array_to_marker(self, id, p, v=0, a=0): 207 | marker = Marker() 208 | marker.header.frame_id = "map" 209 | marker.type = marker.CUBE 210 | marker.action = marker.ADD 211 | marker.id = id 212 | marker.scale.x = self.resolution 213 | marker.scale.y = self.resolution 214 | marker.scale.z = 0.1 215 | marker.color.r = v / 72.0 216 | marker.color.g = 0 217 | marker.color.b = 0.5 218 | marker.color.a = a 219 | marker.pose.orientation.w = 1.0 220 | marker.pose.position.x = p[0] 221 | marker.pose.position.y = p[1] 222 | marker.pose.position.z = p[2] 223 | marker.lifetime = rospy.Time(10) 224 | 225 | return marker 226 | 227 | 228 | def node_to_marker(self, id, node): 229 | marker = Marker() 230 | marker.header.frame_id = "map" 231 | marker.type = marker.SPHERE 232 | marker.action = marker.ADD 233 | marker.id = id 234 | marker.scale.x = 0.4 235 | marker.scale.y = 0.4 236 | marker.scale.z = 0.4 237 | marker.color.r = node.gain / 72.0 238 | marker.color.g = 0.0 239 | marker.color.b = 0.5 240 | marker.color.a = 1.0 241 | marker.pose.orientation.w = 1.0 242 | marker.pose.position.x = node.position.x 243 | marker.pose.position.y = node.position.y 244 | marker.pose.position.z = node.position.z 245 | marker.lifetime = rospy.Time(1.2) 246 | 247 | return marker 248 | 249 | 250 | 251 | 252 | if __name__ == '__main__': 253 | rospy.init_node('pigain', anonymous=True) 254 | pigain = PIGain() 255 | rospy.spin() 256 | -------------------------------------------------------------------------------- /pigain/nodes/test_gp.py: -------------------------------------------------------------------------------- 1 | import gp 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | 5 | 6 | hyperparam = gp.HyperParam(l = 0.3, sigma_f = 1, sigma_n = 0.1) 7 | 8 | x = np.array([[0.4, 0.4], [-0.6, -0.6]]) 9 | y = np.array([0.719, -0.044]) 10 | 11 | xstar = np.empty([0, 2]) 12 | xi = np.arange(-1, 1, 0.01) 13 | for i in xi: 14 | xstar = np.vstack((xstar, i*np.hstack([1,1]))) 15 | 16 | mu, sigma2 = gp.gp(y, x, xstar, hyperparam, gp.sqexpkernel) 17 | sigma = np.sqrt(sigma2) 18 | 19 | plt.plot(xi, mu) 20 | plt.fill_between(xi, mu - 2*sigma, mu + 2*sigma, facecolor = '#D3D3D3', color = "#808080") 21 | plt.show() 22 | -------------------------------------------------------------------------------- /pigain/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pigain 4 | 0.0.1 5 | The pigain package 6 | 7 | 8 | 9 | 10 | mseln 11 | 12 | 13 | 14 | 15 | 16 | TODO 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 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | rospy 53 | std_msgs 54 | geometry_msgs 55 | rospy 56 | std_msgs 57 | message_generation 58 | rospy 59 | std_msgs 60 | geometry_msgs 61 | message_runtime 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /pigain/srv/BestNode.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Point pos 2 | float64 threshold 3 | --- 4 | geometry_msgs/Point[] best_node 5 | float64 gain 6 | -------------------------------------------------------------------------------- /pigain/srv/Query.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Point point 2 | --- 3 | float64 mu 4 | float64 sigma 5 | float64 yaw 6 | -------------------------------------------------------------------------------- /rpl_exploration/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rpl_exploration) 3 | 4 | add_definitions(-std=c++11) 5 | 6 | ## Find catkin macros and libraries 7 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 8 | ## is used, also find other catkin packages 9 | find_package(catkin REQUIRED COMPONENTS 10 | roscpp 11 | rospy 12 | tf 13 | std_msgs 14 | sensor_msgs 15 | geometry_msgs 16 | octomap_msgs 17 | nodelet 18 | 19 | genmsg 20 | actionlib_msgs 21 | actionlib 22 | aeplanner 23 | aeplanner_evaluation 24 | rrtplanner 25 | ) 26 | 27 | find_package(octomap REQUIRED) 28 | find_package(PCL REQUIRED) 29 | 30 | ## System dependencies are found with CMake's conventions 31 | # find_package(Boost REQUIRED COMPONENTS system) 32 | 33 | 34 | ## Uncomment this if the package has a setup.py. This macro ensures 35 | ## modules and global scripts declared therein get installed 36 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 37 | # catkin_python_setup() 38 | 39 | ################################################ 40 | ## Declare ROS messages, services and actions ## 41 | ################################################ 42 | 43 | ## To declare and build messages, services or actions from within this 44 | ## package, follow these steps: 45 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 46 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 47 | ## * In the file package.xml: 48 | ## * add a build_depend tag for "message_generation" 49 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 50 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 51 | ## but can be declared for certainty nonetheless: 52 | ## * add a run_depend tag for "message_runtime" 53 | ## * In this file (CMakeLists.txt): 54 | ## * add "message_generation" and every package in MSG_DEP_SET to 55 | ## find_package(catkin REQUIRED COMPONENTS ...) 56 | ## * add "message_runtime" and every package in MSG_DEP_SET to 57 | ## catkin_package(CATKIN_DEPENDS ...) 58 | ## * uncomment the add_*_files sections below as needed 59 | ## and list every .msg/.srv/.action file to be processed 60 | ## * uncomment the generate_messages entry below 61 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 62 | 63 | # find_package(catkin REQUIRED genmsg actionlib_msgs actionlib) 64 | add_action_files(DIRECTORY action FILES FlyTo.action) 65 | generate_messages(DEPENDENCIES actionlib_msgs geometry_msgs) 66 | ## Generate messages in the 'msg' folder 67 | # add_message_files( 68 | # FILES 69 | # Message1.msg 70 | # Message2.msg 71 | # ) 72 | 73 | ## Generate services in the 'srv' folder 74 | # add_service_files( 75 | # FILES 76 | # Service1.srv 77 | # Service2.srv 78 | # ) 79 | 80 | ## Generate actions in the 'action' folder 81 | # add_action_files( 82 | # FILES 83 | # Action1.action 84 | # Action2.action 85 | # ) 86 | 87 | ## Generate added messages and services with any dependencies listed here 88 | # generate_messages( 89 | # DEPENDENCIES 90 | # sensor_msgs 91 | # std_msgs 92 | #) 93 | 94 | ################################################ 95 | ## Declare ROS dynamic reconfigure parameters ## 96 | ################################################ 97 | 98 | ## To declare and build dynamic reconfigure parameters within this 99 | ## package, follow these steps: 100 | ## * In the file package.xml: 101 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 102 | ## * In this file (CMakeLists.txt): 103 | ## * add "dynamic_reconfigure" to 104 | ## find_package(catkin REQUIRED COMPONENTS ...) 105 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 106 | ## and list every .cfg file to be processed 107 | 108 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 109 | # generate_dynamic_reconfigure_options( 110 | # cfg/DynReconf1.cfg 111 | # cfg/DynReconf2.cfg 112 | # ) 113 | 114 | ################################### 115 | ## catkin specific configuration ## 116 | ################################### 117 | ## The catkin_package macro generates cmake config files for your package 118 | ## Declare things to be passed to dependent projects 119 | ## INCLUDE_DIRS: uncomment this if you package contains header files 120 | ## LIBRARIES: libraries you create in this project that dependent projects also need 121 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 122 | ## DEPENDS: system dependencies of this project that dependent projects also need 123 | catkin_package( 124 | # INCLUDE_DIRS include 125 | # LIBRARIES rpl_exploration 126 | # CATKIN_DEPENDS octomap roscpp sensor_msgs std_msgs 127 | # DEPENDS system_lib 128 | ) 129 | 130 | ########### 131 | ## Build ## 132 | ########### 133 | 134 | ## Specify additional locations of header files 135 | ## Your package locations should be listed before other locations 136 | # include_directories(include) 137 | include_directories(${catkin_INCLUDE_DIRS}) 138 | include_directories(${OCTOMAP_INCLUDE_DIRS}) 139 | include_directories(${PCL_INCLUDE_DIRS}) 140 | add_definitions(${PCL_DEFINITIONS}) 141 | 142 | ## Declare a C++ library 143 | # add_library(rpl_exploration 144 | # src/${PROJECT_NAME}/rpl_exploration.cpp 145 | # ) 146 | 147 | ## Add cmake target dependencies of the library 148 | ## as an example, code may need to be generated before libraries 149 | ## either from message generation or dynamic reconfigure 150 | # add_dependencies(rpl_exploration ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 151 | 152 | ## Declare a C++ executable 153 | 154 | ## Add cmake target dependencies of the executable 155 | ## same as for the library above 156 | # add_dependencies(rpl_exploration_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 157 | 158 | ## Specify libraries to link a library or executable target against 159 | add_library(laserscan_to_pointcloud src/laserscan_to_pointcloud.cpp) 160 | target_link_libraries(laserscan_to_pointcloud ${catkin_LIBRARIES}) 161 | if(catkin_EXPORTED_LIBRARIES) 162 | add_dependencies(laserscan_to_pointcloud ${catkin_EXPORTED_LIBRARIES}) 163 | endif() 164 | 165 | add_executable(fly_to src/fly_to.cpp) 166 | target_link_libraries( fly_to ${catkin_LIBRARIES}) 167 | add_dependencies(fly_to rpl_exploration_generate_messages_cpp) 168 | 169 | add_executable(rpl_exploration src/rpl_exploration.cpp) 170 | target_link_libraries(rpl_exploration ${catkin_LIBRARIES}) 171 | add_dependencies(rpl_exploration ${catkin_EXPORTED_TARGETS}) 172 | add_dependencies(rpl_exploration rpl_exploration_generate_messages_cpp) 173 | 174 | #if(catkin_EXPORTED_LIBRARIES) 175 | # add_dependencies(fly_to ${catkin_EXPORTED_LIBRARIES}) 176 | #endif() 177 | 178 | # add_dependencies( 179 | # fly_to 180 | # ${rpl_exploration_actionlib_EXPORTED_TARGETS} 181 | #) 182 | ############# 183 | ## Install ## 184 | ############# 185 | 186 | # all install targets should use catkin DESTINATION variables 187 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 188 | 189 | ## Mark executable scripts (Python etc.) for installation 190 | ## in contrast to setup.py, you can choose the destination 191 | # install(PROGRAMS 192 | # scripts/my_python_script 193 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 194 | # ) 195 | 196 | ## Mark executables and/or libraries for installation 197 | # install(TARGETS rpl_exploration rpl_exploration_node 198 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 199 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 200 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 201 | # ) 202 | 203 | ## Mark cpp header files for installation 204 | # install(DIRECTORY include/${PROJECT_NAME}/ 205 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 206 | # FILES_MATCHING PATTERN "*.h" 207 | # PATTERN ".svn" EXCLUDE 208 | # ) 209 | 210 | ## Mark other files for installation (e.g. launch and bag files, etc.) 211 | # install(FILES 212 | # # myfile1 213 | # # myfile2 214 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 215 | # ) 216 | 217 | ############# 218 | ## Testing ## 219 | ############# 220 | 221 | ## Add gtest based cpp test target and link libraries 222 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_rpl_exploration.cpp) 223 | # if(TARGET ${PROJECT_NAME}-test) 224 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 225 | # endif() 226 | 227 | ## Add folders to be run by python nosetests 228 | # catkin_add_nosetests(test) 229 | -------------------------------------------------------------------------------- /rpl_exploration/action/FlyTo.action: -------------------------------------------------------------------------------- 1 | # Define the goal 2 | geometry_msgs/PoseStamped pose 3 | --- 4 | # Define the result 5 | --- 6 | # Define a feedback message 7 | float32 distance_to_target 8 | -------------------------------------------------------------------------------- /rpl_exploration/config/exploration.yaml: -------------------------------------------------------------------------------- 1 | camera/horizontal_fov: 85.0 2 | camera/vertical_fov: 58.0 3 | 4 | raycast/dr: 0.1 5 | raycast/dphi: 10 6 | raycast/dtheta: 10 7 | 8 | system/bbx/r: 0.75 9 | system/bbx/overshoot: 0.25 10 | 11 | aep/gain/r_min: 0 12 | aep/gain/r_max: 5 13 | aep/gain/zero: 25 14 | aep/gain/lambda: 0.75 15 | aep/gain/sigma_thresh: 0.2 16 | aep/tree/extension_range: 1 17 | aep/tree/max_sampling_radius: 10 18 | aep/tree/initial_iterations: 15 19 | aep/tree/cutoff_iterations: 30 20 | 21 | rrt/min_nodes: 100 22 | 23 | # bounding box: necessary to limit the simulation 24 | # scenario (smaller than actual gazebo scenario) 25 | boundary/min: [-30.0, -30.0, 0.0] 26 | boundary/max: [ 30.0, 30.0, 2.0] 27 | 28 | visualize_rays: false 29 | visualize_tree: true 30 | visualize_exploration_area: true 31 | 32 | robot_frame: "fcu" 33 | world_frame: "map" 34 | 35 | -------------------------------------------------------------------------------- /rpl_exploration/launch/exploration.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /rpl_exploration/launch/fly_to.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /rpl_exploration/launch/laser_tf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /rpl_exploration/launch/laserscan_to_pointcloud.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /rpl_exploration/launch/octomap_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /rpl_exploration/launch/primesense_tf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /rpl_exploration/launch/rpl_exploration.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /rpl_exploration/launch/rpl_inspector.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 | -------------------------------------------------------------------------------- /rpl_exploration/launch/teleop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /rpl_exploration/nodes/image_maxing.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | from sensor_msgs.msg import Image 4 | import cv2 5 | from cv_bridge import CvBridge, CvBridgeError 6 | import math 7 | 8 | pub = None 9 | bridge = CvBridge() 10 | max_meters = 7 11 | 12 | 13 | def imageCallback(msg): 14 | try: 15 | cv_image = bridge.imgmsg_to_cv2(msg, "32FC1") 16 | except CvBridgeError as e: 17 | print(e) 18 | 19 | height, width = cv_image.shape[0:2] 20 | for x in range(0, width): 21 | for y in range(0, height): 22 | if math.isnan(cv_image[y, x]): 23 | cv_image[y, x] = max_meters 24 | 25 | new_msg = bridge.cv2_to_imgmsg(cv_image, "32FC1") 26 | new_msg.header = msg.header 27 | 28 | pub.publish(new_msg) 29 | 30 | 31 | def image_maxing(): 32 | global pub 33 | pub = rospy.Publisher("/camera/depth/image_raw2", Image, queue_size=10) 34 | rospy.init_node("image_maxing", anonymous=True) 35 | 36 | sub = rospy.Subscriber("/camera/depth/image_raw", Image, imageCallback) 37 | 38 | rospy.spin() 39 | 40 | 41 | if __name__ == '__main__': 42 | try: 43 | image_maxing() 44 | except rospy.ROSInterruptException: 45 | pass 46 | -------------------------------------------------------------------------------- /rpl_exploration/nodes/teleop.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | import math 4 | 5 | from sfml import sf 6 | from geometry_msgs.msg import TwistStamped 7 | 8 | def publish_cmd_vel(key_map): 9 | cmd_vel = TwistStamped() 10 | 11 | if(key_map['w']): 12 | cmd_vel.twist.linear.x += 0.5 13 | if(key_map['s']): 14 | cmd_vel.twist.linear.x -= 0.5 15 | 16 | if(key_map['a']): 17 | cmd_vel.twist.linear.y += 0.5 18 | if(key_map['d']): 19 | cmd_vel.twist.linear.y -= 0.5 20 | 21 | if(key_map['z']): 22 | cmd_vel.twist.linear.z += 0.5 23 | if(key_map['x']): 24 | cmd_vel.twist.linear.z -= 0.5 25 | 26 | if(key_map['q']): 27 | cmd_vel.twist.angular.z += 1 28 | if(key_map['e']): 29 | cmd_vel.twist.angular.z -= 1 30 | 31 | pub.publish(cmd_vel) 32 | 33 | def key_poller(): 34 | 35 | shutdown = False 36 | 37 | key_map = {} 38 | for c in range(97, 123): 39 | key_map[chr(c)] = False 40 | 41 | rate = rospy.Rate(10) 42 | 43 | w = sf.RenderWindow(sf.VideoMode(640, 480), "tele-op", sf.Style.TITLEBAR | sf.Style.RESIZE) 44 | while not rospy.is_shutdown() and not shutdown: 45 | for event in w.events: 46 | # Handle shutdown 47 | if type(event) is sf.KeyEvent and event.code is sf.Keyboard.ESCAPE: 48 | w.close() 49 | shutdown = True 50 | 51 | if type(event) is sf.KeyEvent and 0 <= event.code and event.code < 26: 52 | key = chr(event.code + 97) 53 | if(event.pressed): 54 | key_map[key] = True 55 | 56 | if(event.released): 57 | key_map[key] = False 58 | 59 | publish_cmd_vel(key_map) 60 | sf.sleep(sf.seconds(0.05)) 61 | 62 | 63 | 64 | 65 | if __name__ == '__main__': 66 | rospy.init_node("teleop") 67 | pub = rospy.Publisher('cmd_vel', TwistStamped, queue_size=10) 68 | key_poller() 69 | 70 | 71 | -------------------------------------------------------------------------------- /rpl_exploration/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rpl_exploration 4 | 0.0.0 5 | The rpl_exploration package 6 | 7 | 8 | 9 | 10 | mseln 11 | 12 | 13 | 14 | 15 | 16 | BSD-3 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 | catkin 43 | octomap 44 | octomap_msgs 45 | roscpp 46 | sensor_msgs 47 | std_msgs 48 | nodelet 49 | actionlib 50 | actionlib_msgs 51 | tf 52 | aeplanner 53 | rrtplanner 54 | nav_msgs 55 | 56 | octomap 57 | octomap_msgs 58 | roscpp 59 | sensor_msgs 60 | std_msgs 61 | nodelet 62 | actionlib 63 | actionlib_msgs 64 | tf 65 | aeplanner 66 | rrtplanner 67 | nav_msgs 68 | 69 | 70 | rospy 71 | opencv2 72 | cv_bridge 73 | rospy 74 | opencv2 75 | cv_bridge 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | -------------------------------------------------------------------------------- /rpl_exploration/rpl_exploration_nodelets.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Nodelet for transforming laserscan to pointcloud. 7 | 8 | 9 | 10 | 11 | 14 | 15 | Nodelet for doing exploration in a grid manner 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /rpl_exploration/src/fly_to.cpp: -------------------------------------------------------------------------------- 1 | #include // Note: "Action" is appended 2 | #include 3 | 4 | #include 5 | #include 6 | 7 | namespace rpl_exploration { 8 | class FlyTo 9 | { 10 | private: 11 | ros::NodeHandle nh_; 12 | ros::Publisher pub_; 13 | actionlib::SimpleActionServer as_; 14 | 15 | tf::TransformListener listener; 16 | public: 17 | FlyTo() : pub_(nh_.advertise("fly_to_cmd", 1000)), 18 | as_(nh_, "fly_to", boost::bind(&FlyTo::execute, this, _1, &as_), false) 19 | { 20 | ROS_INFO("Starting fly to server"); 21 | as_.start(); 22 | } 23 | void execute(const rpl_exploration::FlyToGoalConstPtr& goal, 24 | actionlib::SimpleActionServer * as) 25 | { 26 | ROS_INFO_STREAM("Got new goal: Fly to (" << goal->pose.pose.position.x << ", " 27 | << goal->pose.pose.position.y << ", " 28 | << goal->pose.pose.position.z << ") "); 29 | 30 | ros::Rate r(20); 31 | geometry_msgs::Point p = goal->pose.pose.position; 32 | 33 | float distance_to_goal = 9001; // Distance is over 9000 34 | float yaw_diff = M_PI; 35 | 36 | tf::StampedTransform transform; 37 | transform.setOrigin( tf::Vector3(0.0, 0.0, 0.0) ); 38 | transform.setRotation( tf::Quaternion(0, 0, 0, 1) ); 39 | 40 | tf::Quaternion gq( 41 | goal->pose.pose.orientation.x, 42 | goal->pose.pose.orientation.y, 43 | goal->pose.pose.orientation.z, 44 | goal->pose.pose.orientation.w); 45 | tf::Matrix3x3 m(gq); 46 | double roll, pitch, goal_yaw; 47 | m.getRPY(roll, pitch, goal_yaw); 48 | 49 | // Check if target is reached... 50 | do 51 | { 52 | ROS_INFO_STREAM("Publishing goal to (" << p.x << ", " << p.y << ", " << p.z << ") "); 53 | pub_.publish(goal->pose); 54 | 55 | listener.waitForTransform("/map", "/base_link", ros::Time(0), ros::Duration(10.0) ); 56 | listener.lookupTransform("/map", "/base_link", ros::Time(0), transform); 57 | 58 | geometry_msgs::Point q; 59 | q.x = (float)transform.getOrigin().x(); 60 | q.y = (float)transform.getOrigin().y(); 61 | q.z = (float)transform.getOrigin().z(); 62 | 63 | geometry_msgs::Quaternion tq; 64 | tq.x = (float)transform.getRotation().x(); 65 | tq.y = (float)transform.getRotation().y(); 66 | tq.z = (float)transform.getRotation().z(); 67 | tq.w = (float)transform.getRotation().w(); 68 | tf::Quaternion cq( tq.x, tq.y, tq.z, tq.w); 69 | tf::Matrix3x3 m(cq); 70 | double current_yaw; 71 | m.getRPY(roll, pitch, current_yaw); 72 | 73 | ROS_INFO_STREAM("Current position: (" << q.x << ", " << q.y << ", " << q.z << ") "); 74 | geometry_msgs::Point d; 75 | d.x = p.x - q.x; 76 | d.y = p.y - q.y; 77 | d.z = p.z - q.z; 78 | 79 | distance_to_goal = sqrt(d.x*d.x + d.y*d.y + d.z*d.z); 80 | ROS_INFO_STREAM("Distance to goal: " << distance_to_goal); 81 | yaw_diff = fabs(atan2(sin(goal_yaw-current_yaw), cos(goal_yaw-current_yaw))); 82 | 83 | r.sleep(); 84 | } while(distance_to_goal > 0.8 or yaw_diff > 0.6*M_PI); 85 | 86 | 87 | as->setSucceeded(); 88 | } 89 | }; 90 | 91 | 92 | } 93 | 94 | int main(int argc, char** argv) 95 | { 96 | ros::init(argc, argv, "fly_to_server"); 97 | 98 | rpl_exploration::FlyTo fly_to; 99 | 100 | ros::spin(); 101 | return 0; 102 | } 103 | -------------------------------------------------------------------------------- /rpl_exploration/src/laserscan_to_pointcloud.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | namespace laserscan_to_pointcloud { 13 | class LTPNodelet : public nodelet::Nodelet 14 | { 15 | private: 16 | ros::Subscriber scan_sub_; 17 | 18 | ros::Publisher cloud_pub_; 19 | 20 | private: 21 | virtual void onInit() 22 | { 23 | ros::NodeHandle & nh = getNodeHandle(); 24 | ros::NodeHandle & nh_priv = getPrivateNodeHandle(); 25 | 26 | std::string scan_topic; 27 | if (!nh_priv.getParam("scan_topic", scan_topic)) 28 | { 29 | NODELET_ERROR("Failed to get param 'scan_topic'"); 30 | } 31 | std::string cloud_topic; 32 | if (!nh_priv.getParam("cloud_topic", cloud_topic)) 33 | { 34 | NODELET_ERROR("Failed to get param 'cloud_topic'"); 35 | } 36 | 37 | scan_sub_ = nh.subscribe(scan_topic, 2, <PNodelet::scanCallback, this); 38 | cloud_pub_ = nh.advertise >(cloud_topic, 1); 39 | } 40 | 41 | void scanCallback(const sensor_msgs::LaserScan::ConstPtr & scan) 42 | { 43 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 44 | cloud->header.frame_id = scan->header.frame_id; 45 | cloud->header.stamp = pcl_conversions::toPCL(scan->header.stamp); 46 | 47 | cloud->is_dense = false; 48 | cloud->reserve(scan->ranges.size()); 49 | 50 | double current_angle = scan->angle_min - scan->angle_increment; 51 | for (size_t i = 0; i < scan->ranges.size(); ++i) 52 | { 53 | current_angle += scan->angle_increment; 54 | 55 | pcl::PointXYZ point; 56 | 57 | if (scan->ranges[i] < scan->range_min) 58 | { 59 | // Invalid range 60 | point.x = std::numeric_limits::quiet_NaN(); 61 | point.y = std::numeric_limits::quiet_NaN(); 62 | point.z = std::numeric_limits::quiet_NaN(); 63 | cloud->is_dense = false; 64 | } 65 | else if (scan->ranges[i] > scan->range_max) 66 | { 67 | point.x = (scan->range_max+0.001) * std::cos(current_angle); // + 1 mm to make it a bit longer than max length 68 | point.y = (scan->range_max+0.001) * std::sin(current_angle); // so entire scan will be classified as free 69 | point.z = 0; 70 | } 71 | else 72 | { 73 | point.x = scan->ranges[i] * std::cos(current_angle); 74 | point.y = scan->ranges[i] * std::sin(current_angle); 75 | point.z = 0; 76 | } 77 | 78 | cloud->points.push_back(point); 79 | } 80 | 81 | cloud_pub_.publish(cloud); 82 | } 83 | }; 84 | } 85 | 86 | PLUGINLIB_EXPORT_CLASS(laserscan_to_pointcloud::LTPNodelet, nodelet::Nodelet) 87 | -------------------------------------------------------------------------------- /rpl_exploration/src/rpl_exploration.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | #include 19 | #include 20 | 21 | int main(int argc, char** argv) 22 | { 23 | ros::init(argc, argv, "exploration"); 24 | ros::NodeHandle nh; 25 | ROS_INFO("Started exploration"); 26 | 27 | // Open logfile; 28 | std::string path = ros::package::getPath("rpl_exploration"); 29 | std::ofstream logfile, pathfile; 30 | logfile.open(path + "/data/logfile.csv"); 31 | pathfile.open(path + "/data/path.csv"); 32 | 33 | ros::Publisher pub(nh.advertise("/mavros/" 34 | "setpoint_position/" 35 | "local", 36 | 1000)); 37 | 38 | ros::ServiceClient coverage_srv = 39 | nh.serviceClient("/get_coverage"); 40 | 41 | // wait for fly_to server to start 42 | // ROS_INFO("Waiting for fly_to action server"); 43 | actionlib::SimpleActionClient ac("fly_to", true); 44 | // ac.waitForServer(); //will wait for infinite time 45 | // ROS_INFO("Fly to ction server started!"); 46 | 47 | // wait for aep server to start 48 | ROS_INFO("Waiting for aeplanner action server"); 49 | actionlib::SimpleActionClient aep_ac("make_plan", 50 | true); 51 | aep_ac.waitForServer(); // will wait for infinite time 52 | ROS_INFO("aeplanner action server started!"); 53 | 54 | // wait for fly_to server to start 55 | ROS_INFO("Waiting for rrt action server"); 56 | actionlib::SimpleActionClient rrt_ac("rrt", true); 57 | // rrt_ac.waitForServer(); //will wait for infinite time 58 | ROS_INFO("rrt Action server started!"); 59 | 60 | // Get current pose 61 | geometry_msgs::PoseStamped::ConstPtr init_pose = 62 | ros::topic::waitForMessage("/mavros/" 63 | "local_position/pose"); 64 | double init_yaw = tf2::getYaw(init_pose->pose.orientation); 65 | // Up 2 meters and then forward one meter 66 | double initial_positions[8][4] = { 67 | { init_pose->pose.position.x, init_pose->pose.position.y, 68 | init_pose->pose.position.z + 2.0, init_yaw }, 69 | { init_pose->pose.position.x + 1.0 * std::cos(init_yaw), 70 | init_pose->pose.position.y + 1.0 * std::sin(init_yaw), 71 | init_pose->pose.position.z + 2.0, init_yaw }, 72 | }; 73 | 74 | // This is the initialization motion, necessary that the known free space 75 | // allows the planning of initial paths. 76 | ROS_INFO("Starting the planner: Performing initialization motion"); 77 | geometry_msgs::PoseStamped last_pose; 78 | 79 | for (int i = 0; i < 2; ++i) 80 | { 81 | rpl_exploration::FlyToGoal goal; 82 | goal.pose.pose.position.x = initial_positions[i][0]; 83 | goal.pose.pose.position.y = initial_positions[i][1]; 84 | goal.pose.pose.position.z = initial_positions[i][2]; 85 | goal.pose.pose.orientation = 86 | tf::createQuaternionMsgFromYaw(initial_positions[i][3]); 87 | last_pose.pose = goal.pose.pose; 88 | 89 | ROS_INFO_STREAM("Sending initial goal..."); 90 | ac.sendGoal(goal); 91 | 92 | ac.waitForResult(ros::Duration(0)); 93 | } 94 | 95 | // Start planning: The planner is called and the computed path sent to the 96 | // controller. 97 | int iteration = 0; 98 | int actions_taken = 1; 99 | 100 | ros::Time start = ros::Time::now(); 101 | while (ros::ok()) 102 | { 103 | ROS_INFO_STREAM("Planning iteration " << iteration); 104 | aeplanner::aeplannerGoal aep_goal; 105 | aep_goal.header.stamp = ros::Time::now(); 106 | aep_goal.header.seq = iteration; 107 | aep_goal.header.frame_id = "map"; 108 | aep_goal.actions_taken = actions_taken; 109 | aep_ac.sendGoal(aep_goal); 110 | 111 | while (!aep_ac.waitForResult(ros::Duration(0.05))) 112 | { 113 | pub.publish(last_pose); 114 | } 115 | 116 | ros::Duration fly_time; 117 | if (aep_ac.getResult()->is_clear) 118 | { 119 | actions_taken = 0; 120 | 121 | ros::Time s = ros::Time::now(); 122 | geometry_msgs::PoseStamped goal_pose = aep_ac.getResult()->pose; 123 | // Write path to file 124 | pathfile << goal_pose.pose.position.x << ", " << goal_pose.pose.position.y 125 | << ", " << goal_pose.pose.position.z << ", n" << std::endl; 126 | 127 | last_pose.pose = goal_pose.pose; 128 | rpl_exploration::FlyToGoal goal; 129 | goal.pose = goal_pose; 130 | ac.sendGoal(goal); 131 | 132 | ac.waitForResult(ros::Duration(0)); 133 | 134 | fly_time = ros::Time::now() - s; 135 | } 136 | else 137 | { 138 | rrtplanner::rrtGoal rrt_goal; 139 | rrt_goal.start.header.stamp = ros::Time::now(); 140 | rrt_goal.start.header.frame_id = "map"; 141 | rrt_goal.start.pose = last_pose.pose; 142 | if (!aep_ac.getResult()->frontiers.poses.size()) 143 | { 144 | ROS_WARN("Exploration complete!"); 145 | break; 146 | } 147 | for (auto it = aep_ac.getResult()->frontiers.poses.begin(); 148 | it != aep_ac.getResult()->frontiers.poses.end(); ++it) 149 | { 150 | rrt_goal.goal_poses.poses.push_back(*it); 151 | } 152 | 153 | rrt_ac.sendGoal(rrt_goal); 154 | while (!rrt_ac.waitForResult(ros::Duration(0.05))) 155 | { 156 | pub.publish(last_pose); 157 | } 158 | nav_msgs::Path path = rrt_ac.getResult()->path; 159 | 160 | ros::Time s = ros::Time::now(); 161 | for (int i = path.poses.size() - 1; i >= 0; --i) 162 | { 163 | geometry_msgs::Pose goal_pose = path.poses[i].pose; 164 | // Write path to file 165 | pathfile << goal_pose.position.x << ", " << goal_pose.position.y << ", " 166 | << goal_pose.position.z << ", f" << std::endl; 167 | 168 | last_pose.pose = goal_pose; 169 | rpl_exploration::FlyToGoal goal; 170 | goal.pose.pose = goal_pose; 171 | ac.sendGoal(goal); 172 | 173 | ac.waitForResult(ros::Duration(0)); 174 | } 175 | actions_taken = -1; 176 | fly_time = ros::Time::now() - s; 177 | } 178 | 179 | ros::Duration elapsed = ros::Time::now() - start; 180 | 181 | ROS_INFO_STREAM("Iteration: " << iteration << " " << 182 | "Time: " << elapsed << " " << 183 | "Sampling: " << aep_ac.getResult()->sampling_time.data << " " << 184 | "Planning: " << aep_ac.getResult()->planning_time.data << " " << 185 | "Collision check: " << aep_ac.getResult()->collision_check_time.data << " " << 186 | "Flying: " << fly_time << " " << 187 | "Tree size: " << aep_ac.getResult()->tree_size); 188 | 189 | logfile << iteration << ", " 190 | << elapsed << ", " 191 | << aep_ac.getResult()->sampling_time.data << ", " 192 | << aep_ac.getResult()->planning_time.data << ", " 193 | << aep_ac.getResult()->collision_check_time.data << ", " 194 | << fly_time << ", " 195 | << aep_ac.getResult()->tree_size << std::endl; 196 | 197 | iteration++; 198 | } 199 | 200 | pathfile.close(); 201 | logfile.close(); 202 | } 203 | -------------------------------------------------------------------------------- /rrtplanner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rrtplanner) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | visualization_msgs 13 | message_generation 14 | genmsg 15 | actionlib_msgs 16 | actionlib 17 | 18 | kdtree 19 | nav_msgs 20 | ) 21 | find_package(octomap REQUIRED) 22 | find_package(Eigen3 REQUIRED) 23 | 24 | ## System dependencies are found with CMake's conventions 25 | # find_package(Boost REQUIRED COMPONENTS system) 26 | 27 | 28 | ## Uncomment this if the package has a setup.py. This macro ensures 29 | ## modules and global scripts declared therein get installed 30 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 31 | # catkin_python_setup() 32 | 33 | ################################################ 34 | ## Declare ROS messages, services and actions ## 35 | ################################################ 36 | 37 | ## To declare and build messages, services or actions from within this 38 | ## package, follow these steps: 39 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 40 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 41 | ## * In the file package.xml: 42 | ## * add a build_depend tag for "message_generation" 43 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 44 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 45 | ## but can be declared for certainty nonetheless: 46 | ## * add a exec_depend tag for "message_runtime" 47 | ## * In this file (CMakeLists.txt): 48 | ## * add "message_generation" and every package in MSG_DEP_SET to 49 | ## find_package(catkin REQUIRED COMPONENTS ...) 50 | ## * add "message_runtime" and every package in MSG_DEP_SET to 51 | ## catkin_package(CATKIN_DEPENDS ...) 52 | ## * uncomment the add_*_files sections below as needed 53 | ## and list every .msg/.srv/.action file to be processed 54 | ## * uncomment the generate_messages entry below 55 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 56 | 57 | ## Generate messages in the 'msg' folder 58 | # add_message_files( 59 | # FILES 60 | # Message1.msg 61 | # Message2.msg 62 | # ) 63 | 64 | ## Generate services in the 'srv' folder 65 | # add_service_files( 66 | # FILES 67 | # Service1.srv 68 | # Service2.srv 69 | # ) 70 | 71 | ## Generate actions in the 'action' folder 72 | add_action_files( 73 | DIRECTORY action 74 | FILES rrt.action 75 | ) 76 | 77 | ## Generate added messages and services with any dependencies listed here 78 | generate_messages( 79 | DEPENDENCIES 80 | std_msgs 81 | geometry_msgs 82 | visualization_msgs 83 | actionlib_msgs 84 | nav_msgs 85 | ) 86 | 87 | ################################################ 88 | ## Declare ROS dynamic reconfigure parameters ## 89 | ################################################ 90 | 91 | ## To declare and build dynamic reconfigure parameters within this 92 | ## package, follow these steps: 93 | ## * In the file package.xml: 94 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 95 | ## * In this file (CMakeLists.txt): 96 | ## * add "dynamic_reconfigure" to 97 | ## find_package(catkin REQUIRED COMPONENTS ...) 98 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 99 | ## and list every .cfg file to be processed 100 | 101 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 102 | # generate_dynamic_reconfigure_options( 103 | # cfg/DynReconf1.cfg 104 | # cfg/DynReconf2.cfg 105 | # ) 106 | 107 | ################################### 108 | ## catkin specific configuration ## 109 | ################################### 110 | ## The catkin_package macro generates cmake config files for your package 111 | ## Declare things to be passed to dependent projects 112 | ## INCLUDE_DIRS: uncomment this if your package contains header files 113 | ## LIBRARIES: libraries you create in this project that dependent projects also need 114 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 115 | ## DEPENDS: system dependencies of this project that dependent projects also need 116 | catkin_package( 117 | INCLUDE_DIRS include ${OCTOMAP_INCLUDE_DIRS} ${Eigen_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} 118 | LIBRARIES rrtplanner ${catkin_LIBRARIES} ${OCTOMAP_LIBRARIES} 119 | CATKIN_DEPENDS message_runtime roscpp geometry_msgs visualization_msgs kdtree 120 | ) 121 | 122 | ########### 123 | ## Build ## 124 | ########### 125 | 126 | ## Specify additional locations of header files 127 | ## Your package locations should be listed before other locations 128 | include_directories( 129 | include 130 | ${catkin_INCLUDE_DIRS} 131 | ) 132 | 133 | ## Declare a C++ library 134 | # add_library(${PROJECT_NAME} 135 | # src/${PROJECT_NAME}/rrtplanner.cpp 136 | # ) 137 | add_library(rrtplanner src/rrt.cpp) 138 | target_link_libraries(rrtplanner 139 | ${catkin_LIBRARIES} 140 | ${OCTOMAP_LIBRARIES} 141 | ) 142 | add_dependencies(rrtplanner 143 | ${catkin_EXPORTED_TARGETS} 144 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 145 | rrtplanner_generate_messages_cpp 146 | ) 147 | 148 | 149 | 150 | ## Add cmake target dependencies of the library 151 | ## as an example, code may need to be generated before libraries 152 | ## either from message generation or dynamic reconfigure 153 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 154 | 155 | ## Declare a C++ executable 156 | ## With catkin_make all packages are built within a single CMake context 157 | ## The recommended prefix ensures that target names across packages don't collide 158 | add_executable(rrtplanner_node src/rrtplanner_node.cpp src/rrt.cpp) 159 | target_link_libraries(rrtplanner_node 160 | ${catkin_LIBRARIES} 161 | ${OCTOMAP_LIBRARIES} 162 | ) 163 | add_dependencies(rrtplanner_node 164 | ${catkin_EXPORTED_TARGETS} 165 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 166 | rrtplanner_generate_messages_cpp 167 | ) 168 | 169 | 170 | ## Rename C++ executable without prefix 171 | ## The above recommended prefix causes long target names, the following renames the 172 | ## target back to the shorter version for ease of user use 173 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 174 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 175 | 176 | ## Add cmake target dependencies of the executable 177 | ## same as for the library above 178 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 179 | 180 | ## Specify libraries to link a library or executable target against 181 | # target_link_libraries(${PROJECT_NAME}_node 182 | # ${catkin_LIBRARIES} 183 | # ) 184 | 185 | ############# 186 | ## Install ## 187 | ############# 188 | 189 | # all install targets should use catkin DESTINATION variables 190 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 191 | 192 | ## Mark executable scripts (Python etc.) for installation 193 | ## in contrast to setup.py, you can choose the destination 194 | # install(PROGRAMS 195 | # scripts/my_python_script 196 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 197 | # ) 198 | 199 | ## Mark executables and/or libraries for installation 200 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 201 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 202 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 203 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 204 | # ) 205 | 206 | ## Mark cpp header files for installation 207 | # install(DIRECTORY include/${PROJECT_NAME}/ 208 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 209 | # FILES_MATCHING PATTERN "*.h" 210 | # PATTERN ".svn" EXCLUDE 211 | # ) 212 | 213 | ## Mark other files for installation (e.g. launch and bag files, etc.) 214 | # install(FILES 215 | # # myfile1 216 | # # myfile2 217 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 218 | # ) 219 | 220 | ############# 221 | ## Testing ## 222 | ############# 223 | 224 | ## Add gtest based cpp test target and link libraries 225 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_rrtplanner.cpp) 226 | # if(TARGET ${PROJECT_NAME}-test) 227 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 228 | # endif() 229 | 230 | ## Add folders to be run by python nosetests 231 | # catkin_add_nosetests(test) 232 | -------------------------------------------------------------------------------- /rrtplanner/action/rrt.action: -------------------------------------------------------------------------------- 1 | # Define the goal 2 | geometry_msgs/PoseStamped start 3 | geometry_msgs/PoseArray goal_poses 4 | ------- 5 | # Define the result 6 | nav_msgs/Path path 7 | ------- 8 | # Define a feedback message 9 | -------------------------------------------------------------------------------- /rrtplanner/include/rrtplanner/rrt.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef _RRT_H_ 3 | #define _RRT_H_ 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | namespace aeplanner_ns 20 | { 21 | struct RrtNode 22 | { 23 | Eigen::Vector3d pos; 24 | RrtNode *parent; 25 | std::vector children; 26 | ~RrtNode() 27 | { 28 | for (typename std::vector::iterator node_it = children.begin(); 29 | node_it != children.end(); ++node_it) 30 | { 31 | delete (*node_it); 32 | (*node_it) = NULL; 33 | } 34 | } 35 | 36 | double cost() 37 | { 38 | if (parent) 39 | return (pos - parent->pos).norm() + parent->cost(); 40 | return 0; 41 | } 42 | }; 43 | 44 | class Rrt 45 | { 46 | public: 47 | Rrt(const ros::NodeHandle &nh); 48 | void octomapCallback(const octomap_msgs::Octomap &msg); 49 | 50 | void execute(const rrtplanner::rrtGoalConstPtr &goal); 51 | void visualizeGoals(std::vector goals); 52 | void visualizeNode(geometry_msgs::Point pos, int id = 0); 53 | void visualizePose(geometry_msgs::Pose pose, int id = 0); 54 | void visualizeEdge(RrtNode *node, int id = 0); 55 | void visualizePath(RrtNode *node); 56 | 57 | Eigen::Vector3d sample(); 58 | RrtNode *chooseParent(kdtree *kd_tree, Eigen::Vector3d, double l); 59 | void rewire(kdtree *kd_tree, RrtNode *new_node, double l, double r, double r_os); 60 | Eigen::Vector3d getNewPos(Eigen::Vector3d sampled, Eigen::Vector3d parent, double l); 61 | bool collisionLine(Eigen::Vector3d p1, Eigen::Vector3d p2, double r); 62 | RrtNode *addNodeToTree(kdtree *kd_tree, RrtNode *parent, Eigen::Vector3d new_pos); 63 | RrtNode *getGoal(kdtree *goal_tree, RrtNode *new_node, double l, double r, double r_os); 64 | nav_msgs::Path getBestPath(std::vector goals); 65 | std::vector checkIfGoalReached(kdtree *goal_tree, RrtNode *new_node, double l, double r, double r_os); 66 | 67 | private: 68 | ros::NodeHandle nh_; 69 | std::shared_ptr ot_; 70 | 71 | ros::Subscriber octomap_sub_; 72 | actionlib::SimpleActionServer as_; 73 | 74 | std::string frame_id_; 75 | 76 | ros::Publisher path_pub_; 77 | double min_nodes_; 78 | double bounding_radius_; 79 | double bounding_overshoot_; 80 | double extension_range_; 81 | std::vector boundary_min_; 82 | std::vector boundary_max_; 83 | }; 84 | 85 | float CylTest_CapsFirst(const octomap::point3d &pt1, 86 | const octomap::point3d &pt2, 87 | float lsq, float rsq, const octomap::point3d &pt); 88 | } // namespace aeplanner_ns 89 | 90 | #endif 91 | -------------------------------------------------------------------------------- /rrtplanner/launch/rrtplanner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /rrtplanner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rrtplanner 4 | 0.0.0 5 | The rrtplanner package 6 | 7 | 8 | 9 | 10 | Magnus Selin 11 | 12 | 13 | 14 | 15 | 16 | TODO 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 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | roscpp 55 | message_generation 56 | geometry_msgs 57 | visualization_msgs 58 | octomap_world 59 | kdtree 60 | nav_msgs 61 | 62 | roscpp 63 | message_runtime 64 | geometry_msgs 65 | visualization_msgs 66 | octomap_world 67 | kdtree 68 | nav_msgs 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | -------------------------------------------------------------------------------- /rrtplanner/src/rrt.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace aeplanner_ns 4 | { 5 | Rrt::Rrt(const ros::NodeHandle& nh) 6 | : nh_(nh) 7 | , frame_id_("map") 8 | , path_pub_(nh_.advertise("rrt_path", 1000)) 9 | , octomap_sub_(nh_.subscribe("octomap", 1, &Rrt::octomapCallback, this)) 10 | , as_(nh_, "rrt", boost::bind(&Rrt::execute, this, _1), false) 11 | { 12 | std::string ns = ros::this_node::getNamespace(); 13 | bounding_radius_ = 0.5; 14 | bounding_overshoot_ = 0.5; 15 | extension_range_ = 1.0; 16 | min_nodes_ = 100; 17 | if (!ros::param::get(ns + "/rrt/min_nodes", min_nodes_)) 18 | ROS_WARN("No minimum nodes specified default is 100"); 19 | if (!ros::param::get(ns + "/system/bbx/r", bounding_radius_)) 20 | ROS_WARN("No bounding radius specified default is 0.5 m"); 21 | if (!ros::param::get(ns + "/system/bbx/overshoot", bounding_overshoot_)) 22 | ROS_WARN("No overshoot paramerer specified, default is 0.5 m"); 23 | if (!ros::param::get(ns + "/aep/tree/extension_range", extension_range_)) 24 | ROS_WARN("No extension range specified, default is 1.0 m"); 25 | if (!ros::param::get(ns + "/boundary/min", boundary_min_)) 26 | ROS_WARN("No boundary min specified."); 27 | if (!ros::param::get(ns + "/boundary/max", boundary_max_)) 28 | ROS_WARN("No boundary max specified."); 29 | 30 | ot_ = std::make_shared( 31 | 1); // Create dummy OcTree to prevent crash due to ot_ tree not initialized 32 | as_.start(); 33 | } 34 | 35 | void Rrt::execute(const rrtplanner::rrtGoalConstPtr& goal) 36 | { 37 | rrtplanner::rrtResult result; 38 | if (!ot_) 39 | { 40 | ROS_WARN("No octomap received"); 41 | as_.setSucceeded(result); 42 | return; 43 | } 44 | if (!goal->goal_poses.poses.size()) 45 | { 46 | ROS_WARN("No goals received"); 47 | as_.setSucceeded(result); 48 | return; 49 | } 50 | 51 | int N = min_nodes_; 52 | double l = extension_range_; 53 | double r = bounding_radius_; 54 | double r_os = bounding_overshoot_; 55 | std::vector found_goals; 56 | 57 | kdtree* kd_tree = kd_create(3); // Initalize tree 58 | kdtree* goal_tree = kd_create(3); // kd tree with all goals 59 | for (int i = 0; i < goal->goal_poses.poses.size(); ++i) 60 | { 61 | Eigen::Vector3d* g = new Eigen::Vector3d(goal->goal_poses.poses[i].position.x, 62 | goal->goal_poses.poses[i].position.y, 63 | goal->goal_poses.poses[i].position.z); 64 | kd_insert3(goal_tree, (*g)[0], (*g)[1], (*g)[2], g); 65 | } 66 | 67 | // Initialize root position 68 | RrtNode* root = new RrtNode; 69 | root->pos[0] = goal->start.pose.position.x; 70 | root->pos[1] = goal->start.pose.position.y; 71 | root->pos[2] = goal->start.pose.position.z; 72 | root->parent = NULL; 73 | 74 | kd_insert3(kd_tree, root->pos[0], root->pos[1], root->pos[2], root); 75 | 76 | visualizeNode(goal->start.pose.position, 1000); 77 | visualizeGoals(goal->goal_poses.poses); 78 | 79 | for (int i = 0; i < N /* or !found_goals.size() */; ++i) 80 | { 81 | // Sample new position 82 | Eigen::Vector3d z_samp = sample(); 83 | 84 | // Get nearest neighbour 85 | RrtNode* z_parent = chooseParent(kd_tree, z_samp, l); 86 | if (!z_parent) 87 | continue; 88 | 89 | // Calculate position for new node 90 | Eigen::Vector3d new_pos = getNewPos(z_samp, z_parent->pos, l); 91 | Eigen::Vector3d direction = new_pos - z_parent->pos; 92 | 93 | if (!collisionLine(z_parent->pos, new_pos + direction.normalized() * r_os, r)) 94 | { 95 | // Add node to tree 96 | RrtNode* z_new = addNodeToTree(kd_tree, z_parent, new_pos); 97 | rewire(kd_tree, z_new, l, r, r_os); 98 | 99 | visualizeEdge(z_new, i); 100 | 101 | // Check if goal has been reached 102 | RrtNode* tmp_goal = getGoal(goal_tree, z_new, l, r, r_os); 103 | if (tmp_goal) 104 | { 105 | found_goals.push_back(tmp_goal); 106 | } 107 | } 108 | else 109 | { 110 | --i; 111 | } 112 | } 113 | 114 | result.path = getBestPath(found_goals); 115 | 116 | delete root; 117 | kd_free(kd_tree); 118 | kd_free(goal_tree); 119 | 120 | as_.setSucceeded(result); 121 | } 122 | 123 | void Rrt::octomapCallback(const octomap_msgs::Octomap& msg) 124 | { 125 | octomap::AbstractOcTree* aot = octomap_msgs::msgToMap(msg); 126 | octomap::OcTree* ot = (octomap::OcTree*)aot; 127 | ot_ = std::make_shared(*ot); 128 | 129 | if (ot) 130 | delete ot; 131 | } 132 | 133 | Eigen::Vector3d Rrt::sample() 134 | { 135 | Eigen::Vector3d x_samp; 136 | for (int i = 0; i < 3; ++i) 137 | { 138 | do 139 | { 140 | x_samp[i] = 60 * (((double)rand()) / ((double)RAND_MAX) - 0.5); 141 | } while(x_samp[i] < boundary_min_[i] or x_samp[i] > boundary_max_[i]); 142 | } 143 | 144 | return x_samp; 145 | } 146 | 147 | RrtNode* Rrt::chooseParent(kdtree* kd_tree, Eigen::Vector3d node, double l) 148 | { 149 | kdres* nearest = kd_nearest_range3(kd_tree, node[0], node[1], node[2], l + 0.5); 150 | if (kd_res_size(nearest) <= 0) 151 | { 152 | nearest = kd_nearest3(kd_tree, node[0], node[1], node[2]); 153 | } 154 | if (kd_res_size(nearest) <= 0) 155 | { 156 | kd_res_free(nearest); 157 | return NULL; 158 | } 159 | 160 | RrtNode* node_nn = (RrtNode*)kd_res_item_data(nearest); 161 | int i = 0; 162 | 163 | RrtNode* best_node = node_nn; 164 | while (!kd_res_end(nearest)) 165 | { 166 | node_nn = (RrtNode*)kd_res_item_data(nearest); 167 | if (best_node and node_nn->cost() < best_node->cost()) 168 | best_node = node_nn; 169 | 170 | kd_res_next(nearest); 171 | } 172 | 173 | kd_res_free(nearest); 174 | return best_node; 175 | } 176 | 177 | void Rrt::rewire(kdtree* kd_tree, RrtNode* new_node, double l, double r, double r_os) 178 | { 179 | RrtNode* node_nn; 180 | kdres* nearest = kd_nearest_range3(kd_tree, new_node->pos[0], new_node->pos[1], 181 | new_node->pos[2], l + 0.5); 182 | while (!kd_res_end(nearest)) 183 | { 184 | node_nn = (RrtNode*)kd_res_item_data(nearest); 185 | if (node_nn->cost() > new_node->cost() + (node_nn->pos - new_node->pos).norm()) 186 | { 187 | if (!collisionLine( 188 | new_node->pos, 189 | node_nn->pos + (node_nn->pos - new_node->pos).normalized() * r_os, r)) 190 | node_nn->parent = new_node; 191 | } 192 | kd_res_next(nearest); 193 | } 194 | } 195 | 196 | Eigen::Vector3d Rrt::getNewPos(Eigen::Vector3d sampled, Eigen::Vector3d parent, 197 | double l) 198 | { 199 | Eigen::Vector3d direction = sampled - parent; 200 | if (direction.norm() > l) 201 | direction = l * direction.normalized(); 202 | 203 | return parent + direction; 204 | } 205 | 206 | RrtNode* Rrt::addNodeToTree(kdtree* kd_tree, RrtNode* parent, 207 | Eigen::Vector3d new_pos) 208 | { 209 | RrtNode* new_node = new RrtNode; 210 | new_node->pos = new_pos; 211 | 212 | new_node->parent = parent; 213 | parent->children.push_back(new_node); 214 | kd_insert3(kd_tree, new_node->pos[0], new_node->pos[1], new_node->pos[2], 215 | new_node); 216 | 217 | return new_node; 218 | } 219 | 220 | RrtNode* Rrt::getGoal(kdtree* goal_tree, RrtNode* new_node, double l, double r, 221 | double r_os) 222 | { 223 | kdres* nearest_goal = 224 | kd_nearest3(goal_tree, new_node->pos[0], new_node->pos[1], new_node->pos[2]); 225 | if (kd_res_size(nearest_goal) <= 0) 226 | { 227 | kd_res_free(nearest_goal); 228 | return NULL; 229 | } 230 | Eigen::Vector3d* g_nn = (Eigen::Vector3d*)kd_res_item_data(nearest_goal); 231 | kd_res_free(nearest_goal); 232 | 233 | if ((*g_nn - new_node->pos).norm() < 1.5) 234 | if (!collisionLine(new_node->pos, 235 | *g_nn + (*g_nn - new_node->pos).normalized() * r_os, r)) 236 | return new_node; 237 | 238 | return NULL; 239 | } 240 | 241 | nav_msgs::Path Rrt::getBestPath(std::vector goals) 242 | { 243 | nav_msgs::Path path; 244 | if (goals.size() == 0) 245 | { 246 | return path; 247 | } 248 | 249 | RrtNode* best_node = goals[0]; 250 | 251 | for (int i = 0; i < goals.size(); ++i) 252 | if (best_node->cost() > goals[i]->cost()) 253 | best_node = goals[i]; 254 | 255 | RrtNode* n = best_node; 256 | for (int id = 0; n->parent; ++id) 257 | { 258 | geometry_msgs::PoseStamped p; 259 | p.pose.position.x = n->pos[0]; 260 | p.pose.position.y = n->pos[1]; 261 | p.pose.position.z = n->pos[2]; 262 | Eigen::Quaternion q; 263 | Eigen::Vector3d init(1.0, 0.0, 0.0); 264 | // Zero out rotation along 265 | // x and y axis so only 266 | // yaw is kept 267 | Eigen::Vector3d dir(n->pos[0] - n->parent->pos[0], n->pos[1] - n->parent->pos[1], 268 | 0); 269 | q.setFromTwoVectors(init, dir); 270 | 271 | p.pose.orientation.x = q.x(); 272 | p.pose.orientation.y = q.y(); 273 | p.pose.orientation.z = q.z(); 274 | p.pose.orientation.w = q.w(); 275 | 276 | path.poses.push_back(p); 277 | visualizePose(p.pose, id); 278 | 279 | n = n->parent; 280 | } 281 | 282 | visualizePath(best_node); 283 | return path; 284 | } 285 | 286 | std::vector Rrt::checkIfGoalReached(kdtree* goal_tree, 287 | RrtNode* new_node, double l, 288 | double r, double r_os) 289 | { 290 | std::vector path; 291 | 292 | kdres* nearest_goal = 293 | kd_nearest3(goal_tree, new_node->pos[0], new_node->pos[1], new_node->pos[2]); 294 | if (kd_res_size(nearest_goal) <= 0) 295 | { 296 | kd_res_free(nearest_goal); 297 | return path; 298 | } 299 | Eigen::Vector3d* g_nn = (Eigen::Vector3d*)kd_res_item_data(nearest_goal); 300 | kd_res_free(nearest_goal); 301 | 302 | if ((*g_nn - new_node->pos).norm() < 2 * l) 303 | { 304 | if (!collisionLine(new_node->pos, 305 | *g_nn + (*g_nn - new_node->pos).normalized() * r_os, r)) 306 | { 307 | RrtNode* n = new_node; 308 | for (int id = 0; n->parent; ++id) 309 | { 310 | geometry_msgs::Pose p; 311 | p.position.x = n->pos[0]; 312 | p.position.y = n->pos[1]; 313 | p.position.z = n->pos[2]; 314 | Eigen::Quaternion q; 315 | Eigen::Vector3d init(1.0, 0.0, 0.0); 316 | // Zero out rotation 317 | // along x and y axis 318 | // so only yaw is kept 319 | Eigen::Vector3d dir(n->pos[0] - n->parent->pos[0], 320 | n->pos[1] - n->parent->pos[1], 0); 321 | q.setFromTwoVectors(init, dir); 322 | 323 | p.orientation.x = q.x(); 324 | p.orientation.y = q.y(); 325 | p.orientation.z = q.z(); 326 | p.orientation.w = q.w(); 327 | 328 | path.push_back(p); 329 | visualizePose(p, id); 330 | 331 | n = n->parent; 332 | } 333 | 334 | visualizePath(new_node); 335 | } 336 | } 337 | 338 | return path; 339 | } 340 | 341 | bool Rrt::collisionLine(Eigen::Vector3d p1, Eigen::Vector3d p2, double r) 342 | { 343 | std::shared_ptr ot = ot_; 344 | 345 | octomap::point3d start(p1[0], p1[1], p1[2]); 346 | octomap::point3d end(p2[0], p2[1], p2[2]); 347 | octomap::point3d min(std::min(p1[0], p2[0]) - r, std::min(p1[1], p2[1]) - r, 348 | std::min(p1[2], p2[2]) - r); 349 | octomap::point3d max(std::max(p1[0], p2[0]) + r, std::max(p1[1], p2[1]) + r, 350 | std::max(p1[2], p2[2]) + r); 351 | double lsq = (end - start).norm_sq(); 352 | double rsq = r * r; 353 | 354 | octomap::point3d query(p2[0], p2[1], p2[2]); 355 | octomap::OcTreeNode* ot_res = ot->search(query); 356 | if (!ot_res) 357 | return true; 358 | 359 | for (octomap::OcTree::leaf_bbx_iterator it = ot->begin_leafs_bbx(min, max), 360 | it_end = ot->end_leafs_bbx(); 361 | it != it_end; ++it) 362 | { 363 | octomap::point3d pt(it.getX(), it.getY(), it.getZ()); 364 | 365 | if (it->getLogOdds() > 0) 366 | { // Node is occupied 367 | if (CylTest_CapsFirst(start, end, lsq, rsq, pt) > 0 or (end - pt).norm() < r) 368 | { 369 | return true; 370 | } 371 | } 372 | } 373 | 374 | // Check all unknown nodes 375 | /* 376 | octomap::point3d_list 377 | node_centers; 378 | ot_->getUnknownLeafCenters(node_centers, 379 | min, max); 380 | std::list::iterator 381 | it; for (it = 382 | node_centers.begin(); it 383 | != node_centers.end(); 384 | ++it){ 385 | if(CylTest_CapsFirst(start, 386 | end, lsq, rsq, *it) > 0 or 387 | (end-*it).norm() < r) { 388 | return true; 389 | } 390 | } 391 | */ 392 | 393 | return false; 394 | } 395 | 396 | //----------------------------------------------------------------------------- 397 | // Name: CylTest_CapsFirst 398 | // Orig: Greg James - 399 | // gjames@NVIDIA.com Lisc: 400 | // Free code - no warranty & 401 | // no money back. Use it all 402 | // you want Desc: 403 | // This function tests if 404 | // the 3D point 'pt' lies 405 | // within an arbitrarily 406 | // oriented cylinder. The 407 | // cylinder is defined by an 408 | // axis from 'pt1' to 'pt2', 409 | // the axis having a length 410 | // squared of 'lsq' 411 | // (pre-compute for each 412 | // cylinder to avoid repeated 413 | // work!), and radius squared 414 | // of 'rsq'. 415 | // The function tests 416 | // against the end caps 417 | // first, which is cheap -> 418 | // only 419 | // a single dot product to 420 | // test against the parallel 421 | // cylinder caps. If the 422 | // point is within these, more 423 | // work is done to find the 424 | // distance of the point from 425 | // the cylinder axis. 426 | // Fancy Math (TM) makes 427 | // the whole test possible 428 | // with only two 429 | // dot-products 430 | // a subtract, and two 431 | // multiplies. For clarity, 432 | // the 2nd mult is kept as a 433 | // divide. It might be faster 434 | // to change this to a mult by 435 | // also passing in 1/lengthsq 436 | // and using that instead. 437 | // Elminiate the first 3 438 | // subtracts by specifying 439 | // the cylinder as a base 440 | // point on one end cap and a 441 | // vector to the other end cap 442 | // (pass in {dx,dy,dz} instead 443 | // of 'pt2' ). 444 | // 445 | // The dot product is 446 | // constant along a plane 447 | // perpendicular to a 448 | // vector. The magnitude of 449 | // the cross product 450 | // divided by one vector 451 | // length is 452 | // constant along a cylinder 453 | // surface defined by the 454 | // other vector as axis. 455 | // 456 | // Return: -1.0 if point is 457 | // outside the cylinder 458 | // Return: distance squared 459 | // from cylinder axis if point 460 | // is inside. 461 | // 462 | //----------------------------------------------------------------------------- 463 | float CylTest_CapsFirst(const octomap::point3d& pt1, const octomap::point3d& pt2, 464 | float lsq, float rsq, const octomap::point3d& pt) 465 | { 466 | float dx, dy, 467 | dz; // vector d from 468 | // line segment 469 | // point 1 to point 470 | // 2 471 | float pdx, pdy, 472 | pdz; // vector pd from 473 | // point 1 to test 474 | // point 475 | float dot, dsq; 476 | 477 | dx = pt2.x() - pt1.x(); // translate 478 | // so pt1 is 479 | // origin. 480 | // Make vector 481 | // from 482 | dy = pt2.y() - pt1.y(); // pt1 to 483 | // pt2. Need 484 | // for this 485 | // is easily 486 | // eliminated 487 | dz = pt2.z() - pt1.z(); 488 | 489 | pdx = pt.x() - pt1.x(); // vector from 490 | // pt1 to test 491 | // point. 492 | pdy = pt.y() - pt1.y(); 493 | pdz = pt.z() - pt1.z(); 494 | 495 | // Dot the d and pd vectors 496 | // to see if point lies 497 | // behind the cylinder cap 498 | // at pt1.x, pt1.y, pt1.z 499 | 500 | dot = pdx * dx + pdy * dy + pdz * dz; 501 | 502 | // If dot is less than zero 503 | // the point is behind the 504 | // pt1 cap. If greater than 505 | // the cylinder axis line 506 | // segment length squared 507 | // then the point is outside 508 | // the other end cap at pt2. 509 | 510 | if (dot < 0.0f || dot > lsq) 511 | { 512 | return (-1.0f); 513 | } 514 | else 515 | { 516 | // Point lies within the 517 | // parallel caps, so find 518 | // distance squared from 519 | // point to line, using 520 | // the fact that sin^2 + 521 | // cos^2 = 1 the dot = 522 | // cos() * |d||pd|, and 523 | // cross*cross = sin^2 * 524 | // |d|^2 * |pd|^2 525 | // Carefull: '*' means 526 | // mult for scalars and 527 | // dotproduct for vectors 528 | // In short, where dist is 529 | // pt distance to cyl 530 | // axis: dist = sin( pd to 531 | // d ) * |pd| distsq = dsq 532 | // = (1 - cos^2( pd to d)) 533 | // * |pd|^2 dsq = ( 1 - 534 | // (pd * d)^2 / (|pd|^2 * 535 | // |d|^2) ) * |pd|^2 dsq = 536 | // pd * pd - dot * dot / 537 | // lengthsq 538 | // where lengthsq is d*d 539 | // or |d|^2 that is 540 | // passed into this 541 | // function 542 | 543 | // distance squared to the 544 | // cylinder axis: 545 | 546 | dsq = (pdx * pdx + pdy * pdy + pdz * pdz) - dot * dot / lsq; 547 | 548 | if (dsq > rsq) 549 | { 550 | return (-1.0f); 551 | } 552 | else 553 | { 554 | return (dsq); // return 555 | // distance 556 | // squared 557 | // to 558 | // axis 559 | } 560 | } 561 | } 562 | 563 | void Rrt::visualizeNode(geometry_msgs::Point pos, int id) 564 | { 565 | visualization_msgs::Marker a; 566 | a.header.stamp = ros::Time::now(); 567 | a.header.seq = id; 568 | a.header.frame_id = frame_id_; 569 | a.id = id; 570 | a.ns = "nodes"; 571 | a.type = visualization_msgs::Marker::SPHERE; 572 | a.action = visualization_msgs::Marker::ADD; 573 | a.pose.position = pos; 574 | 575 | a.scale.x = 0.2; 576 | a.scale.y = 0.2; 577 | a.scale.z = 0.2; 578 | a.color.r = 0.2; 579 | a.color.g = 0.7; 580 | a.color.b = 0.2; 581 | ; 582 | a.color.a = 1; 583 | a.lifetime = ros::Duration(5.0); 584 | a.frame_locked = false; 585 | path_pub_.publish(a); 586 | } 587 | 588 | void Rrt::visualizePose(geometry_msgs::Pose pose, int id) 589 | { 590 | visualization_msgs::Marker a; 591 | a.header.stamp = ros::Time::now(); 592 | a.header.seq = id; 593 | a.header.frame_id = frame_id_; 594 | a.id = id; 595 | a.ns = "pose"; 596 | a.type = visualization_msgs::Marker::ARROW; 597 | a.action = visualization_msgs::Marker::ADD; 598 | a.pose = pose; 599 | a.scale.x = 0.4; 600 | a.scale.y = 0.1; 601 | a.scale.z = 0.1; 602 | a.color.r = 1.0; 603 | a.color.g = 0.0; 604 | a.color.b = 0.0; 605 | a.color.a = 1.0; 606 | a.lifetime = ros::Duration(5.0); 607 | a.frame_locked = false; 608 | 609 | path_pub_.publish(a); 610 | } 611 | 612 | void Rrt::visualizeEdge(RrtNode* node, int id) 613 | { 614 | visualization_msgs::Marker a; 615 | a.header.stamp = ros::Time::now(); 616 | a.header.seq = id; 617 | a.header.frame_id = frame_id_; 618 | a.id = id; 619 | a.ns = "vp_branches"; 620 | a.type = visualization_msgs::Marker::ARROW; 621 | a.action = visualization_msgs::Marker::ADD; 622 | a.pose.position.x = node->parent->pos[0]; 623 | a.pose.position.y = node->parent->pos[1]; 624 | a.pose.position.z = node->parent->pos[2]; 625 | Eigen::Quaternion q; 626 | Eigen::Vector3d init(1.0, 0.0, 0.0); 627 | Eigen::Vector3d dir(node->pos[0] - node->parent->pos[0], 628 | node->pos[1] - node->parent->pos[1], 629 | node->pos[2] - node->parent->pos[2]); 630 | q.setFromTwoVectors(init, dir); 631 | q.normalize(); 632 | a.pose.orientation.x = q.x(); 633 | a.pose.orientation.y = q.y(); 634 | a.pose.orientation.z = q.z(); 635 | a.pose.orientation.w = q.w(); 636 | a.scale.x = dir.norm(); 637 | a.scale.y = 0.05; 638 | a.scale.z = 0.05; 639 | a.color.r = 1.0; 640 | a.color.g = 0.3; 641 | a.color.b = 0.7; 642 | a.color.a = 1.0; 643 | a.lifetime = ros::Duration(5.0); 644 | a.frame_locked = false; 645 | 646 | path_pub_.publish(a); 647 | } 648 | 649 | void Rrt::visualizePath(RrtNode* node) 650 | { 651 | for (int id = 0; node->parent; ++id) 652 | { 653 | visualization_msgs::Marker a; 654 | a.header.stamp = ros::Time::now(); 655 | a.header.seq = id; 656 | a.header.frame_id = frame_id_; 657 | a.id = id; 658 | a.ns = "path"; 659 | a.type = visualization_msgs::Marker::ARROW; 660 | a.action = visualization_msgs::Marker::ADD; 661 | a.pose.position.x = node->parent->pos[0]; 662 | a.pose.position.y = node->parent->pos[1]; 663 | a.pose.position.z = node->parent->pos[2]; 664 | Eigen::Quaternion q; 665 | Eigen::Vector3d init(1.0, 0.0, 0.0); 666 | Eigen::Vector3d dir(node->pos[0] - node->parent->pos[0], 667 | node->pos[1] - node->parent->pos[1], 668 | node->pos[2] - node->parent->pos[2]); 669 | q.setFromTwoVectors(init, dir); 670 | q.normalize(); 671 | a.pose.orientation.x = q.x(); 672 | a.pose.orientation.y = q.y(); 673 | a.pose.orientation.z = q.z(); 674 | a.pose.orientation.w = q.w(); 675 | a.scale.x = dir.norm(); 676 | a.scale.y = 0.07; 677 | a.scale.z = 0.07; 678 | a.color.r = 0.7; 679 | a.color.g = 0.7; 680 | a.color.b = 0.3; 681 | a.color.a = 1.0; 682 | a.lifetime = ros::Duration(100.0); 683 | a.frame_locked = false; 684 | 685 | path_pub_.publish(a); 686 | 687 | node = node->parent; 688 | } 689 | } 690 | 691 | void Rrt::visualizeGoals(std::vector goals) 692 | { 693 | for (int i = 0; i < goals.size(); ++i) 694 | { 695 | visualization_msgs::Marker a; 696 | a.header.stamp = ros::Time::now(); 697 | a.header.seq = i; 698 | a.header.frame_id = frame_id_; 699 | a.id = i; 700 | a.ns = "goals"; 701 | a.type = visualization_msgs::Marker::ARROW; 702 | a.action = visualization_msgs::Marker::ADD; 703 | a.pose = goals[i]; 704 | 705 | a.scale.x = 0.2; 706 | a.scale.y = 0.1; 707 | a.scale.z = 0.1; 708 | a.color.r = 1.0; 709 | a.color.g = 0.3; 710 | a.color.b = 0.7; 711 | a.color.a = 1; 712 | a.lifetime = ros::Duration(100.0); 713 | a.frame_locked = false; 714 | path_pub_.publish(a); 715 | } 716 | } 717 | 718 | } // namespace aeplanner_ns 719 | -------------------------------------------------------------------------------- /rrtplanner/src/rrtplanner_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char** argv) 4 | { 5 | ros::init(argc, argv, "rrt"); 6 | ros::NodeHandle nh; 7 | 8 | aeplanner_ns::Rrt rrt(nh); 9 | 10 | ros::spin(); 11 | return 0; 12 | } 13 | --------------------------------------------------------------------------------