0
$\begingroup$

Rosanswers logo

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.


Originally posted by mohsen on ROS Answers with karma: 249 on 2018-01-07

Post score: 0


Original comments

Comment by gvdhoorn on 2018-01-07:\

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.

Can you explain why you believe this is necessary?

In any case the 'control loop' will have to ..

Comment by gvdhoorn on 2018-01-07:
.. 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'?

Comment by jayess on 2018-01-07:
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.

Comment by PeteBlackerThe3rd on 2018-01-07:
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.

Comment by mohsen on 2018-01-08:
@PeteBlackerThe3rd: Which sensor fusion package do you suggest? I found robot_localization package which provides sensor fusion using EKF.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

This is how I managed to do it. The idea is to publish each sensor reading twice and read new data only after we've done so.

 import rospy
 import sensor_library
 from sensor_msgs.msg import MagneticField

 rospy.init_node('sensor_node') 
 pub = rospy.Publisher('data', MagneticField, queue_size = 1)

 out = MagneticField()
 rate = rospy.Rate(200)
 c = 2;

 while not rospy.is_shutdown():
       if c == 2:
                 # assign the value of sensor readings to out.
                   c = 0
       c += 1
       pub.publish(out)
       rate.sleep()

Originally posted by mohsen with karma: 249 on 2018-02-06

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by jayess on 2018-02-06:
I'm not understanding this solution. Where is the subscriber and callback for it and where is out being set?

Comment by mohsen on 2018-02-06:
I no longer need the second node, which is what I was asking for in the question.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.