i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 08:47:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added lidar_deskewing node and nodelet. Added "deskewing" option for icp_odometry. Updated velodyne and ouster examples with deskewing option. Added velodyne+T265 deskewing example.

Browse Source
This commit is contained in:
matlabbe
2022-10-16 21:36:11 -07:00
parent cf50e93195
commit 44bbaa2cef
15 changed files with 931 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+5
View File
@@ -263,6 +263,7 @@ SET(rtabmap_plugins_lib_src
src/nodelets/undistort_depth.cpp src/nodelets/undistort_depth.cpp
src/nodelets/imu_to_tf.cpp src/nodelets/imu_to_tf.cpp
src/nodelets/rgbdx_sync.cpp src/nodelets/rgbdx_sync.cpp
src/nodelets/lidar_deskewing.cpp
) )
IF(${cv_bridge_VERSION_MAJOR} GREATER 1 OR ${cv_bridge_VERSION_MINOR} GREATER 10) IF(${cv_bridge_VERSION_MAJOR} GREATER 1 OR ${cv_bridge_VERSION_MINOR} GREATER 10)
@@ -386,6 +387,10 @@ add_executable(rtabmap_imu_to_tf src/ImuToTFNode.cpp)
target_link_libraries(rtabmap_imu_to_tf ${Libraries}) target_link_libraries(rtabmap_imu_to_tf ${Libraries})
set_target_properties(rtabmap_imu_to_tf PROPERTIES OUTPUT_NAME "imu_to_tf") set_target_properties(rtabmap_imu_to_tf PROPERTIES OUTPUT_NAME "imu_to_tf")
add_executable(rtabmap_lidar_deskewing src/LidarDeskewingNode.cpp)
target_link_libraries(rtabmap_lidar_deskewing ${Libraries})
set_target_properties(rtabmap_lidar_deskewing PROPERTIES OUTPUT_NAME "lidar_deskewing")
IF(NOT WIN32) IF(NOT WIN32)
add_executable(rtabmap_wifi_signal_pub src/WifiSignalPubNode.cpp) add_executable(rtabmap_wifi_signal_pub src/WifiSignalPubNode.cpp)
target_link_libraries(rtabmap_wifi_signal_pub rtabmap_ros) target_link_libraries(rtabmap_wifi_signal_pub rtabmap_ros)
include/rtabmap_ros/MsgConversion.h
+14
View File
@@ -264,6 +264,20 @@ bool convertScan3dMsg(
int maxPoints = 0, int maxPoints = 0,
float maxRange = 0.0f); float maxRange = 0.0f);
bool deskew(
const sensor_msgs::PointCloud2 & input,
sensor_msgs::PointCloud2 & output,
const std::string & fixedFrameId,
tf::TransformListener & listener,
double waitForTransform,
bool slerp = false);
bool deskew(
const sensor_msgs::PointCloud2 & input,
sensor_msgs::PointCloud2 & output,
double previousStamp,
const rtabmap::Transform & velocity);
} }
#endif /* MSGCONVERSION_H_ */ #endif /* MSGCONVERSION_H_ */
include/rtabmap_ros/OdometryROS.h
+4
View File
@@ -70,6 +70,7 @@ public:
const std::string & frameId() const {return frameId_;} const std::string & frameId() const {return frameId_;}
const std::string & odomFrameId() const {return odomFrameId_;} const std::string & odomFrameId() const {return odomFrameId_;}
const std::string & guessFrameId() const {return guessFrameId_;}
const rtabmap::ParametersMap & parameters() const {return parameters_;} const rtabmap::ParametersMap & parameters() const {return parameters_;}
bool isPaused() const {return paused_;} bool isPaused() const {return paused_;}
rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const; rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const;
@@ -80,6 +81,9 @@ protected:
virtual void flushCallbacks() = 0; virtual void flushCallbacks() = 0;
tf::TransformListener & tfListener() {return tfListener_;} tf::TransformListener & tfListener() {return tfListener_;}
double waitForTransformDuration() const {return waitForTransform_?waitForTransformDuration_:0.0;}
rtabmap::Transform velocityGuess() const;
double previousStamp() const {return previousStamp_;}
virtual void postProcessData(const rtabmap::SensorData & data, const std_msgs::Header & header) const {} virtual void postProcessData(const rtabmap::SensorData & data, const std_msgs::Header & header) const {}
private: private:
launch/demo/demo_hector_mapping.launch
+1
View File
@@ -69,6 +69,7 @@
<remap from="odom_info" to="/rtabmap/odom_info"/> <remap from="odom_info" to="/rtabmap/odom_info"/>
<param name="frame_id" type="string" value="base_footprint"/> <param name="frame_id" type="string" value="base_footprint"/>
<param name="deskewing" type="string" value="true"/>
<param if="$(arg odom_guess)" name="odom_frame_id" type="string" value="icp_odom"/> <param if="$(arg odom_guess)" name="odom_frame_id" type="string" value="icp_odom"/>
<param if="$(arg odom_guess)" name="guess_frame_id" type="string" value="odom"/> <param if="$(arg odom_guess)" name="guess_frame_id" type="string" value="odom"/>
launch/rtabmap.launch
+16 -3
View File
@@ -96,8 +96,10 @@
<arg name="scan_cloud_topic" default="/scan_cloud"/> <arg name="scan_cloud_topic" default="/scan_cloud"/>
<arg name="subscribe_scan_descriptor" default="false"/> <arg name="subscribe_scan_descriptor" default="false"/>
<arg name="scan_descriptor_topic" default="/scan_descriptor"/> <arg name="scan_descriptor_topic" default="/scan_descriptor"/>
<arg name="scan_deskewing" default="false"/>
<arg name="scan_deskewing_slerp" default="false"/>
<arg name="scan_cloud_max_points" default="0"/> <arg name="scan_cloud_max_points" default="0"/>
<arg name="scan_cloud_filtered" default="false"/> <!-- use filtered cloud from icp_odometry for mapping --> <arg name="scan_cloud_filtered" default="$(arg scan_deskewing)"/> <!-- use filtered cloud from icp_odometry for mapping -->
<arg name="gen_scan" default="false"/> <!-- only works with depth image and if not subscribing to scan topic--> <arg name="gen_scan" default="false"/> <!-- only works with depth image and if not subscribing to scan topic-->
<arg name="gen_depth" default="false" /> <!-- Generate depth image from scan_cloud --> <arg name="gen_depth" default="false" /> <!-- Generate depth image from scan_cloud -->
@@ -312,6 +314,17 @@
<param name="scan_cloud_max_points" type="int" value="$(arg scan_cloud_max_points)"/> <param name="scan_cloud_max_points" type="int" value="$(arg scan_cloud_max_points)"/>
<param name="expected_update_rate" type="double" value="$(arg odom_expected_rate)"/> <param name="expected_update_rate" type="double" value="$(arg odom_expected_rate)"/>
<param name="max_update_rate" type="double" value="$(arg odom_max_rate)"/> <param name="max_update_rate" type="double" value="$(arg odom_max_rate)"/>
<param name="deskewing" type="bool" value="$(arg scan_deskewing)"/>
<param name="deskewing_slerp" type="bool" value="$(arg scan_deskewing_slerp)"/>
</node>
<node if="$(eval not icp_odometry and scan_deskewing and subscribe_scan_cloud)" pkg="rtabmap_ros" type="lidar_deskewing" name="lidar_deskewing" clear_params="$(arg clear_params)" output="$(arg output)">
<param name="wait_for_transform" value="$(arg wait_for_transform)"/>
<param if="$(arg visual_odometry)" name="fixed_frame_id" value="$(arg vo_frame_id)"/>
<param unless="$(arg visual_odometry)" name="fixed_frame_id" value="$(arg odom_frame_id)"/>
<param name="slerp" value="$(arg scan_deskewing_slerp)"/>
<remap from="input_cloud" to="$(arg scan_cloud_topic)"/>
<remap from="$(arg scan_cloud_topic)/deskewed" to="odom_filtered_input_scan"/>
</node> </node>
<node if="$(arg scan_cloud_assembling)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" clear_params="$(arg clear_params)" output="$(arg output)"> <node if="$(arg scan_cloud_assembling)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" clear_params="$(arg clear_params)" output="$(arg output)">
@@ -432,8 +445,8 @@
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/> <remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap unless="$(arg icp_odometry)" from="scan" to="$(arg scan_topic)"/> <remap unless="$(arg icp_odometry)" from="scan" to="$(arg scan_topic)"/>
<remap if="$(arg icp_odometry)" from="scan_cloud" to="odom_filtered_input_scan"/> <remap if="$(eval icp_odometry or scan_cloud_filtered)" from="scan_cloud" to="odom_filtered_input_scan"/>
<remap unless="$(arg icp_odometry)" from="scan_cloud" to="$(arg scan_cloud_topic)"/> <remap unless="$(eval icp_odometry or scan_cloud_filtered)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap from="scan_descriptor" to="$(arg scan_descriptor_topic)"/> <remap from="scan_descriptor" to="$(arg scan_descriptor_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/> <remap from="odom" to="$(arg odom_topic)"/>
</node> </node>
launch/tests/test_ouster.launch
+6 -5
View File
@@ -12,14 +12,15 @@
coming from the first cloud sent by os1_cloud_node, which may be poorly synchronized with IMU data. coming from the first cloud sent by os1_cloud_node, which may be poorly synchronized with IMU data.
--> -->
<!-- Required: --> <arg name="use_sim_time" default="false"/>
<arg name="os1_hostname"/>
<arg name="os1_udp_dest"/>
<arg name="frame_id" default="os1_sensor"/> <!-- Required: -->
<arg unless="$(arg use_sim_time)" name="os1_hostname"/>
<arg unless="$(arg use_sim_time)" name="os1_udp_dest"/>
<arg name="frame_id" default="os1_sensor"/>
<arg name="rtabmapviz" default="true"/> <arg name="rtabmapviz" default="true"/>
<arg name="scan_20_hz" default="true"/> <arg name="scan_20_hz" default="true"/>
<arg name="use_sim_time" default="false"/>
<param if="$(arg use_sim_time)" name="use_sim_time" value="true"/> <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
<!-- Ouster --> <!-- Ouster -->
launch/tests/test_ouster_gen2.launch
+24 -11
View File
@@ -40,11 +40,14 @@
<arg name="frame_id" default="os_sensor"/> <arg name="frame_id" default="os_sensor"/>
<arg name="rtabmapviz" default="true"/> <arg name="rtabmapviz" default="true"/>
<arg name="deskewing" default="true"/>
<arg name="slerp" default="false"/>
<arg name="scan_20_hz" default="true"/> <arg name="scan_20_hz" default="true"/>
<arg name="voxel_size" default="0.15"/> <!-- indoor: 0.1 to 0.3, outdoor: 0.3 to 0.5 --> <arg name="voxel_size" default="0.15"/> <!-- indoor: 0.1 to 0.3, outdoor: 0.3 to 0.5 -->
<arg name="assemble" default="false"/> <arg name="assemble" default="false"/>
<arg name="ptp" default="false"/> <!-- See comments in header to start before launching the launch --> <arg name="ptp" default="false"/> <!-- See comments in header to start before launching the launch -->
<arg name="distortion_correction" default="false"/> <!-- Requires this pull request: https://github.com/ouster-lidar/ouster_example/pull/245 --> <arg name="imu_topic" default="/os_cloud_node/imu"/>
<arg name="scan_topic" default="/os_cloud_node/points"/>
<param if="$(arg use_sim_time)" name="use_sim_time" value="true"/> <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
@@ -56,13 +59,12 @@
<arg if="$(arg scan_20_hz)" name="lidar_mode" value="1024x20"/> <arg if="$(arg scan_20_hz)" name="lidar_mode" value="1024x20"/>
<arg unless="$(arg scan_20_hz)" name="lidar_mode" value="1024x10"/> <arg unless="$(arg scan_20_hz)" name="lidar_mode" value="1024x10"/>
<arg if="$(arg ptp)" name="timestamp_mode" value="TIME_FROM_PTP_1588"/> <arg if="$(arg ptp)" name="timestamp_mode" value="TIME_FROM_PTP_1588"/>
<arg if="$(arg distortion_correction)" name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
</include> </include>
<!-- IMU orientation estimation and publish tf accordingly to os_sensor frame --> <!-- IMU orientation estimation and publish tf accordingly to os_sensor frame -->
<node pkg="nodelet" type="nodelet" name="imu_nodelet_manager" args="manager"> <node pkg="nodelet" type="nodelet" name="imu_nodelet_manager" args="manager">
<remap from="imu/data_raw" to="/os_cloud_node/imu"/> <remap from="imu/data_raw" to="$(arg imu_topic)"/>
<remap from="imu/data" to="/os_cloud_node/imu/data"/> <remap from="imu/data" to="$(arg imu_topic)/filtered"/>
</node> </node>
<node pkg="nodelet" type="nodelet" name="imu_filter" args="load imu_filter_madgwick/ImuFilterNodelet imu_nodelet_manager"> <node pkg="nodelet" type="nodelet" name="imu_filter" args="load imu_filter_madgwick/ImuFilterNodelet imu_nodelet_manager">
<param name="use_mag" value="false"/> <param name="use_mag" value="false"/>
@@ -70,15 +72,26 @@
<param name="publish_tf" value="false"/> <param name="publish_tf" value="false"/>
</node> </node>
<node pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_ros/imu_to_tf imu_nodelet_manager"> <node pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_ros/imu_to_tf imu_nodelet_manager">
<remap from="imu/data" to="/os_cloud_node/imu/data"/> <remap from="imu/data" to="$(arg imu_topic)/filtered"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/> <param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="base_frame_id" value="$(arg frame_id)"/> <param name="base_frame_id" value="$(arg frame_id)"/>
</node> </node>
<!-- Lidar Deskewing -->
<node if="$(arg deskewing)" pkg="nodelet" type="nodelet" name="lidar_deskewing" args="standalone rtabmap_ros/lidar_deskewing" output="screen">
<param name="wait_for_transform" value="0.01"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="slerp" value="$(arg slerp)"/>
<remap from="input_cloud" to="$(arg scan_topic)"/>
</node>
<arg if="$(arg deskewing)" name="scan_topic_deskewed" default="$(arg scan_topic)/deskewed"/>
<arg unless="$(arg deskewing)" name="scan_topic_deskewed" default="$(arg scan_topic)"/>
<group ns="rtabmap"> <group ns="rtabmap">
<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen"> <node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
<remap from="scan_cloud" to="/os_cloud_node/points"/> <remap from="scan_cloud" to="$(arg scan_topic_deskewed)"/>
<remap from="imu" to="/os_cloud_node/imu/data"/> <remap from="imu" to="$(arg imu_topic)/filtered"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/> <param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="odom"/> <param name="odom_frame_id" type="string" value="odom"/>
<param if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/> <param if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
@@ -116,8 +129,8 @@
<param name="approx_sync" type="bool" value="false"/> <param name="approx_sync" type="bool" value="false"/>
<remap if="$(arg assemble)" from="scan_cloud" to="assembled_cloud"/> <remap if="$(arg assemble)" from="scan_cloud" to="assembled_cloud"/>
<remap unless="$(arg assemble)" from="scan_cloud" to="/os_cloud_node/points"/> <remap unless="$(arg assemble)" from="scan_cloud" to="$(arg scan_topic_deskewed)"/>
<remap from="imu" to="/os_cloud_node/imu/data"/> <remap from="imu" to="$(arg imu_topic)/filtered"/>
<!-- RTAB-Map's parameters --> <!-- RTAB-Map's parameters -->
<param if="$(arg assemble)" name="Rtabmap/DetectionRate" type="string" value="0"/> <!-- already set 1 Hz in point_cloud_assembler --> <param if="$(arg assemble)" name="Rtabmap/DetectionRate" type="string" value="0"/> <!-- already set 1 Hz in point_cloud_assembler -->
@@ -158,7 +171,7 @@
</node> </node>
<node if="$(arg assemble)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" output="screen"> <node if="$(arg assemble)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" output="screen">
<remap from="cloud" to="/os_cloud_node/points"/> <remap from="cloud" to="$(arg scan_topic_deskewed)"/>
<remap from="odom" to="odom"/> <remap from="odom" to="odom"/>
<param name="assembling_time" type="double" value="1" /> <param name="assembling_time" type="double" value="1" />
<param name="fixed_frame_id" type="string" value="" /> <param name="fixed_frame_id" type="string" value="" />
@@ -170,7 +183,7 @@
<param name="subscribe_odom_info" type="bool" value="true"/> <param name="subscribe_odom_info" type="bool" value="true"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/> <param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/> <param name="approx_sync" type="bool" value="false"/>
<remap from="scan_cloud" to="/os_cloud_node/points"/> <remap from="scan_cloud" to="odom_filtered_input_scan"/>
</node> </node>
</group> </group>
launch/tests/test_velodyne.launch
+27 -13
View File
@@ -14,7 +14,9 @@
<arg name="use_imu" default="false"/> <!-- Assuming IMU fixed to lidar with /velodyne -> /imu_link TF --> <arg name="use_imu" default="false"/> <!-- Assuming IMU fixed to lidar with /velodyne -> /imu_link TF -->
<arg name="imu_topic" default="/imu/data"/> <arg name="imu_topic" default="/imu/data"/>
<arg name="scan_20_hz" default="false"/> <!-- If we launch the velodyne with "rpm:=1200" argument --> <arg name="scan_20_hz" default="false"/> <!-- If we launch the velodyne with "rpm:=1200" argument -->
<arg name="organize_cloud" default="false"/> <arg name="deskewing" default="true"/>
<arg name="slerp" default="false"/> <!-- If true, a slerp between the first and last time will be used to deskew each point, which is faster than using tf for every point but less accurate -->
<arg name="organize_cloud" default="$(arg deskewing)"/> <!-- Should be organized if deskewing is enabled -->
<arg name="scan_topic" default="/velodyne_points"/> <arg name="scan_topic" default="/velodyne_points"/>
<arg name="use_sim_time" default="false"/> <arg name="use_sim_time" default="false"/>
<param if="$(arg use_sim_time)" name="use_sim_time" value="true"/> <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
@@ -24,7 +26,7 @@
<arg name="queue_size_odom" default="1"/> <!-- Set to 100 for kitti dataset to make sure all scans are processed --> <arg name="queue_size_odom" default="1"/> <!-- Set to 100 for kitti dataset to make sure all scans are processed -->
<arg name="loop_ratio" default="0.2"/> <arg name="loop_ratio" default="0.2"/>
<arg name="resolution" default="0.1"/> <!-- set 0.1-0.3 for indoor, set 0.3-0.5 for outdoor (0.4 for kitti) --> <arg name="resolution" default="0.05"/> <!-- set 0.05-0.3 for indoor, set 0.3-0.5 for outdoor (0.4 for kitti) -->
<arg name="iterations" default="10"/> <arg name="iterations" default="10"/>
<!-- Grid parameters --> <!-- Grid parameters -->
@@ -34,7 +36,7 @@
<!-- For F2M Odometry --> <!-- For F2M Odometry -->
<arg name="ground_normals_up" default="false"/> <!-- set to true when velodyne is always horizontal to ground (ground robot, car, kitti) --> <arg name="ground_normals_up" default="false"/> <!-- set to true when velodyne is always horizontal to ground (ground robot, car, kitti) -->
<arg name="local_map_size" default="15000"/> <arg name="local_map_size" default="15000"/>
<arg name="key_frame_thr" default="0.8"/> <arg name="key_frame_thr" default="0.6"/>
<!-- For FLOAM Odometry --> <!-- For FLOAM Odometry -->
<arg name="floam" default="false"/> <!-- RTAB-Map should be built with FLOAM http://official-rtab-map-forum.206.s1.nabble.com/icp-odometry-with-LOAM-crash-tp8261p8563.html --> <arg name="floam" default="false"/> <!-- RTAB-Map should be built with FLOAM http://official-rtab-map-forum.206.s1.nabble.com/icp-odometry-with-LOAM-crash-tp8261p8563.html -->
@@ -46,9 +48,18 @@
<arg name="organize_cloud" value="$(arg organize_cloud)"/> <arg name="organize_cloud" value="$(arg organize_cloud)"/>
</include> </include>
<!-- IMU orientation estimation and publish tf accordingly to os1_sensor frame --> <!-- IMU orientation estimation and publish tf -->
<node if="$(arg use_imu)" pkg="rtabmap_ros" type="imu_to_tf" name="imu_to_tf"> <node if="$(arg use_imu)" pkg="nodelet" type="nodelet" name="imu_nodelet_manager" args="manager">
<remap from="imu/data" to="$(arg imu_topic)"/> <remap from="imu/data_raw" to="$(arg imu_topic)"/>
<remap from="imu/data" to="$(arg imu_topic)/filtered"/>
</node>
<node if="$(arg use_imu)" pkg="nodelet" type="nodelet" name="imu_filter" args="load imu_filter_madgwick/ImuFilterNodelet imu_nodelet_manager">
<param name="use_mag" value="false"/>
<param name="world_frame" value="enu"/>
<param name="publish_tf" value="false"/>
</node>
<node if="$(arg use_imu)" pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_ros/imu_to_tf imu_nodelet_manager">
<remap from="imu/data" to="$(arg imu_topic)/filtered"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/> <param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="base_frame_id" value="$(arg frame_id)"/> <param name="base_frame_id" value="$(arg frame_id)"/>
</node> </node>
@@ -58,12 +69,14 @@
<remap from="scan_cloud" to="$(arg scan_topic)"/> <remap from="scan_cloud" to="$(arg scan_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/> <param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="odom"/> <param name="odom_frame_id" type="string" value="odom"/>
<param name="deskewing" type="bool" value="$(arg deskewing)"/>
<param name="deskewing_slerp" type="bool" value="$(arg slerp)"/>
<param name="queue_size" type="int" value="$(arg queue_size_odom)"/> <param name="queue_size" type="int" value="$(arg queue_size_odom)"/>
<param name="wait_for_transform_duration" type="double" value="0.2"/> <param name="wait_for_transform_duration" type="double" value="0.2"/>
<param if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/> <param if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
<param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/> <param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>
<remap if="$(arg use_imu)" from="imu" to="$(arg imu_topic)"/> <remap if="$(arg use_imu)" from="imu" to="$(arg imu_topic)/filtered"/>
<param if="$(arg use_imu)" name="guess_frame_id" type="string" value="$(arg frame_id)_stabilized"/> <param if="$(arg use_imu)" name="guess_frame_id" type="string" value="$(arg frame_id)_stabilized"/>
<param if="$(arg use_imu)" name="wait_imu_to_init" type="bool" value="true"/> <param if="$(arg use_imu)" name="wait_imu_to_init" type="bool" value="true"/>
@@ -92,8 +105,9 @@
<param name="OdomF2M/ScanMaxSize" type="string" value="$(arg local_map_size)"/> <param name="OdomF2M/ScanMaxSize" type="string" value="$(arg local_map_size)"/>
<param name="OdomLOAM/Sensor" type="string" value="$(arg floam_sensor)"/> <param name="OdomLOAM/Sensor" type="string" value="$(arg floam_sensor)"/>
<param name="OdomLOAM/Resolution" type="string" value="$(arg resolution)"/> <param name="OdomLOAM/Resolution" type="string" value="$(arg resolution)"/>
<param if="$(arg scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.05"/> <param if="$(eval not deskewing and scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.05"/>
<param unless="$(arg scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.1"/> <param if="$(eval not deskewing and not scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.1"/>
<param if="$(arg deskewing)" name="OdomLOAM/ScanPeriod" type="string" value="0"/>
</node> </node>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d"> <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
@@ -105,7 +119,7 @@
<param name="wait_for_transform_duration" type="double" value="0.2"/> <param name="wait_for_transform_duration" type="double" value="0.2"/>
<remap from="scan_cloud" to="assembled_cloud"/> <remap from="scan_cloud" to="assembled_cloud"/>
<remap from="imu" to="$(arg imu_topic)"/> <remap from="imu" to="$(arg imu_topic)/filtered"/>
<!-- RTAB-Map's parameters --> <!-- RTAB-Map's parameters -->
<param name="Rtabmap/DetectionRate" type="string" value="1"/> <param name="Rtabmap/DetectionRate" type="string" value="1"/>
@@ -127,7 +141,7 @@
<param name="Optimizer/GravitySigma" type="string" value="0.3"/> <param name="Optimizer/GravitySigma" type="string" value="0.3"/>
<!-- ICP parameters --> <!-- ICP parameters -->
<param name="Icp/VoxelSize" type="string" value="0"/> <!-- already voxelized by point_cloud_assembler below --> <param name="Icp/VoxelSize" type="string" value="$(arg resolution)"/>
<param name="Icp/PointToPlaneK" type="string" value="20"/> <param name="Icp/PointToPlaneK" type="string" value="20"/>
<param name="Icp/PointToPlaneRadius" type="string" value="0"/> <param name="Icp/PointToPlaneRadius" type="string" value="0"/>
<param name="Icp/PointToPlane" type="string" value="true"/> <param name="Icp/PointToPlane" type="string" value="true"/>
@@ -151,12 +165,12 @@
</node> </node>
<node pkg="nodelet" type="nodelet" name="point_cloud_assembler" args="standalone rtabmap_ros/point_cloud_assembler" output="screen"> <node pkg="nodelet" type="nodelet" name="point_cloud_assembler" args="standalone rtabmap_ros/point_cloud_assembler" output="screen">
<remap from="cloud" to="$(arg scan_topic)"/> <remap if="$(arg deskewing)" from="cloud" to="odom_filtered_input_scan"/>
<remap unless="$(arg deskewing)" from="cloud" to="$(arg scan_topic)"/>
<remap from="odom" to="odom"/> <remap from="odom" to="odom"/>
<param if="$(arg scan_20_hz)" name="max_clouds" type="int" value="20" /> <param if="$(arg scan_20_hz)" name="max_clouds" type="int" value="20" />
<param unless="$(arg scan_20_hz)" name="max_clouds" type="int" value="10" /> <param unless="$(arg scan_20_hz)" name="max_clouds" type="int" value="10" />
<param name="fixed_frame_id" type="string" value="" /> <param name="fixed_frame_id" type="string" value="" />
<param name="voxel_size" type="double" value="$(arg resolution)" />
<param name="queue_size" type="int" value="$(arg queue_size)" /> <param name="queue_size" type="int" value="$(arg queue_size)" />
</node> </node>
</group> </group>
launch/tests/test_velodyne_t265_deskewing.launch
+184
View File
@@ -0,0 +1,184 @@
<!-- -->
<launch>
<!--
Hand-held 3D lidar mapping example using only a Velodyne PUCK and external odometry (using t265 as example).
Prerequisities: rtabmap should be built with libpointmatcher
We use T265 only for lidar deskewing and icp_odometry guess in this example.
Example:
$ roslaunch rtabmap_ros test_velodyne_t265_deskewing.launch
$ rosrun rviz rviz -f map
$ Show TF and /rtabmap/cloud_map topics
-->
<arg name="rtabmapviz" default="true"/>
<arg name="scan_20_hz" default="false"/> <!-- If we launch the velodyne with "rpm:=1200" argument -->
<arg name="deskewing" default="true"/>
<arg name="slerp" default="false"/> <!-- If true, a slerp between the first and last time will be used to deskew each point, which is faster than using tf for every point but less accurate -->
<arg name="scan_topic" default="/velodyne_points"/>
<arg name="use_sim_time" default="false"/>
<param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
<arg name="odom_frame_id" default="t265_odom_frame"/> <!-- input odometry: here we use T265 odometry, but it could be wheel odometry -->
<arg name="frame_id" default="velodyne"/> <!-- base frame of the robot: for this example, we use velodyne as base frame -->
<arg name="queue_size" default="10"/>
<arg name="queue_size_odom" default="1"/>
<arg name="loop_ratio" default="0.2"/>
<arg name="resolution" default="0.05"/> <!-- set 0.05-0.3 for indoor, set 0.3-0.5 for outdoor -->
<arg name="iterations" default="10"/>
<!-- Grid parameters -->
<arg name="ground_is_obstacle" default="true"/>
<arg name="grid_max_range" default="20"/>
<!-- For F2M Odometry -->
<arg name="ground_normals_up" default="false"/> <!-- set to true when velodyne is always horizontal to ground (ground robot, car) -->
<arg name="local_map_size" default="15000"/>
<arg name="key_frame_thr" default="0.6"/>
<!-- For FLOAM Odometry -->
<arg name="floam" default="false"/> <!-- RTAB-Map should be built with FLOAM http://official-rtab-map-forum.206.s1.nabble.com/icp-odometry-with-LOAM-crash-tp8261p8563.html -->
<arg name="floam_sensor" default="0"/> <!-- 0=16 rings (VLP16), 1=32 rings, 2=64 rings -->
<!-- Static transform between velodyne and T265: TODO: Adjust with real position/orientation!!! -->
<node unless="$(arg use_sim_time)" pkg="tf" type="static_transform_publisher" name="T265_to_velodyne_tf" args="-0.01 0 0.055 0 0 0 t265_link velodyne 100"/>
<!-- Velodyne sensor VLP16 -->
<include unless="$(arg use_sim_time)" file="$(find velodyne_pointcloud)/launch/VLP16_points.launch">
<arg if="$(arg scan_20_hz)" name="rpm" value="1200"/>
<arg unless="$(arg scan_20_hz)" name="rpm" value="600"/>
<arg name="organize_cloud" value="true"/> <!-- should be organized for deskewing -->
</include>
<!-- T265 -->
<group unless="$(arg use_sim_time)" ns="t265">
<include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
<arg name="device_type" value="t265"/>
<arg name="serial_no" value=""/>
<arg name="tf_prefix" value="t265"/>
<arg name="initial_reset" value="false"/>
<arg name="enable_fisheye1" value="false"/>
<arg name="enable_fisheye2" value="false"/>
<arg name="topic_odom_in" value=""/>
<arg name="calib_odom_file" value=""/>
<arg name="enable_pose" value="true"/>
</include>
</group>
<!-- Lidar Deskewing -->
<node if="$(arg deskewing)" pkg="nodelet" type="nodelet" name="lidar_deskewing" args="standalone rtabmap_ros/lidar_deskewing" output="screen">
<param name="wait_for_transform" value="0.01"/>
<param name="fixed_frame_id" value="$(arg odom_frame_id)"/>
<param name="slerp" value="$(arg slerp)"/>
<remap from="input_cloud" to="$(arg scan_topic)"/>
</node>
<arg if="$(arg deskewing)" name="scan_topic_deskewed" default="$(arg scan_topic)/deskewed"/>
<arg unless="$(arg deskewing)" name="scan_topic_deskewed" default="$(arg scan_topic)"/>
<group ns="rtabmap">
<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
<remap from="scan_cloud" to="$(arg scan_topic_deskewed)"/>
<param name="guess_frame_id" type="string" value="$(arg odom_frame_id)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param name="queue_size" type="int" value="$(arg queue_size_odom)"/>
<param name="wait_for_transform_duration" type="double" value="0.2"/>
<param if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
<param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>
<!-- ICP parameters -->
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/Iterations" type="string" value="$(arg iterations)"/>
<param if="$(arg floam)" name="Icp/VoxelSize" type="string" value="0"/>
<param unless="$(arg floam)" name="Icp/VoxelSize" type="string" value="$(arg resolution)"/>
<param name="Icp/DownsamplingStep" type="string" value="1"/> <!-- cannot be increased with ring-like lidar -->
<param name="Icp/Epsilon" type="string" value="0.001"/>
<param if="$(arg floam)" name="Icp/PointToPlaneK" type="string" value="0"/>
<param unless="$(arg floam)" name="Icp/PointToPlaneK" type="string" value="20"/>
<param name="Icp/PointToPlaneRadius" type="string" value="0"/>
<param name="Icp/MaxTranslation" type="string" value="2"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="$(eval resolution*10)"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
<param if="$(arg ground_normals_up)" name="Icp/PointToPlaneGroundNormalsUp" type="string" value="0.8"/>
<!-- Odom parameters -->
<param name="Odom/ScanKeyFrameThr" type="string" value="$(arg key_frame_thr)"/>
<param if="$(arg floam)" name="Odom/Strategy" type="string" value="11"/>
<param unless="$(arg floam)" name="Odom/Strategy" type="string" value="0"/>
<param name="OdomF2M/ScanSubtractRadius" type="string" value="$(arg resolution)"/>
<param name="OdomF2M/ScanMaxSize" type="string" value="$(arg local_map_size)"/>
<param name="OdomLOAM/Sensor" type="string" value="$(arg floam_sensor)"/>
<param name="OdomLOAM/Resolution" type="string" value="$(arg resolution)"/>
<param if="$(eval not deskewing and scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.05"/>
<param if="$(eval not deskewing and not scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.1"/>
<param if="$(arg deskewing)" name="OdomLOAM/ScanPeriod" type="string" value="0"/>
</node>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgb" type="bool" value="false"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
<param name="wait_for_transform_duration" type="double" value="0.2"/>
<remap from="scan_cloud" to="assembled_cloud"/>
<!-- RTAB-Map's parameters -->
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="RGBD/NeighborLinkRefining" type="string" value="false"/>
<param name="RGBD/ProximityBySpace" type="string" value="true"/>
<param name="RGBD/ProximityMaxGraphDepth" type="string" value="0"/>
<param name="RGBD/ProximityPathMaxNeighbors" type="string" value="1"/>
<param name="RGBD/AngularUpdate" type="string" value="0.05"/>
<param name="RGBD/LinearUpdate" type="string" value="0.05"/>
<param name="Mem/NotLinkedNodesKept" type="string" value="false"/>
<param name="Mem/STMSize" type="string" value="30"/>
<param name="Mem/LaserScanNormalK" type="string" value="20"/>
<param name="Reg/Strategy" type="string" value="1"/>
<param name="Grid/CellSize" type="string" value="$(arg resolution)"/>
<param name="Grid/RangeMax" type="string" value="$(arg grid_max_range)"/>
<param name="Grid/ClusterRadius" type="string" value="1"/>
<param name="Grid/GroundIsObstacle" type="string" value="$(arg ground_is_obstacle)"/>
<param name="Optimizer/GravitySigma" type="string" value="0.3"/>
<!-- ICP parameters -->
<param name="Icp/VoxelSize" type="string" value="$(arg resolution)"/>
<param name="Icp/PointToPlaneK" type="string" value="20"/>
<param name="Icp/PointToPlaneRadius" type="string" value="0"/>
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/Iterations" type="string" value="$(arg iterations)"/>
<param name="Icp/Epsilon" type="string" value="0.001"/>
<param name="Icp/MaxTranslation" type="string" value="3"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="$(eval resolution*10)"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
<param name="Icp/CorrespondenceRatio" type="string" value="$(arg loop_ratio)"/>
</node>
<node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
<remap from="scan_cloud" to="odom_filtered_input_scan"/>
<remap from="odom_info" to="odom_info"/>
</node>
<node pkg="nodelet" type="nodelet" name="point_cloud_assembler" args="standalone rtabmap_ros/point_cloud_assembler" output="screen">
<remap from="cloud" to="$(arg scan_topic_deskewed)"/>
<remap from="odom" to="odom"/>
<param if="$(arg scan_20_hz)" name="max_clouds" type="int" value="20" />
<param unless="$(arg scan_20_hz)" name="max_clouds" type="int" value="10" />
<param name="fixed_frame_id" type="string" value="" />
<param name="queue_size" type="int" value="$(arg queue_size)" />
</node>
</group>
</launch>
nodelet_plugins.xml
+8
View File
@@ -176,6 +176,14 @@
</description> </description>
</class> </class>
<class name="rtabmap_ros/lidar_deskewing"
type="rtabmap_ros::LidarDeskewing"
base_class_type="nodelet::Nodelet">
<description>
This is my nodelet.
</description>
</class>
</library> </library>
<library path="lib/librtabmap_sync"> <library path="lib/librtabmap_sync">
src/LidarDeskewingNode.cpp
+47
View File
@@ -0,0 +1,47 @@
/*
Copyright (c) 2010-2016, 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 "nodelet/loader.h"
int main(int argc, char **argv)
{
ros::init(argc, argv, "lidar_deskewing");
nodelet::V_string nargv;
for(int i=1;i<argc;++i)
{
nargv.push_back(argv[i]);
}
nodelet::Loader nodelet;
nodelet::M_string remap(ros::names::getRemappings());
std::string nodelet_name = ros::this_node::getName();
nodelet.load(nodelet_name, "rtabmap_ros/lidar_deskewing", remap, nargv);
ros::spin();
return 0;
}
src/MsgConversion.cpp
+416
View File
@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Compression.h> #include <rtabmap/core/Compression.h>
#include <rtabmap/utilite/UStl.h> #include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/ULogger.h> #include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UTimer.h>
#include <pcl_conversions/pcl_conversions.h> #include <pcl_conversions/pcl_conversions.h>
#include <eigen_conversions/eigen_msg.h> #include <eigen_conversions/eigen_msg.h>
#include <tf_conversions/tf_eigen.h> #include <tf_conversions/tf_eigen.h>
@@ -2487,4 +2488,419 @@ bool convertScan3dMsg(
return true; return true;
} }
bool deskew_impl(
const sensor_msgs::PointCloud2 & input,
sensor_msgs::PointCloud2 & output,
const std::string & fixedFrameId,
tf::TransformListener * listener,
double waitForTransform,
bool slerp,
const rtabmap::Transform & velocity,
double previousStamp)
{
if(listener != 0)
{
if(input.header.frame_id.empty())
{
ROS_ERROR("Input cloud has empty frame_id!");
return false;
}
if(fixedFrameId.empty())
{
ROS_ERROR("fixedFrameId parameter should be set!");
return false;
}
}
else
{
if(!slerp)
{
ROS_ERROR("slerp should be true when constant velocity model is used!");
return false;
}
if(previousStamp <= 0.0)
{
ROS_ERROR("previousStamp should be >0 when constant velocity model is used!");
return false;
}
if(velocity.isNull())
{
ROS_ERROR("velocity should be valid when constant velocity model is used!");
return false;
}
}
int offsetTime = -1;
int offsetX = -1;
int offsetY = -1;
int offsetZ = -1;
int timeDatatype = 6;
for(size_t i=0; i<input.fields.size(); ++i)
{
if(input.fields[i].name.compare("t") == 0)
{
if(offsetTime != -1)
{
ROS_WARN("The input cloud should have only one of these fields: t, time or stamps. Overriding with %s.", input.fields[i].name.c_str());
}
offsetTime = input.fields[i].offset;
timeDatatype = input.fields[i].datatype;
}
else if(input.fields[i].name.compare("time") == 0)
{
if(offsetTime != -1)
{
ROS_WARN("The input cloud should have only one of these fields: t, time or stamps. Overriding with %s.", input.fields[i].name.c_str());
}
offsetTime = input.fields[i].offset;
timeDatatype = input.fields[i].datatype;
}
else if(input.fields[i].name.compare("stamps") == 0)
{
if(offsetTime != -1)
{
ROS_WARN("The input cloud should have only one of these fields: t, time or stamps. Overriding with %s.", input.fields[i].name.c_str());
}
offsetTime = input.fields[i].offset;
timeDatatype = input.fields[i].datatype;
}
else if(input.fields[i].name.compare("x") == 0)
{
ROS_ASSERT(input.fields[i].datatype==7);
offsetX = input.fields[i].offset;
}
else if(input.fields[i].name.compare("y") == 0)
{
ROS_ASSERT(input.fields[i].datatype==7);
offsetY = input.fields[i].offset;
}
else if(input.fields[i].name.compare("z") == 0)
{
ROS_ASSERT(input.fields[i].datatype==7);
offsetZ = input.fields[i].offset;
}
}
if(offsetTime < 0)
{
ROS_ERROR("Input cloud doesn't have \"t\" or \"time\" field!");
return false;
}
if(offsetX < 0)
{
ROS_ERROR("Input cloud doesn't have \"x\" field!");
return false;
}
if(offsetY < 0)
{
ROS_ERROR("Input cloud doesn't have \"y\" field!");
return false;
}
if(offsetZ < 0)
{
ROS_ERROR("Input cloud doesn't have \"z\" field!");
return false;
}
if(input.height == 0)
{
ROS_ERROR("Input cloud height is zero!");
return false;
}
if(input.width == 0)
{
ROS_ERROR("Input cloud width is zero!");
return false;
}
bool timeOnColumns = input.width > input.height;
// Get latest timestamp
ros::Time firstStamp;
ros::Time lastStamp;
if(timeDatatype == 6) // UINT32
{
unsigned int nsec = *((const unsigned int*)(&input.data[0]+offsetTime));
firstStamp = input.header.stamp+ros::Duration(0, nsec);
nsec = *((const unsigned int*)(&input.data[timeOnColumns?(input.width-1)*input.point_step:(input.height-1)*input.row_step]+offsetTime));
lastStamp = input.header.stamp+ros::Duration(0, nsec);
}
else if(timeDatatype == 7) // FLOAT32
{
float sec = *((const float*)(&input.data[0]+offsetTime));
firstStamp = input.header.stamp+ros::Duration().fromSec(sec);
sec = *((const float*)(&input.data[timeOnColumns?(input.width-1)*input.point_step:(input.height-1)*input.row_step]+offsetTime));
lastStamp = input.header.stamp+ros::Duration().fromSec(sec);
}
else
{
ROS_ERROR("Not supported time datatype %d!", timeDatatype);
return false;
}
if(lastStamp <= firstStamp)
{
ROS_ERROR("First and last stamps in the scan are the same!");
return false;
}
std::string errorMsg;
if(listener != 0 &&
waitForTransform>0.0 &&
!listener->waitForTransform(
input.header.frame_id,
firstStamp,
input.header.frame_id,
lastStamp,
fixedFrameId,
ros::Duration(waitForTransform),
ros::Duration(0.01),
&errorMsg))
{
ROS_ERROR("Could not estimate motion of %s accordingly to fixed frame %s between stamps %f and %f! (%s)",
input.header.frame_id.c_str(),
fixedFrameId.c_str(),
firstStamp.toSec(),
lastStamp.toSec(),
errorMsg.c_str());
return false;
}
rtabmap::Transform firstPose;
rtabmap::Transform lastPose;
double scanTime = 0;
if(slerp)
{
if(listener != 0)
{
firstPose = rtabmap_ros::getTransform(
input.header.frame_id,
fixedFrameId,
firstStamp,
input.header.stamp,
*listener,
0);
lastPose = rtabmap_ros::getTransform(
input.header.frame_id,
fixedFrameId,
lastStamp,
input.header.stamp,
*listener,
0);
}
else
{
float vx,vy,vz, vroll,vpitch,vyaw;
velocity.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
// We need three poses:
// 1- The pose of base frame in odom frame at first stamp
// 2- The pose of base frame in odom frame at msg stamp
// 3- The pose of base frame in odom frame at last stamp
UASSERT(firstStamp.toSec() >= previousStamp);
UASSERT(lastStamp.toSec() > previousStamp);
double dt1 = firstStamp.toSec() - previousStamp;
double dt2 = input.header.stamp.toSec() - previousStamp;
double dt3 = lastStamp.toSec() - previousStamp;
rtabmap::Transform p1(vx*dt1, vy*dt1, vz*dt1, vroll*dt1, vpitch*dt1, vyaw*dt1);
rtabmap::Transform p2(vx*dt2, vy*dt2, vz*dt2, vroll*dt2, vpitch*dt2, vyaw*dt2);
rtabmap::Transform p3(vx*dt3, vy*dt3, vz*dt3, vroll*dt3, vpitch*dt3, vyaw*dt3);
// First and last poses are relative to stamp of the msg
firstPose = p2.inverse() * p1;
lastPose = p2.inverse() * p3;
}
if(firstPose.isNull())
{
ROS_ERROR("Could not get transform of %s accordingly to %s between stamps %f and %f!",
input.header.frame_id.c_str(),
fixedFrameId.empty()?"velocity":fixedFrameId.c_str(),
firstStamp.toSec(),
input.header.stamp.toSec());
return false;
}
if(lastPose.isNull())
{
ROS_ERROR("Could not get transform of %s accordingly to %s between stamps %f and %f!",
input.header.frame_id.c_str(),
fixedFrameId.empty()?"velocity":fixedFrameId.c_str(),
lastStamp.toSec(),
input.header.stamp.toSec());
return false;
}
scanTime = lastStamp.toSec() - firstStamp.toSec();
}
//else tf will be used to get more accurate transforms
output = input;
ros::Time stamp;
UTimer processingTime;
if(timeOnColumns)
{
// ouster point cloud:
// t1 t2 ...
// ring1 ring1 ...
// ring2 ring2 ...
// ring3 ring4 ...
// ring4 ring3 ...
for(size_t u=0; u<output.width; ++u)
{
if(timeDatatype == 6) // UINT32
{
unsigned int nsec = *((const unsigned int*)(&output.data[u*output.point_step]+offsetTime));
stamp = input.header.stamp+ros::Duration(0, nsec);
}
else
{
float sec = *((const float*)(&output.data[u*output.point_step]+offsetTime));
stamp = input.header.stamp+ros::Duration().fromSec(sec);
}
rtabmap::Transform transform;
if(slerp)
{
transform = firstPose.interpolate((stamp-firstStamp).toSec() / scanTime, lastPose);
}
else
{
transform = rtabmap_ros::getTransform(
output.header.frame_id,
fixedFrameId,
stamp,
output.header.stamp,
*listener,
0);
if(transform.isNull())
{
ROS_ERROR("Could not get transform of %s accordingly to %s between stamps %f and %f!",
output.header.frame_id.c_str(),
fixedFrameId.c_str(),
stamp.toSec(),
output.header.stamp.toSec());
return false;
}
}
for(size_t v=0; v<input.height; ++v)
{
unsigned char * dataPtr = &output.data[v*output.row_step + u*output.point_step];
float & x = *((float*)(dataPtr+offsetX));
float & y = *((float*)(dataPtr+offsetY));
float & z = *((float*)(dataPtr+offsetZ));
pcl::PointXYZ pt(x,y,z);
pt = rtabmap::util3d::transformPoint(pt, transform);
x = pt.x;
y = pt.y;
z = pt.z;
// set delta stamp to zero so that on downstream they know the cloud is deskewed
if(timeDatatype == 6) // UINT32
{
*((unsigned int*)(dataPtr+offsetTime)) = 0;
}
else
{
*((float*)(dataPtr+offsetTime)) = 0;
}
}
}
}
else // time on rows
{
// velodyne point cloud:
// t1 ring1 ring2 ring3 ring4
// t2 ring1 ring2 ring3 ring4
// t3 ring1 ring2 ring3 ring4
// t4 ring1 ring2 ring3 ring4
// ... ... ... ... ...
for(size_t v=0; v<output.height; ++v)
{
if(timeDatatype == 6) // UINT32
{
unsigned int nsec = *((const unsigned int*)(&output.data[v*output.row_step]+offsetTime));
stamp = input.header.stamp+ros::Duration(0, nsec);
}
else
{
float sec = *((const float*)(&output.data[v*output.row_step]+offsetTime));
stamp = input.header.stamp+ros::Duration().fromSec(sec);
}
rtabmap::Transform transform;
if(slerp)
{
transform = firstPose.interpolate((stamp-firstStamp).toSec() / scanTime, lastPose);
}
else
{
transform = rtabmap_ros::getTransform(
output.header.frame_id,
fixedFrameId,
stamp,
output.header.stamp,
*listener,
0);
if(transform.isNull())
{
ROS_ERROR("Could not get transform of %s accordingly to %s between stamps %f and %f!",
output.header.frame_id.c_str(),
fixedFrameId.c_str(),
stamp.toSec(),
output.header.stamp.toSec());
return false;
}
}
for(size_t u=0; u<input.width; ++u)
{
unsigned char * dataPtr = &output.data[v*output.row_step + u*output.point_step];
float & x = *((float*)(dataPtr+offsetX));
float & y = *((float*)(dataPtr+offsetY));
float & z = *((float*)(dataPtr+offsetZ));
pcl::PointXYZ pt(x,y,z);
pt = rtabmap::util3d::transformPoint(pt, transform);
x = pt.x;
y = pt.y;
z = pt.z;
// set delta stamp to zero so that on downstream they know the cloud is deskewed
if(timeDatatype == 6) // UINT32
{
*((unsigned int*)(dataPtr+offsetTime)) = 0;
}
else
{
*((float*)(dataPtr+offsetTime)) = 0;
}
}
}
}
ROS_DEBUG("Lidar deskewing time=%fs", processingTime.elapsed());
return true;
}
bool deskew(
const sensor_msgs::PointCloud2 & input,
sensor_msgs::PointCloud2 & output,
const std::string & fixedFrameId,
tf::TransformListener & listener,
double waitForTransform,
bool slerp)
{
return deskew_impl(input, output, fixedFrameId, &listener, waitForTransform, slerp, rtabmap::Transform(), 0);
}
bool deskew(
const sensor_msgs::PointCloud2 & input,
sensor_msgs::PointCloud2 & output,
double previousStamp,
const rtabmap::Transform & velocity)
{
return deskew_impl(input, output, "", 0, 0, true, velocity, previousStamp);
}
} }
src/OdometryROS.cpp
+9
View File
@@ -423,6 +423,15 @@ Transform OdometryROS::getTransform(const std::string & fromFrameId, const std::
return transform; return transform;
} }
rtabmap::Transform OdometryROS::velocityGuess() const
{
if(odometry_)
{
return odometry_->getVelocityGuess();
}
return rtabmap::Transform();
}
void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg) void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
{ {
if(!this->isPaused()) if(!this->isPaused())
src/nodelets/icp_odometry.cpp
+61 -2
View File
@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/LaserScan.h> #include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h> #include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h> #include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include "rtabmap_ros/MsgConversion.h" #include "rtabmap_ros/MsgConversion.h"
#include "rtabmap_ros/PluginInterface.h" #include "rtabmap_ros/PluginInterface.h"
@@ -68,6 +69,8 @@ public:
scanNormalK_(0), scanNormalK_(0),
scanNormalRadius_(0.0), scanNormalRadius_(0.0),
scanNormalGroundUp_(0.0), scanNormalGroundUp_(0.0),
deskewing_(false),
deskewingSlerp_(false),
plugin_loader_("rtabmap_ros", "rtabmap_ros::PluginInterface"), plugin_loader_("rtabmap_ros", "rtabmap_ros::PluginInterface"),
scanReceived_(false), scanReceived_(false),
cloudReceived_(false) cloudReceived_(false)
@@ -96,6 +99,8 @@ private:
pnh.param("scan_normal_k", scanNormalK_, scanNormalK_); pnh.param("scan_normal_k", scanNormalK_, scanNormalK_);
pnh.param("scan_normal_radius", scanNormalRadius_, scanNormalRadius_); pnh.param("scan_normal_radius", scanNormalRadius_, scanNormalRadius_);
pnh.param("scan_normal_ground_up", scanNormalGroundUp_, scanNormalGroundUp_); pnh.param("scan_normal_ground_up", scanNormalGroundUp_, scanNormalGroundUp_);
pnh.param("deskewing", deskewing_, deskewing_);
pnh.param("deskewing_slerp", deskewingSlerp_, deskewingSlerp_);
if (pnh.hasParam("plugins")) if (pnh.hasParam("plugins"))
{ {
@@ -142,6 +147,8 @@ private:
NODELET_INFO("IcpOdometry: scan_normal_k = %d", scanNormalK_); NODELET_INFO("IcpOdometry: scan_normal_k = %d", scanNormalK_);
NODELET_INFO("IcpOdometry: scan_normal_radius = %f m", scanNormalRadius_); NODELET_INFO("IcpOdometry: scan_normal_radius = %f m", scanNormalRadius_);
NODELET_INFO("IcpOdometry: scan_normal_ground_up = %f", scanNormalGroundUp_); NODELET_INFO("IcpOdometry: scan_normal_ground_up = %f", scanNormalGroundUp_);
NODELET_INFO("IcpOdometry: deskewing = %s", deskewing_?"true":"false");
NODELET_INFO("IcpOdometry: deskewing_slerp = %s", deskewingSlerp_?"true":"false");
scan_sub_ = nh.subscribe("scan", queueSize, &ICPOdometry::callbackScan, this); scan_sub_ = nh.subscribe("scan", queueSize, &ICPOdometry::callbackScan, this);
cloud_sub_ = nh.subscribe("scan_cloud", queueSize, &ICPOdometry::callbackCloud, this); cloud_sub_ = nh.subscribe("scan_cloud", queueSize, &ICPOdometry::callbackCloud, this);
@@ -328,7 +335,7 @@ private:
// make sure the frame of the laser is updated too // make sure the frame of the laser is updated too
Transform localScanTransform = getTransform(this->frameId(), Transform localScanTransform = getTransform(this->frameId(),
scanMsg->header.frame_id, scanMsg->header.frame_id,
scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment)); scanMsg->header.stamp);
if(localScanTransform.isNull()) if(localScanTransform.isNull())
{ {
ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting odometry update.", scanMsg->header.stamp.toSec()); ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting odometry update.", scanMsg->header.stamp.toSec());
@@ -338,7 +345,35 @@ private:
//transform in frameId_ frame //transform in frameId_ frame
sensor_msgs::PointCloud2 scanOut; sensor_msgs::PointCloud2 scanOut;
laser_geometry::LaserProjection projection; laser_geometry::LaserProjection projection;
projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener());
if(deskewing_ && !guessFrameId().empty())
{
projection.transformLaserScanToPointCloud(deskewing_&&!guessFrameId().empty()?guessFrameId():scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener());
sensor_msgs::PointCloud2 scanOutDeskewed;
if(!pcl_ros::transformPointCloud(scanMsg->header.frame_id, scanOut, scanOutDeskewed, this->tfListener()))
{
ROS_ERROR("Cannot transform back projected scan from \"%s\" frame to \"%s\" frame at time %fs.",
guessFrameId().c_str(), scanMsg->header.frame_id.c_str(), scanMsg->header.stamp.toSec());
return;
}
scanOut = scanOutDeskewed;
}
else
{
projection.projectLaser(*scanMsg, scanOut, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp);
if(previousStamp() > 0 && !velocityGuess().isNull())
{
// deskew with constant velocity model
sensor_msgs::PointCloud2 scanOutDeskewed;
if(!deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess()))
{
ROS_ERROR("Failed to deskew input cloud, aborting odometry update!");
return;
}
}
}
bool hasIntensity = false; bool hasIntensity = false;
for(unsigned int i=0; i<scanOut.fields.size(); ++i) for(unsigned int i=0; i<scanOut.fields.size(); ++i)
@@ -531,6 +566,28 @@ private:
cloudMsg = *pointCloudMsg; cloudMsg = *pointCloudMsg;
} }
if(deskewing_)
{
if(!guessFrameId().empty())
{
// deskew with TF
if(!deskew(*pointCloudMsg, cloudMsg, guessFrameId(), tfListener(), waitForTransformDuration(), deskewingSlerp_))
{
ROS_ERROR("Failed to deskew input cloud, aborting odometry update!");
return;
}
}
else if(previousStamp() > 0 && !velocityGuess().isNull())
{
// deskew with constant velocity model
if(!deskew(*pointCloudMsg, cloudMsg, previousStamp(), velocityGuess()))
{
ROS_ERROR("Failed to deskew input cloud, aborting odometry update!");
return;
}
}
}
LaserScan scan; LaserScan scan;
bool hasNormals = false; bool hasNormals = false;
bool hasIntensity = false; bool hasIntensity = false;
@@ -760,6 +817,8 @@ private:
int scanNormalK_; int scanNormalK_;
double scanNormalRadius_; double scanNormalRadius_;
double scanNormalGroundUp_; double scanNormalGroundUp_;
bool deskewing_;
bool deskewingSlerp_;
std::vector<boost::shared_ptr<rtabmap_ros::PluginInterface> > plugins_; std::vector<boost::shared_ptr<rtabmap_ros::PluginInterface> > plugins_;
pluginlib::ClassLoader<rtabmap_ros::PluginInterface> plugin_loader_; pluginlib::ClassLoader<rtabmap_ros::PluginInterface> plugin_loader_;
bool scanReceived_ = false; bool scanReceived_ = false;
src/nodelets/lidar_deskewing.cpp
+108
View File
@@ -0,0 +1,108 @@
#include <ros/ros.h>
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/LaserScan.h>
#include <laser_geometry/laser_geometry.h>
#include <pcl_ros/transforms.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap_ros/MsgConversion.h>
namespace rtabmap_ros
{
class LidarDeskewing : public nodelet::Nodelet
{
public:
LidarDeskewing() :
waitForTransformDuration_(0.01),
slerp_(false),
tfListener_(0)
{
}
virtual ~LidarDeskewing()
{
}
private:
virtual void onInit()
{
tfListener_ = new tf::TransformListener();
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
pnh.param("wait_for_transform", waitForTransformDuration_, waitForTransformDuration_);
pnh.param("slerp", slerp_, slerp_);
NODELET_INFO("fixed_frame_id: %s", fixedFrameId_.c_str());
NODELET_INFO("wait_for_transform: %fs", waitForTransformDuration_);
NODELET_INFO("slerp: %s", slerp_?"true":"false");
if(fixedFrameId_.empty())
{
NODELET_FATAL("fixed_frame_id parameter cannot be empty!");
}
pubScan_ = nh.advertise<sensor_msgs::PointCloud2>(nh.resolveName("input_scan") + "/deskewed", 1);
pubCloud_ = nh.advertise<sensor_msgs::PointCloud2>(nh.resolveName("input_cloud") + "/deskewed", 1);
subScan_ = nh.subscribe("input_scan", 1, &LidarDeskewing::callbackScan, this);
subCloud_ = nh.subscribe("input_cloud", 1, &LidarDeskewing::callbackCloud, this);
}
void callbackScan(const sensor_msgs::LaserScanConstPtr & msg)
{
sensor_msgs::PointCloud2 scanOut;
laser_geometry::LaserProjection projection;
projection.transformLaserScanToPointCloud(fixedFrameId_, *msg, scanOut, *tfListener_);
sensor_msgs::PointCloud2 scanOutDeskewed;
if(!pcl_ros::transformPointCloud(msg->header.frame_id, scanOut, scanOutDeskewed, *tfListener_))
{
ROS_ERROR("Cannot transform back projected scan from \"%s\" frame to \"%s\" frame at time %fs.",
fixedFrameId_.c_str(), msg->header.frame_id.c_str(), msg->header.stamp.toSec());
return;
}
pubScan_.publish(scanOutDeskewed);
}
void callbackCloud(const sensor_msgs::PointCloud2ConstPtr & msg)
{
sensor_msgs::PointCloud2 msgDeskewed;
if(deskew(*msg, msgDeskewed, fixedFrameId_, *tfListener_, waitForTransformDuration_, slerp_))
{
pubCloud_.publish(msgDeskewed);
}
else
{
// Just republish the msg to not breakdown downstream
// A warning should be already shown (see deskew() source code)
ROS_WARN("deskewing failed! returning possible skewed cloud!");
pubCloud_.publish(msg);
}
}
private:
ros::Publisher pubScan_;
ros::Publisher pubCloud_;
ros::Subscriber subScan_;
ros::Subscriber subCloud_;
std::string fixedFrameId_;
double waitForTransformDuration_;
bool slerp_;
tf::TransformListener * tfListener_;
};
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::LidarDeskewing, nodelet::Nodelet);
}