publishing sensory data out of sync
Hi all,
I am working on a project with the ROBOTIS turtlebot3 burger and am trying to include a temperature sensor with its build.
I have uploaded some arduino code into the OpenCR1.0 to read sensory data for serial communication. This data is being read on my raspberry pi with ubuntu server 20.04. I am also using ROS2 foxy as well.
#define PORT4_SIG1 73
#define PORT4_SIG2 74
#define PORT4_ADC 75
#define OLLO_SLEEP 46
void setup() {
Serial.begin(115200);
pinMode(OLLO_SLEEP, OUTPUT);
digitalWrite(OLLO_SLEEP, HIGH);
pinMode(PORT4_ADC, INPUT_ANALOG);
pinMode(PORT4_SIG1, OUTPUT);
pinMode(PORT4_SIG2, OUTPUT);
}
void loop() {
uint32_t analogValue;
analogValue = analogRead(PORT4_ADC);
word long vvalue = (4095 - analogValue) * 10000 / analogValue;
Serial.println(float(vvalue / 1000.0));
}
Then I have a python script to confirm I can read its value and print into the terminal.
#!/usr/bin/env python3
import serial
import time
if __name__ == '__main__':
ser = serial.Serial('/dev/ttyACM0', 115200, timeout=1)
ser.reset_input_buffer()
while True:
line = ser.readline().decode('utf-8').rstrip()
print(line)
I can see the temperature changing properly as I grip onto the sensor probe and let go. Now I want to implement this with ROS2. Here is my attempt in reading in the serial data and publishing it:
#!/usr.bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32
import serial
class tps_node(Node):
def __init__(self):
super().__init__("tps_node")
self.ser = serial.Serial('/dev/ttyACM0', 115200, timeout=1)
self.ser.reset_input_buffer()
self.tps_publisher = self.create_publisher(
Float32, "tps_node", 10
)
self.pub_timer = self.create_timer(0.1, self.callback_temp_publisher)
self.get_logger().info("TPS10 node started")
def callback_temp_publisher(self):
if self.ser.in_waiting > 0:
line = self.ser.readline().decode('utf-8').rstrip()
try:
line = float(line)
except:
return
temp = Float32()
temp.data = line
self.tps_publisher.publish(temp)
print(line)
def main(args=None):
rclpy.init(args=args)
node = tps_node()
rclpy.spin(node)
rclpy.shutdown()
if __name__ == "__main__":
main()
My issue is that I think my timer is not in sync with the serial communication. I get temperature outputs but it does not increase with me holding onto the sensor probe. I think the output data is too far back in the stack and cannot keep up with the communication. So my question is, what is the best way to get sensory data from my OpenCR1.0 (an arduino board) to my pi so I can publish it in as near real time as possible or to get the latest value to get the most up to date value.
Please let me know if I have left out any detail and need to provide more information.
Thanks in advance!