Subscribe to 2 topics and store outputs (different frequencies?)
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
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.")
Don't. Ever. Create. Subscribers. In. Callbacks. (or Publishers).
Just create the Subsribers in your
__init__and store the 'latest' message on/vel_desiredin 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.How can I store the latest message of vel_desired in a class member?
How are you currently storing the
vel_ref_x? Messages are just simple Python objects. You can store them in exactly the same way.Using self? Can you please write a piece of code? Sorry but I don’t know a lot of Python+ROS
This has nothing to do with ROS.
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.
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?
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.
@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.
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.
Thats how i would have done.
main
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.
@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
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.
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.