Question

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

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

Comments

Answers