i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-09-13 06:40:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

Updated for rtabmap 0.8.12. Updated synchronization of messages (laser scan and images) with odometry pose stamp. Removed data_recorder node, use data_recorder.launch instead.

Browse Source
This commit is contained in:
Mathieu Labbe
2015-05-03 18:23:56 -04:00
parent 55ce1db43a
commit db4b4a9397
10 changed files with 215 additions and 1037 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+1 -5
View File
@@ -13,7 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
find_package(RTABMap 0.8.11 REQUIRED)
find_package(RTABMap 0.8.12 REQUIRED)
find_package(octomap REQUIRED)
#Qt stuff
@@ -175,9 +175,6 @@ target_link_libraries(camera ${Libraries})
add_executable(rtabmapviz src/GuiNode.cpp src/GuiWrapper.cpp src/PreferencesDialogROS.cpp)
target_link_libraries(rtabmapviz rtabmap_ros ${QT_LIBRARIES} ${Libraries})
add_executable(data_recorder src/DataRecorderNode.cpp)
target_link_libraries(data_recorder rtabmap_ros ${QT_LIBRARIES} ${Libraries})
add_executable(data_player src/DbPlayerNode.cpp)
target_link_libraries(data_player rtabmap_ros ${QT_LIBRARIES} ${Libraries})
@@ -208,7 +205,6 @@ install(TARGETS
map_assembler
grid_map_assembler
map_optimizer
data_recorder
data_player
camera
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
launch/data_recorder.launch
+51 -35
View File
@@ -1,40 +1,56 @@
<launch>
<node name="data_recorder" pkg="rtabmap_ros" type="data_recorder" output="screen">
<param name="output_file_name" value="output.db" type="string"/>
<param name="frame_id" type="string" value="camera_link"/>
<param name="subscribe_odometry" type="bool" value="false"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="false"/>
<remap from="rgb/image" to="data_throttled_image"/>
<remap from="depth/image" to="data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="data_throttled_camera_info"/>
<param name="rgb/image_transport" type="string" value="raw"/>
<param name="depth/image_transport" type="string" value="raw"/>
<param name="queue_size" type="int" value="10"/>
</node>
<!-- Throttling messages -->
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager"/>
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle standalone_nodelet" output="screen">
<param name="rate" type="double" value="10.0"/>
<remap from="rgb/image_in" to="/camera/rgb/image_rect_color"/>
<remap from="depth/image_in" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info_in" to="/camera/depth_registered/camera_info"/>
<remap from="rgb/image_out" to="data_throttled_image"/>
<remap from="depth/image_out" to="data_throttled_image_depth"/>
<remap from="rgb/camera_info_out" to="data_throttled_camera_info"/>
</node>
<!-- arguments -->
<arg name="subscribe_odometry" default="false"/>
<arg name="subscribe_depth" default="true"/>
<arg name="subscribe_stereo" default="false"/>
<arg name="subscribe_laserScan" default="false"/>
<node pkg="image_view" type="image_view" name="image_view">
<remap from="image" to="data_throttled_image"/>
</node>
<arg name="frame_id" default="camera_link"/>
<arg name="odom_frame_id" default=""/> <!-- use topic when not set, otherwise use TF if set -->
<arg name="output_path" default="output.db"/>
<arg name="record_in_RAM" default="true"/>
<arg name="queue_size" default="10"/>
<arg name="max_rate" default="0"/> <!-- Record as fast as possible -->
<arg name="camera_prefix" default="camera"/>
<arg name="odom_topic" default="odom"/>
<arg name="scan_topic" default="scan"/>
<node name="data_recorder" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<!-- Disable any processing -->
<param name="Mem/RehearsalSimilarity" type="string" value="1.0"/> <!-- desactivate rehearsal -->
<param name="Kp/WordsPerImage" type="string" value="-1"/> <!-- desactivate keypoints extraction -->
<param name="Rtabmap/MaxRetrieved" type="string" value="0"/> <!-- desactivate global retrieval -->
<param name="RGBD/MaxLocalRetrieved" type="string" value="0"/> <!-- desactivate local retrieval -->
<param name="Rtabmap/MemoryThr" type="string" value="1"/> <!-- keep the WM empty -->
<param name="Mem/STMSize" type="string" value="1"/> <!-- STM=1 -->
<param name="publish_tf" type="bool" value="false"/> <!-- don't publish TF -->
<param unless="$(arg subscribe_odometry)" name="RGBD/Enabled" type="string" value="false"/>
<param name="Rtabmap/DetectionRate" type="string" value="$(arg max_rate)"/>
<param name="DbSqlite3/InMemory" type="string" value="$(arg record_in_RAM)"/>
<param name="database_path" type="string" value="$(arg output_path)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_depth" type="bool" value="$(arg subscribe_depth)"/>
<param name="subscribe_laserScan" type="bool" value="$(arg subscribe_laserScan)"/>
<param name="subscribe_stereo" type="bool" value="$(arg subscribe_stereo)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<!-- Hack to use a fake odom_frame_id=frame_id if subscribe_odometry = false -->
<param if="$(arg subscribe_odometry)" name="odom_frame_id" type="string" value="$(arg odom_frame_id)"/>
<param unless="$(arg subscribe_odometry)" name="odom_frame_id" type="string" value="$(arg frame_id)"/>
<remap from="rgb/image" to="$(arg camera_prefix)/rgb/image_rect_color"/>
<remap from="rgb/camera_info" to="$(arg camera_prefix)/rgb/camera_info"/>
<remap from="depth/image" to="$(arg camera_prefix)/depth_registered/image_raw"/>
<remap from="left/image_rect" to="$(arg camera_prefix)/left/image_rect"/>
<remap from="left/camera_info" to="$(arg camera_prefix)/left/camera_info"/>
<remap from="right/image_rect" to="$(arg camera_prefix)/right/image_rect"/>
<remap from="right/camera_info" to="$(arg camera_prefix)/right/camera_info"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>
</node>
</launch>
launch/demo/demo_data_recorder.launch
+24 -22
View File
@@ -5,27 +5,29 @@
<!-- This demo works with demo_mapping.bag -->
<param name="use_sim_time" type="bool" value="True"/>
<node name="data_recorder" pkg="rtabmap_ros" type="data_recorder" output="screen">
<param name="output_file_name" value="output.db" type="string"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="subscribe_odometry" type="bool" value="true"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="true"/>
<remap from="odom" to="/az3/base_controller/odom"/>
<remap from="scan" to="/jn0/base_scan"/>
<remap from="rgb/image" to="data_throttled_image"/>
<remap from="depth/image" to="data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="data_throttled_camera_info"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
<param name="queue_size" type="int" value="10"/>
</node>
<include file="$(find rtabmap_ros)/launch/data_recorder.launch">
<arg name="subscribe_odometry" value="true"/>
<arg name="subscribe_depth" value="true"/>
<arg name="subscribe_stereo" value="false"/>
<arg name="subscribe_laserScan" value="true"/>
<arg name="frame_id" value="base_footprint"/>
<arg name="odom_frame_id" value=""/> <!-- use topic when not set, otherwise use TF if set -->
<arg name="output_path" value="output.db"/>
<arg name="record_in_RAM" value="false"/>
<arg name="queue_size" value="10"/>
<arg name="max_rate" value="0"/>
<arg name="camera_prefix" value="/camera"/>
<arg name="odom_topic" value="/az3/base_controller/odom"/>
<arg name="scan_topic" value="/jn0/base_scan"/>
</include>
<!-- uncompress image data -->
<node name="cameraInfo_relay" type="relay" pkg="topic_tools" args="/data_throttled_camera_info /camera/rgb/camera_info"/>
<node name="republish_rgb" type="republish" pkg="image_transport" args="compressed in:=/data_throttled_image raw out:=/camera/rgb/image_rect_color" />
<node name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=/data_throttled_image_depth raw out:=/camera/depth_registered/image_raw" />
</launch>
launch/rgbd_localization.launch
-106
View File
@@ -1,106 +0,0 @@
<launch>
<!-- RGB-D LOCALIZATION VERSION -->
<!-- Choose visualization -->
<arg name="rviz" default="false" />
<arg name="rtabmapviz" default="true" />
<!-- ODOMETRY MAIN ARGUMENTS:
-"strategy" : Strategy: 0=BOW (bag-of-words) 1=Optical Flow
-"feature" : Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
-"nn" : Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE
Set to 1 for float descriptor like SIFT/SURF
Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK
-"max_depth" : Maximum features depth (m)
-"min_inliers" : Minimum visual correspondences to accept a transformation (m)
-"inlier_distance" : RANSAC maximum inliers distance (m)
-"local_map" : Local map size: number of unique features to keep track
-"odom_info_data" : Fill odometry info messages with inliers/outliers data.
-->
<arg name="strategy" default="0" />
<arg name="feature" default="6" />
<arg name="nn" default="3" />
<arg name="max_depth" default="4.0" />
<arg name="min_inliers" default="20" />
<arg name="inlier_distance" default="0.02" />
<arg name="local_map" default="1000" />
<arg name="odom_info_data" default="true" />
<!-- TF FRAMES -->
<node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf"
args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /camera_link 100" />
<group ns="rtabmap">
<!-- Odometry -->
<node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
<remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
<param name="Odom/FeatureType" type="string" value="$(arg feature)"/>
<param name="OdomBow/NNType" type="string" value="$(arg nn)"/>
<param name="Odom/MaxDepth" type="string" value="$(arg max_depth)"/>
<param name="Odom/MinInliers" type="string" value="$(arg min_inliers)"/>
<param name="Odom/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/>
<param name="Odom/FillInfoData" type="string" value="$(arg odom_info_data)"/>
</node>
<!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="false"/>
<remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
<param name="Rtabmap/DatabasePath" type="string" value="~/.ros/rtabmap.db"/> <!-- Database used for localization -->
<param name="Mem/STMSize" type="string" value="1"/> <!-- 1 location in short-term memory -->
<param name="Mem/IncrementalMemory" type="string" value="false"/> <!-- false = Localization mode-->
<param name="LccBow/MinInliers" type="string" value="10"/> <!-- Minimum inliers to accept a loop closure -->
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_odom_info" type="bool" value="$(arg odom_info_data)"/>
<remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
</node>
</group>
<!-- Visualization RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>
<!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) -->
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap_ros/data_odom_sync standalone_nodelet">
<remap from="rgb/image_in" to="camera/rgb/image_rect_color"/>
<remap from="depth/image_in" to="camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info_in" to="camera/rgb/camera_info"/>
<remap from="odom_in" to="rtabmap/odom"/>
<remap from="rgb/image_out" to="data_odom_sync/image"/>
<remap from="depth/image_out" to="data_odom_sync/depth"/>
<remap from="rgb/camera_info_out" to="data_odom_sync/camera_info"/>
<remap from="odom_out" to="odom_sync"/>
</node>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
<remap from="rgb/image" to="data_odom_sync/image"/>
<remap from="depth/image" to="data_odom_sync/depth"/>
<remap from="rgb/camera_info" to="data_odom_sync/camera_info"/>
<remap from="cloud" to="voxel_cloud" />
<param name="voxel_size" type="double" value="0.01"/>
</node>
</launch>
launch/rgbd_mapping.launch
+11 -9
View File
@@ -5,9 +5,12 @@
<!-- WARNING : Database is automatically deleted on each startup -->
<!-- See "delete_db_on_start" option below... -->
<!-- Choose visualization -->
<arg name="rviz" default="false" />
<arg name="rtabmapviz" default="true" />
<!-- Choose visualization -->
<arg name="rviz" default="false" />
<arg name="rtabmapviz" default="true" />
<!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
<arg name="frame_id" default="camera_link"/>
<!-- ODOMETRY MAIN ARGUMENTS:
-"strategy" : Strategy: 0=BOW (bag-of-words) 1=Optical Flow
@@ -29,11 +32,7 @@
<arg name="inlier_distance" default="0.02" />
<arg name="local_map" default="1000" />
<arg name="odom_info_data" default="true" />
<!-- TF FRAMES -->
<node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf"
args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /camera_link 100" />
<group ns="rtabmap">
<!-- Odometry -->
@@ -42,6 +41,7 @@
<remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
<param name="Odom/FeatureType" type="string" value="$(arg feature)"/>
<param name="OdomBow/NNType" type="string" value="$(arg nn)"/>
@@ -56,19 +56,21 @@
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
<param name="LccBow/MinInliers" type="string" value="10"/>
<param name="LccBow/InlierDistance" type="string" value="0.02"/>
<param name="LccBow/InlierDistance" type="string" value="$(arg inlier_distance)"/>
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_odom_info" type="bool" value="$(arg odom_info_data)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/>
launch/rgbd_mapping_kinect2.launch
+12 -10
View File
@@ -15,10 +15,13 @@
-->
<arg name="rgb_prefix" default="rgb_lowres" />
<arg name="depth_prefix" default="depth_lowres" />
<!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
<arg name="frame_id" default="kinect2_link"/>
<!-- Choose visualization -->
<arg name="rviz" default="false" />
<arg name="rtabmapviz" default="true" />
<!-- Choose visualization -->
<arg name="rviz" default="false" />
<arg name="rtabmapviz" default="true" />
<!-- ODOMETRY MAIN ARGUMENTS:
-"strategy" : Strategy: 0=BOW (bag-of-words) 1=Optical Flow
@@ -35,17 +38,13 @@
<arg name="strategy" default="0" />
<arg name="feature" default="6" />
<arg name="nn" default="3" />
<arg name="max_depth" default="4.0" />
<arg name="max_depth" default="8.0" />
<arg name="min_inliers" default="20" />
<arg name="inlier_distance" default="0.02" />
<arg name="local_map" default="1000" />
<arg name="gftt_max_corners" default="500" />
<arg name="gftt_min_distance" default="7" />
<!-- TF FRAMES -->
<node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf"
args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /kinect2_link 100" />
<group ns="rtabmap">
<!-- Odometry -->
@@ -54,6 +53,7 @@
<remap from="depth/image" to="/kinect2/$(arg depth_prefix)/image"/>
<remap from="rgb/camera_info" to="/kinect2/$(arg rgb_prefix)/camera_info"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="approx_sync" type="bool" value="true"/>
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
@@ -72,6 +72,7 @@
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<remap from="rgb/image" to="/kinect2/$(arg rgb_prefix)/image"/>
<remap from="depth/image" to="/kinect2/$(arg depth_prefix)/image"/>
@@ -80,7 +81,7 @@
<param name="approx_sync" type="bool" value="true"/>
<param name="LccBow/MinInliers" type="string" value="10"/>
<param name="LccBow/InlierDistance" type="string" value="0.02"/>
<param name="LccBow/InlierDistance" type="string" value="$(arg inlier_distance)"/>
</node>
<!-- Visualisation RTAB-Map -->
@@ -88,6 +89,7 @@
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="true"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<remap from="rgb/image" to="/kinect2/$(arg rgb_prefix)/image"/>
<remap from="depth/image" to="/kinect2/$(arg depth_prefix)/image"/>
package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package>
<name>rtabmap_ros</name>
<version>0.8.11</version>
<version>0.8.12</version>
<description>RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
<maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
src/CoreWrapper.cpp
+113 -35
View File
@@ -185,6 +185,10 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
pnh.param("map_cache_cleanup", mapCacheCleanup_, mapCacheCleanup_);
ROS_INFO("rtabmap: frame_id = %s", frameId_.c_str());
if(!odomFrameId_.empty())
{
ROS_INFO("rtabmap: odom_frame_id = %s", odomFrameId_.c_str());
}
ROS_INFO("rtabmap: map_frame_id = %s", mapFrameId_.c_str());
ROS_INFO("rtabmap: queue_size = %d", queueSize);
ROS_INFO("rtabmap: tf_delay = %f", tfDelay);
@@ -305,6 +309,12 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
Parameters::kMemNotLinkedNodesKept().c_str());
parameters_.at(Parameters::kMemNotLinkedNodesKept())= vStr;
}
else if(iter->compare("RGBD/LocalLoopDetectionMaxDiffID") == 0)
{
ROS_WARN("Parameter name changed: RGBD/LocalLoopDetectionMaxDiffID -> %s. Please update your launch file accordingly.",
Parameters::kRGBDLocalLoopDetectionMaxGraphDepth().c_str());
parameters_.at(Parameters::kRGBDLocalLoopDetectionMaxGraphDepth())= vStr;
}
}
}
@@ -331,17 +341,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
"detection on images-only.");
}
}
else
{
// loop closure detection (images-only)
if(subscribeDepth || subscribeLaserScan || subscribeStereo)
{
ROS_WARN("ROS param subscribe_depth, subscribe_laserScan or subscribe_stereo is true, but RTAB-Map "
"parameter \"RGBD/Enabled\" is false! Please set subscribe_depth, subscribe_laserScan and subscribe_stereo "
"to false to use rtabmap node for loop closure detection on images-only, or set \"RGBD/Enabled\" to true "
"for RGB-D SLAM.");
}
}
if(paused_)
{
ROS_WARN("Node paused... don't forget to call service \"resume\" to start rtabmap.");
@@ -577,6 +577,7 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
}
lastPose_ = odom;
lastPoseStamp_ = odomMsg->header.stamp;
float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
if(uIsFinite(rotVariance) && rotVariance > rotVariance_)
@@ -638,6 +639,7 @@ bool CoreWrapper::commonOdomTFUpdate(const ros::Time & stamp)
}
lastPose_ = odom;
lastPoseStamp_ = stamp;
// Throttle
if(rate_>0.0f)
{
@@ -652,7 +654,7 @@ bool CoreWrapper::commonOdomTFUpdate(const ros::Time & stamp)
return false;
}
Transform CoreWrapper::getLocalTransform(const std::string & sensorFrameId, const ros::Time & stamp) const
Transform CoreWrapper::getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const
{
// TF ready?
Transform localTransform;
@@ -660,15 +662,15 @@ Transform CoreWrapper::getLocalTransform(const std::string & sensorFrameId, cons
{
if(waitForTransform_)
{
if(!tfListener_.waitForTransform(frameId_, sensorFrameId, stamp, ros::Duration(1)))
if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), sensorFrameId.c_str());
ROS_WARN("Could not get transform from %s to %s after 1 second!", fromFrameId.c_str(), toFrameId.c_str());
return localTransform;
}
}
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, sensorFrameId, stamp, tmp);
tfListener_.lookupTransform(fromFrameId, toFrameId, stamp, tmp);
localTransform = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
@@ -697,17 +699,38 @@ void CoreWrapper::commonDepthCallback(
return;
}
Transform localTransform = getLocalTransform(depthMsg->header.frame_id, depthMsg->header.stamp);
//for sync transform
Transform odomT = getTransform(odomFrameId, frameId_, lastPoseStamp_);
if(odomT.isNull() && !odomFrameId_.empty())
{
ROS_WARN("Could not get TF transform from %s to %s, sensors will not be synchronized with odometry pose.",
odomFrameId.c_str(), frameId_.c_str());
}
Transform localTransform = getTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp);
if(localTransform.isNull())
{
return;
}
// sync with odometry stamp
if(lastPoseStamp_ != depthMsg->header.stamp)
{
if(!odomT.isNull())
{
Transform sensorT = getTransform(odomFrameId, frameId_, depthMsg->header.stamp);
if(sensorT.isNull())
{
return;
}
localTransform = odomT.inverse() * sensorT * localTransform;
}
}
cv::Mat scan;
if(scanMsg.get() != 0)
{
// make sure the frame of the laser is updated too
if(getLocalTransform(scanMsg->header.frame_id, scanMsg->header.stamp).isNull())
if(getTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp).isNull())
{
return;
}
@@ -716,9 +739,25 @@ void CoreWrapper::commonDepthCallback(
sensor_msgs::PointCloud2 scanOut;
laser_geometry::LaserProjection projection;
projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
pcl::PointCloud<pcl::PointXYZ> pclScan;
pcl::fromROSMsg(scanOut, pclScan);
scan = util3d::laserScanFromPointCloud(pclScan);
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(scanOut, *pclScan);
// sync with odometry stamp
if(lastPoseStamp_ != scanMsg->header.stamp)
{
if(!odomT.isNull())
{
Transform sensorT = getTransform(odomFrameId, frameId_, scanMsg->header.stamp);
if(sensorT.isNull())
{
return;
}
Transform t = odomT.inverse() * sensorT;
pclScan = util3d::transformPointCloud<pcl::PointXYZ>(pclScan, t);
}
}
scan = util3d::laserScanFromPointCloud(*pclScan);
}
cv_bridge::CvImageConstPtr ptrImage;
@@ -741,7 +780,7 @@ void CoreWrapper::commonDepthCallback(
float cy = model.cy();
process(ptrImage->header.seq,
scanMsg.get() != 0?scanMsg->header.stamp:ptrImage->header.stamp,
scanMsg.get() != 0?scanMsg->header.stamp:ptrDepth->header.stamp,
ptrImage->image,
lastPose_,
odomFrameId,
@@ -754,7 +793,7 @@ void CoreWrapper::commonDepthCallback(
cy,
localTransform,
scan,
(int)scanMsg->ranges.size());
scanMsg.get() != 0?(int)scanMsg->ranges.size():0);
rotVariance_ = 0;
transVariance_ = 0;
}
@@ -780,17 +819,39 @@ void CoreWrapper::commonStereoCallback(
return;
}
Transform localTransform = getLocalTransform(leftImageMsg->header.frame_id, leftImageMsg->header.stamp);
//for sync transform
Transform odomT = getTransform(odomFrameId, frameId_, lastPoseStamp_);
if(odomT.isNull() && !odomFrameId_.empty())
{
ROS_WARN("Could not get TF transform from %s to %s, sensors will not be synchronized with odometry pose.",
odomFrameId.c_str(), frameId_.c_str());
}
Transform localTransform = getTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp);
if(localTransform.isNull())
{
return;
}
// sync with odometry stamp
if(lastPoseStamp_ != leftImageMsg->header.stamp)
{
if(!odomT.isNull())
{
Transform sensorT = getTransform(odomFrameId, frameId_, leftImageMsg->header.stamp);
if(sensorT.isNull())
{
return;
}
localTransform = odomT.inverse() * sensorT * localTransform;
}
}
cv::Mat scan;
if(scanMsg.get() != 0)
{
// make sure the frame of the laser is updated too
if(getLocalTransform(scanMsg->header.frame_id, scanMsg->header.stamp).isNull())
if(getTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp).isNull())
{
return;
}
@@ -798,9 +859,25 @@ void CoreWrapper::commonStereoCallback(
sensor_msgs::PointCloud2 scanOut;
laser_geometry::LaserProjection projection;
projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
pcl::PointCloud<pcl::PointXYZ> pclScan;
pcl::fromROSMsg(scanOut, pclScan);
scan = util3d::laserScanFromPointCloud(pclScan);
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(scanOut, *pclScan);
// sync with odometry stamp
if(lastPoseStamp_ != scanMsg->header.stamp)
{
if(!odomT.isNull())
{
Transform sensorT = getTransform(odomFrameId, frameId_, scanMsg->header.stamp);
if(sensorT.isNull())
{
return;
}
Transform t = odomT.inverse() * sensorT;
pclScan = util3d::transformPointCloud<pcl::PointXYZ>(pclScan, t);
}
}
scan = util3d::laserScanFromPointCloud(*pclScan);
}
cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage;
@@ -824,7 +901,7 @@ void CoreWrapper::commonStereoCallback(
float baseline = model.baseline();
process(leftImageMsg->header.seq,
leftImageMsg->header.stamp,
scanMsg.get() != 0?scanMsg->header.stamp:leftImageMsg->header.stamp,
ptrLeftImage->image,
lastPose_,
odomFrameId,
@@ -837,7 +914,7 @@ void CoreWrapper::commonStereoCallback(
cy,
localTransform,
scan,
(int)scanMsg->ranges.size());
scanMsg.get() != 0?(int)scanMsg->ranges.size():0);
rotVariance_ = 0;
transVariance_ = 0;
}
@@ -1121,12 +1198,13 @@ void CoreWrapper::process(
{
timeRtabmap = timer.ticks();
}
ROS_INFO("rtabmap: Update rate=%fs, Limit=%fs, RTAB-Map=%fs, Publish=%fs (%d local nodes)",
1.0f/rate_,
rtabmap_.getTimeThreshold()/1000.0f,
timeRtabmap,
timer.ticks(),
rtabmap_.getWMSize()+rtabmap_.getSTMSize());
ROS_INFO("rtabmap: Rate=%.2fs, Limit=%.3fs, RTAB-Map=%.4fs, Pub=%.4fs (local map=%d, WM=%d)",
rate_>0?1.0f/rate_:0,
rtabmap_.getTimeThreshold()/1000.0f,
timeRtabmap,
timer.ticks(),
(int)rtabmap_.getLocalOptimizedPoses().size(),
rtabmap_.getWMSize()+rtabmap_.getSTMSize());
}
else if(!rtabmap_.isIDsGenerated())
{
src/CoreWrapper.h
+2 -1
View File
@@ -94,7 +94,7 @@ private:
bool commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg);
bool commonOdomTFUpdate(const ros::Time & stamp); // TF odom
rtabmap::Transform getLocalTransform(const std::string & sensorFrameId, const ros::Time & stamp) const;
rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const;
void commonDepthCallback(
const std::string & odomFrameId,
@@ -223,6 +223,7 @@ private:
rtabmap::Rtabmap rtabmap_;
bool paused_;
rtabmap::Transform lastPose_;
ros::Time lastPoseStamp_;
float rotVariance_;
float transVariance_;
rtabmap::Transform currentMetricGoal_;
src/DataRecorderNode.cpp
-813
View File
@@ -1,813 +0,0 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include "rtabmap/utilite/ULogger.h"
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/Odometry.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <image_geometry/stereo_camera_model.h>
#include "rtabmap_ros/MsgConversion.h"
#include <pcl_ros/transforms.h>
#include <pcl_conversions/pcl_conversions.h>
#include <laser_geometry/laser_geometry.h>
#include <rtabmap/core/util3d.h>
#include <QtGui/QApplication>
#include "rtabmap/gui/DataRecorder.h"
using namespace rtabmap;
float max3( const float& a, const float& b, const float& c)
{
float m=a>b?a:b;
return m>c?m:c;
}
class DataRecorderWrapper
{
public:
DataRecorderWrapper() :
fileName_("output.db"),
frameId_("base_link"),
odomFrameId_(""), // null = use topic, otherwise use referred TF.
waitForTransform_(false),
depthOdomScanSync_(0),
depthSync_(0),
depthImageSync_(0)
{
ros::NodeHandle pnh("~");
bool subscribeOdometry = false;
bool subscribeLaserScan = false;
bool subscribeDepth = false;
bool subscribeStereo = false;
int queueSize = 10;
double agePenalty = 0.1;
pnh.param("subscribe_odometry", subscribeOdometry, subscribeOdometry);
pnh.param("subscribe_depth", subscribeDepth, subscribeDepth);
pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo);
pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("age_penalty", agePenalty, agePenalty);
pnh.param("output_file_name", fileName_, fileName_);
pnh.param("frame_id", frameId_, frameId_);
pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
if(!subscribeOdometry)
{
UWARN("odom_frame_id set without setting subscribe_odometry to true. Ignoring odometry from TF.");
odomFrameId_.clear();
}
setupCallbacks(subscribeOdometry, subscribeDepth, subscribeStereo, subscribeLaserScan, queueSize, agePenalty);
}
bool init()
{
return recorder_.init(fileName_.c_str());
}
virtual ~DataRecorderWrapper()
{
if(depthOdomScanSync_)
delete depthOdomScanSync_;
if(depthSync_)
delete depthSync_;
if(depthImageSync_)
delete depthImageSync_;
}
private:
void setupCallbacks(
bool subscribeOdom,
bool subscribeDepth,
bool subscribeStereo,
bool subscribeLaserScan,
int queueSize,
double agePenalty)
{
if(subscribeStereo)
{
ros::NodeHandle nh; // public
ros::NodeHandle pnh("~"); // private
ros::NodeHandle left_nh(nh, "left");
ros::NodeHandle right_nh(nh, "right");
ros::NodeHandle left_pnh(pnh, "left");
ros::NodeHandle right_pnh(pnh, "right");
image_transport::ImageTransport left_it(left_nh);
image_transport::ImageTransport right_it(right_nh);
image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
imageSub_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft);
imageRightSub_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight);
cameraInfoSub_.subscribe(left_nh, "camera_info", 1);
cameraInfoRightSub_.subscribe(right_nh, "camera_info", 1);
if(subscribeOdom)
{
ROS_INFO("Registering Stereo+Odom callback...");
odomSub_.subscribe(nh, "odom", 1);
stereoOdomSync_ = new message_filters::Synchronizer<MyStereoOdomSyncPolicy>(MyStereoOdomSyncPolicy(queueSize), imageSub_, imageRightSub_, cameraInfoSub_, cameraInfoRightSub_, odomSub_);
stereoOdomSync_->setAgePenalty(agePenalty);
stereoOdomSync_->registerCallback(boost::bind(&DataRecorderWrapper::stereoOdomCallback, this, _1, _2, _3, _4, _5));
}
else
{
ROS_INFO("Registering Stereo callback...");
stereoSync_ = new message_filters::Synchronizer<MyStereoSyncPolicy>(MyStereoSyncPolicy(queueSize), imageSub_, imageRightSub_, cameraInfoSub_, cameraInfoRightSub_);
stereoSync_->registerCallback(boost::bind(&DataRecorderWrapper::stereoCallback, this, _1, _2, _3, _4));
}
}
else
{
ros::NodeHandle nh; // public
ros::NodeHandle pnh("~"); // private
ros::NodeHandle rgb_nh(nh, "rgb");
ros::NodeHandle depth_nh(nh, "depth");
ros::NodeHandle rgb_pnh(pnh, "rgb");
ros::NodeHandle depth_pnh(pnh, "depth");
image_transport::ImageTransport rgb_it(rgb_nh);
image_transport::ImageTransport depth_it(depth_nh);
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
if(subscribeOdom && subscribeDepth && subscribeLaserScan)
{
ROS_INFO("Registering Depth+LaserScan callback...");
imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
scanSub_.subscribe(nh, "scan", 1);
if(odomFrameId_.empty())
{
odomSub_.subscribe(nh, "odom", 1);
depthOdomScanSync_ = new message_filters::Synchronizer<MyDepthOdomScanSyncPolicy>(MyDepthOdomScanSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
depthOdomScanSync_->setAgePenalty(agePenalty);
depthOdomScanSync_->registerCallback(boost::bind(&DataRecorderWrapper::depthOdomScanCallback, this, _1, _2, _3, _4, _5));
}
else
{
depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(MyDepthScanSyncPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
depthScanSync_->setAgePenalty(agePenalty);
depthScanSync_->registerCallback(boost::bind(&DataRecorderWrapper::depthScanCallback, this, _1, _2, _3, _4));
}
}
else if(subscribeOdom && subscribeDepth && !subscribeLaserScan)
{
ROS_INFO("Registering Depth callback...");
imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
odomSub_.subscribe(nh, "odom", 1);
depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(MyDepthSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_);
depthSync_->setAgePenalty(agePenalty);
depthSync_->registerCallback(boost::bind(&DataRecorderWrapper::depthCallback, this, _1, _2, _3, _4));
}
else if(!subscribeOdom && subscribeDepth)
{
ROS_INFO("Registering to depth without odometry callback...");
imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
depthImageSync_ = new message_filters::Synchronizer<MyDepthImageSyncPolicy>(MyDepthImageSyncPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
depthImageSync_->setAgePenalty(agePenalty);
depthImageSync_->registerCallback(boost::bind(&DataRecorderWrapper::depthImageCallback, this, _1, _2, _3));
}
else
{
if(subscribeOdom && !subscribeDepth && subscribeLaserScan)
{
ROS_WARN("Cannot record only laser scan without depth images...");
}
ROS_INFO("Registering default callback (\"image\" only)...");
defaultSub_ = rgb_it.subscribe("image", 1, &DataRecorderWrapper::defaultCallback, this);
}
}
}
void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg)
{
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
rtabmap::SensorData data(ptrImage->image.clone());
recorder_.addData(data);
}
void depthImageCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
{
// TF ready?
Transform localTransform;
try
{
if(waitForTransform_)
{
if(!tfListener_.waitForTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), depthMsg->header.frame_id.c_str());
return;
}
}
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
localTransform = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return;
}
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfoMsg);
float fx = model.fx();
float fy = model.fy();
float cx = model.cx();
float cy = model.cy();
cv::Mat depth16;
if(ptrDepth->image.type() != CV_16UC1)
{
if(ptrDepth->image.type() == CV_32FC1)
{
//convert to 16 bits
depth16 = util3d::cvtDepthFromFloat(ptrDepth->image);
static bool shown = false;
if(!shown)
{
ROS_WARN("Use depth image with \"unsigned short\" type to "
"avoid conversion. This message is only printed once...");
shown = true;
}
}
else
{
ROS_ERROR("Depth image must be of type \"unsigned short\"!");
return;
}
}
else
{
depth16 = ptrDepth->image.clone();
}
rtabmap::SensorData data(
ptrImage->image.clone(),
depth16,
fx,
fy,
cx,
cy,
localTransform,
Transform(),
1.0f,
1.0f,
0,
rtabmap_ros::timestampFromROS(imageMsg->header.stamp));
recorder_.addData(data);
}
void depthCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
{
// TF ready?
Transform localTransform;
try
{
if(waitForTransform_)
{
if(!tfListener_.waitForTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), depthMsg->header.frame_id.c_str());
return;
}
}
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
localTransform = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return;
}
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfoMsg);
float fx = model.fx();
float fy = model.fy();
float cx = model.cx();
float cy = model.cy();
cv::Mat depth16;
if(ptrDepth->image.type() != CV_16UC1)
{
if(ptrDepth->image.type() == CV_32FC1)
{
//convert to 16 bits
depth16 = util3d::cvtDepthFromFloat(ptrDepth->image);
static bool shown = false;
if(!shown)
{
ROS_WARN("Use depth image with \"unsigned short\" type to "
"avoid conversion. This message is only printed once...");
shown = true;
}
}
else
{
ROS_ERROR("Depth image must be of type \"unsigned short\"!");
return;
}
}
else
{
depth16 = ptrDepth->image.clone();
}
float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
rtabmap::SensorData data(
ptrImage->image.clone(),
depth16,
fx,
fy,
cx,
cy,
localTransform,
rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
uIsFinite(rotVariance) && rotVariance>0?rotVariance:1,
uIsFinite(transVariance) && transVariance>0?transVariance:1,
0,
rtabmap_ros::timestampFromROS(odomMsg->header.stamp));
recorder_.addData(data);
}
void depthOdomScanCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg)
{
// TF ready?
Transform localTransform;
try
{
if(waitForTransform_)
{
if(!tfListener_.waitForTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), scanMsg->header.frame_id.c_str());
return;
}
if(!tfListener_.waitForTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), depthMsg->header.frame_id.c_str());
return;
}
}
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, tmp);
tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
localTransform = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return;
}
//transform in frameId_ frame
sensor_msgs::PointCloud2 scanOut;
laser_geometry::LaserProjection projection;
projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
pcl::PointCloud<pcl::PointXYZ> pclScan;
pcl::fromROSMsg(scanOut, pclScan);
cv::Mat scan = util3d::laserScanFromPointCloud(pclScan);
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfoMsg);
float fx = model.fx();
float fy = model.fy();
float cx = model.cx();
float cy = model.cy();
cv::Mat depth16;
if(ptrDepth->image.type() != CV_16UC1)
{
if(ptrDepth->image.type() == CV_32FC1)
{
//convert to 16 bits
depth16 = util3d::cvtDepthFromFloat(ptrDepth->image);
static bool shown = false;
if(!shown)
{
ROS_WARN("Use depth image with \"unsigned short\" type to "
"avoid conversion. This message is only printed once...");
shown = true;
}
}
else
{
ROS_ERROR("Depth image must be of type \"unsigned short\"!");
return;
}
}
else
{
depth16 = ptrDepth->image.clone();
}
float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
rtabmap::SensorData data(
scan,
(int)scanMsg->ranges.size(),
ptrImage->image.clone(),
depth16,
fx,
fy,
cx,
cy,
localTransform,
rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
uIsFinite(rotVariance) && rotVariance>0?rotVariance:1,
uIsFinite(transVariance) && transVariance>0?transVariance:1,
0,
rtabmap_ros::timestampFromROS(odomMsg->header.stamp));
recorder_.addData(data);
}
void depthScanCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg)
{
// TF ready?
Transform localTransform;
Transform odom;
try
{
if(waitForTransform_)
{
if(!tfListener_.waitForTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), scanMsg->header.frame_id.c_str());
return;
}
if(!tfListener_.waitForTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), depthMsg->header.frame_id.c_str());
return;
}
if(!odomFrameId_.empty())
{
if(!tfListener_.waitForTransform(odomFrameId_, frameId_, scanMsg->header.stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", odomFrameId_.c_str(), frameId_.c_str());
return;
}
}
}
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, tmp);
tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
localTransform = rtabmap_ros::transformFromTF(tmp);
if(!odomFrameId_.empty())
{
tfListener_.lookupTransform(odomFrameId_, frameId_, scanMsg->header.stamp, tmp);
odom = rtabmap_ros::transformFromTF(tmp);
}
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return;
}
//transform in frameId_ frame
sensor_msgs::PointCloud2 scanOut;
laser_geometry::LaserProjection projection;
projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
pcl::PointCloud<pcl::PointXYZ> pclScan;
pcl::fromROSMsg(scanOut, pclScan);
cv::Mat scan = util3d::laserScanFromPointCloud(pclScan);
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfoMsg);
float fx = model.fx();
float fy = model.fy();
float cx = model.cx();
float cy = model.cy();
cv::Mat depth16;
if(ptrDepth->image.type() != CV_16UC1)
{
if(ptrDepth->image.type() == CV_32FC1)
{
//convert to 16 bits
depth16 = util3d::cvtDepthFromFloat(ptrDepth->image);
static bool shown = false;
if(!shown)
{
ROS_WARN("Use depth image with \"unsigned short\" type to "
"avoid conversion. This message is only printed once...");
shown = true;
}
}
else
{
ROS_ERROR("Depth image must be of type \"unsigned short\"!");
return;
}
}
else
{
depth16 = ptrDepth->image.clone();
}
rtabmap::SensorData data(
scan,
(int)scanMsg->ranges.size(),
ptrImage->image.clone(),
depth16,
fx,
fy,
cx,
cy,
localTransform,
odom,
1,
1,
0,
rtabmap_ros::timestampFromROS(scanMsg->header.stamp));
recorder_.addData(data);
}
void stereoOdomCallback(
const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg,
const nav_msgs::OdometryConstPtr & odomMsg)
{
// TF ready?
Transform localTransform;
try
{
if(waitForTransform_)
{
if(!tfListener_.waitForTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), leftImageMsg->header.frame_id.c_str());
return;
}
}
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tmp);
localTransform = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return;
}
cv_bridge::CvImageConstPtr ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "bgr8");
cv_bridge::CvImageConstPtr ptrRightImage = cv_bridge::toCvShare(rightImageMsg, "mono8");
image_geometry::StereoCameraModel model;
model.fromCameraInfo(*leftCameraInfoMsg, *rightCameraInfoMsg);
float fx = model.right().fx();
float baseline = model.baseline();
float cx = model.right().cx();
float cy = model.right().cy();
float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
rtabmap::SensorData data(
ptrLeftImage->image.clone(),
ptrRightImage->image.clone(),
fx,
baseline,
cx,
cy,
localTransform,
rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
uIsFinite(rotVariance) && rotVariance>0?rotVariance:1,
uIsFinite(transVariance) && transVariance>0?transVariance:1,
0,
rtabmap_ros::timestampFromROS(odomMsg->header.stamp));
recorder_.addData(data);
}
void stereoCallback(
const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg)
{
// TF ready?
Transform localTransform;
try
{
if(waitForTransform_)
{
if(!tfListener_.waitForTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), leftImageMsg->header.frame_id.c_str());
return;
}
}
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tmp);
localTransform = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return;
}
cv_bridge::CvImageConstPtr ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "bgr8");
cv_bridge::CvImageConstPtr ptrRightImage = cv_bridge::toCvShare(rightImageMsg, "mono8");
image_geometry::StereoCameraModel model;
model.fromCameraInfo(*leftCameraInfoMsg, *rightCameraInfoMsg);
float fx = model.right().fx();
float baseline = model.baseline();
float cx = model.right().cx();
float cy = model.right().cy();
rtabmap::SensorData data(
ptrLeftImage->image.clone(),
ptrRightImage->image.clone(),
fx,
baseline,
cx,
cy,
localTransform,
Transform(),
1.0f,
1.0f,
0,
rtabmap_ros::timestampFromROS(leftImageMsg->header.stamp));
recorder_.addData(data);
}
private:
DataRecorder recorder_;
std::string fileName_;
std::string frameId_;
std::string odomFrameId_;
bool waitForTransform_;
image_transport::Subscriber defaultSub_;
image_transport::SubscriberFilter imageSub_;
image_transport::SubscriberFilter imageDepthSub_;
image_transport::SubscriberFilter imageRightSub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRightSub_;
message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
nav_msgs::Odometry,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::LaserScan> MyDepthOdomScanSyncPolicy;
message_filters::Synchronizer<MyDepthOdomScanSyncPolicy> * depthOdomScanSync_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::LaserScan> MyDepthScanSyncPolicy;
message_filters::Synchronizer<MyDepthScanSyncPolicy> * depthScanSync_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
nav_msgs::Odometry,
sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepthSyncPolicy;
message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_;
//without odom
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepthImageSyncPolicy;
message_filters::Synchronizer<MyDepthImageSyncPolicy> * depthImageSync_;
//stereo with odometry
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::CameraInfo,
nav_msgs::Odometry> MyStereoOdomSyncPolicy;
message_filters::Synchronizer<MyStereoOdomSyncPolicy> * stereoOdomSync_;
//stereo without odometry
typedef message_filters::sync_policies::ExactTime<
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::CameraInfo> MyStereoSyncPolicy;
message_filters::Synchronizer<MyStereoSyncPolicy> * stereoSync_;
tf::TransformListener tfListener_;
};
int main(int argc, char** argv)
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kInfo);
ros::init(argc, argv, "data_recorder");
QApplication app(argc, argv);
DataRecorderWrapper recorder;
if(recorder.init())
{
ros::spin();
}
else
{
ROS_ERROR("Cannot initialize the recorder! Make sure the parameter output_file_name is set!");
}
}