Using camera image from Gazebo

[ghost]

Issue description

Hi,
I'm using ROS2 Humble on Ubuntu 22.04.2 LTS and have a setup with a robot spawned in gazebo (version 11.10.2). I would like to test/use stella_vslam_ros and followed instructions here (https://github.com/stella-cv/stella_vslam_ros/blob/ros2/doc/tutorial_socket.md).
I'm using the libgazebo_ros_camera.so plugin to publish camera image on /camera/image_raw topic.

Following the instructions, I first start the server

~$ docker run --rm -it --name stella_vslam-ros-server --net=host stella_vslam-ros-server

Here it works as expected

WebSocket: listening on *:3000
HTTP server: listening on *:3001

Then, knowing that the camera image is already published on the topic /camera/image_raw from Gazebo, I execute

~$ docker run --rm -it --name stella_vslam-ros-socket --net=host stella_vslam-ros-socket
/ros2_ws# source /opt/ros/${ROS_DISTRO}/setup.bash
/ros2_ws# source /ros2_ws/install/setup.bash
/ros2_ws# ros2 run stella_vslam_ros run_slam -v ./orb_vocab.fbow -c /path/to/config.yaml --map-db-out map.msg

At this point, it tells me it is connected to the server:

[2023-06-09 18:54:30.801] [I] loading ORB vocabulary: ./orb_vocab.fbow
[2023-06-09 18:54:30.819] [I] load orb_params "ORB feature extraction setting"
[2023-06-09 18:54:30.819] [I] startup SLAM system
[2023-06-09 18:54:30.819] [I] start mapping module
[2023-06-09 18:54:30.820] [I] start global optimization module
[2023-06-09 18:54:30] [connect] Successful connection
[2023-06-09 18:54:30] [connect] WebSocket Connection 127.0.0.1:3000 v-2 "WebSocket++/0.8.1" /socket.io/?EIO=4&transport=websocket&t=1686336870 101
[2023-06-09 18:54:30.839] [I] connected to server

and from the server side, everything looks good as well:

WebSocket: listening on *:3000
HTTP server: listening on *:3001
Connected - ID: CWMiJcOkOX-aVfHcAAAB

However, I do ne see anything at localhost:3001

Additional information

While everything is running, I get the following information

~$  ros2 topic info /camera/image_raw
Type: sensor_msgs/msg/Image
Publisher count: 1
Subscription count: 1

The subscription here is most likely from RViz.

~$ ros2 topic hz /camera/image_raw
average rate: 35.510
	min: 0.026s max: 0.032s std dev: 0.00155s window: 37
average rate: 33.599
	min: 0.026s max: 0.059s std dev: 0.00617s window: 69
average rate: 34.185
	min: 0.026s max: 0.059s std dev: 0.00514s window: 105
average rate: 33.979
	min: 0.026s max: 0.059s std dev: 0.00560s window: 139
average rate: 33.832
	min: 0.026s max: 0.059s std dev: 0.00547s window: 173
average rate: 33.827
	min: 0.026s max: 0.059s std dev: 0.00567s window: 207
average rate: 33.987
	min: 0.026s max: 0.059s std dev: 0.00528s window: 243
...

What is weird is the following:

~$ ros2 node info /run_slam
Unable to find node '/run_slam'

The list of the node is the following:

~$ ros2 node list
/RPLIDAR_A1M8/bytes_laserscan
/bno055/bytes_imu
/controller_manager
/gazebo
/gazebo_ros2_control
/intel_realsense/imu/intel_realsense_d435i_imu_driver
/intel_realsense_d435i_depth_driver
/joint_position_controller
/joint_state_broadcaster
/joint_states
/robot_state_publisher
/rviz2
/transform_listener_impl_556434036bd0

Here below is the config.yaml file

#============================================================================#
# Camera Model (from https://github.com/stella-cv/stella_vslam_ros/pull/106) #
#============================================================================#
Camera:
  name: "RealSense D435"
  setup: "monocular"
  model: "perspective"
  fx: 1386.4138492513919
  fy: 1386.4138492513919
  cx: 960.5
  cy: 540.5
  k1: 0.0
  k2: 0.0
  p1: 0.0
  p2: 0.0
  k3: 0.0
  fps: 15.0
  cols: 800
  rows: 600
  color_order: "RGB"
#=====================#
# Tracking Parameters #
#=====================#
Preprocessing:
  min_size: 500
#================#
# ORB Parameters #
#================#
Feature:
  name: "ORB feature extraction setting"
  scale_factor: 1.2
  num_levels: 8
  ini_fast_threshold: 20
  min_fast_threshold: 7
#====================#
# Mapping Parameters #
#====================#
Mapping:
  baseline_dist_thr_ratio: 0.02
  redundant_obs_ratio_thr: 0.9
  num_covisibilities_for_landmark_generation: 20
  num_covisibilities_for_landmark_fusion: 20
#===========================#
# PangolinViewer Parameters #
#===========================#
PangolinViewer:
  keyframe_size: 0.05
  keyframe_line_width: 1
  graph_line_width: 1
  point_size: 2
  camera_size: 0.08
  camera_line_width: 3
  viewpoint_x: 0
  viewpoint_y: -0.9
  viewpoint_z: -1.9
  viewpoint_f: 400

and finally the gazebo setup for the camera plugin:

      <sensor name="d435i" type="depth">          
          <pose>0.12 0 0.0915 0 0 0</pose>
          <alwaysOn>1</alwaysOn>
          <updateRate>15.0</updateRate>                     
          <camera name="d435i">
              <horizontal_fov>1.46608</horizontal_fov>
              <image>
                  <width>800</width>
                  <height>600</height>
                  <format>R8G8B8</format>
              </image>
              <clip>
                <near>0.02</near>
                <far>300</far>
              </clip>
              <noise>
                <type>gaussian</type>
                <mean>0.0</mean>
                <stddev>0.007</stddev>
              </noise>              
          </camera>
	  
	  <plugin name="intel_realsense_d435i_depth_driver" filename="libgazebo_ros_camera.so">
	    <ros>
		<namespace></namespace>
		<remapping>d435i/image_raw:=camera/image_raw</remapping>
		<remapping>d435i/depth/image_raw:=camera/depth/image_raw</remapping>
		<remapping>d435i/camera_info:=camera/camera_info</remapping>
		<remapping>d435i/depth/camera_info:=camera/depth/camera_info</remapping>
		<remapping>d435i/points:=camera/depth/points</remapping>		
		
	    </ros>    
	    <camera_name>d435i</camera_name>
	    <frame_name>camera_optical_link</frame_name>
	    <hack_baseline>0</hack_baseline>
	    <min_depth>0.05</min_depth>
	    <max_depth>8.0</max_depth>    
   	  </plugin>
      </sensor>   

Thanks in advance for your help.
Regards.

[ymd-stella]

Is gazebo running inside a docker container?
Is --net=host attached?

[ghost]

No, Gazebo is running on the host.

[ymd-stella]

I have tried, but the following does not reproduce in my environment. Have you changed the DDS settings on the host?

What is weird is the following:

~$ ros2 node info /run_slam
Unable to find node '/run_slam'

[ymd-stella]

Are ROS_DOMAIN_ID and ROS_LOCALHOST_ONLY not set on the host?

[ghost]

Thanks for your help and your time! I didn’t change DDS settings. Regarding the env. variables I will check and report to you what I have. Regards.

[ghost]

Ok, here is what I have:
On the host, ROS_DOMAIN_ID is unset:

~$ echo $ROS_DOMAIN_ID

ROS_LOCALHOST_ONLY is set to 0:

~$ echo $ROS_LOCALHOST_ONLY
0

Note: the same applies to stella_vslam-ros-socket

As you pointed out DDS settings as a possible root cause of the issue, I did some searches and found https://stackoverflow.com/questions/65900201/troubles-communicating-with-ros2-node-in-docker-container.

One of the solution proposed is to launch the container (here, stella_vslam-ros-socket) with sharing of /dev/shm, i.e:

~$ docker run --rm -it --name stella_vslam-ros-socket --net=host -v /dev/shm:/dev/shm stella_vslam-ros-socket

When executing the container, the user is root. However, the user on the host is 'grocher' (from which are executed Gazebo, RViz, etc.). So, I created the user 'grocher' in the container (host and container UID being identical) and executed the node:

root:/ros2_ws# adduser grocher
...
root:/ros2_ws# su grocher

grocher:/ros2_ws# source /opt/ros/${ROS_DISTRO}/setup.bash
grocher:/ros2_ws# source /ros2_ws/install/setup.bash
grocher:/ros2_ws# ros2 run stella_vslam_ros run_slam -v ./orb_vocab.fbow -c ./config.yaml --map-db-out map.msg

On the host, now I see info for the node run_slam

~$ ros2 node info /run_slam
/run_slam
  Subscribers:
    /camera/image_raw: sensor_msgs/msg/Image
    /initialpose: geometry_msgs/msg/PoseWithCovarianceStamped
    /parameter_events: rcl_interfaces/msg/ParameterEvent
  Publishers:
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /rosout: rcl_interfaces/msg/Log
    /run_slam/camera_pose: nav_msgs/msg/Odometry
    /tf: tf2_msgs/msg/TFMessage
  Service Servers:
    /run_slam/describe_parameters: rcl_interfaces/srv/DescribeParameters
    /run_slam/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
    /run_slam/get_parameters: rcl_interfaces/srv/GetParameters
    /run_slam/list_parameters: rcl_interfaces/srv/ListParameters
    /run_slam/set_parameters: rcl_interfaces/srv/SetParameters
    /run_slam/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically
  Service Clients:

  Action Servers:

  Action Clients:

Just adding user 'grocher' and not sharing /dev/shm leads a core dump in the container to occur.

Now, in the viewer I get the image camera/

but still, nothing is displayed on the grid.

Interestingly, when removing the sphere from the scene I get (which is expected here):

...
[2023-06-11 15:00:54.396] [W] preprocess: cannot extract any keypoints
[2023-06-11 15:00:54.416] [W] preprocess: cannot extract any keypoints
[2023-06-11 15:00:54.436] [W] preprocess: cannot extract any keypoints
[2023-06-11 15:00:54.476] [W] preprocess: cannot extract any keypoints
[2023-06-11 15:00:54.496] [W] preprocess: cannot extract any keypoints
[2023-06-11 15:00:54.515] [W] preprocess: cannot extract any keypoints
[2023-06-11 15:00:54.536] [W] preprocess: cannot extract any keypoints
...

I will investigate by modifying settings in the viewer , add textures, etc.
I will keep progresses posted here.

Regards.

[ghost]

Ok, I finally got it to work :-) by adding textured elements to the Gazebo environment.

Thanks again for your help!
Regards.