diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..56a30d3 --- /dev/null +++ b/.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' +} +... diff --git a/README.md b/README.md new file mode 100644 index 0000000..908d09c --- /dev/null +++ b/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) diff --git a/amcl3d/CMakeLists.txt b/amcl3d/CMakeLists.txt new file mode 100644 index 0000000..33809b9 --- /dev/null +++ b/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}) \ No newline at end of file diff --git a/amcl3d/launch/amcl3d.launch b/amcl3d/launch/amcl3d.launch new file mode 100644 index 0000000..b86ba14 --- /dev/null +++ b/amcl3d/launch/amcl3d.launch @@ -0,0 +1,128 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/amcl3d/launch/amcl3d_test.launch b/amcl3d/launch/amcl3d_test.launch new file mode 100644 index 0000000..7bedf4f --- /dev/null +++ b/amcl3d/launch/amcl3d_test.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/amcl3d/package.xml b/amcl3d/package.xml new file mode 100644 index 0000000..875eb91 --- /dev/null +++ b/amcl3d/package.xml @@ -0,0 +1,30 @@ + + + + amcl3d + 1.0.0 + +

+ Adaptive Monte Carlo Localization (AMCL) in 3D. +

+

+ 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. +

+ + http://wiki.ros.org/amcl3d + Francisco J.Perez-Grau + Francisco Cuesta Rodriguez + Paloma Carrasco Fernandez + TODO + + catkin + + visualization_msgs + nav_msgs + octomap_ros + pcl_ros + rosinrange_msg + + diff --git a/amcl3d/rviz/amcl3d.rviz b/amcl3d/rviz/amcl3d.rviz new file mode 100644 index 0000000..555f9b6 --- /dev/null +++ b/amcl3d/rviz/amcl3d.rviz @@ -0,0 +1,296 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /TF1/Frames1 + Splitter Ratio: 0.554795027 + Tree Height: 617 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 + - Class: rviz/Tool Properties + Expanded: ~ + Name: Tool Properties + Splitter Ratio: 0.5 +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 100 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_link: + Value: true + grid3d: + Value: true + lidar_points: + Value: true + odom: + Value: true + os1_imu: + Value: true + os1_lidar: + Value: true + os1_sensor: + Value: true + vicon_real: + Value: true + world: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + world: + grid3d: + {} + odom: + base_link: + lidar_points: + {} + vicon_real: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Arrow Length: 0.300000012 + Axes Length: 0.300000012 + Axes Radius: 0.00999999978 + Class: rviz/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.0700000003 + Head Radius: 0.0299999993 + Name: Particles + Shaft Length: 0.230000004 + Shaft Radius: 0.00999999978 + Shape: Arrow (Flat) + Topic: /amcl3d_node/particle_cloud + Unreliable: false + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /amcl3d_node/range + Name: Ranges + Namespaces: + amcl3d: true + Queue Size: 100 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 5.625 + Min Value: -0.875 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.00999999978 + Style: Flat Squares + Topic: /amcl3d_node/map_point_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 2302 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.0500000007 + Style: Flat Squares + Topic: /lidar_pointcloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.5663619 + Min Value: -0.343192577 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.100000001 + Style: Flat Squares + Topic: /amcl3d_node/pointcloud_voxelfiltered + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.00999999978 + Style: Flat Squares + Topic: /amcl3d_node/pointcloud_passfiltered + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 25.9345531 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 1.65662098 + Y: 0.212233126 + Z: 1.25997341 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 1.56979632 + Target Frame: + Value: Orbit (rviz) + Yaw: 2.88044262 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1148 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 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 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1855 + X: 65 + Y: 24 diff --git a/amcl3d/src/Grid3d.cpp b/amcl3d/src/Grid3d.cpp new file mode 100644 index 0000000..33f55bf --- /dev/null +++ b/amcl3d/src/Grid3d.cpp @@ -0,0 +1,331 @@ +#include "Grid3d.h" + +#include +#include + +namespace amcl3d +{ +Grid3d::Grid3d(const double sensor_dev) : sensor_dev_(sensor_dev) +{ +} + +Grid3d::~Grid3d() +{ +} + +bool Grid3d::open(const std::string& map_path) +{ + //! Load octomap + if (!loadOctomap(map_path)) + return false; + + //! Compute the point-cloud associated to the octomap + computePointCloud(); + + //! Try to load the associated grid-map from file + std::string grid_path; + if (map_path.compare(map_path.length() - 3, 3, ".bt") == 0) + grid_path = map_path.substr(0, map_path.find(".bt")) + ".grid"; + if (map_path.compare(map_path.length() - 3, 3, ".ot") == 0) + grid_path = map_path.substr(0, map_path.find(".ot")) + ".grid"; + + if (loadGrid(grid_path)) + return true; + + //! Compute the gridMap using kdtree search over the point-cloud + computeGrid(); + + //! Save grid on file + saveGrid(grid_path); + + return true; +} + +bool Grid3d::buildGridSliceMsg(const float z, nav_msgs::OccupancyGrid& msg) const +{ + if (!grid_) + return false; + + if (z < 0 || z > max_z_) + return false; + + msg.header.frame_id = "grid3d"; + msg.info.map_load_time = ros::Time::now(); + msg.info.resolution = resolution_; + msg.info.width = grid_size_x_; + msg.info.height = grid_size_y_; + msg.info.origin.position.x = 0.; + msg.info.origin.position.y = 0.; + msg.info.origin.position.z = static_cast(z); + msg.info.origin.orientation.x = 0.; + msg.info.origin.orientation.y = 0.; + msg.info.origin.orientation.z = 0.; + msg.info.origin.orientation.w = 1.; + msg.data.resize(grid_size_x_ * grid_size_y_); + + //! Extract max probability + const uint32_t offset = static_cast(z * one_div_res_) * grid_size_x_ * grid_size_y_; + const uint32_t end = offset + grid_size_x_ * grid_size_y_; + float max_prob = -1.0; + for (uint32_t i = offset; i < end; ++i) + if (grid_[i].prob > max_prob) + max_prob = grid_[i].prob; + + //! Copy data into grid msg and scale the probability to [0, 100] + if (max_prob < 0.000001f) + max_prob = 0.000001f; + max_prob = 100.f / max_prob; + for (uint32_t i = 0; i < grid_size_x_ * grid_size_y_; ++i) + msg.data[i] = static_cast(grid_[i + offset].prob * max_prob); + + return true; +} + +bool Grid3d::buildMapPointCloudMsg(sensor_msgs::PointCloud2& msg) const +{ + if (!cloud_) + return false; + + pcl::toROSMsg(*cloud_, msg); + + msg.header.frame_id = "grid3d"; + + return true; +} + +void Grid3d::buildGrid3d2WorldTf(const std::string& global_frame_id, tf::StampedTransform& tf) const +{ + tf::Transform grid3d_2_world_tf; + grid3d_2_world_tf.setOrigin(tf::Vector3(min_oct_x_, min_oct_y_, min_oct_z_)); + grid3d_2_world_tf.setRotation(tf::Quaternion(0, 0, 0, 1)); + + tf = tf::StampedTransform(grid3d_2_world_tf, ros::Time::now(), global_frame_id, "grid3d"); +} + +float Grid3d::computeCloudWeight(const std::vector& points) const +{ + if (!grid_) + return 0; + + float weight = 0.; + int n = 0; + + for (uint32_t i = 0; i < points.size(); ++i) + { + const pcl::PointXYZ& p = points[i]; + if (p.x >= 0.f && p.y >= 0.f && p.z >= 0.f && p.x < max_x_ && p.y < max_y_ && p.z < max_z_) + { + weight += grid_[point2grid(p.x, p.y, p.z)].prob; + ++n; + } + } + + return (n <= 10) ? 0 : weight / n; +} + +bool Grid3d::isIntoMap(const float x, const float y, const float z) const +{ + return (x >= 0.f && y >= 0.f && z >= 0.f && x <= max_x_ && y <= max_y_ && z <= max_z_); +} + +void Grid3d::getMinOctomap(float& x, float& y, float& z) const +{ + x = min_oct_x_; + y = min_oct_y_; + z = min_oct_z_; +} + +bool Grid3d::loadOctomap(const std::string& map_path) +{ + if (map_path.length() <= 3) + return false; + + if (map_path.compare(map_path.length() - 3, 3, ".bt") == 0) + { + octomap_.reset(new octomap::OcTree(0.1)); + } + else if (map_path.compare(map_path.length() - 3, 3, ".ot") == 0) + { + octomap_.reset(dynamic_cast(octomap::AbstractOcTree::read(map_path))); + } + + if (!octomap_) + { + ROS_ERROR("[%s] Error: NULL octomap!!", ros::this_node::getName().data()); + return false; + } + + if (!octomap_->readBinary(map_path) || octomap_->size() <= 1) + return false; + + ROS_INFO("[%s] Octomap loaded", ros::this_node::getName().data()); + + // Get map parameters + double min_x, min_y, min_z, max_x, max_y, max_z; + octomap_->getMetricMin(min_x, min_y, min_z); + octomap_->getMetricMax(max_x, max_y, max_z); + max_x_ = static_cast(max_x - min_x); + max_y_ = static_cast(max_y - min_y); + max_z_ = static_cast(max_z - min_z); + min_oct_x_ = static_cast(min_x); + min_oct_y_ = static_cast(min_y); + min_oct_z_ = static_cast(min_z); + resolution_ = static_cast(octomap_->getResolution()); + one_div_res_ = 1.0f / resolution_; + + ROS_INFO("[%s]" + "\n Map size:" + "\n X: %lf to %lf" + "\n Y: %lf to %lf" + "\n Z: %lf to %lf" + "\n Res: %lf", + ros::this_node::getName().data(), min_x, max_x, min_y, max_y, min_z, max_z, resolution_); + + return true; +} + +void Grid3d::computePointCloud() +{ + //! Load the octomap in PCL for easy nearest neighborhood computation + //! The point-cloud is shifted to have (0,0,0) as min values + cloud_.reset(new pcl::PointCloud()); + cloud_->width = static_cast(octomap_->size()); + cloud_->height = 1; + cloud_->points.resize(cloud_->width * cloud_->height); + + uint32_t i = 0; + for (octomap::OcTree::leaf_iterator it = octomap_->begin_leafs(); it != octomap_->end_leafs(); ++it) + { + if (it != nullptr && octomap_->isNodeOccupied(*it)) + { + cloud_->points[i].x = static_cast(it.getX()) - min_oct_x_; + cloud_->points[i].y = static_cast(it.getY()) - min_oct_y_; + cloud_->points[i].z = static_cast(it.getZ()) - min_oct_z_; + ++i; + } + } + cloud_->width = i; + cloud_->points.resize(i); +} + +bool Grid3d::saveGrid(const std::string& grid_path) +{ + auto pf = fopen(grid_path.c_str(), "wb"); + if (!pf) + { + ROS_ERROR("[%s] Error opening file %s for writing", ros::this_node::getName().data(), grid_path.c_str()); + return false; + } + + //! Write grid general info + fwrite(&grid_size_, sizeof(uint32_t), 1, pf); + fwrite(&grid_size_x_, sizeof(uint32_t), 1, pf); + fwrite(&grid_size_y_, sizeof(uint32_t), 1, pf); + fwrite(&grid_size_z_, sizeof(uint32_t), 1, pf); + fwrite(&sensor_dev_, sizeof(double), 1, pf); + + //! Write grid cells + fwrite(grid_.get(), sizeof(GridCell), grid_size_, pf); + + fclose(pf); + + ROS_INFO("[%s] Grid map successfully saved on %s", ros::this_node::getName().data(), grid_path.c_str()); + + return true; +} + +bool Grid3d::loadGrid(const std::string& grid_path) +{ + auto pf = fopen(grid_path.c_str(), "rb"); + if (!pf) + { + ROS_ERROR("[%s] Error opening file %s for reading", ros::this_node::getName().data(), grid_path.c_str()); + return false; + } + + //! Read grid general info + double sensor_dev; + fread(&grid_size_, sizeof(uint32_t), 1, pf); + fread(&grid_size_x_, sizeof(uint32_t), 1, pf); + fread(&grid_size_y_, sizeof(uint32_t), 1, pf); + fread(&grid_size_z_, sizeof(uint32_t), 1, pf); + fread(&sensor_dev, sizeof(double), 1, pf); + + if (std::fabs(sensor_dev - sensor_dev_) >= std::numeric_limits::epsilon()) + { + ROS_ERROR("[%s] Loaded sensorDev is different", ros::this_node::getName().data()); + return false; + } + + grid_step_y_ = grid_size_x_; + grid_step_z_ = grid_size_x_ * grid_size_y_; + + //! Read grid cells + grid_.reset(new GridCell[grid_size_]); + fread(grid_.get(), sizeof(GridCell), grid_size_, pf); + + fclose(pf); + + ROS_INFO("[%s] Grid map successfully loaded from %s", ros::this_node::getName().data(), grid_path.c_str()); + + return true; +} + +void Grid3d::computeGrid() +{ + ROS_INFO("[%s] Computing 3D occupancy grid. This will take some time...", ros::this_node::getName().data()); + + //! Alloc the 3D grid + grid_size_x_ = static_cast(max_x_ * one_div_res_); + grid_size_y_ = static_cast(max_y_ * one_div_res_); + grid_size_z_ = static_cast(max_z_ * one_div_res_); + grid_size_ = grid_size_x_ * grid_size_y_ * grid_size_z_; + grid_step_y_ = grid_size_x_; + grid_step_z_ = grid_size_x_ * grid_size_y_; + + grid_.reset(new GridCell[grid_size_]); + + //! Setup kdtree + pcl::KdTreeFLANN kdtree; + kdtree.setInputCloud(cloud_); + + //! Compute the distance to the closest point of the grid + const float gauss_const1 = static_cast(1. / (sensor_dev_ * sqrt(2 * M_PI))); + const float gauss_const2 = static_cast(1. / (2. * sensor_dev_ * sensor_dev_)); + uint32_t index; + float dist; + pcl::PointXYZ search_point; + std::vector point_idx_nkn_search(1); + std::vector point_nkn_squared_distance(1); + for (uint32_t iz = 0; iz < grid_size_z_; ++iz) + for (uint32_t iy = 0; iy < grid_size_y_; ++iy) + for (uint32_t ix = 0; ix < grid_size_x_; ++ix) + { + search_point.x = ix * resolution_; + search_point.y = iy * resolution_; + search_point.z = iz * resolution_; + index = ix + iy * grid_step_y_ + iz * grid_step_z_; + + if (kdtree.nearestKSearch(search_point, 1, point_idx_nkn_search, point_nkn_squared_distance) > 0) + { + dist = point_nkn_squared_distance[0]; + grid_[index].dist = dist; + grid_[index].prob = gauss_const1 * expf(-dist * dist * gauss_const2); + } + else + { + grid_[index].dist = -1.0; + grid_[index].prob = 0.0; + } + } + + ROS_INFO("[%s] Computing 3D occupancy grid done!", ros::this_node::getName().data()); +} + +inline uint32_t Grid3d::point2grid(const float x, const float y, const float z) const +{ + return static_cast(x * one_div_res_) + static_cast(y * one_div_res_) * grid_step_y_ + + static_cast(z * one_div_res_) * grid_step_z_; +} + +} // namespace amcl3d diff --git a/amcl3d/src/Grid3d.h b/amcl3d/src/Grid3d.h new file mode 100644 index 0000000..eb86f2b --- /dev/null +++ b/amcl3d/src/Grid3d.h @@ -0,0 +1,63 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace amcl3d +{ +class Grid3d +{ +public: + explicit Grid3d(const double sensor_dev); + virtual ~Grid3d(); + + bool open(const std::string& map_path); + + bool buildGridSliceMsg(const float z, nav_msgs::OccupancyGrid& msg) const; + bool buildMapPointCloudMsg(sensor_msgs::PointCloud2& msg) const; + void buildGrid3d2WorldTf(const std::string& global_frame_id, tf::StampedTransform& tf) const; + + float computeCloudWeight(const std::vector& points) const; + bool isIntoMap(const float x, const float y, const float z) const; + void getMinOctomap(float& x, float& y, float& z) const; + +private: + bool loadOctomap(const std::string& map_path); + void computePointCloud(); + + bool saveGrid(const std::string& grid_path); + bool loadGrid(const std::string& grid_path); + void computeGrid(); + + inline uint32_t point2grid(const float x, const float y, const float z) const; + + //! Init parameter + double sensor_dev_; + + //! Octomap parameters + std::unique_ptr octomap_; + float max_x_, max_y_, max_z_; + float min_oct_x_, min_oct_y_, min_oct_z_; + float resolution_, one_div_res_; + + //! 3D point cloud representation of the map + pcl::PointCloud::Ptr cloud_; + + //! 3D probabilistic grid cell + struct GridCell + { + float dist, prob; + GridCell() : dist(-1.f), prob(0.f) + { + } + }; + std::unique_ptr grid_; + uint32_t grid_size_, grid_size_x_, grid_size_y_, grid_size_z_; + uint32_t grid_step_y_, grid_step_z_; +}; + +} // namespace amcl3d diff --git a/amcl3d/src/Node.cpp b/amcl3d/src/Node.cpp new file mode 100644 index 0000000..ab0c7d3 --- /dev/null +++ b/amcl3d/src/Node.cpp @@ -0,0 +1,533 @@ +#include "Node.h" + +#include +#include +#include +#include +#include + +namespace amcl3d +{ +Node::Node() : grid3d_(parameters_.sensor_dev), pf_(), nh_(ros::this_node::getName()) +{ + ROS_DEBUG("[%s] Node::Node()", ros::this_node::getName().data()); +} + +Node::~Node() +{ + ROS_DEBUG("[%s] Node::~Node()", ros::this_node::getName().data()); +} + +void Node::spin() +{ + ROS_DEBUG("[%s] Node::spin()", ros::this_node::getName().data()); + + if (!grid3d_.open(parameters_.map_path)) + return; + + if (parameters_.publish_grid_slice_rate != 0 && grid3d_.buildGridSliceMsg(parameters_.grid_slice, grid_slice_msg_)) + { + grid_slice_pub_ = nh_.advertise("grid_slice", 1, true); + grid_slice_pub_timer_ = + nh_.createTimer(ros::Duration(ros::Rate(parameters_.publish_grid_slice_rate)), &Node::publishGridSlice, this); + } + + if (parameters_.publish_point_cloud_rate != 0 && grid3d_.buildMapPointCloudMsg(map_point_cloud_msg_)) + { + map_point_cloud_pub_ = nh_.advertise("map_point_cloud", 1, true); + map_point_cloud_pub_timer_ = nh_.createTimer(ros::Duration(ros::Rate(parameters_.publish_point_cloud_rate)), + &Node::publishMapPointCloud, this); + } + + grid3d_.buildGrid3d2WorldTf(parameters_.globalFrameId_, grid_to_world_tf_); + + if (parameters_.publish_grid_tf_rate != 0) + { + grid_to_world_tf_timer_ = + nh_.createTimer(ros::Duration(ros::Rate(parameters_.publish_grid_tf_rate)), &Node::publishGridTf, this); + } + + point_sub_ = nh_.subscribe(parameters_.inCloudTopic, 1, &Node::pointcloudCallback, this); + odom_sub_ = nh_.subscribe(parameters_.inOdomTopic, 1, &Node::odomCallback, this); + range_sub_ = nh_.subscribe(parameters_.inRangeTopic, 1, &Node::rangeCallback, this); + initialpose_sub_ = nh_.subscribe("initial_pose", 2, &Node::initialPoseReceived, this); + + particles_pose_pub_ = nh_.advertise("particle_cloud", 1, true); + range_markers_pub_ = nh_.advertise("range", 0); + odom_base_pub_ = nh_.advertise("base_transform", 1); + + cloud_filter_pub_ = nh_.advertise("pointcloud_voxelfiltered", 0); + cloud_passfilter_pub_ = nh_.advertise("pointcloud_passfiltered", 0); + + while (ros::ok()) + { + ros::spinOnce(); + usleep(10000); + } + + nh_.shutdown(); +} + +void Node::publishMapPointCloud(const ros::TimerEvent&) +{ + ROS_DEBUG("[%s] Node::publishMapPointCloud()", ros::this_node::getName().data()); + + map_point_cloud_msg_.header.stamp = ros::Time::now(); + map_point_cloud_pub_.publish(map_point_cloud_msg_); +} + +void Node::publishGridSlice(const ros::TimerEvent&) +{ + ROS_DEBUG("[%s] Node::publishGridSlice()", ros::this_node::getName().data()); + + grid_slice_msg_.header.stamp = ros::Time::now(); + grid_slice_pub_.publish(grid_slice_msg_); +} + +void Node::publishGridTf(const ros::TimerEvent&) +{ + ROS_DEBUG("[%s] Node::publishGridSlice()", ros::this_node::getName().data()); + + grid_to_world_tf_.stamp_ = ros::Time::now(); + + tf::TransformBroadcaster tf_broadcaster_; + tf_broadcaster_.sendTransform(grid_to_world_tf_); +} + +void Node::pointcloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg) +{ + if (!is_odom_) + { + ROS_WARN("Odometry transform not received"); + return; + } + + //! If the filter is not initialized then exit + if (!pf_.isInitialized()) + { + ROS_WARN("Filter not initialized yet, waiting for initial pose."); + if (parameters_.setInitialPose_) + { + tf::Transform initPose; + initPose.setOrigin(tf::Vector3(parameters_.initX_, parameters_.initY_, parameters_.initZ_)); + initPose.setRotation(tf::Quaternion(0.0, 0.0, sin(parameters_.initA_ * 0.5), cos(parameters_.initA_ * 0.5))); + setInitialPose(initPose, parameters_.initXDev_, parameters_.initYDev_, parameters_.initZDev_, + parameters_.initADev_); + } + return; + } + + //! Check if an update must be performed or not + if (!checkUpdateThresholds()) + return; + + static const ros::Duration updateInterval(1.0 / parameters_.updateRate_); + nextupdate_time_ = ros::Time::now() + updateInterval; + + //! Apply pass-though and voxel grid + clock_t begin_filter = clock(); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + pcl::fromROSMsg(*msg, *cloud); + pcl::PassThrough pass; + pass.setInputCloud(cloud); + pass.setFilterFieldName("z"); + pass.setFilterLimits(-10, 0); + pass.setNegative(false); + pcl::PointCloud::Ptr cloud_filtered(new pcl::PointCloud); + pass.filter(*cloud_filtered); + cloud_filtered->header = cloud->header; + sensor_msgs::PointCloud2 passCloud; + pcl::toROSMsg(*cloud_filtered, passCloud); + cloud_passfilter_pub_.publish(passCloud); + + pcl::PointCloud::Ptr cloud_src(new pcl::PointCloud); + pcl::fromROSMsg(passCloud, *cloud_src); + pcl::VoxelGrid sor; + sor.setInputCloud(cloud_src); + sor.setLeafSize(parameters_.voxelSize_, parameters_.voxelSize_, parameters_.voxelSize_); + pcl::PointCloud::Ptr cloud_down(new pcl::PointCloud); + sor.filter(*cloud_down); + cloud_down->header = cloud_src->header; + sensor_msgs::PointCloud2 downCloud; + pcl::toROSMsg(*cloud_down, downCloud); + cloud_filter_pub_.publish(downCloud); + clock_t end_filter = clock(); + double elapsed_secs = double(end_filter - begin_filter) / CLOCKS_PER_SEC; + ROS_INFO("Filter time: [%lf] sec", elapsed_secs); + + //! Perform particle prediction based on odometry + odom_increment_tf = lastupdatebase_2_odom_tf_.inverse() * base_2_odom_tf_; + const double delta_x = odom_increment_tf.getOrigin().getX(); + const double delta_y = odom_increment_tf.getOrigin().getY(); + const double delta_z = odom_increment_tf.getOrigin().getZ(); + const double delta_a = getYawFromTf(odom_increment_tf); + + clock_t begin_predict = clock(); + pf_.predict(parameters_.odomXMod_, parameters_.odomYMod_, parameters_.odomZMod_, parameters_.odomAMod_, delta_x, + delta_y, delta_z, delta_a); + clock_t end_predict = clock(); + elapsed_secs = double(end_predict - begin_predict) / CLOCKS_PER_SEC; + ROS_INFO("Predict time: [%lf] sec", elapsed_secs); + + //! Compensate for the current roll and pitch of the base-link + const float sr = sin(roll_); + const float cr = cos(roll_); + const float sp = sin(pitch_); + const float cp = cos(pitch_); + float r00, r01, r02, r10, r11, r12, r20, r21, r22; + r00 = cp; + r01 = sp * sr; + r02 = cr * sp; + r10 = 0; + r11 = cr; + r12 = -sr; + r20 = -sp; + r21 = cp * sr; + r22 = cp * cr; + sensor_msgs::PointCloud2ConstIterator iterX(downCloud, "x"); + sensor_msgs::PointCloud2ConstIterator iterY(downCloud, "y"); + sensor_msgs::PointCloud2ConstIterator iterZ(downCloud, "z"); + std::vector points; + points.resize(downCloud.width); + for (uint32_t i = 0; i < downCloud.width; ++i, ++iterX, ++iterY, ++iterZ) + { + points[i].x = *iterX * r00 + *iterY * r01 + *iterZ * r02; + points[i].y = *iterX * r10 + *iterY * r11 + *iterZ * r12; + points[i].z = *iterX * r20 + *iterY * r21 + *iterZ * r22; + } + + //! Perform particle update based on current point-cloud + clock_t begin_update = clock(); + pf_.update(grid3d_, points, range_data, parameters_.alpha_, parameters_.sensor_range); + clock_t end_update = clock(); + elapsed_secs = double(end_update - begin_update) / CLOCKS_PER_SEC; + ROS_INFO("Update time: [%lf] sec", elapsed_secs); + + mean_p_ = pf_.getMean(); + + //! Clean the range buffer + range_data.clear(); + + //! Update time and transform information + lastupdatebase_2_odom_tf_ = base_2_odom_tf_; + + //! Do the resampling if needed + clock_t begin_resample = clock(); + static int n_updates = 0; + if (++n_updates > parameters_.resampleInterval_) + { + n_updates = 0; + pf_.resample(); + } + clock_t end_resample = clock(); + elapsed_secs = double(end_resample - begin_resample) / CLOCKS_PER_SEC; + ROS_INFO("Resample time: [%lf] sec", elapsed_secs); + + //! Publish particles + publishParticles(); +} + +void Node::odomCallback(const geometry_msgs::TransformStampedConstPtr& msg) +{ + base_2_odom_tf_.setOrigin( + tf::Vector3(msg->transform.translation.x, msg->transform.translation.y, msg->transform.translation.z)); + base_2_odom_tf_.setRotation(tf::Quaternion(msg->transform.rotation.x, msg->transform.rotation.y, + msg->transform.rotation.z, msg->transform.rotation.w)); + + //! Update roll and pitch from odometry + double yaw; + base_2_odom_tf_.getBasis().getRPY(roll_, pitch_, yaw); + + static tf::TransformBroadcaster tfBr; + tfBr.sendTransform( + tf::StampedTransform(base_2_odom_tf_, ros::Time::now(), parameters_.odomFrameId_, parameters_.baseFrameId_)); + + if (!is_odom_) + { + is_odom_ = true; + + lastbase_2_world_tf_ = initodom_2_world_tf_ * base_2_odom_tf_; + lastodom_2_world_tf_ = initodom_2_world_tf_; + } + + if (!pf_.isInitialized()) + { + ROS_WARN("Filter not initialized yet, not publishing output TF"); + return; + } + + static bool has_takenoff = false; + if (!has_takenoff) + { + ROS_WARN("Not <> yet"); + + //! Check takeoff height + has_takenoff = base_2_odom_tf_.getOrigin().getZ() > parameters_.takeOffHeight_; + + lastbase_2_world_tf_ = initodom_2_world_tf_ * base_2_odom_tf_; + lastodom_2_world_tf_ = initodom_2_world_tf_; + + lastmean_p_ = mean_p_; // for not 'jumping' whenever has_takenoff is true + } + else + { + //! Check if AMCL went wrong (nan, inf) + if (std::isnan(mean_p_.x) || std::isnan(mean_p_.y) || std::isnan(mean_p_.z) || std::isnan(mean_p_.a)) + { + ROS_WARN("AMCL NaN detected"); + amcl_out_ = true; + } + if (std::isinf(mean_p_.x) || std::isinf(mean_p_.y) || std::isinf(mean_p_.z) || std::isinf(mean_p_.a)) + { + ROS_WARN("AMCL Inf detected"); + amcl_out_ = true; + } + + //! Check jumps + if (fabs(mean_p_.x - lastmean_p_.x) > 1.) + { + ROS_WARN_STREAM("AMCL Jump detected in X"); + amcl_out_ = true; + } + if (fabs(mean_p_.y - lastmean_p_.y) > 1.) + { + ROS_WARN_STREAM("AMCL Jump detected in Y"); + amcl_out_ = true; + } + if (fabs(mean_p_.z - lastmean_p_.z) > 1.) + { + ROS_WARN_STREAM("AMCL Jump detected in Z"); + amcl_out_ = true; + } + if (fabs(mean_p_.a - lastmean_p_.a) > 1.) + { + ROS_WARN_STREAM("AMCL Jump detected in Yaw"); + amcl_out_ = true; + } + + if (!amcl_out_) + { + geometry_msgs::Point32 grid3d; + grid3d_.getMinOctomap(grid3d.x, grid3d.y, grid3d.z); + + tf::Transform base_2_world_tf; + base_2_world_tf.setOrigin(tf::Vector3(mean_p_.x + grid3d.x, mean_p_.y + grid3d.y, mean_p_.z + grid3d.z)); + tf::Quaternion q; + q.setRPY(roll_, pitch_, mean_p_.a); + base_2_world_tf.setRotation(q); + + base_2_world_tf = base_2_world_tf * lastupdatebase_2_odom_tf_.inverse() * base_2_odom_tf_; + + lastmean_p_ = mean_p_; + + lastbase_2_world_tf_ = base_2_world_tf; + lastodom_2_world_tf_ = base_2_world_tf * base_2_odom_tf_.inverse(); + + amcl_out_lastbase_2_odom_tf_ = lastupdatebase_2_odom_tf_; + } + else + { + lastbase_2_world_tf_ = lastbase_2_world_tf_ * amcl_out_lastbase_2_odom_tf_.inverse() * base_2_odom_tf_; + amcl_out_lastbase_2_odom_tf_ = base_2_odom_tf_; + } + } + + //! Publish transform + geometry_msgs::TransformStamped odom_2_base_tf; + odom_2_base_tf.header.stamp = msg->header.stamp; + odom_2_base_tf.header.frame_id = parameters_.globalFrameId_; + odom_2_base_tf.child_frame_id = parameters_.baseFrameId_; + odom_2_base_tf.transform.translation.x = lastbase_2_world_tf_.getOrigin().getX(); + odom_2_base_tf.transform.translation.y = lastbase_2_world_tf_.getOrigin().getY(); + odom_2_base_tf.transform.translation.z = lastbase_2_world_tf_.getOrigin().getZ(); + odom_2_base_tf.transform.rotation.x = lastbase_2_world_tf_.getRotation().getX(); + odom_2_base_tf.transform.rotation.y = lastbase_2_world_tf_.getRotation().getY(); + odom_2_base_tf.transform.rotation.z = lastbase_2_world_tf_.getRotation().getZ(); + odom_2_base_tf.transform.rotation.w = lastbase_2_world_tf_.getRotation().getW(); + odom_base_pub_.publish(odom_2_base_tf); + + tfBr.sendTransform(tf::StampedTransform(lastodom_2_world_tf_, ros::Time::now(), parameters_.globalFrameId_, + parameters_.odomFrameId_)); +} + +void Node::initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg) +{ + //! We only accept initial pose estimates in the global frame + if (msg->header.frame_id != parameters_.globalFrameId_) + { + ROS_WARN("Ignoring initial pose in frame \"%s\"; " + "initial poses must be in the global frame, \"%s\"", + msg->header.frame_id.c_str(), parameters_.globalFrameId_.c_str()); + return; + } + + //! Transform into the global frame + tf::Transform pose; + tf::poseMsgToTF(msg->pose.pose, pose); + + ROS_INFO("Setting pose (%.6f): %.3f %.3f %.3f %.3f", ros::Time::now().toSec(), pose.getOrigin().x(), + pose.getOrigin().y(), pose.getOrigin().z(), getYawFromTf(pose)); + + //! Initialize the filter + setInitialPose(pose, parameters_.initXDev_, parameters_.initYDev_, parameters_.initZDev_, parameters_.initADev_); +} + +void Node::rangeCallback(const rosinrange_msg::range_poseConstPtr& msg) +{ + const int node = msg->destination_id; + + geometry_msgs::Point32 grid3d; + grid3d_.getMinOctomap(grid3d.x, grid3d.y, grid3d.z); + + geometry_msgs::Point anchor; + anchor.x = msg->position.x; + anchor.y = msg->position.y; + anchor.z = msg->position.z; + + float ax, ay, az; + ax = msg->position.x - grid3d.x; + ay = msg->position.y - grid3d.y; + az = msg->position.z - grid3d.z; + + range_data.push_back(Range(static_cast(msg->range), ax, ay, az)); + + geometry_msgs::Point uav; + uav.x = mean_p_.x + grid3d.x; + uav.y = mean_p_.y + grid3d.y; + uav.z = mean_p_.z + grid3d.z; + + RvizMarkerPublish(msg->destination_id, static_cast(msg->range), uav, anchor); +} + +bool Node::checkUpdateThresholds() +{ + std::cout << "Checking for AMCL3D update" << std::endl; + + if (ros::Time::now() < nextupdate_time_) + return false; + + odom_increment_tf = lastupdatebase_2_odom_tf_.inverse() * base_2_odom_tf_; + + //! Check translation threshold + if (odom_increment_tf.getOrigin().length() > parameters_.dTh_) + { + ROS_INFO("Translation update"); + return true; + } + + //! Check yaw threshold + double yaw, pitch, roll; + odom_increment_tf.getBasis().getRPY(roll, pitch, yaw); + if (fabs(yaw) > parameters_.aTh_) + { + ROS_INFO("Rotation update"); + return true; + } + + return false; +} + +void Node::publishParticles() +{ + //! If the filter is not initialized then exit + if (!pf_.isInitialized()) + return; + + geometry_msgs::Point32 grid3d; + grid3d_.getMinOctomap(grid3d.x, grid3d.y, grid3d.z); + + //! Build the msg based on the particles position and orinetation + geometry_msgs::PoseArray msg; + pf_.buildParticlesPoseMsg(grid3d, msg); + msg.header.stamp = ros::Time::now(); + msg.header.frame_id = parameters_.globalFrameId_; + + //! Publish particle cloud + particles_pose_pub_.publish(msg); +} + +void Node::setInitialPose(const tf::Transform& init_pose, const float x_dev, const float y_dev, const float z_dev, + const float a_dev) +{ + initodom_2_world_tf_ = init_pose; + + geometry_msgs::Point32 grid3d; + grid3d_.getMinOctomap(grid3d.x, grid3d.y, grid3d.z); + + const tf::Vector3 t = init_pose.getOrigin(); + + const float xInit = t.x() - grid3d.x; + const float yInit = t.y() - grid3d.y; + const float zInit = t.z() - grid3d.z + parameters_.initZOffset_; + const float aInit = static_cast(getYawFromTf(init_pose)); + + pf_.init(parameters_.num_particles, xInit, yInit, zInit, aInit, x_dev, y_dev, z_dev, a_dev); + + mean_p_ = pf_.getMean(); + lastmean_p_ = mean_p_; + + //! Extract TFs for future updates + //! Reset lastupdatebase_2_odom_tf_ + lastupdatebase_2_odom_tf_ = base_2_odom_tf_; + + //! Publish particles + publishParticles(); +} + +double Node::getYawFromTf(const tf::Transform& tf) +{ + double yaw, pitch, roll; + tf.getBasis().getRPY(roll, pitch, yaw); + + return yaw; +} + +void Node::RvizMarkerPublish(uint32_t anchor_id, float r, geometry_msgs::Point uav, geometry_msgs::Point anchor) +{ + visualization_msgs::Marker marker; + marker.header.frame_id = parameters_.globalFrameId_; + marker.header.stamp = ros::Time::now(); + marker.ns = "amcl3d"; + marker.id = anchor_id; + marker.type = visualization_msgs::Marker::LINE_STRIP; + marker.action = visualization_msgs::Marker::ADD; + marker.scale.x = 0.1; + marker.scale.y = r; + marker.scale.z = r; + marker.color.a = 0.5; + if (amcl_out_) //! Indicate if AMCL was lost + { + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + } + else + { + switch (anchor_id) + { + case 0: + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + break; + case 1: + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + break; + case 2: + marker.color.r = 0.0; + marker.color.g = 0.0; + marker.color.b = 1.0; + break; + } + } + marker.points.clear(); + marker.points.push_back(uav); + marker.points.push_back(anchor); + + //! Publish marker + range_markers_pub_.publish(marker); +} + +} // namespace amcl3d \ No newline at end of file diff --git a/amcl3d/src/Node.h b/amcl3d/src/Node.h new file mode 100644 index 0000000..77a1e65 --- /dev/null +++ b/amcl3d/src/Node.h @@ -0,0 +1,95 @@ +/*! + * @file Node.h + * @copyright Copyright (c) 2019, FADA-CATEC + * + * 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. + */ + +#pragma once + +#include "Parameters.h" +#include "ParticleFilter.h" //! Include Grid.hpp + +#include +#include + +namespace amcl3d +{ +class Node +{ +public: + explicit Node(); + virtual ~Node(); + + void spin(); + +private: + void publishMapPointCloud(const ros::TimerEvent&); + void publishGridSlice(const ros::TimerEvent&); + void publishGridTf(const ros::TimerEvent&); + void publishParticles(); + + void pointcloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg); + void odomCallback(const geometry_msgs::TransformStampedConstPtr& msg); + void initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg); + void rangeCallback(const rosinrange_msg::range_poseConstPtr& msg); + + //! Check motion and time thresholds for AMCL update + bool checkUpdateThresholds(); + + //! Set the initial pose of the particle filter + void setInitialPose(const tf::Transform& init_pose, const float x_dev, const float y_dev, const float z_dev, + const float a_dev); + + //! Return yaw from a given TF + double getYawFromTf(const tf::Transform& tf); + + //! To show range sensors in Rviz + void RvizMarkerPublish(uint32_t anchor_id, const float r, geometry_msgs::Point uav, geometry_msgs::Point anchor); + + Parameters parameters_; + Grid3d grid3d_; + ParticleFilter pf_; + + ros::NodeHandle nh_; + + sensor_msgs::PointCloud2 map_point_cloud_msg_; + ros::Publisher map_point_cloud_pub_; + ros::Timer map_point_cloud_pub_timer_; + + nav_msgs::OccupancyGrid grid_slice_msg_; + ros::Publisher grid_slice_pub_; + ros::Timer grid_slice_pub_timer_; + + tf::StampedTransform grid_to_world_tf_; + ros::Timer grid_to_world_tf_timer_; + + bool is_odom_{ false }; + bool amcl_out_{ false }; + double roll_{ 0 }, pitch_{ 0 }; + + std::vector range_data; + Particle mean_p_, lastmean_p_; + + ros::Subscriber point_sub_, odom_sub_, range_sub_, initialpose_sub_; + ros::Publisher particles_pose_pub_, range_markers_pub_, odom_base_pub_; + + tf::Transform lastbase_2_world_tf_, initodom_2_world_tf_, lastodom_2_world_tf_, amcl_out_lastbase_2_odom_tf_, + lastupdatebase_2_odom_tf_, base_2_odom_tf_, odom_increment_tf; + + ros::Time nextupdate_time_; + + ros::Publisher cloud_filter_pub_, cloud_passfilter_pub_; +}; + +} // namespace amcl3d diff --git a/amcl3d/src/Parameters.cpp b/amcl3d/src/Parameters.cpp new file mode 100644 index 0000000..9a201d9 --- /dev/null +++ b/amcl3d/src/Parameters.cpp @@ -0,0 +1,234 @@ +#include "Parameters.h" + +#include + +namespace amcl3d +{ +Parameters::Parameters() +{ + if (!ros::param::get("~inCloudTopic", inCloudTopic)) + { + exitWithParameterError("inCloudTopic"); + } + + if (!ros::param::get("~inOdomTopic", inOdomTopic)) + { + exitWithParameterError("inOdomTopic"); + } + + if (!ros::param::get("~inRangeTopic", inRangeTopic)) + { + exitWithParameterError("inRangeTopic"); + } + if (!ros::param::get("~baseFrameId_", baseFrameId_)) + { + exitWithParameterError("baseFrameId_"); + } + + if (!ros::param::get("~odomFrameId_", odomFrameId_)) + { + exitWithParameterError("odomFrameId_"); + } + + if (!ros::param::get("~globalFrameId_", globalFrameId_)) + { + exitWithParameterError("globalFrameId_"); + } + + if (!ros::param::get("~map_path", map_path)) + { + exitWithParameterError("map_path"); + } + + if (!ros::param::get("~setInitialPose_", setInitialPose_)) + { + exitWithParameterError("setInitialPose_"); + } + + if (!ros::param::get("~initX_", initX_)) + { + exitWithParameterError("initX_"); + } + + if (!ros::param::get("~initY_", initY_)) + { + exitWithParameterError("initY_"); + } + + if (!ros::param::get("~initZ_", initZ_)) + { + exitWithParameterError("initZ_"); + } + + if (!ros::param::get("~initA_", initA_)) + { + exitWithParameterError("initA_"); + } + + if (!ros::param::get("~initZOffset_", initZOffset_)) + { + exitWithParameterError("initZOffset_"); + } + + if (!ros::param::get("~initXDev_", initXDev_)) + { + exitWithParameterError("initXDev_"); + } + + if (!ros::param::get("~initYDev_", initYDev_)) + { + exitWithParameterError("initYDev_"); + } + + if (!ros::param::get("~initZDev_", initZDev_)) + { + exitWithParameterError("initZDev_"); + } + + if (!ros::param::get("~initADev_", initADev_)) + { + exitWithParameterError("initADev_"); + } + + if (!ros::param::get("~publish_point_cloud_rate", publish_point_cloud_rate)) + { + exitWithParameterError("publish_point_cloud_rate"); + } + + if (!ros::param::get("~grid_slice", grid_slice)) + { + exitWithParameterError("grid_slice"); + } + + if (!ros::param::get("~publish_grid_slice_rate", publish_grid_slice_rate)) + { + exitWithParameterError("publish_grid_slice_rate"); + } + + if (!ros::param::get("~publish_grid_tf_rate", publish_grid_tf_rate)) + { + exitWithParameterError("publish_grid_tf_rate"); + } + + if (!ros::param::get("~sensor_dev", sensor_dev)) + { + exitWithParameterError("sensor_dev"); + } + + if (!ros::param::get("~sensor_range", sensor_range)) + { + exitWithParameterError("sensor_range"); + } + + if (!ros::param::get("~voxelSize_", voxelSize_)) + { + exitWithParameterError("voxelSize_"); + } + + if (!ros::param::get("~num_particles", num_particles)) + { + exitWithParameterError("num_particles"); + } + + if (!ros::param::get("~odomXMod_", odomXMod_)) + { + exitWithParameterError("odomXMod_"); + } + + if (!ros::param::get("~odomYMod_", odomYMod_)) + { + exitWithParameterError("odomYMod_"); + } + + if (!ros::param::get("~odomZMod_", odomZMod_)) + { + exitWithParameterError("odomZMod_"); + } + + if (!ros::param::get("~odomAMod_", odomAMod_)) + { + exitWithParameterError("odomAMod_"); + } + + if (!ros::param::get("~resampleInterval_", resampleInterval_)) + { + exitWithParameterError("resampleInterval_"); + } + + if (!ros::param::get("~updateRate_", updateRate_)) + { + exitWithParameterError("updateRate_"); + } + + if (!ros::param::get("~dTh_", dTh_)) + { + exitWithParameterError("dTh_"); + } + + if (!ros::param::get("~aTh_", aTh_)) + { + exitWithParameterError("aTh_"); + } + + if (!ros::param::get("~takeOffHeight_", takeOffHeight_)) + { + exitWithParameterError("takeOffHeight_"); + } + + if (!ros::param::get("~alpha_", alpha_)) + { + exitWithParameterError("alpha_"); + } + + ROS_INFO("[%s]" + "\n Parameters:" + "\n inCloudTopic=%s" + "\n inOdomTopic=%s" + "\n inRangeTopic=%s" + "\n baseFrameId_=%s" + "\n odomFrameId_=%s" + "\n globalFrameId_=%s" + "\n map_path=%s" + "\n setInitialPose_=%d" + "\n initX_=%lf" + "\n initY_=%lf" + "\n initZ_=%lf" + "\n initA_=%lf" + "\n initZOffset_=%lf" + "\n initXDev_=%lf" + "\n initYDev_=%lf" + "\n initZDev_=%lf" + "\n initADev_=%lf" + "\n publish_point_cloud_rate=%lf" + "\n grid_slice=%f" + "\n publish_grid_slice_rate=%lf" + "\n publish_grid_tf_rate=%lf" + "\n sensor_dev=%lf" + "\n sensor_range=%lf" + "\n voxelSize_=%lf" + "\n num_particles=%d" + "\n odomXMod_=%lf" + "\n odomYMod_=%lf" + "\n odomZMod_=%lf" + "\n odomAMod_=%lf" + "\n resampleInterval_=%d" + "\n updateRate_=%lf" + "\n dTh_=%lf" + "\n aTh_=%lf" + "\n takeOffHeight_=%lf" + "\n alpha_=%lf", + ros::this_node::getName().data(), inCloudTopic.c_str(), inOdomTopic.c_str(), inRangeTopic.c_str(), + baseFrameId_.c_str(), odomFrameId_.c_str(), globalFrameId_.c_str(), map_path.c_str(), (int)setInitialPose_, + initX_, initY_, initZ_, initA_, initZOffset_, initXDev_, initYDev_, initZDev_, initADev_, + publish_point_cloud_rate, grid_slice, publish_grid_slice_rate, publish_grid_tf_rate, sensor_dev, + sensor_range, voxelSize_, num_particles, odomXMod_, odomYMod_, odomZMod_, odomAMod_, resampleInterval_, + updateRate_, dTh_, aTh_, takeOffHeight_, alpha_); +} + +void Parameters::exitWithParameterError(const char* parameterStr) +{ + ROS_ERROR("[%s] `%s` parameter not set!", ros::this_node::getName().data(), parameterStr); + exit(EXIT_FAILURE); +} + +} // namespace amcl3d diff --git a/amcl3d/src/Parameters.h b/amcl3d/src/Parameters.h new file mode 100644 index 0000000..1a56d6c --- /dev/null +++ b/amcl3d/src/Parameters.h @@ -0,0 +1,50 @@ +#pragma once + +#include + +namespace amcl3d +{ +class Parameters +{ +public: + explicit Parameters(); + + std::string inCloudTopic; + std::string inOdomTopic; + std::string inRangeTopic; + std::string baseFrameId_; + std::string odomFrameId_; + std::string globalFrameId_; + std::string map_path; + + bool setInitialPose_; + double initX_, initY_, initZ_, initA_, initZOffset_; + double initXDev_, initYDev_, initZDev_, initADev_; + + float grid_slice; + double publish_point_cloud_rate; + double publish_grid_slice_rate; + double publish_grid_tf_rate; + + double sensor_dev; + double sensor_range; + double voxelSize_; + + int num_particles; + + double odomXMod_, odomYMod_, odomZMod_, odomAMod_; + + int resampleInterval_; + + double updateRate_; + double dTh_, aTh_; + + double takeOffHeight_; + + double alpha_; + +private: + void exitWithParameterError(const char* parameterStr); +}; + +} // namespace amcl3d diff --git a/amcl3d/src/ParticleFilter.cpp b/amcl3d/src/ParticleFilter.cpp new file mode 100644 index 0000000..2678ef5 --- /dev/null +++ b/amcl3d/src/ParticleFilter.cpp @@ -0,0 +1,240 @@ +#include "ParticleFilter.h" + +#include + +namespace amcl3d +{ +ParticleFilter::ParticleFilter() +{ + //! Setup random number generator from GSL + gsl_rng_env_setup(); + random_value_ = gsl_rng_alloc(gsl_rng_default); +} + +ParticleFilter::~ParticleFilter() +{ + gsl_rng_free(random_value_); +} + +void ParticleFilter::buildParticlesPoseMsg(const geometry_msgs::Point32& offset, geometry_msgs::PoseArray& msg) const +{ + msg.poses.resize(p_.size()); + + for (uint32_t i = 0; i < p_.size(); ++i) + { + msg.poses[i].position.x = static_cast(p_[i].x) + offset.x; + msg.poses[i].position.y = static_cast(p_[i].y) + offset.y; + msg.poses[i].position.z = static_cast(p_[i].z) + offset.z; + msg.poses[i].orientation.x = 0.; + msg.poses[i].orientation.y = 0.; + msg.poses[i].orientation.z = sin(static_cast(p_[i].a * 0.5f)); + msg.poses[i].orientation.w = cos(static_cast(p_[i].a * 0.5f)); + } +} + +void ParticleFilter::init(const int num_particles, const float x_init, const float y_init, const float z_init, + const float a_init, const float x_dev, const float y_dev, const float z_dev, + const float a_dev) +{ + //! Resize particle set + p_.resize(abs(num_particles)); + + //! Sample the given pose + const float dev = std::max(std::max(x_dev, y_dev), z_dev); + const float gaussConst1 = 1. / (dev * sqrt(2 * M_PI)); + const float gaussConst2 = 1. / (2 * dev * dev); + + p_[0].x = x_init; + p_[0].y = y_init; + p_[0].z = z_init; + p_[0].a = a_init; + p_[0].w = gaussConst1; + + float wt = p_[0].w; + float dist; + + for (uint32_t i = 1; i < p_.size(); ++i) + { + p_[i].x = p_[0].x + gsl_ran_gaussian(random_value_, x_dev); + p_[i].y = p_[0].y + gsl_ran_gaussian(random_value_, y_dev); + p_[i].z = p_[0].z + gsl_ran_gaussian(random_value_, z_dev); + p_[i].a = p_[0].a + gsl_ran_gaussian(random_value_, a_dev); + + dist = sqrt((p_[i].x - p_[0].x) * (p_[i].x - p_[0].x) + (p_[i].y - p_[0].y) * (p_[i].y - p_[0].y) + + (p_[i].z - p_[0].z) * (p_[i].z - p_[0].z)); + + p_[i].w = gaussConst1 * exp(-dist * dist * gaussConst2); + + wt += p_[i].w; + } + + Particle mean_p; + for (uint32_t i = 0; i < p_.size(); ++i) + { + p_[i].w /= wt; + + mean_p.x += p_[i].w * p_[i].x; + mean_p.y += p_[i].w * p_[i].y; + mean_p.z += p_[i].w * p_[i].z; + mean_p.a += p_[i].w * p_[i].a; + } + mean_ = mean_p; + + initialized_ = true; +} + +void ParticleFilter::predict(const double odom_x_mod, const double odom_y_mod, const double odom_z_mod, + const double odom_a_mod, const double delta_x, const double delta_y, const double delta_z, + const double delta_a) +{ + const double x_dev = fabs(delta_x * odom_x_mod); + const double y_dev = fabs(delta_y * odom_y_mod); + const double z_dev = fabs(delta_z * odom_z_mod); + const double a_dev = fabs(delta_a * odom_a_mod); + + //! Make a prediction for all particles according to the odometry + float sa, ca, rand_x, rand_y; + for (uint32_t i = 0; i < p_.size(); ++i) + { + sa = sin(p_[i].a); + ca = cos(p_[i].a); + rand_x = delta_x + gsl_ran_gaussian(random_value_, x_dev); + rand_y = delta_y + gsl_ran_gaussian(random_value_, y_dev); + p_[i].x += ca * rand_x - sa * rand_y; + p_[i].y += sa * rand_x + ca * rand_y; + p_[i].z += delta_z + gsl_ran_gaussian(random_value_, z_dev); + p_[i].a += delta_a + gsl_ran_gaussian(random_value_, a_dev); + } +} + +void ParticleFilter::update(const Grid3d& grid3d, const std::vector& points, + const std::vector& range_data, const double& alpha, const double& sigma_) +{ + //! Incorporate measurements + float wtp = 0, wtr = 0; + std::vector new_points; + new_points.resize(points.size()); + + for (uint32_t i = 0; i < p_.size(); ++i) + { + //! Get particle information + float tx = p_[i].x; + float ty = p_[i].y; + float tz = p_[i].z; + float sa = sin(p_[i].a); + float ca = cos(p_[i].a); + + //! Check the particle is into the map + if (!grid3d.isIntoMap(tx, ty, tz)) + { + // std::cout << "Not into map: " << grid3d_.isIntoMap(tx, ty, tz-1.0) << std::endl; + p_[i].w = 0; + continue; + } + + //! Transform every point to current particle position + for (uint32_t j = 0; j < points.size(); ++j) + { + //! Get point + const pcl::PointXYZ& p = points[j]; + + //! Translate and rotate it in yaw + new_points[j].x = ca * p.x - sa * p.y + tx; + new_points[j].y = sa * p.x + ca * p.y + ty; + new_points[j].z = p.z + tz; + } + + //! Evaluate the weight of the point cloud + p_[i].wp = grid3d.computeCloudWeight(new_points); + + //! Evaluate the weight of the range sensors + p_[i].wr = computeRangeWeight(tx, ty, tz, range_data, sigma_); + + //! Increase the summatory of weights + wtp += p_[i].wp; + wtr += p_[i].wr; + } + + //! Normalize all weights + float wt = 0; + for (uint32_t i = 0; i < p_.size(); ++i) + { + if (wtp > 0) + p_[i].wp /= wtp; + else + p_[i].wp = 0; + + if (wtr > 0) + p_[i].wr /= wtr; + else + p_[i].wr = 0; + + p_[i].w = p_[i].wp * alpha + p_[i].wr * (1 - alpha); + wt += p_[i].w; + } + + Particle mean_p; + for (uint32_t i = 0; i < p_.size(); ++i) + { + if (wt > 0) + p_[i].w /= wt; + else + p_[i].w = 0; + + mean_p.x += p_[i].w * p_[i].x; + mean_p.y += p_[i].w * p_[i].y; + mean_p.z += p_[i].w * p_[i].z; + mean_p.a += p_[i].w * p_[i].a; + } + mean_ = mean_p; +} + +void ParticleFilter::resample() +{ + std::vector newP(p_.size()); + const float factor = 1.f / p_.size(); + const float r = factor * gsl_rng_uniform(random_value_); + float c = p_[0].w; + float u; + + //! Do resamplig + for (uint32_t m = 0, i = 0; m < p_.size(); ++m) + { + u = r + factor * m; + while (u > c) + { + if (++i >= p_.size()) + break; + c += p_[i].w; + } + newP[m] = p_[i]; + newP[m].w = factor; + } + + //! Asign the new particles set + p_ = newP; +} + +float ParticleFilter::computeRangeWeight(const float x, const float y, const float z, + const std::vector& range_data, const double sigma_) +{ + if (range_data.empty()) + return 0; + + float w = 1; + const float k1 = 1.f / (sigma_ * sqrt(2 * M_PI)); + const float k2 = 0.5f / (sigma_ * sigma_); + float ax, ay, az, r; + for (uint32_t i = 0; i < range_data.size(); ++i) + { + ax = range_data[i].ax; + ay = range_data[i].ay; + az = range_data[i].az; + r = sqrt((x - ax) * (x - ax) + (y - ay) * (y - ay) + (z - az) * (z - az)); + w *= k1 * exp(-k2 * (r - range_data[i].r) * (r - range_data[i].r)); + } + + return w; +} + +} // namespace amcl3d diff --git a/amcl3d/src/ParticleFilter.h b/amcl3d/src/ParticleFilter.h new file mode 100644 index 0000000..fe5f286 --- /dev/null +++ b/amcl3d/src/ParticleFilter.h @@ -0,0 +1,93 @@ +#pragma once + +#include "Grid3d.h" +#include "Parameters.h" + +#include + +#include +#include +#include +#include + +namespace amcl3d +{ +//! Struct that contains the data concerning one particle +struct Particle +{ + //! Position + float x, y, z; + + //! Yaw angle + float a; + + //! Weight + float w, wp, wr; + + Particle() : x(0), y(0), z(0), a(0), w(0), wp(0), wr(0) + { + } +}; + +//! Struct that contains the data concerning one range meaurement +struct Range +{ + float r, ax, ay, az; + + Range(const float r_, const float ax_, const float ay_, const float az_) : r(r_), ax(ax_), ay(ay_), az(az_) + { + } +}; + +class ParticleFilter +{ +public: + explicit ParticleFilter(); + virtual ~ParticleFilter(); + + bool isInitialized() const + { + return initialized_; + } + + //! Get mean value from particles + Particle getMean() const + { + return mean_; + } + + void buildParticlesPoseMsg(const geometry_msgs::Point32& offset, geometry_msgs::PoseArray& msg) const; + + //! Set the initial pose of the particle filter + void init(const int num_particles, const float x_init, const float y_init, const float z_init, const float a_init, + const float x_dev, const float y_dev, const float z_dev, const float a_dev); + + //! This function implements the PF prediction stage. + //! Translation in X, Y and Z in meters and yaw angle incremenet in rad + void predict(const double odom_x_mod, const double odom_y_mod, const double odom_z_mod, const double odom_a_mod, + const double delta_x, const double delta_y, const double delta_z, const double delta_a); + + //! Update Particles with a pointcloud update + void update(const Grid3d& grid3d, const std::vector& points, const std::vector& range_data, + const double& alpha, const double& sigma_); + + //! Resample the set of particles using low-variance sampling + void resample(); + +private: + float computeRangeWeight(const float x, const float y, const float z, const std::vector& range_data, + const double sigma_); + + //! Indicates if the filter was initialized + bool initialized_{ false }; + + //! Particles + std::vector p_; + + Particle mean_; // nuevos + + //! Random number generator + gsl_rng* random_value_; +}; + +} // namespace amcl3d diff --git a/amcl3d/src/main.cpp b/amcl3d/src/main.cpp new file mode 100644 index 0000000..47bf2c4 --- /dev/null +++ b/amcl3d/src/main.cpp @@ -0,0 +1,21 @@ +#include "Node.h" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "amcl3d_node"); + + ROS_INFO("[%s] Node initialization.", ros::this_node::getName().data()); + + if (!ros::master::check()) + { + ROS_ERROR("[%s] Roscore is not running.", ros::this_node::getName().data()); + return EXIT_FAILURE; + } + + amcl3d::Node node; + node.spin(); + + ROS_INFO("[%s] Node finished.", ros::this_node::getName().data()); + + return EXIT_SUCCESS; +} diff --git a/amcl3d/test/Test.cpp b/amcl3d/test/Test.cpp new file mode 100755 index 0000000..3a7683b --- /dev/null +++ b/amcl3d/test/Test.cpp @@ -0,0 +1,85 @@ +#include "../test/Test.h" + +#include "tf/transform_listener.h" + +namespace amcl3d +{ +Test::Test() +{ +} + +Test::~Test() +{ +} + +void Test::spin() +{ + ROS_DEBUG("[%s] Test::spin()", ros::this_node::getName().data()); + + //! Initialize transforms and variables + vicon_tf_.setIdentity(); + vicon_init_tf_.setIdentity(); + vicon_relative_tf_.setIdentity(); + + //! Initialize subscribers and publishers + vicon_sub_ = nh_.subscribe("vicon_client/ROSIN_F550/pose", 1, &Test::baseCallback, this); + vicon_pub_ = nh_.advertise("vicon_odometry", 1); + + pointcloud_sub_ = nh_.subscribe("/os1_cloud_node/points", 1, &Test::cloudCallback, this); + pointcloud_pub_ = nh_.advertise("lidar_pointcloud", 1); + + while (ros::ok()) + { + ros::spinOnce(); + } + + nh_.shutdown(); +}; + +void Test::cloudCallback(const sensor_msgs::PointCloud2Ptr& msg) +{ + //! To correct publishing the pointcloud + msg->header.stamp = ros::Time(0); + msg->header.frame_id = "lidar_points"; + + pointcloud_pub_.publish(msg); +}; + +void Test::baseCallback(const geometry_msgs::PoseStampedConstPtr& msg) +{ + //! To send the odometry from /odom + + vicon_tf_.setOrigin(tf::Vector3(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z)); + vicon_tf_.setRotation(tf::Quaternion(msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, + msg->pose.orientation.w)); + + if (!got_vicon_init_) + { + vicon_init_tf_.setOrigin(tf::Vector3(msg->pose.position.x, msg->pose.position.y, 0)); + vicon_init_tf_.setRotation(tf::Quaternion(msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, + msg->pose.orientation.w)); + got_vicon_init_ = true; + } + + vicon_relative_tf_ = vicon_init_tf_.inverse() * vicon_tf_; + + tf::Vector3 position = vicon_relative_tf_.getOrigin(); + tf::Quaternion orientation = vicon_relative_tf_.getRotation(); + + geometry_msgs::TransformStamped vicon_relative; + vicon_relative.header.stamp.sec = ros::Time::now().sec; + vicon_relative.header.stamp.nsec = ros::Time::now().nsec; + vicon_relative.header.frame_id = "vicon_odometry"; + vicon_relative.transform.translation.x = position.getX(); + vicon_relative.transform.translation.y = position.getY(); + vicon_relative.transform.translation.z = position.getZ(); + vicon_relative.transform.rotation.x = orientation.getX(); + vicon_relative.transform.rotation.y = orientation.getY(); + vicon_relative.transform.rotation.z = orientation.getZ(); + vicon_relative.transform.rotation.w = orientation.getW(); + + vicon_pub_.publish(vicon_relative); + + br_.sendTransform(tf::StampedTransform(vicon_tf_, ros::Time::now(), "world", "vicon_real")); +}; +} // namespace amcl3d diff --git a/amcl3d/test/Test.h b/amcl3d/test/Test.h new file mode 100644 index 0000000..21298c0 --- /dev/null +++ b/amcl3d/test/Test.h @@ -0,0 +1,37 @@ +#pragma once + +#include +#include +#include + +namespace amcl3d +{ +class Test +{ +public: + explicit Test(); + virtual ~Test(); + + void spin(); + +private: + void cloudCallback(const sensor_msgs::PointCloud2Ptr& msg); + void baseCallback(const geometry_msgs::PoseStampedConstPtr& msg); + + ros::NodeHandle nh_; + + ros::Subscriber vicon_sub_; + ros::Subscriber pointcloud_sub_; + + ros::Publisher vicon_pub_; + ros::Publisher pointcloud_pub_; + + bool got_vicon_init_{ false }; + + tf::Transform vicon_tf_; + tf::Transform vicon_init_tf_; + tf::Transform vicon_relative_tf_; + + tf::TransformBroadcaster br_; +}; +} // namespace amcl3d diff --git a/amcl3d/test/main_test.cpp b/amcl3d/test/main_test.cpp new file mode 100644 index 0000000..7deb4ef --- /dev/null +++ b/amcl3d/test/main_test.cpp @@ -0,0 +1,21 @@ +#include "../test/Test.h" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "test_node"); + + ROS_INFO("[%s] Test initialization.", ros::this_node::getName().data()); + + if (!ros::master::check()) + { + ROS_ERROR("[%s] roscore is not running.", ros::this_node::getName().data()); + return EXIT_FAILURE; + } + + amcl3d::Test node; + node.spin(); + + ROS_INFO("[%s] Test finished.", ros::this_node::getName().data()); + + return EXIT_SUCCESS; +} diff --git a/rosinrange_msg/CMakeLists.txt b/rosinrange_msg/CMakeLists.txt new file mode 100644 index 0000000..f3c5296 --- /dev/null +++ b/rosinrange_msg/CMakeLists.txt @@ -0,0 +1,37 @@ +project(rosinrange_msg) +cmake_minimum_required(VERSION 2.8.3) + +################################## +## Configure CATKIN dependecies ## +################################## + +find_package(catkin REQUIRED + COMPONENTS + geometry_msgs + message_generation +) + +####################### +## Generate messages ## +####################### + +add_message_files(DIRECTORY msg) + +########################################## +## Generate added messages and services ## +########################################## + +generate_messages( + DEPENDENCIES + geometry_msgs +) + +################################### +## CATKIN specific configuration ## +################################### + +catkin_package( + CATKIN_DEPENDS + geometry_msgs + message_runtime +) \ No newline at end of file diff --git a/rosinrange_msg/msg/range_pose.msg b/rosinrange_msg/msg/range_pose.msg new file mode 100644 index 0000000..3a725dd --- /dev/null +++ b/rosinrange_msg/msg/range_pose.msg @@ -0,0 +1,6 @@ +Header header + +uint64 source_id +uint64 destination_id +float64 range +geometry_msgs/Point position \ No newline at end of file diff --git a/rosinrange_msg/package.xml b/rosinrange_msg/package.xml new file mode 100644 index 0000000..8766b66 --- /dev/null +++ b/rosinrange_msg/package.xml @@ -0,0 +1,26 @@ + + + + rosinrange_msg + 1.0.0 + +

+ ROSIN radio-range sensor message. +

+

+ It is the message that amcl3d uses to receive distance measures and positions of radio-range sensors. +

+ + http://wiki.ros.org/amcl3d + Paloma Carrasco Fernandez + Francisco J.Perez-Grau + Francisco Cuesta Rodriguez + TODO + + catkin + + geometry_msgs + message_generation + message_runtime + +