cliff_detector not working

asked 2019-03-24 16:28:56 -0500

hdbot gravatar image

updated 2019-03-26 11:23:05 -0500


I am trying to detect down the stairs as an obstacle. I am using depth_nav_tools for doing this task, however, I can't seem to detect it as obstacle. For some reason cliff_detector and depth_sensor_pose are not getting data usable from the camera. I am able to open the cliff_detector and depth_sensor_pose in the rviz. However, I still get an error to detect stairs.

[ERROR] [1553614230.243212450]: Ground points not detected
[ERROR] [1553614230.243258533]: height = -0.0000 angle = 0.0000

My launch file code is:


<!--EKF Launch -->
<include file="$(find robot_localization)/launch/UKF_Odom_zed.launch" />

<!--Zed Camera Launch -->
<include file="$(find zed_wrapper)/launch/zed.launch" />

<arg name="base_frame"              default="zed_camera_center"/>

<!-- Convert depth image to laser scan -->
    <node name="laserscan_kinect" pkg="laserscan_kinect" type="laserscan_kinect" > 
      <remap from="image" to="/zed/depth/depth_registered"/>
      <remap from="camera_info" to="/zed/rgb/camera_info"/>
      <rosparam command="load" file="$(find laserscan_kinect)/config/params.yaml" />

<!--Stair Detection -->
<node name="cliff_detector" pkg="cliff_detector" type="cliff_detector" >
      <remap from="image" to="/zed/depth/depth_registered"/>
      <remap from="camera_info" to="/zed/rgb/camera_info"/>
        <rosparam command="load" file="$(find cliff_detector)/config/params.yaml" />

<!--Depth Sensor-->

<node name="depth_sensor_pose" pkg="depth_sensor_pose" type="depth_sensor_pose" output="screen">
      <remap from="image" to="/zed/depth/depth_registered"/>
      <remap from="camera_info" to="/zed/rgb/camera_info"/>
      <rosparam command="load" file="$(find depth_sensor_pose)/config/params.yaml"/>
<!-- SLAM -->

<arg name="rviz" default="true" />
<arg name="rtabmapviz" default="false" />

  <group ns="rtabmap">
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
          <param name="frame_id" type="string" value="zed_camera_center"/>

          <param name="subscribe_depth" type="bool" value="true"/>
          <param name="subscribe_scan" type="bool" value="true"/>

          <remap from="odom" to="/zed/odom"/>
          <remap from="scan" to="/scan"/>

          <remap from="rgb/image"       to="/zed/rgb/image_rect_color"/>
          <remap from="depth/image"     to="/zed/depth/depth_registered"/>
          <remap from="rgb/camera_info" to="/zed/rgb/camera_info"/>
      <remap from="depth/camera_info" to="/zed/depth/camera_info"/>

          <param name="queue_size" type="int" value="10"/>
      <param name="approx_sync" value="false"/>
      <param name="visual_odometry" value="false"/> -->

         <!--  RTAB-Map's parameters -->

     <param name="Rtabmap/TimeThr" type="string" value="700"/>
         <param name="Rtabmap/DetectionRate" type="string" value="1"/>

         <param name="Kp/WordsPerImage" type="string" value="200"/>
         <param name="Kp/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
         <param name="Kp/DetectorStrategy" type="string" value="0"/>   <!-- use SURF -->
          <param name="Kp/NNStrategy" type="string" value="1"/>      <!--   kdTree -->

          <param name="SURF/HessianThreshold" type="string" value="1000"/>


<!--RVIZ launch -->
<node pkg="rviz" type="rviz" name="rviz"/>


This is what I get when I try to detect stairs. The small hallway patch is the stairs.

image description

I know it is just the way I am connecting them there is an issue with it, but I can't seem to understand how to fix it. Can anyone please help me figure out my mistake in the code?

Thank you, hdbot

edit retag flag offensive close merge delete