First version rtabmap_launch working

rtabmap_msgs/CMakeLists.txt

@@ -0,0 +1,79 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(rtabmap_msgs)
+
+##################
+## Dependencies ##
+##################
+
+find_package(catkin REQUIRED COMPONENTS
+             std_msgs std_srvs geometry_msgs sensor_msgs genmsg
+)
+
+#######################################
+## Declare ROS messages and services ##
+#######################################
+
+## Generate messages in the 'msg' folder
+add_message_files(
+   FILES
+   Info.msg
+   KeyPoint.msg
+   GlobalDescriptor.msg
+   ScanDescriptor.msg
+   MapData.msg
+   MapGraph.msg
+   NodeData.msg
+   Link.msg
+   OdomInfo.msg
+   Point2f.msg
+   Point3f.msg
+   Goal.msg
+   RGBDImage.msg
+   RGBDImages.msg
+   UserData.msg
+   GPS.msg
+   Path.msg
+   EnvSensor.msg
+   CameraModel.msg
+   CameraModels.msg
+)
+
+## Generate services in the 'srv' folder
+ add_service_files(
+   FILES
+   GetMap.srv
+   GetMap2.srv
+   ListLabels.srv
+   PublishMap.srv
+   ResetPose.srv
+   SetGoal.srv
+   SetLabel.srv
+   RemoveLabel.srv
+   GetPlan.srv
+   AddLink.srv
+   GetNodeData.srv
+   GetNodesInRadius.srv
+   LoadDatabase.srv
+   DetectMoreLoopClosures.srv
+   GlobalBundleAdjustment.srv
+   CleanupLocalGrids.srv
+ )
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+   DEPENDENCIES
+   std_msgs
+   geometry_msgs
+   sensor_msgs
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(
+  CATKIN_DEPENDS std_msgs std_srvs geometry_msgs sensor_msgs
+)
+
+###########
+## Build ##
+###########

rtabmap_msgs/msg/CameraModel.msg

@@ -0,0 +1,4 @@
+
+sensor_msgs/CameraInfo camera_info
+geometry_msgs/Transform local_transform
+

rtabmap_msgs/msg/CameraModels.msg

@@ -0,0 +1,3 @@
+
+CameraModel[] models
+

rtabmap_msgs/msg/EnvSensor.msg

@@ -0,0 +1,6 @@
+
+Header header
+
+# EnvSensor
+int32 type
+float64 value

rtabmap_msgs/msg/GPS.msg

@@ -0,0 +1,7 @@
+
+float64 stamp      # in seconds
+float64 longitude  # DD format
+float64 latitude   # DD format
+float64 altitude   # in meters
+float64 error      # in meters
+float64 bearing    # North 0->360 deg

rtabmap_msgs/msg/GlobalDescriptor.msg

@@ -0,0 +1,8 @@
+
+Header header
+
+# compressed global descriptor
+# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
+int32 type
+uint8[] info
+uint8[] data

rtabmap_msgs/msg/Goal.msg

@@ -0,0 +1,9 @@
+
+Header header
+
+# Set either node_id or node_label
+int32 node_id
+string node_label
+
+# optional: if not set, the base frame of the robot is used
+string frame_id

rtabmap_msgs/msg/Info.msg

@@ -0,0 +1,50 @@
+
+########################################
+# RTAB-Map info with statistics 
+########################################
+
+Header header
+
+int32 refId
+int32 loopClosureId
+int32 proximityDetectionId
+int32 landmarkId
+
+geometry_msgs/Transform loopClosureTransform
+
+####
+# For statistics...
+####
+# State (node IDs) of the current Working Memory (including STM)
+int32[] wmState
+
+# std::map<int, float> posterior;
+int32[] posteriorKeys
+float32[] posteriorValues
+
+# std::map<int, float> likelihood;
+int32[] likelihoodKeys
+float32[] likelihoodValues
+
+# std::map<int, float> rawLikelihood;
+int32[] rawLikelihoodKeys
+float32[] rawLikelihoodValues
+
+# std::map<int, int> weights;
+int32[] weightsKeys
+int32[] weightsValues
+
+# std::map<int, std::string> labels;
+int32[] labelsKeys
+string[] labelsValues
+
+# std::map<std::string, float> stats
+string[] statsKeys
+float32[] statsValues
+
+# std::vector<int> localPath
+int32[] localPath
+int32 currentGoalId
+
+# std::vector<int> odomCache
+MapGraph odom_cache

rtabmap_msgs/msg/KeyPoint.msg

@@ -0,0 +1,16 @@
+#class cv::KeyPoint
+#{
+#    Point2f pt;
+#    float size;
+#    float angle;
+#    float response;
+#    int octave;
+#    int class_id;
+#}
+
+Point2f pt
+float32 size
+float32 angle
+float32 response
+int32 octave
+int32 class_id

rtabmap_msgs/msg/Link.msg

@@ -0,0 +1,14 @@
+#class rtabmap::Link
+#{
+#   int from;
+#   int to;
+#   Type type;
+#   Transform transform;
+#   cv::Mat(6,6,CV_64FC1) information;
+#}
+
+int32 fromId
+int32 toId
+int32 type
+geometry_msgs/Transform transform
+float64[36] information

rtabmap_msgs/msg/MapData.msg

@@ -0,0 +1,15 @@
+
+Header header
+
+##################
+# Optimized graph
+##################
+MapGraph graph
+
+##################
+# Graph data
+##################
+NodeData[] nodes
+
+
+

rtabmap_msgs/msg/MapGraph.msg

@@ -0,0 +1,18 @@
+
+Header header
+
+##
+# /map to /odom transform
+# Always identity when the graph is optimized from the latest pose.
+##
+geometry_msgs/Transform mapToOdom
+
+# The poses
+int32[] posesId
+geometry_msgs/Pose[] poses
+
+# The links
+Link[] links
+
+
+

rtabmap_msgs/msg/NodeData.msg

@@ -0,0 +1,70 @@
+
+int32 id
+int32 mapId
+int32 weight
+float64 stamp
+string label
+
+# Pose from odometry not corrected
+geometry_msgs/Pose pose
+
+# Ground truth (optional)
+geometry_msgs/Pose groundTruthPose
+
+# GPS (optional)
+GPS gps
+
+# compressed image in /camera_link frame
+# use rtabmap::util3d::uncompressImage() from "rtabmap/core/util3d.h"
+uint8[] image
+
+# compressed depth image in /camera_link frame
+# use rtabmap::util3d::uncompressImage() from "rtabmap/core/util3d.h"
+uint8[] depth
+
+# Camera models
+float32[] fx
+float32[] fy
+float32[] cx
+float32[] cy
+float32[] width
+float32[] height
+float32[] baseline
+# local transform (/base_link -> /camera_link)
+geometry_msgs/Transform[] localTransform
+
+# compressed 2D or 3D laser scan
+# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
+uint8[] laserScan
+int32 laserScanMaxPts
+float32 laserScanMaxRange
+int32 laserScanFormat
+# local transform (/base_link -> /base_laser)
+geometry_msgs/Transform laserScanLocalTransform
+
+# compressed user data
+# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
+uint8[] userData
+
+# compressed occupancy grid
+# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
+uint8[] grid_ground
+uint8[] grid_obstacles
+uint8[] grid_empty_cells
+float32 grid_cell_size
+Point3f grid_view_point
+
+# std::multimap<wordId, index>
+# std::vector<cv::Keypoint>
+# std::vector<cv::Point3f>
+int32[] wordIdKeys
+int32[] wordIdValues
+KeyPoint[] wordKpts
+Point3f[] wordPts
+# compressed descriptors
+# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
+uint8[] wordDescriptors
+
+GlobalDescriptor[] globalDescriptors
+
+EnvSensor[] env_sensors

rtabmap_msgs/msg/OdomInfo.msg

@@ -0,0 +1,69 @@
+
+Header header
+
+bool lost
+int32 matches
+int32 inliers
+float32 icpInliersRatio
+float32 icpRotation
+float32 icpTranslation
+float32 icpStructuralComplexity
+float32 icpStructuralDistribution
+int32 icpCorrespondences
+float64[36] covariance
+int32 features
+int32 localMapSize
+int32 localScanMapSize
+int32 localKeyFrames
+int32 localBundleOutliers
+int32 localBundleConstraints
+float32 localBundleTime
+bool keyFrameAdded
+float32 timeEstimation
+float32 timeParticleFiltering
+float32 stamp
+float32 interval
+float32 distanceTravelled
+int32 memoryUsage # MB
+float32 gravityRollError
+float32 gravityPitchError
+
+# Local bundle camera ids
+int32[] localBundleIds
+
+# Local bundle camera models
+CameraModels[] localBundleModels
+
+# Local bundle camera poses
+geometry_msgs/Pose[] localBundlePoses
+
+geometry_msgs/Transform transform
+geometry_msgs/Transform transformFiltered
+geometry_msgs/Transform transformGroundTruth
+geometry_msgs/Transform guess
+
+# 0=F2M, 1=F2F
+int32 type
+
+# F2M odometry
+#   std::multimap<int, cv::KeyPoint> words;
+#   std::vector<int> wordMatches;
+#   std::vector<int> wordInliers;
+int32[] wordsKeys
+KeyPoint[] wordsValues
+int32[] wordMatches
+int32[] wordInliers
+int32[] localMapKeys
+Point3f[] localMapValues
+
+# local scan map data
+sensor_msgs/PointCloud2 localScanMap
+
+# F2F odometry
+#   std::vector<cv::Point2f> refCorners;
+#   std::vector<cv::Point2f> newCorners;
+#   std::vector<int> cornerInliers;
+Point2f[] refCorners
+Point2f[] newCorners
+int32[] cornerInliers
+

rtabmap_msgs/msg/Path.msg

@@ -0,0 +1,6 @@
+
+Header header
+
+int32[] nodeIds
+geometry_msgs/Pose[] poses
+

rtabmap_msgs/msg/Point2f.msg

@@ -0,0 +1,8 @@
+#class cv::Point2f
+#{
+#    float x;
+#    float y;
+#}
+
+float32 x
+float32 y

rtabmap_msgs/msg/Point3f.msg

@@ -0,0 +1,10 @@
+#class cv::Point3f
+#{
+#    float x;
+#    float y;
+#    float z;
+#}
+
+float32 x
+float32 y
+float32 z

rtabmap_msgs/msg/RGBDImage.msg

@@ -0,0 +1,25 @@
+
+Header header
+
+# For stereo, rgb corresponds to left camera, and depth the right camera.
+
+# camera info
+sensor_msgs/CameraInfo rgb_camera_info
+sensor_msgs/CameraInfo depth_camera_info
+
+# Raw
+sensor_msgs/Image rgb
+sensor_msgs/Image depth
+
+# Compressed
+sensor_msgs/CompressedImage rgb_compressed
+sensor_msgs/CompressedImage depth_compressed
+
+# Local features
+KeyPoint[] key_points
+Point3f[] points
+# compressed descriptors
+# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
+uint8[] descriptors
+
+GlobalDescriptor global_descriptor

rtabmap_msgs/msg/RGBDImages.msg

@@ -0,0 +1,4 @@
+
+Header header
+
+RGBDImage[] rgbd_images

rtabmap_msgs/msg/ScanDescriptor.msg

@@ -0,0 +1,8 @@
+
+Header header
+
+# scan or scan_cloud is set
+sensor_msgs/LaserScan scan
+sensor_msgs/PointCloud2 scan_cloud
+
+GlobalDescriptor global_descriptor

rtabmap_msgs/msg/UserData.msg

@@ -0,0 +1,12 @@
+
+Header header
+
+# OpenCV matrix containing the user data. A matrix of type CV_8UC1 
+# with 1 row is considered to be compressed (with rtabmap::compressData() method).
+# If you have one dimension unsigned 8 bits uncompressed data, make sure to transpose it
+# (to have multiple rows instead of multiple columns) in order to be detected as
+# not compressed.
+uint32 rows
+uint32 cols
+uint32 type
+uint8[] data

rtabmap_msgs/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>rtabmap_msgs</name>
+  <version>0.1.0</version>
+  <description>RTAB-Map's msgs package.</description>
+  <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
+  <author>Mathieu Labbe</author>
+  <license>BSD</license>
+  <url type="bugtracker">https://github.com/introlab/rtabmap_ros/issues</url>
+  <url type="repository">https://github.com/introlab/rtabmap_ros</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>genmsg</buildtool_depend>
+
+  <build_depend>message_generation</build_depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  
+  <exec_depend>message_runtime</exec_depend>
+</package>

rtabmap_msgs/srv/AddLink.srv

@@ -0,0 +1,4 @@
+#request
+Link link
+---
+#response

rtabmap_msgs/srv/CleanupLocalGrids.srv

@@ -0,0 +1,24 @@
+#  Cleanup local grids service
+# 
+#     Clear empty space from local occupancy grids 
+#     (and laser scans) based on the current optimized global 2d grid map.
+#     If the map needs to be regenerated in the future (e.g., when
+#     we re-use the map in SLAM mode), removed obstacles won't reappear.
+#     Use this with care and only when you know that the map doesn't have errors, 
+#     otherwise some real obstacles/walls may be cleared if there is too much 
+#     drift in the map.
+#
+
+# Radius in cells around empty cell without obstacles to clear underlying obstacles, default 1 cell if not set.
+int32 radius
+
+# Filter also the scans, default false if not set.
+# The filtered laser scans will be used for localization,
+# so if dynamic obstacles have been removed, localization won't try to
+# match them anymore. Filtering the laser scans cannot be reverted, 
+# but grids can (see DatabaseViewer->Edit menu).
+bool filter_scans
+
+---
+# return the number of grids or scans modified, -1 if there is an error
+int32 modified

rtabmap_msgs/srv/DetectMoreLoopClosures.srv

@@ -0,0 +1,27 @@
+# Detect more loop closures service
+#
+#     Based on the current optimized graph, 
+#     this process will try to find more nodes corresponding with each 
+#     other, and thus finding more loop closures to add to graph.
+# 
+
+# Cluster radius (m),     default 1 m if not set
+float32 cluster_radius_max
+
+# Cluster radius min (m), default 0 m if not set
+float32 cluster_radius_min
+
+# Cluster angle (deg),    default 0 deg if not set
+float32 cluster_angle
+
+# Iterations,             default 1 if not set
+int32 iterations
+
+# Add only intra session loop closures
+bool intra_only
+
+# Add only inter session loop closures
+bool inter_only
+---
+# return the number of loop closures detected, or -1 if it failed.
+int32 detected

rtabmap_msgs/srv/GetMap.srv

@@ -0,0 +1,7 @@
+#request
+bool global
+bool optimized
+bool graphOnly
+---
+#response
+MapData data

rtabmap_msgs/srv/GetMap2.srv

@@ -0,0 +1,12 @@
+#request
+bool global
+bool optimized
+bool with_images
+bool with_scans
+bool with_user_data
+bool with_grids
+bool with_words
+bool with_global_descriptors
+---
+#response
+MapData data

rtabmap_msgs/srv/GetNodeData.srv

@@ -0,0 +1,9 @@
+#request
+int32[] ids
+bool images
+bool scan
+bool grid
+bool user_data
+---
+#response
+NodeData[] data

rtabmap_msgs/srv/GetNodesInRadius.srv

@@ -0,0 +1,31 @@
+#request
+
+# In mapping mode (Mem/IncrementalMemory=true), if target pose 
+#   and node_id are all zeros, poses around the latest node 
+#   in the graph are returned.
+# In localization mode (Mem/IncrementalMemory=false), if target pose 
+#   and node_id are all zeros, poses around the latest localization 
+#   pose are returned.
+# If node_id is not zero, target pose is ignored.
+
+# Node id
+int32 node_id
+
+# Target pose:
+float32 x
+float32 y
+float32 z
+
+# Radius, <=0 means that RGBD/LocalRadius will be used 
+# if k is also 0. If k>0 and a radius of 0 means all nearest 
+# poses up to k.
+float32 radius
+
+# Maximum number of nearest poses
+int32 k
+
+---
+#response
+int32[] ids
+geometry_msgs/Pose[] poses
+float32[] distsSqr

rtabmap_msgs/srv/GetPlan.srv

@@ -0,0 +1,11 @@
+# Get a plan from the current position to the goal node or pose 
+
+# The final node of the goal (set 0 to use pose instead)
+int32 goal_node
+# The final pose of the goal position (used only if goalNodeId=0)
+geometry_msgs/PoseStamped goal
+
+# How many meters from the map's graph we can plan (0=infinite)
+float32 tolerance
+---
+Path plan

rtabmap_msgs/srv/GlobalBundleAdjustment.srv

@@ -0,0 +1,22 @@
+# Global Bundle Adjustment service
+#
+#       Perform global bundle adjustment. Note that as soon as the map 
+#       is modified again, the graph is re-optimized the standard way (without SBA).
+#       It then makes only sense to use this after a mapping run (and after a call 
+#       to /rtabmap/pause) when you know that the robot will restart in localization 
+#       mode the next time, or at the beginning of the localization session.
+#
+
+# Optimizer type (0=g2o, 1=CVSBA), default 0
+int32 type
+
+# Iterations,        default 0 (use Optimizer/Iterations already loaded in the node)
+int32 iterations
+
+# Pixel variance,    default 0 (use g2o/PixelVariance already loaded in the node)
+float32 pixel_variance
+
+# Use vocabulary matches, default false (rematch all features between frames)
+bool voc_matches
+---
+# return false if failure

rtabmap_msgs/srv/ListLabels.srv

@@ -0,0 +1,5 @@
+#request
+---
+#response 
+int32[] ids
+string[] labels

rtabmap_msgs/srv/LoadDatabase.srv

@@ -0,0 +1,11 @@
+#request
+
+# Local database path on which rtabmap is running.
+# If the path doesn't exist, a new database will be created.
+string database_path
+
+# If the database already exists, data will be cleared if true.
+bool clear
+
+---
+#response, return false on rtabmap initialization failure.

rtabmap_msgs/srv/PublishMap.srv

@@ -0,0 +1,6 @@
+#request
+bool global
+bool optimized
+bool graphOnly
+---
+#response

rtabmap_msgs/srv/RemoveLabel.srv

@@ -0,0 +1,4 @@
+#request
+string label
+---
+#response 

rtabmap_msgs/srv/ResetPose.srv

@@ -0,0 +1,9 @@
+#request
+float32 x
+float32 y
+float32 z
+float32 roll
+float32 pitch
+float32 yaw
+---
+#response

rtabmap_msgs/srv/SetGoal.srv

@@ -0,0 +1,15 @@
+#request
+
+# Set either node_id or node_label
+int32 node_id
+string node_label
+
+# optional: if not set, the base frame of the robot is used
+string frame_id
+
+---
+
+#response 
+int32[] path_ids
+geometry_msgs/Pose[] path_poses
+float32 planning_time

rtabmap_msgs/srv/SetLabel.srv

@@ -0,0 +1,6 @@
+#request
+# Set node_id = 0 to set label to last node
+int32 node_id
+string node_label
+---
+#response