Robotics StackExchange | Archived questions

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

I have 2 topics: /veldesired 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 /veldesired, 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 /veldesired, then to /ardrone/navdata, then to /veldesired, 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

        self.velocity_publisher.publish(vel_command)

    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)   

    try:

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

            #print "a***"

            #rate.sleep()

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

Asked by Dylan on 2019-08-21 07:04:37 UTC

Comments

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.

Asked by gvdhoorn on 2019-08-21 07:57:01 UTC

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

Asked by Dylan on 2019-08-21 11:38:40 UTC

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

Asked by gvdhoorn on 2019-08-21 11:39:54 UTC

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

Asked by Dylan on 2019-08-21 11:43:10 UTC

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.

Asked by gvdhoorn on 2019-08-21 12:46:48 UTC

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?

Asked by Dylan on 2019-08-21 20:58:00 UTC

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.

Asked by PeteBlackerThe3rd on 2019-08-22 01:48:05 UTC

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

Asked by gvdhoorn on 2019-08-22 02:18:56 UTC

I know that I should know how to implement it, but I don’t and I need to test this code today. Please, I really need your help. I already read python tutorials about classes but I don’t get it. In my other scripts, when I called 2 subscribers it only keeps in a loop in one of them and the other is never updated. If you could edit my code it would be really appreciated. I edited the main post with your suggestions.

Asked by Dylan on 2019-08-22 06:37:13 UTC

Thats how i would have done.

class main_control_vel():

    def __init__(self)
         self.velocity =Twist()
         self.navdata = Navdata()   //You need to check specific type of msg for this
         rospy.Subscriber( ' /vel_desired', Twist, self.vel_callback)
         rospy.Subscriber('/ardrone/Navdata', Navdata, self.nav_callback)
         self.looprate = rospy.Rate(5)    // To run loop at 5 Hz

    def vel_callback(self, vel_msg)
         self.velocity = vel_msg

    def nav_callback(self, nav_msg)
         self.navdata = nav_msg

    def start(self)
         while not rospy.is_shutdown():
         // Do here whatever you want to do with self.velocity and self.navdata
         // They will keep on updating with freq they are being published
        self.looprate.sleep()

main

 rospy.init_node('go_to', anonymous=True)
 m_land = main_control_vel()
 m_land.start()

Asked by usamamaq on 2019-08-22 16:33:33 UTC

I couldnt verify whole. If it solves your problem do let me know i will add this to answer.

P.S. i myself was in your condition not so many days ago. I hope it helps.

Asked by usamamaq on 2019-08-22 16:39:34 UTC

@usamamaq this is what I did and I think it works, but it doesn't print 10 message of the topic published at 200 Hz and 1 of the topic at 20 Hz, it prints something like: 20 at 200 Hz, 1 at 20 Hz, then 5 at 200 Hz, 1 at 20 Hz, then 7 at 200 Hz, 1 at 20 Hz and so on. It's in the main post

Asked by Dylan on 2019-08-22 16:45:10 UTC

Ok if your velocity and navdata message are being published asynchronously then this can happen or can be many other reasons. I think there are two options:

1) If both messages are time stamped then subscriber message filters can be used so velocity message at timestamp closer to navdata stamp is only used for next calculations.

2)Or may be you can initiate a timer in class init and update the velocity and navdata message after specific time has passed in callbacks.

Really couldnt test it right now otherwise could have helped you better.

Asked by usamamaq on 2019-08-22 17:07:41 UTC

One more thing: Why you want to specifically get 20 msgs before updating velocity message. You want to control the ardrone velocity inputs. Let navdata get updated at whatever time navdata is being sent by ardrone and just control publishing of ref velocity. May be if you start controlling ardrone on specific events of navdata (getting 20 messages) it starts delaying your vel_command.

Asked by usamamaq on 2019-08-22 17:19:24 UTC

that's what I did. The reference value is updated at 20 Hz and navdata at 200 Hz, and I send a new velocity at 200 Hz

Asked by Dylan on 2019-08-22 18:26:51 UTC

For now your code in navdata callback is small so you can think you are publishing velocity at 200 Hz because navdata is being updated at 200 Hz. But i will recommend just updating the navdata in callback and doing all calculation and publishing work in separate class member. And controlling you publishing rate by looprate.sleep(). As i have recommended in code above.

Asked by usamamaq on 2019-08-23 00:24:50 UTC

Answers