Subscribe to 2 topics and store outputs (different frequencies?)

asked 2019-08-21 07:04:37 -0600

Dylan gravatar image

updated 2019-08-22 16:46:02 -0600

I have 2 topics: /vel_desired and /ardrone/navdata. The first one's type is Twist and updates at f=20Hz. The second one updates at f=200Hz. Ideally, I would like to take 1 Twist message from /vel_desired, store it somewhere and use it 10 times looking at /ardrone/navdata (i.e, publishing based on the data read)

I don't know how to do that, so I made a script that subscribes to /vel_desired, then to /ardrone/navdata, then to /vel_desired, then to /ardrone/navdata, etc. but I don't know if that is correct and what frequency is it working (I guess it's more than 200 Hz).

I can subscribe to each topic at 200 Hz, because the first topic will keep it's value until it gets a new one.

Edit: this is the code now:

#!/usr/bin/env python
from __future__ import division
import rospy
import sys
from std_msgs.msg import String
from std_msgs.msg import Empty
from geometry_msgs.msg import Twist
from ardrone_autonomy.msg import Navdata

import threading
import tf
import time
from std_msgs.msg import Empty
from nav_msgs.msg import Odometry
from sensor_msgs.msg import Image, CameraInfo, CompressedImage
from cv_bridge import CvBridge, CvBridgeError
import cv2
from geometry_msgs.msg import Twist
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Pose

from tf.transformations import euler_from_quaternion, quaternion_from_euler

import os

import numpy as np
import math

import time
import datetime

from ar_track_alvar_msgs.msg import AlvarMarkers

class main_control_vel():

    def __init__(self):
        print "aa"

        self.last_vel_twist = Twist()

        self.velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        rospy.Subscriber('/vel_deseada', Twist, self.read_desired_vel)
        rospy.Subscriber("/ardrone/navdata", Navdata, self.control_vel) 

    def control_vel(self, navdata):
        print "------------b"
        vel_command = Twist()

        vel_ref_x = self.last_vel_twist.linear.x
        vel_ref_y = self.last_vel_twist.linear.y
        vel_ref_z = self.last_vel_twist.linear.z

        orientation_ref_x = self.last_vel_twist.angular.x
        orientation_ref_y = self.last_vel_twist.angular.y
        orientation_ref_z = self.last_vel_twist.angular.z
        #orientation_ref_w = self.last_vel_twist.angular.w

        vel_actual_x = navdata.vx
        vel_actual_y = navdata.vy
        vel_actual_z = navdata.vz

        vel_command.linear.x = vel_actual_x - vel_ref_x
        vel_command.linear.y = vel_actual_y - vel_ref_y
        vel_command.linear.z = vel_actual_z - vel_ref_z

        vel_command.angular.x = 0.4
        vel_command.angular.y = 0.4
        vel_command.angular.z = 0.0


    def read_desired_vel(self, vel_twist):
        print "------c"
        self.last_vel_twist = vel_twist

if __name__ == '__main__':

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

    #self.velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
    #rospy.Subscriber('/vel_desired', Twist, self.read_desired_vel)
    #rospy.Subscriber("/ardrone/navdata", Navdata, self.control_vel)   


        #rate = rospy.Rate(1) #este rate creo que me da cada cuanto entra al while not de abajo
        m_land = main_control_vel()
        #while not rospy.is_shutdown():

            #print "a***"


    except rospy.ROSInterruptException:
        rospy.loginfo("Node terminated.")
edit retag flag offensive close merge delete



Don't. Ever. Create. Subscribers. In. Callbacks. (or Publishers).

Just create the Subsribers in your __init__ and store the 'latest' message on /vel_desired in a class member. The other subscriber can then use it whenever its callback gets called.

PS: yes, don't ever say 'never'. But really. Creating subscribers in callbacks can be done for legitimate purposes, but this is not one.

I would also be surprised if this actually works, as the rospy.Subscriber(..)s go out of scope as soon as the callbacks are exited.

gvdhoorn gravatar image gvdhoorn  ( 2019-08-21 07:57:01 -0600 )edit

How can I store the latest message of vel_desired in a class member?

Dylan gravatar image Dylan  ( 2019-08-21 11:38:40 -0600 )edit

How are you currently storing the vel_ref_x? Messages are just simple Python objects. You can store them in exactly the same way.

gvdhoorn gravatar image gvdhoorn  ( 2019-08-21 11:39:54 -0600 )edit

Using self? Can you please write a piece of code? Sorry but I don’t know a lot of Python+ROS

Dylan gravatar image Dylan  ( 2019-08-21 11:43:10 -0600 )edit

This has nothing to do with ROS.

def read_desired_vel(self, vel_twist):
    self.last_vel_twist = vel_twist

You may want to pickup a book about Python and/or follow a few tutorials online about it. That will make a lot of things much more understandable.

gvdhoorn gravatar image gvdhoorn  ( 2019-08-21 12:46:48 -0600 )edit

Sorry but I don't know if I understand what you're saying. And where do I set the frequency? It's approximately 200 Hz, but it can be from 199 Hz to 201 Hz, for example. I added the new code to the main post, is that OK?

Dylan gravatar image Dylan  ( 2019-08-21 20:58:00 -0600 )edit

This code doesn't set the frequency, the callbacks will be called whenever a new message is available. The rate is determined by the node sending the messages not the one receiving them.

I'll second @gvdhoorn I'd really recommend working through some pure python tutorials on classes before trying to build this ROS node, you're problems are all python related.

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2019-08-22 01:48:05 -0600 )edit

@Dylan: conceptually you seem to have a fairly good grasp of what you need to do, but the implementation is the problem here.

As much as it may seem to introduce "unnecessary" or unwanted delays, I'd repeat my suggestion to first brush up on Python itself. It will make things much, much easier.

gvdhoorn gravatar image gvdhoorn  ( 2019-08-22 02:18:56 -0600 )edit