Skip to content

Commit

Permalink
Added lidar_deskewing node and nodelet. Added "deskewing" option for …
Browse files Browse the repository at this point in the history 
…icp_odometry. Updated velodyne and ouster examples with deskewing option. Added velodyne+T265 deskewing example.
  • Loading branch information
@matlabbe
matlabbe committed Oct 17, 2022
1 parent cf50e93 commit 44bbaa2
Show file tree
Hide file tree
Showing 15 changed files with 930 additions and 34 deletions.
5 changes: 5 additions & 0 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -263,6 +263,7 @@ SET(rtabmap_plugins_lib_src
src/nodelets/undistort_depth.cpp
src/nodelets/imu_to_tf.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)
Expand Down Expand Up @@ -386,6 +387,10 @@ add_executable(rtabmap_imu_to_tf src/ImuToTFNode.cpp)
target_link_libraries(rtabmap_imu_to_tf ${Libraries})
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)
add_executable(rtabmap_wifi_signal_pub src/WifiSignalPubNode.cpp)
target_link_libraries(rtabmap_wifi_signal_pub rtabmap_ros)
Expand Down
14 changes: 14 additions & 0 deletions include/rtabmap_ros/MsgConversion.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -264,6 +264,20 @@ bool convertScan3dMsg(
int maxPoints = 0,
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_ */
4 changes: 4 additions & 0 deletions include/rtabmap_ros/OdometryROS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,6 +70,7 @@ class OdometryROS : public nodelet::Nodelet

const std::string & frameId() const {return frameId_;}
const std::string & odomFrameId() const {return odomFrameId_;}
const std::string & guessFrameId() const {return guessFrameId_;}
const rtabmap::ParametersMap & parameters() const {return parameters_;}
bool isPaused() const {return paused_;}
rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const;
Expand All @@ -80,6 +81,9 @@ class OdometryROS : public nodelet::Nodelet

virtual void flushCallbacks() = 0;
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 {}

private:
Expand Down
1 change: 1 addition & 0 deletions launch/demo/demo_hector_mapping.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,7 @@
<remap from="odom_info" to="/rtabmap/odom_info"/>

<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="guess_frame_id" type="string" value="odom"/>
Expand Down
21 changes: 17 additions & 4 deletions launch/rtabmap.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -96,8 +96,10 @@
<arg name="scan_cloud_topic" default="/scan_cloud"/>
<arg name="subscribe_scan_descriptor" default="false"/>
<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_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_depth" default="false" /> <!-- Generate depth image from scan_cloud -->
Expand Down Expand Up @@ -312,6 +314,17 @@
<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="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 if="$(arg scan_cloud_assembling)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" clear_params="$(arg clear_params)" output="$(arg output)">
Expand Down Expand Up @@ -430,10 +443,10 @@
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_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 if="$(arg icp_odometry)" from="scan_cloud" to="odom_filtered_input_scan"/>
<remap unless="$(arg icp_odometry)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap if="$(eval icp_odometry or scan_cloud_filtered)" from="scan_cloud" to="odom_filtered_input_scan"/>
<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="odom" to="$(arg odom_topic)"/>
</node>
Expand Down
9 changes: 5 additions & 4 deletions launch/tests/test_ouster.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,14 +12,15 @@
coming from the first cloud sent by os1_cloud_node, which may be poorly synchronized with IMU data.
-->

<arg name="use_sim_time" default="false"/>

<!-- Required: -->
<arg name="os1_hostname"/>
<arg name="os1_udp_dest"/>
<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="frame_id" default="os1_sensor"/>
<arg name="rtabmapviz" 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"/>

<!-- Ouster -->
Expand Down
35 changes: 24 additions & 11 deletions launch/tests/test_ouster_gen2.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,11 +40,14 @@

<arg name="frame_id" default="os_sensor"/>
<arg name="rtabmapviz" default="true"/>
<arg name="deskewing" default="true"/>
<arg name="slerp" default="false"/>
<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="assemble" default="false"/>
<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"/>

Expand All @@ -56,29 +59,39 @@
<arg if="$(arg scan_20_hz)" name="lidar_mode" value="1024x20"/>
<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 distortion_correction)" name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
</include>

<!-- IMU orientation estimation and publish tf accordingly to os_sensor frame -->
<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" to="/os_cloud_node/imu/data"/>
<remap from="imu/data_raw" to="$(arg imu_topic)"/>
<remap from="imu/data" to="$(arg imu_topic)/filtered"/>
</node>
<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="world_frame" value="enu"/>
<param name="publish_tf" value="false"/>
</node>
<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="base_frame_id" value="$(arg frame_id)"/>
</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">
<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
<remap from="scan_cloud" to="/os_cloud_node/points"/>
<remap from="imu" to="/os_cloud_node/imu/data"/>
<remap from="scan_cloud" to="$(arg scan_topic_deskewed)"/>
<remap from="imu" to="$(arg imu_topic)/filtered"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
Expand Down Expand Up @@ -116,8 +129,8 @@
<param name="approx_sync" type="bool" value="false"/>

<remap if="$(arg assemble)" from="scan_cloud" to="assembled_cloud"/>
<remap unless="$(arg assemble)" from="scan_cloud" to="/os_cloud_node/points"/>
<remap from="imu" to="/os_cloud_node/imu/data"/>
<remap unless="$(arg assemble)" from="scan_cloud" to="$(arg scan_topic_deskewed)"/>
<remap from="imu" to="$(arg imu_topic)/filtered"/>

<!-- RTAB-Map's parameters -->
<param if="$(arg assemble)" name="Rtabmap/DetectionRate" type="string" value="0"/> <!-- already set 1 Hz in point_cloud_assembler -->
Expand Down Expand Up @@ -158,7 +171,7 @@
</node>

<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"/>
<param name="assembling_time" type="double" value="1" />
<param name="fixed_frame_id" type="string" value="" />
Expand All @@ -170,7 +183,7 @@
<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="/os_cloud_node/points"/>
<remap from="scan_cloud" to="odom_filtered_input_scan"/>
</node>
</group>

Expand Down
40 changes: 27 additions & 13 deletions launch/tests/test_velodyne.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,9 @@
<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="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="use_sim_time" default="false"/>
<param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
Expand All @@ -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="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"/>

<!-- Grid parameters -->
Expand All @@ -34,7 +36,7 @@
<!-- 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="local_map_size" default="15000"/>
<arg name="key_frame_thr" default="0.8"/>
<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 -->
Expand All @@ -46,9 +48,18 @@
<arg name="organize_cloud" value="$(arg organize_cloud)"/>
</include>

<!-- IMU orientation estimation and publish tf accordingly to os1_sensor frame -->
<node if="$(arg use_imu)" pkg="rtabmap_ros" type="imu_to_tf" name="imu_to_tf">
<remap from="imu/data" to="$(arg imu_topic)"/>
<!-- IMU orientation estimation and publish tf -->
<node if="$(arg use_imu)" pkg="nodelet" type="nodelet" name="imu_nodelet_manager" args="manager">
<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="base_frame_id" value="$(arg frame_id)"/>
</node>
Expand All @@ -58,12 +69,14 @@
<remap from="scan_cloud" to="$(arg scan_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<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="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"/>

<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="wait_imu_to_init" type="bool" value="true"/>

Expand Down Expand Up @@ -92,8 +105,9 @@
<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="$(arg 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 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">
Expand All @@ -105,7 +119,7 @@
<param name="wait_for_transform_duration" type="double" value="0.2"/>

<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 -->
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
Expand All @@ -127,7 +141,7 @@
<param name="Optimizer/GravitySigma" type="string" value="0.3"/>

<!-- 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/PointToPlaneRadius" type="string" value="0"/>
<param name="Icp/PointToPlane" type="string" value="true"/>
Expand All @@ -151,12 +165,12 @@
</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)"/>
<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"/>
<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="voxel_size" type="double" value="$(arg resolution)" />
<param name="queue_size" type="int" value="$(arg queue_size)" />
</node>
</group>
Expand Down
Loading

0 comments on commit 44bbaa2

Please sign in to comment.