Using this repo with a simulated camera?

[OdedHorowits]

Hi, I am using Gazebo with a camera and trying to modify this repo for that. I have made a similar python node, like the one that reads from the TEST_DATA.

Basically all i did was changing run_py_node() to use images from a subscriber to the ros2 topic that publishes the images. I validated that the images do arrive by saving them into files. I made sure to use a setting file with parameters that matches the camera - I took them from the camera *.sdf model file. However, the ORB-SLAM3: Current Frame shows 'WAITING FOR IMAGES'. What might be the reason?

Here it the modified python node:

#!/usr/bin/env python3

import sys
import os
import time
from pathlib import Path
import argparse
import yaml
import copy
import numpy as np
import cv2

import rclpy
from rclpy.node import Node
from rclpy.parameter import Parameter

from sensor_msgs.msg import Image
from std_msgs.msg import String, Float64
from cv_bridge import CvBridge, CvBridgeError

class MonoDriver(Node):
    def __init__(self, node_name = "mono_py_node_gazebo"):
        super().__init__(node_name)

        self.declare_parameter("settings_name","Gazebo_cam")
        self.settings_name = str(self.get_parameter('settings_name').value)
        print(f'DEBUG: settings_name = {self.settings_name}')
        # Global variables
        self.node_name = node_name

        self.br = CvBridge()

        self.pub_exp_config_name = "/mono_py_driver/experiment_settings"
        self.sub_exp_ack_name = "/mono_py_driver/exp_settings_ack"
        self.pub_img_to_agent_name = "/mono_py_driver/img_msg"
        self.pub_timestep_to_agent_name = "/mono_py_driver/timestep_msg"
        self.send_config = True

        self.publish_exp_config_ = self.create_publisher(String, self.pub_exp_config_name, 1)
        self.subscribe_exp_ack_ = self.create_subscription(String, self.sub_exp_ack_name, self.ack_callback, 10)
        self.publish_img_msg_ = self.create_publisher(Image, self.pub_img_to_agent_name, 1)
        self.publish_timestep_msg_ = self.create_publisher(Float64, self.pub_timestep_to_agent_name, 1)

        self.image_subscription = self.create_subscription(Image, '/drone0/sensor_measurements/hd_camera/image_raw', self.image_callback, 10)
        self.image_subscription

        self.current_image = None
        self.current_timestep = None

        print(f"MonoDriver initialized, attempting handshake with CPP node")

    def ack_callback(self, msg):
        print(f"Got ack: {msg.data}")

        if(msg.data == "ACK"):
            self.send_config = False

    def handshake_with_cpp_node(self):
        if self.send_config:
            msg = String()
            msg.data = self.settings_name
            self.publish_exp_config_.publish(msg)
            time.sleep(0.01)

    def image_callback(self, msg):
        try:
            self.current_image = self.br.imgmsg_to_cv2(msg, "bgr8")
            self.current_timestep = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
            # Save the image to a file for debugging
            # filename = f"debug_image_{self.current_timestep:.6f}.png"
            # cv2.imwrite(filename, self.current_image)
        except CvBridgeError as e:
            self.get_logger().error(f"Could not convert image: {e}")

    def run_py_node(self):
        if self.current_image is not None and self.current_timestep is not None:
            img_msg = self.br.cv2_to_imgmsg(self.current_image, encoding="passthrough")
            timestep_msg = Float64()
            timestep_msg.data = self.current_timestep

            try:
                self.publish_timestep_msg_.publish(timestep_msg)
                self.publish_img_msg_.publish(img_msg)
                self.get_logger().info(f"Published image at time {self.current_timestep}")

            except CvBridgeError as e:
                self.get_logger().error(f"Could not publish image: {e}")

def main(args = None):
    rclpy.init(args=args)
    n = MonoDriver("mono_py_node_gazebo")
    rate = n.create_rate(20)

    while(n.send_config):
        n.handshake_with_cpp_node()
        rclpy.spin_once(n)

    print(f"Handshake complete")

    try:
        while rclpy.ok():
            print('spinning')
            rclpy.spin_once(n)
            n.run_py_node()
            rate.sleep()
    except KeyboardInterrupt:
        pass

    n.destroy_node()
    rclpy.shutdown()

if __name__=="__main__":
    main()

In addition, I run all this in a docker and uses tmuxiator to run both nodes:

<%

# Other parameters
use_sim_time      = true
%>

attach: false
root: ./
startup_window: ros2_orb_slam
windows:
  - ros2_orb_slam_cpp_node:
      layout:
      panes:
        - bash -c "source /home/ros2_test/install/setup.bash && ros2 run ros2_orb_slam3 mono_node_cpp --ros-args -p node_name_arg:=mono_slam_cpp"
  - ros2_orb_slam_python_node:
      layout:
      panes:
        - bash -c "source /home/ros2_test/install/setup.bash && ros2 run ros2_orb_slam3 mono_driver_node_gazebo.py --ros-args -p settings_name:=Gazebo_cam"

I also made sure that the flow over the image_raw topic is going from the gazebo simulator into the orb_slam node using ROS2 network visualizer:

[andremaurell]

I found the same error, I'm trying to get the images based on my camera's topic, but the screen stays black and "waiting for images".

[OdedHorowits]

I really tried to drill it down to located the problematic configuration... with no success until now. I wonder whether it is a synchronization problem or configuration problem.

[andremaurell]

I saw other people with the same problem on other package, apparently, that's a bug from ORB_SLAM3

[OdedHorowits]

I saw other people with the same problem on other package, apparently, that's a bug from ORB_SLAM3

Are you sure you can call that a bug? I don't understand why should it care what the src for the images as long as gets frame after frame.

[OdedHorowits]

Actually, this guy at https://github.com/suchetanrs/ORB-SLAM3-ROS2-Docker claims to manage to work with ORB SLAM3 and gazebo, as seems in the short gif of his README. However, he does not explain how to connect the nodes to make it work. I spent almost 6 days trying to make that work and got stuck with the same problem like I have here - 'waiting for images'.

[OdedHorowits]

So in order to isolate the problem, I made a fake data set using Gazebo camera. I recorded the camera and IMU data using a ros bag and build a data set with the same hierarchy and files as the one provided in this repo. Camera (sensor.yaml) configuration were taken from the camera sdf model. Trying to use this fake dataset gave me the same problem - waiting for images. So, as last chance, I desided to use the configuration of the camera on the example, and it started to work. I say "started to work" and not "worked" since images were streaming to the ORB_SLAM3 app, but they were not going smooth. Trying to go back to my camera configuration, I used the image resolution of the camera on the example - 752X480 - and all other parameters were taken again from the sdf file of the camera, and got the same result - it worked but not smoothly. For my surprise, it worked even the images I used had resolution of 1280X720.

I tried to look for that specific resolution in the cpp node and all over the repo, but didn't find that it was hard coded.

So, my conclusion is that it is all about configuration, just need to find the right ones.

If someone manage to go further, plz update.

[andremaurell]

@OdedHorowits can you send-me your Cmake and Package.xml files?

[OdedHorowits]

If that is what you ask - I added my modified script to the # Install Python executibles section at the end of the CmakeLists.txt file Here is package.xml (didn't change anything here)

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>ros2_orb_slam3</name>
  <version>1.0.0</version>
  <description>ROS2 implementation of ORB SLAM3 V1.0</description>
  <maintainer email="azmyin12@gmail.com">tigerwife</maintainer>
  <license>GPL 3.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>
  <buildtool_depend>ament_cmake_python</buildtool_depend>

  <build_depend>rclcpp</build_depend>
  <build_depend>rclpy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>cv_bridge</build_depend>
  <build_depend>image_transport</build_depend>
  <build_depend>Pangolin</build_depend>
  <build_depend>boost_system</build_depend>
  <build_depend>boost_serialization</build_depend>
  <build_depend>libcrypto</build_depend>
  <build_depend>python3-natsort</build_depend>

  <!-- As suggested by the official tutorial, add your custom message interfaces as follows -->
  <!-- <depend>your_custom_msg_interface</depend>  -->

  <exec_depend>rclcpp</exec_depend>
  <exec_depend>rclpy</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <exec_depend>cv_bridge</exec_depend>
  <exec_depend>image_transport</exec_depend>
  <exec_depend>Pangolin</exec_depend>
  <exec_depend>boost_system</exec_depend>
  <exec_depend>boost_serialization</exec_depend>
  <exec_depend>libcrypto</exec_depend>
  <exec_depend>python3-natsort</exec_depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

and here is CMakeLists.txt:

cmake_minimum_required(VERSION 3.8)
project(ros2_orb_slam3)

# Make sure to set this path before building the  
set(ENV{PYTHONPATH} "/opt/ros/humble/lib/python3.10/site-packages/") # Must be set to match your installation

# Must use C++17 to make it compatible with rclcpp
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3")

# Check C++17 support
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++17" COMPILER_SUPPORTS_CXX17)

if(COMPILER_SUPPORTS_CXX17)
  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17")
  add_definitions(-DCOMPILEDWITHC17)
  message(STATUS "Using flag -std=c++17.")
else()
  message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++17 support. Please use a different C++ compiler.")
endif()

LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) # REDUNDANT?

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclpy REQUIRED)
find_package(std_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
# find_package(your_custom_msg_interface REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(OpenCV 4.2 REQUIRED)
find_package(Eigen3 3.3.0 REQUIRED) # Matched with Sophus
find_package(Pangolin REQUIRED)
find_package(image_transport REQUIRED)

# Header file locations [C++ node]
include_directories(include) # Add .hpp, .h files from include/ros2_orb_slam3
include_directories(${EIGEN3_INCLUDE_DIRS}) # Include headers for eigen3
include_directories(${Pangolin_INCLUDE_DIRS}) # include headers for pangolin
include_directories(${OpenCV_INCLUDE_DIRS}) # include headers for pangolin

# ORB-SLAM3 includes [VSLAM library]
include_directories(
  ${PROJECT_SOURCE_DIR}
  ${PROJECT_SOURCE_DIR}/orb_slam3
  ${PROJECT_SOURCE_DIR}/orb_slam3/include
  ${PROJECT_SOURCE_DIR}/orb_slam3/include/CameraModels
  ${PROJECT_SOURCE_DIR}/orb_slam3/Thirdparty
  ${PROJECT_SOURCE_DIR}/orb_slam3/Thirdparty/Sophus
  ${ament_INCLUDE_DIRS}
)

set(THIS_PACKAGE_INCLUDE_DEPENDS
  rclcpp
  rclpy
  std_msgs
  sensor_msgs
  # your_custom_msg_interface
  cv_bridge
  image_transport
  OpenCV
  Eigen3
  Pangolin
)

#* ORB SLAM3 as a shared library
# Whenever you add a new .h, .hpp or .cc, .cpp file in the ros2_orb_slam3/orb_slam3/include and os2_orb_slam3/orb_slam3/src directories, make sure to add them here as shown below
add_library(orb_slam3_lib SHARED
  orb_slam3/src/System.cc
  orb_slam3/src/Tracking.cc
  orb_slam3/src/LocalMapping.cc
  orb_slam3/src/LoopClosing.cc
  orb_slam3/src/ORBextractor.cc
  orb_slam3/src/ORBmatcher.cc
  orb_slam3/src/FrameDrawer.cc
  orb_slam3/src/Converter.cc
  orb_slam3/src/MapPoint.cc
  orb_slam3/src/KeyFrame.cc
  orb_slam3/src/Atlas.cc
  orb_slam3/src/Map.cc
  orb_slam3/src/MapDrawer.cc
  orb_slam3/src/Optimizer.cc
  orb_slam3/src/Frame.cc
  orb_slam3/src/KeyFrameDatabase.cc
  orb_slam3/src/Sim3Solver.cc
  orb_slam3/src/Viewer.cc
  orb_slam3/src/ImuTypes.cc
  orb_slam3/src/G2oTypes.cc
  orb_slam3/src/CameraModels/Pinhole.cpp
  orb_slam3/src/CameraModels/KannalaBrandt8.cpp
  orb_slam3/src/OptimizableTypes.cpp
  orb_slam3/src/MLPnPsolver.cpp
  orb_slam3/src/GeometricTools.cc
  orb_slam3/src/TwoViewReconstruction.cc
  orb_slam3/src/Config.cc
  orb_slam3/src/Settings.cc
  orb_slam3/include/System.h
  orb_slam3/include/Tracking.h
  orb_slam3/include/LocalMapping.h
  orb_slam3/include/LoopClosing.h
  orb_slam3/include/ORBextractor.h
  orb_slam3/include/ORBmatcher.h
  orb_slam3/include/FrameDrawer.h
  orb_slam3/include/Converter.h
  orb_slam3/include/MapPoint.h
  orb_slam3/include/KeyFrame.h
  orb_slam3/include/Atlas.h
  orb_slam3/include/Map.h
  orb_slam3/include/MapDrawer.h
  orb_slam3/include/Optimizer.h
  orb_slam3/include/Frame.h
  orb_slam3/include/KeyFrameDatabase.h
  orb_slam3/include/Sim3Solver.h
  orb_slam3/include/Viewer.h
  orb_slam3/include/ImuTypes.h
  orb_slam3/include/G2oTypes.h
  orb_slam3/include/CameraModels/GeometricCamera.h
  orb_slam3/include/CameraModels/Pinhole.h
  orb_slam3/include/CameraModels/KannalaBrandt8.h
  orb_slam3/include/OptimizableTypes.h
  orb_slam3/include/MLPnPsolver.h
  orb_slam3/include/GeometricTools.h
  orb_slam3/include/TwoViewReconstruction.h
  orb_slam3/include/SerializationUtils.h
  orb_slam3/include/Config.h
  orb_slam3/include/Settings.h
)

set_target_properties(orb_slam3_lib PROPERTIES VERSION "${orb_slam3_lib_VERSION}") # TODO need to findout why this is required

ament_target_dependencies(orb_slam3_lib
  ${THIS_PACKAGE_INCLUDE_DEPENDS}
)

# Link libraries
target_link_libraries(orb_slam3_lib
  ${OpenCV_LIBS}
  ${EIGEN3_LIBS}
  ${Pangolin_LIBRARIES}
  ${PROJECT_SOURCE_DIR}/orb_slam3/Thirdparty/DBoW2/lib/libDBoW2.so
  ${PROJECT_SOURCE_DIR}/orb_slam3/Thirdparty/g2o/lib/libg2o.so
  -lboost_system
  -lboost_serialization
  -lcrypto 
)

# Find the .so files provided in ros2_orb_slam3/orb_slam3/Thirdparty projects
set(DBoW2_PATH "${PROJECT_SOURCE_DIR}/orb_slam3/Thirdparty/DBoW2/lib/libDBoW2.so")
set(g2o_PATH "${PROJECT_SOURCE_DIR}/orb_slam3/Thirdparty/g2o/lib/libg2o.so")

# Install .so files to the lib directory in the install space
install(FILES ${DBoW2_PATH} DESTINATION lib)
install(FILES ${g2o_PATH} DESTINATION lib)

# Add executable
add_executable(mono_node_cpp
  src/mono_example.cpp
  src/common.cpp
)

ament_target_dependencies(mono_node_cpp
  PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}
)

target_link_libraries(mono_node_cpp PUBLIC orb_slam3_lib) # Link a node with the internal shared library

# Install all the header files in package/package/include
install (DIRECTORY include/ 
        DESTINATION include
)

# Install our node and library
install(TARGETS mono_node_cpp orb_slam3_lib
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION lib/${PROJECT_NAME}
  INCLUDES DESTINATION include/${PROJECT_NAME}
)

# Tell downstream packages where to find the headers
ament_export_include_directories(include)

# Help downstream packages to find transitive dependencies
ament_export_dependencies(
  orb_slam3_lib
  ${THIS_PACKAGE_INCLUDE_DEPENDS}
)

# Python node
# Install Python modules, submodules
ament_python_install_package(${PROJECT_NAME}) # Install the modules in ros2_orb_slam3/ros2_orb_slam3 folder

# Install Python executibles
install(PROGRAMS
  scripts/mono_driver_node.py
  scripts/mono_driver_node_gazebo.py
  DESTINATION lib/${PROJECT_NAME}
)

ament_package()

[Mechazo11]

@OdedHorowits and @andremaurell great discussion, sorry for joining in so late

@OdedHorowits I was reviewing your mono_driver_node.py file and noticed the following

 try:
        while rclpy.ok():
            print('spinning')
            rclpy.spin_once(n)
            n.run_py_node()
            rate.sleep()
    except KeyboardInterrupt:
        pass

Comparing with my version that reads static image files saved in directories

#* Blocking loop to send RGB image and timestep message
    for idx, imgz_name in enumerate(n.imgz_seqz[n.start_frame:n.end_frame]):
        try:
            rclpy.spin_once(n) # Blocking we need a non blocking take care of callbacks
            n.run_py_node(idx, imgz_name)
            rate.sleep()

            # DEBUG, if you want to halt sending images after a certain Frame is reached
            if (n.frame_id>n.frame_stop and n.frame_stop != -1):
                print(f"BREAK!")
                break

        except KeyboardInterrupt:
            break

I noticed that you are just calling n.run_py_node() without passing in the idx or the imgz_name parameters. Hence, no image files are being read and consequently, VSLAM node doesn't receive the first image leading to the WAITING FOR IMAGE status.

Please note that, the way I wrote run_py_node() method, it is not meant to take in live data stream (simulated or real) without modifications.

I would advise you modify your python node or use my original one as is with the new dataset you captured with Gazebo to make sure the cpp and python node performs handshake and then executes the VSLAM pipeline correctly. Then the MonoDriver::run_py_node() needs to be modified to accept live stream (simulated) data.

Hope this helps.

With best, @Mechazo11

[meard]

Hello @Mechazo11 With respect to above discussion, I was able to make ORB SLAM3 work with PX4 and Gazebo. Thanks to your repo, I was able to develop this small project after forking your repository. If your can review the changes, I can send your a PR.

[Mechazo11]

@meard

Happy to hear that you got the system to work with simulated camera stream. For now, I am going to close this issue. I will take a look at your repo but I don't have plan to merge such major change with this repo. As I had mentioned, this repo serves as ROS 2 native implementation of ORB SLAM3