Re: Problems with launch and mapping
Posted by
denzle on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Problems-with-launch-and-mapping-tp6808p7184.html
Here's the lidar launch i've been using
<?xml version="1.0"?>
<!--
Copyright (c) 2018, STEREOLABS.
All rights reserved.
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
OWNER 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.
-->
<!--
Hand-held 3D lidar mapping example using only a Ouster OS-1 (no camera).
Prerequisities: rtabmap should be built with libpointmatcher
Example:
$ roslaunch rtabmap_ros test_ouster.launch os1_hostname:=os1-XXXXXXXXXXXX.local os1_udp_dest:=192.168.1.XXX
$ rosrun rviz rviz -f map
$ Show TF and /rtabmap/cloud_map topics
ISSUE: You may have to reset odometry after receiving the first cloud if the map looks tilted. The problem seems
coming from the first cloud sent by os1_cloud_node, which may be poorly synchronized with IMU data.
-->
<launch>
<node pkg="tf2_ros" type="static_transform_publisher" name="map_to_odom" args="0 0 0 0 0 0 1 map odom" />
<node pkg="tf2_ros" type="static_transform_publisher" name="odom_to_base_link" args="0 0 0 0 0 0 1 odom base_link" />
<node pkg="tf2_ros" type="static_transform_publisher" name="base_link_to_os1_sensor" args="0 0 0 0 0 0 1 base_link os1_sensor" />
<node pkg="tf2_ros" type="static_transform_publisher" name="os1_sensor_to_os1_lidar" args="0 0 0 0 0 0 1 os1_sensor os1_lidar" />
<node pkg="tf2_ros" type="static_transform_publisher" name="os1_sensor_to_os1_imu" args="0 0 0 0 0 0 1 os1_sensor os1_imu" />
<!-- Required: -->
<arg name="os1_hostname" default="os1-991949000023.local"/>
<arg name="os1_udp_dest" default="10.5.5.1"/>
<arg name="frame_id" default="base_link"/>
<arg name="rtabmapviz" default="true"/>
<arg name="scan_20_hz" default="true"/>
<arg name="use_sim_time" default="true"/>
<param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
<arg name="rgb_topic" default="/camera/color/image_raw"/>
<arg name="depth_topic" default="/camera/aligned_depth_to_color/image_raw"/>
<arg name="camera_info_topic" default="/camera/color/camera_info"/>
<arg name="rviz" default="true" />
<arg name="rviz_cfg" default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
<arg name="rgbd_sync" default="false"/>
<arg name="approx_rgbd_sync" default="false"/>
<arg name="subscribe_rgbd" default="$(arg rgbd_sync)"/>
<arg name="approx_sync" default="true"/>
<arg name="subscribe_scan" default="false"/>
<arg name="scan_topic" default="/scan"/>
<arg name="subscribe_scan_cloud" default="true"/>
<arg name="scan_cloud_topic" default="/os1_cloud_node/points"/>
<arg name="scan_cloud_max_points" default="65536"/>
<arg name="scan_cloud_filtered" default="true"/>
<arg name="visual_odometry" default="false"/>
<arg name="icp_odometry" default="true"/>
<arg name="args" default=""/>
<arg name="rtabmap_args" default="$(arg args)"/>
<arg name="launch_prefix" default=""/>
<arg name="voxel_size" default="0.35"/> <!-- indoor: 0.1 to 0.3, outdoor: 0.3 to 0.5 -->
<!-- Ouster -->
<include unless="$(arg use_sim_time)" file="$(find ouster_ros)/os1.launch">
<arg name="os1_hostname" value="$(arg os1_hostname)"/>
<arg name="os1_udp_dest" value="$(arg os1_udp_dest)"/>
<arg if="$(arg scan_20_hz)" name="lidar_mode" value="1024x20"/>
<arg unless="$(arg scan_20_hz)" name="lidar_mode" value="1024x10"/>
</include>
<!-- IMU orientation estimation and publish tf accordingly to os1_sensor frame -->
<node pkg="nodelet" type="nodelet" name="imu_nodelet_manager" args="manager">
<remap from="imu" to="/os1_cloud_node/imu"/>
</node>
<node pkg="nodelet" type="nodelet" name="imu_filter" args="load imu_filter_madgwick/ImuFilterNodelet imu_nodelet_manager">
<param name="use_mag" value="true"/>
<param name="world_frame" value="map"/>
<param name="publish_tf" value="true"/>
</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="/os1_cloud_node/imu/data"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="base_frame_id" value="$(arg frame_id)"/>
</node>
<group ns="rtabmap">
<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
<remap from="scan_cloud" to="/os1_cloud_node/points"/>
<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"/>
<param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>
<!-- <remap from="imu" to="/os1_cloud_node/imu/data"/> -->
<param name="guess_frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_imu_to_init" type="bool" value="false"/>
<param name="Reg/Strategy" type="string" value="1"/>
<!-- ICP parameters -->
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/Iterations" type="string" value="10"/>
<param name="Icp/VoxelSize" type="string" value="0.2"/>
<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 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="1"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.1"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
<!-- Odom parameters -->
<param name="Odom/ScanKeyFrameThr" type="string" value="0.95"/>
<param name="Odom/Strategy" type="string" value="0"/>
<param name="OdomF2M/ScanSubtractRadius" type="string" value="0.2"/>
<param name="OdomF2M/ScanMaxSize" type="string" value="150000"/>
</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="true"/>
<remap from="scan_cloud" to="/os1_cloud_node/points"/>
<!-- RTAB-Map's parameters -->
<param name="Rtabmap/DetectionRate" type="string" value="0"/>
<param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
<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.10"/>
<param name="RGBD/LinearUpdate" type="string" value="0.10"/>
<param name="Mem/NotLinkedNodesKept" type="string" value="false"/>
<param name="Mem/STMSize" type="string" value="30"/>
<param name="Mem/LaserScanVoxelSize" type="string" value="0.1"/>
<param name="Mem/LaserScanNormalK" type="string" value="10"/>
<param name="Mem/LaserScanRadius" type="string" value="0"/>
<param name="Reg/Strategy" type="string" value="1"/>
<param name="Grid/CellSize" type="string" value="0.1"/>
<param name="Grid/RangeMax" type="string" value="20"/>
<param name="Grid/ClusterRadius" type="string" value="1"/>
<param name="Grid/GroundIsObstacle" type="string" value="true"/>
<param name="Grid/FromDepth" type="string" value="false"/>
<!-- ICP parameters -->
<param name="Icp/VoxelSize" type="string" value="0.3"/>
<param name="Icp/PointToPlaneK" type="string" value="20"/>
<param name="Icp/PointToPlaneRadius" type="string" value="0"/>
<param name="Icp/PointToPlane" type="string" value="false"/>
<param name="Icp/Iterations" type="string" value="10"/>
<param name="Icp/Epsilon" type="string" value="0.001"/>
<param name="Icp/MaxTranslation" type="string" value="3"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
<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.4"/>
</node>
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen">
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="rgbd_image" to="rgbd_image"/> <!-- output -->
<param name="approx_sync" type="bool" value="true"/> <!-- false for realsense -->
</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="true"/>
<remap from="scan_cloud" to="/os1_cloud_node/points"/>
</node>
</group>
<node pkg="nodelet" type="nodelet" name="point_cloud_assembler" args="standalone rtabmap_ros/point_cloud_assembler" output="screen">
<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="fusion" />
<remap from="cloud" to="/os1_cloud_node/points"/>
<remap from="odom" to="/odom"/>
</node>
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
</launch>