Initial commit, version 1.0.0

.clang-format

@@ -0,0 +1,66 @@
+---
+BasedOnStyle: Google
+AccessModifierOffset: -2
+ConstructorInitializerIndentWidth: 2
+AlignEscapedNewlinesLeft: false
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: false
+AllowShortIfStatementsOnASingleLine: false
+AllowShortLoopsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: None
+AllowShortLoopsOnASingleLine: false
+AlwaysBreakTemplateDeclarations: true
+AlwaysBreakBeforeMultilineStrings: false
+BreakBeforeBinaryOperators: false
+BreakBeforeTernaryOperators: false
+BreakConstructorInitializersBeforeComma: true
+BinPackParameters: true
+ColumnLimit: 120
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+DerivePointerBinding: false
+PointerBindsToType: true
+ExperimentalAutoDetectBinPacking: false
+IndentCaseLabels: true
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCSpaceBeforeProtocolList: true
+PenaltyBreakBeforeFirstCallParameter: 19
+PenaltyBreakComment: 60
+PenaltyBreakString: 1
+PenaltyBreakFirstLessLess: 1000
+PenaltyExcessCharacter: 1000
+PenaltyReturnTypeOnItsOwnLine: 90
+SpacesBeforeTrailingComments: 2
+Cpp11BracedListStyle: false
+Standard: Auto
+IndentWidth: 2
+TabWidth: 2
+UseTab: Never
+IndentFunctionDeclarationAfterType: false
+SpacesInParentheses: false
+SpacesInAngles: false
+SpaceInEmptyParentheses: false
+SpacesInCStyleCastParentheses: false
+SpaceAfterControlStatementKeyword: true
+SpaceBeforeAssignmentOperators: true
+ContinuationIndentWidth: 4
+SortIncludes: false
+SpaceAfterCStyleCast: false
+
+# Configure each individual brace in BraceWrapping
+BreakBeforeBraces: Custom
+
+# Control of individual brace wrapping cases
+BraceWrapping: {
+    AfterClass: 'true'
+    AfterControlStatement: 'true'
+    AfterEnum : 'true'
+    AfterFunction : 'true'
+    AfterNamespace : 'true'
+    AfterStruct : 'true'
+    AfterUnion : 'true'
+    BeforeCatch : 'true'
+    BeforeElse : 'true'
+    IndentBraces : 'false'
+}
+...

README.md

@@ -0,0 +1,80 @@
+# amcl3d
+
+### Overview
+
+This is a package is a **"Adaptive Monte-Carlo Localization in 3D"**.
+
+It is a particle filter that estimates the localization of a robot moving in a 3D environment without using GPS.
+
+It takes information from an odometry source, point-clouds from an onboard sensor (e.g. laser) and distance measurements from radio-range sensors.
+
+#### License
+
+Apache 2.0
+
+**Author: Paloma Carrasco Fernández (pcarrasco@catec.aero),
+          Francisco Cuesta Rodríguez (fcuesta@catec.aero),
+          Francisco J.Perez-Grau (fjperez@catec.aero)**
+
+**Affiliation: [FADA-CATEC](https://http://www.catec.aero//)**
+
+**Maintainer: Paloma Carrasco Fernández (pcarrasco@catec.aero),
+              Francisco Cuesta Rodríguez (fcuesta@catec.aero)**
+
+The amcl3d package has been tested under [ROS] Kinetic and Ubuntu 16.04.
+
+#### Publications
+
+If you want more information about the algorithm or use this work in your project, please check and cite the following publication:
+
+* Francisco J.Perez-Grau, Fernando Caballero, Antidio Viguria and Anibal Ollero:
+
+	**[Multi-sensor 3D Monte Carlo Localization (MCL) for long-term aerial robot navigation, 2017](https://journals.sagepub.com/doi/pdf/10.1177/1729881417732757)**
+
+#### Detailed Description
+
+To know in more detail the behavior of the package:
+
+* **[amcl3d (Wiki-ROS)](http://wiki.ros.org/amcl3d#preview)**
+
+### Installation
+
+#### Building from Source
+
+##### Building
+
+To build from source, clone the latest version from this repository into your catkin workspace and compile the package using
+
+     cd catkin_workspace/src
+     git clone ...
+     cd ../
+     catkin build
+
+### Tests
+
+Run the test with
+
+     roslaunch ouster_ros os1.launch os1_hostname:=10.5.5.94 replay:=true
+     roslaunch amcl3d amcl3d_test.launch
+
+### Launch files
+
+* **amcl3d.launch:** it contains the start of amcl3d node with a standard configuration of parameters.
+
+          roslaunch amcl3d amcl3d.launch
+		  
+* **amcl3d_test.launch:**  this roslaunch allows you to start the RViz with the aforementioned configuration, the amcl3d node, the test-amcl3d node, the bag player and creates a transformation to relate the point-cloud frame of test-amcl3d node with the robot frame of amcl3d node.
+
+		  roslaunch amcl3d amcl3d_test.launch
+
+### Bugs & Feature Requests
+
+Please report bugs and request features using the [Issue Tracker](https://github.com/fada-catec/amcl3d/issues).
+
+### Acknowledgement
+
+![ROSIN](http://rosin-project.eu/wp-content/uploads/rosin_ack_logo_wide.png)
+
+
+Supported by ROSIN - ROS-Industrial Focused Technical Projects (FTP).  
+More information: [rosin-project.eu](http://rosin-project.eu)

amcl3d/CMakeLists.txt

@@ -0,0 +1,59 @@
+project(amcl3d)
+cmake_minimum_required(VERSION 3.5.1)
+
+add_compile_options(-std=c++11)
+
+##################################
+## Configure CATKIN dependecies ##
+##################################
+
+find_package(catkin REQUIRED
+   COMPONENTS
+      visualization_msgs 
+      nav_msgs 
+      octomap_ros 
+      pcl_ros 
+      rosinrange_msg
+)
+
+###################
+## Configure GSL ##
+###################
+
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(GSL REQUIRED gsl)
+
+###################################
+## CATKIN specific configuration ##
+###################################
+
+catkin_package(
+   CATKIN_DEPENDS 
+      visualization_msgs 
+      nav_msgs 
+      octomap_ros 
+      pcl_ros 
+      rosinrange_msg
+   DEPENDS GSL
+)
+
+###########
+## Build ##
+###########
+
+include_directories(SYSTEM ${catkin_INCLUDE_DIRS})
+include_directories(SYSTEM ${GSL_INCLUDE_DIRS})
+
+file(GLOB_RECURSE UTILS_SRCS     "src/*.c*")
+file(GLOB_RECURSE UTILS_HDRS     "src/*.h*")
+
+add_executable(${PROJECT_NAME}_node          ${UTILS_SRCS} ${UTILS_HDRS})
+add_dependencies(${PROJECT_NAME}_node        ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME}_node   ${catkin_LIBRARIES} ${GSL_LIBRARIES})
+
+file(GLOB_RECURSE TEST_SRCS      "test/*.c*")
+file(GLOB_RECURSE TEST_HDRS      "test/*.h*")
+
+add_executable(${PROJECT_NAME}_testnode         ${TEST_SRCS} ${TEST_HDRS})
+add_dependencies(${PROJECT_NAME}_testnode       ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME}_testnode  ${catkin_LIBRARIES} ${GSL_LIBRARIES})

amcl3d/launch/amcl3d.launch

@@ -0,0 +1,128 @@
+<launch>
+
+   <param name="use_sim_time" value="true"/>
+
+   <!-- Visualization -->
+   <node type="rviz" name="rviz" pkg="rviz" args="-d $(find amcl3d)/rviz/amcl3d.rviz"/>
+
+   <node name="amcl3d_node" pkg="amcl3d" type="amcl3d_node" output="screen">
+
+      <!-- Topics and frames -->
+
+      <!-- Topic which has the information about the pointcloud -->
+      <param name="inCloudTopic" value="/lidar_pointcloud"/>
+
+      <!-- Topic which has the information about the odometry -->
+      <param name="inOdomTopic" value="/vicon_odometry"/>
+
+      <!-- Topic which has the information about the range sensor -->
+      <param name="inRangeTopic" value="/nanotron_node/range"/>
+
+      <!-- Name of uav frame -->
+
+      <!-- To set the name of the robot TF -->
+      <param name="baseFrameId_" value="base_link"/>
+
+      <!-- To set the name of the flight origin of the robot TF -->
+      <param name="odomFrameId_" value="odom"/>
+
+      <!-- To set the name of the test-bed origin TF -->
+      <param name="globalFrameId_" value="world"/>
+
+      <!-- To set the route to the localization of the environment map -->
+      <param name="map_path" value="$(find amcl3d)/test/map/map_file.bt"/>
+
+      <!-- Odometry characterization -->
+
+      <!-- Flag to indicate if the initial pose has been received -->
+      <param name="setInitialPose_" value="true"/>
+
+      <!-- To set the position in X-axis -->
+      <param name="initX_" value="2.87080"/>
+
+      <!-- To set the position in Y-axis -->
+      <param name="initY_" value="1.63822"/>
+
+      <!-- To set the position in Z-axis -->
+      <param name="initZ_" value="0"/>
+
+      <!-- To set the YAW angle -->
+      <param name="initA_" value="0.144"/>
+
+      <!-- To set a initial offset in Z-axis -->
+      <param name="initZOffset_" value="0"/>
+
+      <!-- To set thresholds of X-axis in initial pose -->
+      <param name="initXDev_" value="0.05"/>
+
+      <!-- To set thresholds of Y-axis in initial pose -->
+      <param name="initYDev_" value="0.05"/>
+
+      <!-- To set thresholds of Z-axis in initial pose -->
+      <param name="initZDev_" value="0.05"/>
+
+      <!-- To set thresholds of YAW angle in initial pose -->
+      <param name="initADev_" value="0.1"/>
+
+      <!-- Map and grid -->
+
+      <!-- To set the rate for publishing map point cloud -->
+      <param name="publish_point_cloud_rate" value="10"/>
+
+      <!-- To set the Z-axis for the grid slice -->
+      <param name="grid_slice" value="-1.0"/>
+
+      <!-- To set the rate for publishing the grid slice -->
+      <param name="publish_grid_slice_rate" value="10"/>
+
+      <!-- To set the rate for publishing the grid TF -->
+      <param name="publish_grid_tf_rate" value="10"/>
+
+      <!-- To set the deviation of 3D laser sensor -->
+      <param name="sensor_dev" value="0.05"/>
+
+      <!-- To set the deviation of the range sensor -->
+      <param name="sensor_range" value="0.53"/>
+
+      <!-- To set the size of the Voxel Filter -->
+      <param name="voxelSize_" value="0.7"/>
+
+      <!-- Filter settings-->
+
+      <!-- To set the number of particles in the filter -->
+      <param name="num_particles" value="500"/>
+
+      <!-- To set the thresholds of X-axis where the filter can predict the particles -->
+      <param name="odomXMod_" value="0.1"/>
+
+      <!-- To set the thresholds of Y-axis where the filter can predict the particles -->
+      <param name="odomYMod_" value="0.1"/>
+
+      <!-- To set the thresholds of Z-axis where the filter can predict the particles -->
+      <param name="odomZMod_" value="0.1"/>
+
+      <!-- To set the thresholds of YAW angle where the filter can predict the particles -->
+      <param name="odomAMod_" value="0.3"/>
+
+      <!-- To set resampling control -->
+      <param name="resampleInterval_" value="0"/>
+
+      <!-- To set the filter updating frequency -->
+      <param name="updateRate_" value="100"/>
+
+      <!-- To set the threshold for the update in the distance -->
+      <param name="dTh_" value="0.1"/>
+
+      <!-- To set the threshold for the update in yaw angle -->
+      <param name="aTh_" value="0.3"/>
+
+      <!-- To set the threshold for UAV takeoff -->
+      <param name="takeOffHeight_" value="0.5"/>
+
+      <!-- To set the percentage weight of point cloud and range sensor, alpha_*pointcloud + (1-alpha_)*range_sensor -->
+      <param name="alpha_" value="0.8"/>
+
+   </node>
+
+</launch>
+

amcl3d/launch/amcl3d_test.launch

@@ -0,0 +1,15 @@
+<launch>
+
+   <include file="$(find amcl3d)/launch/amcl3d.launch" />
+
+   <!-- Package to test the algorithm -->
+   <node pkg="amcl3d" name="amcl3d_test" type="amcl3d_testnode" output="screen"/>
+
+   <!-- TF which relations the pointcloud-sensor with the uav localization -->
+   <node name="tf_sensorconnected" pkg="tf" type="static_transform_publisher" args="0 0 0.05 3.1416 0 0 /base_link /lidar_points 100"/>
+
+   <!-- To charge the rosbag -->
+   <node pkg="rosbag" name="Rosbag_p" type="play" args="--clock $(find amcl3d)/test/rosbag/RosbagRosin.bag"/>
+
+</launch>
+

amcl3d/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<package format="2">
+
+   <name>amcl3d</name>
+   <version>1.0.0</version>
+   <description>
+      <p>
+         Adaptive Monte Carlo Localization (AMCL) in 3D.
+      </p>
+      <p>
+         amcl3d is a probabilistic algorithm to localizate a robot moving in 3D.
+         It uses Monte-Carlo Localization, i.e. a particle filter. 
+         This package use a laser sensor and radio-range sensors to localizate a UAV within a known map.
+      </p>
+   </description>
+   <url>http://wiki.ros.org/amcl3d</url>
+   <author email="fjperez@catec.aero">Francisco J.Perez-Grau </author>
+   <maintainer email="fcuesta@catec.aero">Francisco Cuesta Rodriguez </maintainer>
+   <maintainer email="pcarrasco@catec.aero"> Paloma Carrasco Fernandez </maintainer>
+   <license>TODO</license>
+
+   <buildtool_depend>catkin</buildtool_depend>
+
+   <depend>visualization_msgs</depend>
+   <depend>nav_msgs</depend>
+   <depend>octomap_ros</depend>
+   <depend>pcl_ros</depend>
+   <depend>rosinrange_msg</depend>
+
+</package>