Cannot launch rtabmap stereo sync

[stevemartinov]

I have a ZED M camera and I want to send the images remotely to my PC. My TF tree is good and no problem there. When I run the example launch file called zed_rtabmap.launch everything works but when I try to launch this file:

<launch>

 <include file="$(find zed_wrapper)/launch/zedm.launch"/>

  <arg name="rate" default="15"/>
  <arg name="approx_sync" default="true"/>

  <node pkg="nodelet" type="nodelet" name="data_throttle" args="standalone rtabmap_ros/stereo_sync" output="screen">
      <param name="compressed_rate" type="double" value="$(arg rate)"/>
      <param name="approx_sync" type="bool" value="$(arg approx_sync)"/>

      <remap from="left/image_rect" to="zed/zed/left/image_rect_color"/>
      <remap from="right/image_rect" to="zed/zed/right/image_rect_color"/>
      <remap from="left/camera_info" to="zed/zed/left/camera_info"/>
      <remap from="right/camera_info" to="zed/zed/right/camera_info"/>

      <remap from="rgbd_image" to="rgbd_image"/>
    </node>

</launch>

I get this warning and I cannot launch data_throttle:

/data_throttle: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. 
/data_throttle subscribed to (approx sync):
   /zed/zed/left/image_rect_color,
   /zed/zed/right/image_rect_color,
   /zed/zed/left/camera_info,
   /zed/zed/right/camera_info

However all the topics are being published at 30 Hz.

Here is my zedm.launch

<launch>
    <arg name="svo_file"             default="" /> <!-- <arg name="svo_file" default="path/to/svo/file.svo"> -->
    <arg name="stream"               default="" /> <!-- <arg name="stream" default="<ip_address>:<port>"> -->

    <arg name="node_name"            default="zed" />
    <arg name="camera_model"         default="zedm" /> <!-- 'zed' or 'zedm' -->
    <arg name="publish_urdf"         default="true" />

    <group ns="zed">
        <include file="$(find zed_wrapper)/launch/zed_camera.launch">
            <arg name="svo_file"            value="$(arg svo_file)" />
            <arg name="stream"              value="$(arg stream)" />
            <arg name="node_name"           value="$(arg node_name)" />
            <arg name="camera_model"        value="$(arg camera_model)" />
            <arg name="publish_urdf"        value="$(arg publish_urdf)" />
        </include>
    </group>
</launch>

And my zed_camera.launch:

<launch>
    <!-- Camera Model -->
    <arg name="node_name"             default="zed_node" />

    <!-- Camera Model -->
    <arg name="camera_model"          default="zedm" /> <!-- 'zed' or 'zedm' -->

    <!-- Load SVO file -->
    <arg name="svo_file"              default="" /><!-- <arg name="svo_file" default="path/to/svo/file.svo"> -->
    <!-- Remote stream -->
    <arg name="stream"                default="" /> <!-- <arg name="stream" default="<ip_address>:<port>"> -->

    <!-- Publish ZED urdf -->
    <arg name="publish_urdf"          default="true" />

    <!-- Publish ZED urdf -->
    <arg name="publish_tf"            default="false" />
    <arg name="publish_map_tf"        default="false" />

    <arg name="camera_id"             default="-1" />
    <arg name="gpu_id"                default="-1" />

    <!-- ROS URDF description of the ZED -->
    <group if="$(arg publish_urdf)">
        <param name="zed_description" textfile="$(find zed_wrapper)/urdf/$(arg camera_model).urdf" />
        <node name="zed_state_publisher" pkg="robot_state_publisher" type="state_publisher">
            <remap from="robot_description" to="zed_description" />
        </node>
    </group>

    <node name="$(arg node_name)" pkg="zed_wrapper" type="zed_wrapper_node" output="screen" required="true"><!-- launch-prefix="valgrind" -->
        <rosparam file="$(find zed_wrapper)/params/common.yaml" command="load" />
        <rosparam file="$(find zed_wrapper)/params/$(arg camera_model).yaml" command="load" />

        <!-- SVO file path -->
        <param name="svo_file"          value="$(arg svo_file)" />

        <!-- Remote stream -->
        <param name="stream"            value="$(arg stream)" />

        <!-- Publish ZED urdf -->
        <param name="tracking/publish_tf"        value="$(arg publish_tf)" />
        <param name="tracking/publish_map_tf"    value="$(arg publish_map_tf)" />

        <!-- Camera ID -->
        <param name="general/zed_id"             value="$(arg camera_id)" />

        <!-- GPU ID -->
        <param name="general/gpu_id"             value="$(arg gpu_id)" />
        <param name="general/base_frame"         value="base_footprint" />
    </node>
</launch>

[Myzhar]

Hi @stevemartinov, since you are getting the warning about not receiving the topics

   /zed/zed/left/image_rect_color,
   /zed/zed/right/image_rect_color,
   /zed/zed/left/camera_info,
   /zed/zed/right/camera_info

maybe that there is something wrong with the namespaces or the topics remapping.

Can you copy and past the result of the command $ rostopic list so I can understand more about your configuration?

Also the command $ roswtf can help to understand what's going wrong

PS I suggest you to replace in your zedm.launch the parameter <arg name="node_name" default="zed" /> with <arg name="node_name" default="zed_node" /> or <arg name="node_name" default="zedm" /> to be sure that the node name and the namespace name are not equal, since this can generate mismatching

[stevemartinov]

I will update you tomorrow

[stevemartinov]

Hi, I have changed to zed_node and it worked, however this is roswtf:

Online checks summary:

Found 1 warning(s).
Warnings are things that may be just fine, but are sometimes at fault

WARNING The following node subscriptions are unconnected:
 * /zed_state_publisher:
   * /joint_states

And I also get: !!! Failed to get static transforms. Is the 'ROBOT STATE PUBLISHER' node correctly working? Why is that? I have turned of any TF and included only urdf to be published

[Myzhar]

You have turned off the publishing of dynamic TFs (odom->base_link and map->odom), but static TFs are required and are published by Robot State Publisher (zed_state_publisher node) after parsing the URDF.

The warning message and the output of roswtf say that there is something wrong with that node, but it's not easy to understand what from these info.

If zed_state_publisher is working correctly you should get something similar to this TF tree using the command $ rosrun tf view_frames:

Check if the zed_state_publisher is correctly in the zed namespace

[stevemartinov]

Ok I see now. Also, I get this warning: Elaboration takes longer (0.0461412 sec) than requested by the FPS rate (0.033333333 sec). Please consider to lower the 'frame_rate' setting or to reduce the power requirements reducing the resolutions. SHall I just ignore it?

[Myzhar]

Yes, you can ignore it. It simply means that you are not getting data at 30 FPS as requested by node settings, since the average on elaboration time is higher than the 33 msec. In your case you are getting data at about 22FPS

[abylikhsanov]

Hi, I am having a similar issue, can you have a look? roswtf:

WARNING The following node subscriptions are unconnected:
 * /odometry_frame:
   * /right/state
   * /left/state
 * /odom_filter:
   * /set_pose
   * /imu
 * /zed/zed_state_publisher:
   * /zed/joint_states

WARNING The following nodes are unexpectedly connected:
 * /state_publisher->/zed_node (/tf_static)

Here is my launch file with throttle:

<launch>

  <include file="$(find zed_wrapper)/launch/zedm.launch"/>

  <arg name="rate" default="15"/>
  <arg name="approx_sync" default="true"/>

  <node pkg="nodelet" type="nodelet" name="data_throttle" args="standalone rtabmap_ros/stereo_sync" output="screen">
      <param name="compressed_rate" type="double" value="$(arg rate)"/>
      <param name="approx_sync" type="bool" value="$(arg approx_sync)"/>

      <remap from="left/image_rect" to="/zed/zed_node/left/image_rect_color"/>
      <remap from="right/image_rect" to="/zed/zed_node/right/image_rect_color"/>
      <remap from="left/camera_info" to="/zed/zed_node/left/camera_info"/>
      <remap from="right/camera_info" to="/zed/zed_node/right/camera_info"/>

      <remap from="rgbd_image" to="rgbd_image"/>
    </node>

</launch>

my zedm.launch:

<launch>
    <arg name="svo_file"             default="" /> <!-- <arg name="svo_file" default="path/to/svo/file.svo"> -->
    <arg name="stream"               default="" /> <!-- <arg name="stream" default="<ip_address>:<port>"> -->

    <arg name="node_name"            default="zed_node" />
    <arg name="camera_model"         default="zedm" /> <!-- 'zed' or 'zedm' -->
    <arg name="publish_urdf"         default="true" />

    <group ns="zed">
        <include file="$(find zed_wrapper)/launch/zed_camera.launch">
            <arg name="svo_file"            value="$(arg svo_file)" />
            <arg name="stream"              value="$(arg stream)" />
            <arg name="node_name"           value="$(arg node_name)" />
            <arg name="camera_model"        value="$(arg camera_model)" />
            <arg name="publish_urdf"        value="$(arg publish_urdf)" />
        </include>
    </group>
</launch>

My zed_camera.launch:

<launch>
    <!-- Camera Model -->
    <arg name="node_name"             default="" />

    <!-- Camera Model -->
    <arg name="camera_model"          default="zedm" /> <!-- 'zed' or 'zedm' -->

    <!-- Load SVO file -->
    <arg name="svo_file"              default="" /><!-- <arg name="svo_file" default="path/to/svo/file.svo"> -->
    <!-- Remote stream -->
    <arg name="stream"                default="" /> <!-- <arg name="stream" default="<ip_address>:<port>"> -->

    <!-- Publish ZED urdf -->
    <arg name="publish_urdf"          default="true" />

    <!-- Publish ZED urdf -->
    <arg name="publish_tf"            default="false" />
    <arg name="publish_map_tf"        default="false" />

    <arg name="camera_id"             default="-1" />
    <arg name="gpu_id"                default="-1" />

    <!-- ROS URDF description of the ZED -->
    <group if="$(arg publish_urdf)">
        <param name="zed_description" textfile="$(find zed_wrapper)/urdf/$(arg camera_model).urdf" />
        <node name="zed_state_publisher" pkg="robot_state_publisher" type="state_publisher">
            <remap from="robot_description" to="zed_description" />
        </node>
    </group>

    <node name="$(arg node_name)" pkg="zed_wrapper" type="zed_wrapper_node" output="screen" required="true"><!-- launch-prefix="valgrind" -->
        <rosparam file="$(find zed_wrapper)/params/common.yaml" command="load" />
        <rosparam file="$(find zed_wrapper)/params/$(arg camera_model).yaml" command="load" />

        <!-- SVO file path -->
        <param name="svo_file"          value="$(arg svo_file)" />

        <!-- Remote stream -->
        <param name="stream"            value="$(arg stream)" />

        <!-- Publish ZED urdf -->
        <param name="tracking/publish_tf"        value="$(arg publish_tf)" />
        <param name="tracking/publish_map_tf"    value="$(arg publish_map_tf)" />

        <!-- Camera ID -->
        <param name="general/zed_id"             value="$(arg camera_id)" />

        <!-- GPU ID -->
        <param name="general/gpu_id"             value="$(arg gpu_id)" />
        <param name="general/base_frame"         value="base_footprint" />
    </node>
</launch>

And my TF (which seems to look ok): frames.pdf

[Myzhar]

@abylikhsanov what is exactly the error message that you are getting?

[abylikhsanov]

Just the warning that topics are not published:

[ WARN] [1557756687.317796039]: /data_throttle: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. 
/data_throttle subscribed to (approx sync):
   /zed/zed_node/left/image_rect_color,
   /zed/zed_node/right/image_rect_color,
   /zed/zed_node/left/camera_info,
   /zed/zed_node/right/camera_info

[Myzhar]

Can you verify that the topic signals are correctly connected using rqt_graph

[abylikhsanov]

Yes:

[Myzhar]

Have you verified that the topics are correctly published using the standard command $ ros_topic hz /zed/zed_node/left/image_rect_color (and other)?

[abylikhsanov]

Yes of course, they are all published

[abylikhsanov]

Maybe the problem lies here (not connected)? :

* /zed/zed_state_publisher:
   * /zed/joint_states

[Myzhar]

So the error is really weird. Maybe that it's related to namespaces.

This is your remapping:

      <remap from="left/image_rect" to="/zed/zed_node/left/image_rect_color"/>
      <remap from="right/image_rect" to="/zed/zed_node/right/image_rect_color"/>
      <remap from="left/camera_info" to="/zed/zed_node/left/camera_info"/>
      <remap from="right/camera_info" to="/zed/zed_node/right/camera_info"/>

this is instead the remapping of @stevemartinov:

      <remap from="left/image_rect" to="zed/zed/left/image_rect_color"/>
      <remap from="right/image_rect" to="zed/zed/right/image_rect_color"/>
      <remap from="left/camera_info" to="zed/zed/left/camera_info"/>
      <remap from="right/camera_info" to="zed/zed/right/camera_info"/>

You are using absolute topic namespace. Try to remove the / in front of all the ZED topics

[abylikhsanov]

It didn't work. Also, @stevemartinov has this warning:

WARNING The following node subscriptions are unconnected:
 * /zed_state_publisher:
   * /joint_states

Whereas I have with the namespace

[Myzhar]

That warning is not related to topic connections, but to TF. It can be caused by an unclean shutdown where the publisher didn't unregister with the master I believe. Try to shutdown ros_master and to start it again to see if your issue is solved

[abylikhsanov]

Still the same warning. Interesting enough if I launch zed_rtab.launch I do not get this warning

[Myzhar]

What happens if you set approx_sync to false. I remeber I had a similar issue with approx_sync active

[Myzhar]

Maybe I noticed something that can generate conflicts: in your rqt_graph there is a state_publisher and a zed_state_publisher. Who is launching the state_publisher?

[abylikhsanov]

Both. I did not include zed_camera_center link in my URDF and zed_state_publisher published its own base_link->zed_camera_cetner TF

[abylikhsanov]

I tried to use only roscore and why does zed_state_publisher creates a TF between base_link and zed? I have stated base_footprint in my launch file frames.pdf

[abylikhsanov]

I think there are no conflicts as I ran the launch file with only roscore and got this rqt:

And I still get the warning

[Myzhar]

Ok. It's a new feature introduced with the latest version of the ZED Wrapper. Since is easier for the users to add a direct link from base_link to the link of a sensor, we decided to add it directly in the ZED/ZED-M URDF. In this way the odom and the pose topic are directly referred to base_link instead of zed_camera_center and you must not take care of the transformation base_link->zed_camera_center.

You can read the updated documentation here: https://www.stereolabs.com/docs/ros/zed_node/#transform-frame

[abylikhsanov]

Yes but I did not do anything with this TF as I simply used roscore now.

[Myzhar]

What do you meand with I simply used roscore now.?

[abylikhsanov]

I mean with no external packages, simply zed_wrapper to see if my packages conflict with zed_wrapper but I got the same warning. I have attached the TF several comments above

[abylikhsanov]

Also, camera_info has 0 timestamp, is tha correct?

header: 
  seq: 808
  stamp: 
    secs: 0
    nsecs:         0
  frame_id: "zed_right_camera_optical_frame"
height: 720
width: 1280
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [676.9194946289062, 0.0, 638.7445678710938, 0.0, 676.9194946289062, 349.6361999511719, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [676.9194946289062, 0.0, 638.7445678710938, -42.64883804321289, 0.0, 676.9194946289062, 349.6361999511719, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False
---

[Myzhar]

The TF is correct. I replied to your question why does zed_state_publisher creates a TF between base_link and zed?

Normally base_footprint is "before" base_link, so if you are using base_footprint you must create a TF tree that looks like base_footprint -> base_link -> zed_camera_center.

Usually I suggest to not use base_footprint since it's only a projection of 'base_link ' to the floor.

[Myzhar]

@abylikhsanov please do not change the topic of the issue. If you notice something weird not related to this issue please open a new issue. Can you open a new issue about 'camera_info` timestamp, so we can check it?

[abylikhsanov]

Okay but it is not a new issue. This issue could be related to the current issue.