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