Logging multiple-ultrasound data on a MySQL server with (arduino-ROS-Python) framework
I am trying to log the ultrasonic data from a RC car into a MySQL server. I am able to publish and subscribe to the data via rostopic. But I am having problem with the code for uploading the data which is being subscribed. I have 4 sensors published to 4 topics /ultrasoundFL,/ultrasoundFR,/ultrasoundRL,/ultrasoundRR.
import rospy
#from std_msgs.msg import String
from sensor_msgs.msg import Range
import os
from time import *
import time
import threading
import serial
import datetime
import sys
import MySQLdb as mdb
try:
con = mdb.connect('130.127.199.212', 'root', 'Fr24clemson', 'autonomous');
cur = con.cursor()
except:
print "Error opening serial port."
sys.exit(1)
resp = ""
droptable="DROP TABLE autonomous.ultrasonic_sensor_data"
cur.execute(droptable)
createtable="CREATE TABLE autonomous.ultrasonic_sensor_data (n_FL DOUBLE NULL ,w_FR DOUBLE NULL,n_RL DOUBLE NULL ,w_RR DOUBLE NULL,t_unixtime DOUBLE NULL,t_time TIMESTAMP)"
cur.execute(createtable)
try:
def ultrasoundFL_callback(data):
FL=float(data.range)
def ultrasoundFR_callback(data):
FR=float(data.range)
def ultrasoundRL_callback(data):
RL=float(data.range)
def ultrasoundRR_callback(data):
RR=float(data.range)
unixtime=datetime.datetime.now().strftime("%s")
sql = "insert into ultrasonic_sensor_data(n_FL, w_FR, n_RL, w_RR, t_unixtime) values(%s, %s, %s, %s, %s)" % (FL,FR,RL,RR, unixtime)
print sql
cur.execute(sql)
print "Rows inserted: %s" % cur.rowcount
con.commit()
#time.sleep(1)#set to whatever
resp = ""
except:
print sys.exc_info()[0]
def listener():
from sensor_msgs.msg import Range
rospy.init_node('serial', anonymous=True)
rospy.Subscriber("ultrasoundFL", Range, ultrasoundFL_callback)
print ("a")
rospy.Subscriber("ultrasoundFR", Range, ultrasoundFR_callback)
print ("b")
rospy.Subscriber("ultrasoundRL", Range, ultrasoundRL_callback)
print ("c")
rospy.Subscriber("ultrasoundRR", Range, ultrasoundRR_callback)
print ("c")
rospy.spin()
if __name__=='__main__':
listener()
I am getting this error:
[ERROR] [WallTime: 1468975143.127478] bad callback:
Asked by angshumanG on 2016-07-19 19:10:31 UTC
Answers
The variable that you create for each sensor is only valid inside its callback.
Your callbacks will be called asynchronously, whenever there is new data on a topic. This means that you can't assume that callbacks will be called in order.
Instead of trying to write all four reading to your database in the callback for one sensor (where you only have data from one sensor), I suggest a different approach. You have a few choices:
- Gather the data into a single message with all of your sensor readings on the publisher, so that you only need to subscribe to one message
- Store the data from each sensor separately in the database, so that you don't need to collect all four sensor readings
- Use an Approximate Time Synchronizer from the message_filters package to synchronize your sensor readings so that you get a single callback with all four sensor readings. The message_filters python docs may be useful.
- Build your own synchronization mechanism
Asked by ahendrix on 2016-07-20 12:14:00 UTC
Comments
You code is hard to read because it isn't formatted well; can you adjust the indentation so that it matches PEP-8 style?
Asked by ahendrix on 2016-07-19 19:36:32 UTC
You also say that "there is a problem" but you don't describe the problem. That makes it difficult to answer your question, if we don't know what the problem is.
Asked by ahendrix on 2016-07-19 19:37:51 UTC
i am getting error : NameError: global name 'FL' is not defined
Asked by angshumanG on 2016-07-19 19:43:07 UTC