Initial commit

.gitignore

@@ -0,0 +1,2 @@
+*.pyc
+*.vscode/

CMakeLists.txt

@@ -0,0 +1,84 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(uav_frontier_exploration_3d)
+SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+
+find_package(catkin REQUIRED COMPONENTS
+  geometry_msgs
+  nav_msgs
+  roscpp
+  rospy
+  std_msgs
+  tf
+  visualization_msgs
+  message_generation
+  pcl_ros
+  pcl_conversions
+  dynamic_reconfigure
+  geographic_msgs
+  octomap_msgs
+  octomap_ros
+  nodelet
+  mean_shift_clustering
+)
+find_package(Boost)
+find_package(PCL 1.8 REQUIRED COMPONENTS
+    common
+    sample_consensus
+    io
+    ros
+    segmentation
+    filters)
+find_package(octomap 1.7 REQUIRED)
+add_definitions(-DOCTOMAP_NODEBUGOUT)
+find_package(cmake_modules REQUIRED)
+
+generate_messages(
+    DEPENDENCIES
+    std_msgs
+    geographic_msgs
+)
+
+catkin_package(
+    INCLUDE_DIRS include
+    LIBRARIES frontier_exploration_3d
+    CATKIN_DEPENDS octomap_ros pcl_ros
+    DEPENDS octomap PCL)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+  ${OCTOMAP_INCLUDE_DIRS}
+ )
+
+link_directories(
+    ${PCL_LIBRARY_DIRS})
+add_definitions(
+    ${PCL_DEFINITIONS})  
+
+set(LINK_LIBS
+    ${OCTOMAP_LIBRARIES}
+    ${catkin_LIBRARIES}
+    ${PCL_LIBRARIES}
+)
+
+add_library(frontier_exploration_3d 
+    src/FrontierServer.cpp)
+target_link_libraries(frontier_exploration_3d ${LINK_LIBS} yaml-cpp)
+
+add_executable(frontier_server_node src/FrontierServerNode.cpp src/OctomapServer.cpp src/BestFrontier.cpp)
+target_link_libraries(frontier_server_node frontier_exploration_3d ${LINK_LIBS})
+
+install(DIRECTORY include/${PROJECT_NAME}/
+        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+        FILES_MATCHING PATTERN "*.h"
+        PATTERN ".svn" EXCLUDE)
+
+install(DIRECTORY launch/
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+    PATTERN ".svn" EXCLUDE
+)
+

README.md

@@ -0,0 +1,2 @@
+# uav_frontier_exploration_3d
+

config/exploration.lua

@@ -0,0 +1,86 @@
+-- Copyright 2016 The Cartographer Authors
+--
+-- Licensed under the Apache License, Version 2.0 (the "License");
+-- you may not use this file except in compliance with the License.
+-- You may obtain a copy of the License at
+--
+--      http://www.apache.org/licenses/LICENSE-2.0
+--
+-- Unless required by applicable law or agreed to in writing, software
+-- distributed under the License is distributed on an "AS IS" BASIS,
+-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+-- See the License for the specific language governing permissions and
+-- limitations under the License.
+
+include "map_builder.lua"
+include "trajectory_builder.lua"
+namespace = os.getenv("UAV_NAMESPACE")
+
+options = {
+  map_builder = MAP_BUILDER,
+  trajectory_builder = TRAJECTORY_BUILDER,
+  map_frame =  namespace.."/slam/map",
+  tracking_frame =  namespace.."/slam/base_link",
+  published_frame =  namespace.."/slam/base_link",
+  odom_frame = namespace.."/slam/odom",
+  provide_odom_frame = false,
+  publish_frame_projected_to_2d = false,
+  use_pose_extrapolator = true,
+  use_odometry = false,
+  use_nav_sat = false,
+  nav_sat_use_predefined_enu_frame = false,
+  nav_sat_predefined_enu_frame_lat_deg = 45.813902,
+  nav_sat_predefined_enu_frame_lon_deg = 16.038766,
+  nav_sat_predefined_enu_frame_alt_m = 168.259294525,
+  nav_sat_translation_weight = 1.,
+  nav_sat_inverse_covariance_weight = 0.,
+  use_landmarks = false,
+  num_laser_scans = 0,
+  num_multi_echo_laser_scans = 0,
+  num_subdivisions_per_laser_scan = 1,
+  num_point_clouds = 1,
+  lookup_transform_timeout_sec = 0.2,
+  submap_publish_period_sec = 0.3,
+  pose_publish_period_sec = 5e-3,
+  trajectory_publish_period_sec = 30e-3,
+  rangefinder_sampling_ratio = 1.,
+  odometry_sampling_ratio = 1.,
+  fixed_frame_pose_sampling_ratio = 1.,
+  imu_sampling_ratio = 1.,
+  landmarks_sampling_ratio = 1.,
+}
+
+TRAJECTORY_BUILDER.collate_fixed_frame = false
+
+TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1
+TRAJECTORY_BUILDER_3D.ceres_scan_matcher.translation_weight = 10.
+TRAJECTORY_BUILDER_3D.ceres_scan_matcher.rotation_weight = 600
+TRAJECTORY_BUILDER_3D.submaps.high_resolution = 0.05
+TRAJECTORY_BUILDER_3D.submaps.high_resolution_max_range = 40.
+TRAJECTORY_BUILDER_3D.submaps.num_range_data = 50.
+-- TRAJECTORY_BUILDER_3D.submaps.num_range_data = 90.
+TRAJECTORY_BUILDER_3D.high_resolution_adaptive_voxel_filter = {
+    max_length = 2.,
+    min_num_points = 150,
+    max_range = 50.,
+}
+
+TRAJECTORY_BUILDER_3D.low_resolution_adaptive_voxel_filter = {
+    max_length = 4.,
+    min_num_points = 200,
+    max_range = 70,
+}
+
+MAP_BUILDER.use_trajectory_builder_3d = true
+MAP_BUILDER.num_background_threads = 3
+POSE_GRAPH.optimization_problem.huber_scale = 5e2
+POSE_GRAPH.optimize_every_n_nodes = 0
+POSE_GRAPH.constraint_builder.sampling_ratio = 0.02
+POSE_GRAPH.constraint_builder.max_constraint_distance = 30.
+POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 200
+POSE_GRAPH.constraint_builder.min_score = 0.4
+POSE_GRAPH.constraint_builder.global_localization_min_score = 0.66
+POSE_GRAPH.optimization_problem.rotation_weight = 0
+POSE_GRAPH.optimization_problem.acceleration_weight = 1e2
+
+return options

config/kopterworx_exploration_sim_house.yaml

@@ -0,0 +1,56 @@
+exploration:
+  # simulation
+  global_frame: "/red/slam/map"
+  base_link_frame: "/red/slam/base_link"
+  # Exploration depth of octomap tree. 16 is best resolution, slowest exploration
+  # Save frontiers on that level
+  # experience shows that there is significant increase in search time by 
+  # setting this parameter to < 16)
+  depth: 14
+  # if the candidate point is inside the radius of the previous visited points,
+  # the candidate is deleted from the candidate list
+  radius: 2.5
+  # bounding box: necessary to limit the simulation 
+  # scenario (smaller than actual gazebo scenario)
+  # used for frontier detection
+  bbx_minX: -19.0
+  bbx_maxX: 7.0
+  bbx_minY: -7.0
+  bbx_maxY: 27.0
+  bbx_minZ: 1.0
+  bbx_maxZ: 5.0
+
+  # Length of one side of the cube inside which we calculate information gain
+  box_length: 5.0
+  # Gain to scale exp function in information gain
+  k_gain: 100.0
+  # Constant; small lambda means that motion is cheap and gives priority
+  # to the gain of information, lambda -> inf: closer points have priority
+  lambda: 0.1386
+  # Total volume of the environment for exploration in m^3 30x40x10 = 12000
+  volume: 12000
+
+sensor_model:
+  # Field of view in degrees
+  horizontalFOV: 360
+  verticalFOV: 30
+  # VelodyneVLP16 max range in meters
+  max_range: 10
+  pitch_angle: 10.0
+
+  probability_hit: 0.7
+  probability_miss: 0.4
+  clamping_thres_min: 0.12 
+  clamping_thres_max: 0.97
+
+octomap:
+  resolution: 0.5
+  octree_depth: 16
+  # Filename and file path for saving octomap
+  filename: "octomap_house.binvox.bt"
+  file_path: "catkin_ws/src/frontier_exploration_3d/data/"
+
+clustering:
+  # A higher bandwidth will result in fewer but larger clusters
+  # Can not be lower then resolution of voxels (parents)
+  kernel_bandwidth: 2

config/kopterworx_exploration_sim_large_env.yaml

@@ -0,0 +1,54 @@
+exploration:
+  global_frame: "/red/slam/map"
+  base_link_frame: "/red/slam/base_link"
+  # Exploration depth of octomap tree. 16 is best resolution, slowest exploration
+  # Save frontiers on that level
+  # experience shows that there is significant increase in search time by 
+  # setting this parameter to < 16)
+  depth: 14
+  # if the candidate point is inside the radius of the previous visited points,
+  # the candidate is deleted from the candidate list
+  radius: 5.5
+  # bounding box: necessary to limit the simulation 
+  # scenario (smaller than actual gazebo scenario)
+  # used for frontier detection
+  bbx_minX: -65.0
+  bbx_maxX: 65.0
+  bbx_minY: -90.0
+  bbx_maxY: 90.0
+  bbx_minZ: 1.0
+  bbx_maxZ: 5.0
+
+  # Length of one side of the cube inside which we calculate information gain
+  box_length: 8.0
+  # Gain to scale exp function in information gain
+  k_gain: 100.0
+  # Constant; small lambda means that motion is cheap and gives priority
+  lambda: 0.1386 
+
+
+  # Total volume of the environment for exploration in m^3
+  volume: 50000
+sensor_model:
+  # Field of view in degrees
+  horizontalFOV: 360
+  verticalFOV: 30
+  # VelodyneVLP16 max range in meters
+  max_range: 10
+  pitch_angle: 10.0
+
+  probability_hit: 0.7
+  probability_miss: 0.4
+  clamping_thres_min: 0.12 
+  clamping_thres_max: 0.97
+
+octomap:
+  resolution: 0.5
+  octree_depth: 16
+  # Filename and file path for saving octomap
+  filename: "octomap_large.binvox.bt"
+  file_path: "catkin_ws/src/frontier_exploration_3d/data/"
+
+clustering:
+  # A higher bandwidth will result in fewer but larger clusters
+  kernel_bandwidth: 2

config/vlp16-imu.urdf.xacro

@@ -0,0 +1,50 @@
+<?xml version="1.0"?>
+<robot name="stencil" xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:arg name="namespace" default="none"/>
+  <xacro:property name="namespace" value="$(arg namespace)" />
+
+  <material name="orange">
+    <color rgba="1.0 0.5 0.2 1" />
+  </material>
+  <material name="gray">
+    <color rgba="0.2 0.2 0.2 1" />
+  </material>
+
+  <link name="${namespace}/imu_link">
+    <visual>
+      <origin xyz="0.0 0.0 0.0" />
+      <geometry>
+        <box size="0.06 0.04 0.02" />
+      </geometry>
+      <material name="orange" />
+    </visual>
+  </link>
+
+  <link name="${namespace}/velodyne">
+    <visual>
+      <origin xyz="0.0 0.0 0.0" />
+      <geometry>
+        <cylinder length="0.07" radius="0.05" />
+      </geometry>
+      <material name="gray" />
+    </visual>
+  </link>
+
+  <link name="${namespace}/base_link" />
+
+  <joint name="${namespace}/imu_link_joint" type="fixed">
+    <parent link="${namespace}/base_link" />
+    <child link="${namespace}/imu_link" />
+    <origin xyz="0 0 0" rpy="0 0 0" />
+  </joint>
+
+  <joint name="${namespace}/velodyne_link_joint" type="fixed">
+    <parent link="${namespace}/base_link" />
+    <child link="${namespace}/velodyne" />
+    <origin xyz="0.08 0. -0.1578" rpy="3.151592653589793 0.1323284641020683 0." /> <!-- Horizontalni postav -->
+    <!-- origin xyz="0.065 0. -0.135" rpy="1.570796 0. -1.570796" / --> <!-- Vertikalni postav -->
+  </joint>
+
+</robot>
+
+

include/uav_frontier_exploration_3d/BestFrontier.h

@@ -0,0 +1,32 @@
+#ifndef BEST_FRONTIER_H_
+#define BEST_FRONTIER_H_
+
+#include <uav_frontier_exploration_3d/OctomapServer.h>
+
+namespace best_frontier
+{
+  class BestFrontier
+  {
+    public:
+      BestFrontier();
+      bool configureFromFile(string config_filename);
+      point3d bestFrontierInfGain(
+        octomap::OcTree* octree, point3d currentPosition, KeySet& Cells);
+
+    protected:
+      double calculateDistance(const point3d p1, const point3d p2)
+      {		
+        double distance = sqrt(
+        pow(p2.x() - p1.x(), 2) +
+        pow(p2.y() - p1.y(), 2) +
+        pow(p2.z() - p1.z(), 2));
+        return distance;
+      }
+      double calcMIBox(const octomap::OcTree *octree, const point3d &sensorOrigin); 
+
+      string m_configFilename;
+      double m_resolution, m_kGain, m_lambda, m_boxInfGainSize;
+      ofstream m_logfile; 
+    };
+}
+#endif