merge two topics and publish one

asked 2021-05-29 05:29:26 -0600

Hello, I am trying to get the information out of two different topics and publish it on a new one.

Topic 1 is called /objecstInfo which is of a custom msg type find_object_2d/DetectionInfo. I need to extract the filePaths out of this topic/msg.

Header header

# All arrays should have the same size
std_msgs/Int32[] ids
std_msgs/Int32[] widths
std_msgs/Int32[] heights
std_msgs/String[] filePaths
std_msgs/Int32[] inliers
std_msgs/Int32[] outliers
# 3x3 homography matrix: [h11, h12, h13, h21, h22, h23, h31, h32, h33] (h31 = dx and h32 = dy, see QTransform)
std_msgs/Float32MultiArray[] homographies

Topic 2 is called /objectsPose which is of msg type geometry_msgs/PoseStamped. I need to extract the x and y Pose out of this topic.

The topic I want to publish this information is called /objectsTopic. I made a custom msg for this called ObjectTopic.msg which looks like this:

Header header
string objectName
float64 poseX
float64 poseY

in objectName I want to publish filePaths from topic 1 and in poseX and poseY I want to publish pose.x and pose.y from topic 2. I found this method message_filters but could not implement it successfully in my code. In fact I could not even get the topic published. When I run the node and execute rostopic list the topic /objectsTopic is not listed.

This is my python script:

#!/usr/bin/env python
from __future__ import print_function

import sys
import rospy
import message_filters
from find_object_2d.msg     import ObjectPose
from find_object_2d.msg     import DetectionInfo
from geometry_msgs.msg      import PoseStamped

def listener():
    info_sub = message_filters.Subscriber('/objectsInfo', DetectionInfo)
    pose_sub = message_filters.Subscriber('/objectsPose', PoseStamped)

    ts = message_filters.TimeSynchronizer([info_sub, pose_sub], 10)
    ts.registerCallback(callback)
    rospy.spin()

def callback(objectsInfo, objectsPose):
    pub = rospy.Publisher('objectsTopic', ObjectPose, queue_size=10)                       
    msg = ObjectPose()                              
    msg.objectName = objectsInfo.DetectionInfo.filePaths
    msg.poseX = objectsPose.PoseStamped.pose.position.x     
    msg.poseY = objectsPose.PoseStamped.pose.position.y               

    pub.publish(msg)                              
    rospy.sleep(1)

if __name__ == '__main__':
    rospy.init_node('object_topic', anonymous=True)             
    try:
        listener()
    except rospy.ROSInterruptException:
        pass

Help would be really appreciated! If there are other ways of solving this problem then using message_filters feel free to suggest.

Thank you very much.

edit retag flag offensive close merge delete