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:
React
Typescript
Django
Laravel
Facebook
Microsoft
Google
Alibaba
D3
Tencent