[rospy] Messages are not published, any idea why?
Hello people,
I have an issue and I am out of ideas. Let me describe my situation. I have some videos and 1 bag that contains a point cloud topic and a topic that contains timestamps for each frame of the videos and I am trying to republish the data by republishing the point cloud topic as it is and creating the image topics from the video frames with their timestamps. The problem is that although the Image messages are created as they should be, they are not getting published but the velodyne points are published normally.
The code that I am experimenting with is uploaded here at pastebin and as you can see I am a beginner at this and any help or suggestion will be greatly appreciated.
EDIT: here is the code
#!/usr/bin/env python
import cv2
import rospy
from sensor_msgs.msg import Image, PointCloud2, CameraInfo
import os
import yaml
import rosbag
from cv_bridge import CvBridge
rospy.init_node('Translator')
class Translator:
mode = 'republish'
cam_ids = [i for i in range(4)]
video_name = 'econVideo_2021-09-29-10-29-58'
camera_info_msg = CameraInfo()
bridge = CvBridge()
def __init__(self, input_bag, video_path, calibration_files_path):
# Takes as input a ROSbag containing
self.bag = input_bag
self.video_path = video_path
self.calibration_files_path = calibration_files_path
#@staticmethod
#def
#orig = os.path.splitext(bag)[0] + ".orig.bag"
#move(bag, orig)
def translate(self, video_path):
setup_cameras(video_path)
do_translate()
def setup_cameras(self):
self.cameras = dict()
for cid in self.cam_ids:
self.cam_info = dict()
print("{}".format(self.video_path) + "/{}_{}.mkv".format(self.video_name, cid))
self.cam_info['cap'] = cv2.VideoCapture("{}".format(self.video_path) + "/{}_{}.mkv".format(self.video_name, cid))
self.cameras[cid] = self.cam_info
if self.mode == 'republish':
# Setting up camera, camera_info & velodyne publishers. Also setting up cv2.VideoCapture for each video file.
for cid in self.cam_ids:
self.cam_info['publisher'] = rospy.Publisher('/econ_cam_{}/image_raw'.format(cid), Image, queue_size=25)
self.cam_info['publisher_info'] = rospy.Publisher('/econ_cam_{}/camera_info'.format(cid), CameraInfo, queue_size=25)
self.cameras[cid] = self.cam_info
self.velo_publisher = rospy.Publisher('/velodyne_points', PointCloud2, queue_size=10)
print(self.cameras[1]['publisher'])
return self.cameras, self.velo_publisher
@staticmethod
def parse_camera_info_from_yaml(yaml_fname):
info = {}
if os.path.isfile(yaml_fname): # if a file containing camera info exists extract its content.
with open(yaml_fname, "r") as file_handle:
calib_data = yaml.load(file_handle)
# Parse
info['width'] = calib_data["image_width"]
info['height'] = calib_data["image_height"]
info['K'] = calib_data["camera_matrix"]["data"]
info['D'] = calib_data["distortion_coefficients"]["data"]
info['R'] = calib_data["rectification_matrix"]["data"]
info['P'] = calib_data["projection_matrix"]["data"]
info['distortion_model'] = calib_data["distortion_model"]
else: # if a file containing camera info does not exist create dummy camera info.
info['width'] = 0
info['height'] = 0
info['K'] = [0, 0, 0, 0, 0, 0, 0, 0, 0]
info['D'] = [0, 0, 0, 0, 0]
info['R'] = [0, 0, 0, 0, 0, 0, 0, 0, 0]
info['P'] = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
info['distortion_model'] = "plumb_bob"
return info
def cam_info_data(self):
self.info_data = {}
for idx in self.cameras.keys():
yaml_fname = '{}'.format(self.calibration_files_path) + 'cam-{}-camerainfo.yaml'.format(idx)
self.info_data[idx] = self.parse_camera_info_from_yaml(yaml_fname)
#print(self.info_data[idx])
return self.info_data
def ...
External links quickly go bad. Please edit your description and copy/paste the code into it. It's OK if it's many lines, but try not to show us unnecessary stuff. You can format it as code by using the 101010 button.
You edit using the "edit" button at the end of the description.
This code has too many levels of indirection for me to look for your bug. I suggest you create a minimal test program that reads the bag and publishes images to just one topic. This will tell you if your use of the API is correct or not.
do you have a
rate = rospy.Rate(10)anywhere else in your code? It doesn't need to be 10HzTo expand on @osilva point: you must sleep in the main loop, or the background rospy threads never get an opportunity to execute.
I do not use a certain rate. I just read frames from each video file once the topic containing the timestamp of the frames is read (check the method called translate, it is the last one). I used to structure my script, not in OOP fashion, but when I did create this class as an exercise to the OOP principles.
@lfx13 I suggest you read this answer #q264812 to understand how rate() and sleep() are needed when publishing. You are not creating interruptions otherwise. It’s ok to use OOP but you still need to invoke these methods.
@osilva hello, it still does not seem to work; I've added the rate object and the sleep method at the for loop, it seems to take effect and introduce some latency as intended. The pointcloud2 topic is only being published as previously mentioned, but the image messages are not getting published. The post you mentioned explains how rate object works, especially the sleep method, but it does not mention why it is needed other than maintaining a specific rate. I've read here that the publisher should be given enough time and for this task, the rate object helps but it does not make a difference in my case.
The issue seems to be with the publisher, because when I write the topics to a bag, everything seems to be working as intended. But the output bag needs to be reindexed.