search

introlab / rtabmap

RTAB-Map library and standalone application
https://introlab.github.io/rtabmap
Other
2.74k stars 780 forks source link

Rtabmap with emulated ZED Crash #576

Open tomlogan501 opened 4 years ago

tomlogan501 commented 4 years ago

Hi, I'm using only the depth point cloud of the simulated ZED on the robot, it went well and this error occurs

[ INFO] [1595490914.120097131, 1542.035000000]: Assembled 1 obstacle and 0 ground clouds (12235 points, 0.001679s)
[ INFO] [1595490914.120949802, 1542.035000000]: rtabmap (72): Rate=0.33s, Limit=0.000s, RTAB-Map=0.2694s, Maps update=0.0035s pub=0.0025s (local map=53, WM=53)
[ INFO] [1595490914.303735809, 1542.212000000]: Odom: ratio=0.331479, std dev=0.046505m|0.046505rad, update time=0.448586s
[FATAL] (2020-07-23 09:55:15.214) LaserScan.cpp:241::LaserScan() Condition (data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI))) not met! [format=XYZRGB]
terminate called after throwing an instance of 'UException'
  what():  [FATAL] (2020-07-23 09:55:15.214) LaserScan.cpp:241::LaserScan() Condition (data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI))) not met! [format=XYZRGB]
[ INFO] [1595490915.395193399, 1543.196000000]: Odom: ratio=0.349150, std dev=0.041732m|0.041732rad, update time=0.436971s

Here is the launch file used

<?xml version="1.0"?>
<launch>

    <arg name="use_imu"       default="true"/> <!-- 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="use_sim_time"  default="false"/>
    <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>

    <arg name="frame_id" default="base_link_stabilized"/>

    <node pkg="tf" type="static_transform_publisher" name="static" args="0 0 0 0 0 0   /odom_combined /base_link_stabilized 100"/> 

    <group ns="rtabmap">
      <node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
        <remap from="scan_cloud" to="/zed/depth/points"/>
        <param name="publish_tf"            type="string" value="true"/>  
        <param name="odom_frame"            type="string" value="odom"/>  
    <param name="wait_for_transform"        type="string" value="true"/> 
    <param name="guess_frame_id"            type="string" value="odom_combined"/> 

        <remap if="$(arg use_imu)" from="imu" to="$(arg imu_topic)"/>

        <!-- 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.1"/>
        <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.7"/>
        <param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>  

        <!-- Odom parameters -->       
        <param name="Odom/ScanKeyFrameThr"       type="string" value="0.9"/>
        <param name="Odom/Strategy"              type="string" value="0"/>
        <param name="OdomF2M/ScanSubtractRadius" type="string" value="0.2"/>
        <param name="OdomF2M/ScanMaxSize"        type="string" value="15000"/>      
      </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="odom"               to="/husky_velocity_controller/odom"/-->
        <!--remap from="odom"               to="/odometry/filtered"/-->
    <remap from="odom"              to="/odometry/filtered"/>
        <remap from="scan_cloud"    to="/zed/depth/points"/>

        <!-- RTAB-Map's parameters -->
        <param name="Rtabmap/DetectionRate"          type="string" value="3"/>  
        <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.1"/>
    <param name="RGBD/LinearUpdate"              type="string" value="0.05"/>
        <!-- Avec odometrie filtered, pour fermeture de boucle désactivée mode hectorslam  -->
    <param name="RGBD/OptimizeMaxError"          type="string" value="0"/>

        <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/FromDepth"                 type="string" value="false"/>
        <param name="Grid/CellSize"                  type="string" value="0.10"/>
        <param name="Grid/RangeMax"                  type="string" value="10"/>
        <param name="Grid/ClusterRadius"             type="string" value="1"/>
        <param name="Grid/GroundIsObstacle"          type="string" value="false"/>
        <param name="Grid/RayTracing"                type="string" value="true"/>

      </node>
  </group>

</launch>

Should I force the param for the subscribing to laserscan to false ? Or add it ?

matlabbe commented 4 years ago

Which rtabmap version did you use, I cannot reproduce the problem. Back in May, there was some commits about supporting laser scan with RGB channel, maybe it fixed this.

tomlogan501 commented 4 years ago

rtabmap --version RTAB-Map: 0.19.6 PCL: 1.9.1 With VTK: 7.1.1 OpenCV: 3.4.8 With OpenCV nonfree: false With ORB OcTree: true With FastCV: false With Madgwick: true With TORO: true With g2o: false With GTSAM: false With Vertigo: true With CVSBA: false With Ceres: false With OpenNI2: true With Freenect: true With Freenect2: false With K4W2: false With DC1394: true With FlyCapture2: false With ZED: false With RealSense: false With RealSense SLAM: false With RealSense2: false With libpointmatcher: false With octomap: true With cpu-tsdf: false With open chisel: false With Alice Vision: false With LOAM: false With FOVIS: false With Viso2: false With DVO: false With ORB_SLAM2: false With OKVIS: false With MSCKF_VIO: false With VINS-Fusion: false

matlabbe commented 4 years ago

The new released binary version is 0.20.0. You may give a try. sudo apt install ros-[rosdistro]-rtabmap-ros