A sensor node that publishes faster than it samples?[Python]
I am using a sensor with a sample rate of 100 Hz. I would like to use it in a control loop with an update rate of 200 Hz. I believe I eventually have to publish the sensor's data at 200 Hz.
I have done this using two nodes:
1- sensor_node: Receives the sensor data at 100 Hz.
2- rate_converter: Receives the messages published by sensor_node and publishes them at 200 Hz.
Am I doing this right? Is it possible to do this using only one node? (Or some other method)
The first node:
import rospy
import sensor_library
from sensor_msgs.msg import MagneticField
rospy.init_node('sensor_node')
pub = rospy.Publisher('data', MagneticField, queue_size = 1)
rate = rospy.Rate(100)
while not rospy.is_shutdown():
# assign the value of sensor readings to out
pub.publish(out)
rate.sleep()
The second node:
import rospy
from sensor_msgs.msg import MagneticField
def callback(msg):
# Basically, out = msg
out = MagneticField()
rospy.init_node('rate_converter')
sub = rospy.Subscriber('data', MagneticField, callback, queue_size = 1)
pub = rospy.Publisher('data', MagneticField, queue_size = 1)
rate = rospy.Rate(200)
while not rospy.is_shutdown():
pub.publish(out)
rate.sleep()
Update:
Why I think I need higher rates:
1- Isn't it standard practice in quadrotor control? You have many sensors with different sample rates and you are using them in the same control system. Since some sensors are inherently slow (ultrasonic), using stale data is inevitable. Here's a slide from lecture 4.4 of this course showing update rates of a cascaded control loop.
2- I'm also using the madgwick filter from imu_tools package. The output rate of the filter is bounded from above by the lowest input rate. For example, with magnetometer rate of 100 Hz and IMU rate of 200 Hz, I will receive orientation data at 100 Hz.
Since magnetometer update is only used to prevent yaw drift, I'm okay with using stale data. I also read somewhere (not reliable) that this filter needs to update 4-6 time faster than the data.
And as to why I'm not using an internal variable:
For PID controllers, I'm using the pid package by Andy Zelenak. I prefer using the package as is, since modifying the node or writing one from scratch takes time.
Can you explain why you believe this is necessary?
In any case the 'control loop' will have to ..
.. cope with working with 'stale data' for half of its iterations. Why waste network and computational resources to send copies of messages when the 'control loop' could just use an internal variable that stores the 'last received value'?
I'm that course they use a Kalman filter that runs at a higher rate to provide an estimate of the state of the quadcopter. Then they use the sensors to correct the estimation (at a lower rate). Check out the later sections.
One of (the many) reasons for using a Kalman filter is that you don't have to give them all the sensor data each iteration, you can call the filter at the rate appropriate for each sensor with that reading and it will make sense of it all.
@PeteBlackerThe3rd: Which sensor fusion package do you suggest? I found robot_localization package which provides sensor fusion using EKF.