Merge two topics - get information of 2 different topics

edit

I'm simulating in Gazebo a control with noise. The model is ideal, so there's no noise. That's why I created a topic called vel_noise where some noise is published. In another topic, called vel_control, the control of my vehicle is implemented (based on position measurements).

Both topics are Twist, i.e, they have 3 linear velocities and 3 angular velocities.

Now, I want to create a node that subscribes to both topics, add the velocities of each one and publish that information in another topic called cmd_vel.

I found that there's something called message_filters that could do this, but I couldn't make that work. Can anyone please help me, please?

My idea is to have something like this:

vel_merged.linear.x = vel_noise.linear.x + vel_control.linear.x
vel_merged.linear.y = vel_noise.linear.y + vel_control.linear.y
vel_merged.linear.z = vel_noise.linear.z + vel_control.linear.z

(I'm using Python)

I tried something like this, using message filters:

#!/usr/bin/env python
import message_filters
import rospy
from geometry_msgs.msg import Twist

class main_control_noise():  

    def __init__(self):

        image_sub = message_filters.Subscriber('vel_control', Twist)
        info_sub = message_filters.Subscriber('vel_noise', Twist)

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


        return

    def callback(vel_control, vel_noise):
      # Solve all of perception here...
      print "aa"


if __name__ == '__main__':

    rospy.init_node('control_noise', anonymous=True)

    try:
        rate = rospy.Rate(100) #este rate creo que me da cada cuanto entra al while not de abajo

        while not rospy.is_shutdown():

            print "a"
            m_noise = main_control_noise()
            rate.sleep()

    except rospy.ROSInterruptException:
        rospy.loginfo("Node terminated.")

But I don't know a lot of things:

1) What information do I have to give to message_filters.Subscriber? I want to subscribe those 2 nodes (names: vel_control and vel_noise), and the "output" of those topics would be a Twist message: 3 velocities and 4 orientations. I don' know if message_filters.Subscriber('vel_control', Twist) is OK

2) I don't know what do I have to put inside the callback

For example, the vel_noise script is:

class main_noise():

    def __init__(self):

        self.velocity_publisher = rospy.Publisher('/vel_noise', Twist, queue_size=10)

        vel_msg = Twist()

        global counter

        sine_noise = math.sin(counter*math.pi/180)

        vel_msg.linear.x = sine_noise/5

        if counter % 2 == 0:
            vel_msg.linear.y = sine_noise
        else:
            vel_msg.linear.y = -sine_noise

        print "Counter: ", counter
        print "Sin: ", vel_msg.linear.x
        self.velocity_publisher.publish(vel_msg)

        return


if __name__ == '__main__':

    rospy.init_node('noise', anonymous=True)

    global counter
    counter = 0

    try:
        rate = rospy.Rate(100) #100 Hz

        while not rospy.is_shutdown():

            print "a"
            counter = counter+1
            m_noise = main_noise()
            rate.sleep()

    except rospy.ROSInterruptException:
        rospy.loginfo("Node terminated.")