[rospy] Messages are not published, any idea why?

asked 2021-10-04 03:59:25 -0500

Ifx13 gravatar image

updated 2021-10-04 07:13:32 -0500

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


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

    #orig = os.path.splitext(bag)[0] + ".orig.bag"
    #move(bag, orig)

    def translate(self, video_path):

    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)
        return self.cameras, self.velo_publisher

    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)
        return self.info_data

    def ...
edit retag flag offensive close merge delete



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.

Mike Scheutzow gravatar image Mike Scheutzow  ( 2021-10-04 06:48:12 -0500 )edit

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.

Mike Scheutzow gravatar image Mike Scheutzow  ( 2021-10-04 07:44:16 -0500 )edit

do you have a rate = rospy.Rate(10) anywhere else in your code? It doesn't need to be 10Hz

osilva gravatar image osilva  ( 2021-10-04 08:14:15 -0500 )edit

To expand on @osilva point: you must sleep in the main loop, or the background rospy threads never get an opportunity to execute.

Mike Scheutzow gravatar image Mike Scheutzow  ( 2021-10-04 08:27:48 -0500 )edit

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.

Ifx13 gravatar image Ifx13  ( 2021-10-04 08:49:23 -0500 )edit

@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 gravatar image osilva  ( 2021-10-04 16:56:13 -0500 )edit

@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.

Ifx13 gravatar image Ifx13  ( 2021-10-05 01:49:51 -0500 )edit

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.

Ifx13 gravatar image Ifx13  ( 2021-10-05 02:05:00 -0500 )edit