def db_write(pub, state):
"""tms_db_writerにトピックを送る（結果、DBにデータを書き込む)
args:
pub: node.publisher型。tms_db_writerへのトピック送信用
state: int型。1なら測定中、0なら測定停止中
"""
global node, roll, pitch, temp, rate, wave
now_time = datetime.now().isoformat()
send_data = {"temp": temp, "rate": rate, "wave": wave}
db_msg = TmsdbStamped()
db_msg.header.frame_id = "/world"
# TODO: db_msgのヘッダーのstampがわからなかったので書いていない
# clock = Clock()
# db_msg.header.stamp = clock.now()
tmp_data = Tmsdb()
tmp_data.time = now_time
tmp_data.name = "whs1_mybeat"
tmp_data.id = 3021
tmp_data.place = 5001
tmp_data.sensor = 3021
tmp_data.state = state
tmp_data.rr = roll
tmp_data.rp = pitch
tmp_data.ry = 0.0
tmp_data.note = json.dumps(send_data)
db_msg.tmsdb = [tmp_data]
pub.publish(db_msg)
def callback(arg):
rospy.loginfo("get callback")
msg = TmsdbStamped()
msg.header.frame_id = "world_link"
msg.header.stamp = rospy.get_rostime() + rospy.Duration(9 * 60 * 60)
now = datetime.datetime.now().strftime("%Y-%m-%dT%H:%M:%S.%f")
msg.tmsdb.append(Tmsdb())
msg.tmsdb[0].time = now
msg.tmsdb[0].id = 2010
msg.tmsdb[0].name = "wheelchair_blue"
msg.tmsdb[0].place = 5001
msg.tmsdb[0].sensor = 2010
msg.tmsdb[0].state = 1 # 存在する
msg.tmsdb[0].x = arg.position.x
msg.tmsdb[0].y = arg.position.y
msg.tmsdb[0].z = arg.position.z
quat = (
arg.orientation.x,
arg.orientation.y,
arg.orientation.z,
arg.orientation.w)
roll, pitch, yaw = tf.transformations.euler_from_quaternion(quat)
msg.tmsdb[0].rr = roll
msg.tmsdb[0].rp = pitch
msg.tmsdb[0].ry = yaw
db_pub.publish(msg)
def sendDbHistoryInformation(self):
rate = rospy.Rate(100) # 100hz
while not rospy.is_shutdown():
temp_dbdata = Tmsdb()
object_information = TmsdbStamped()
d = datetime.now() - timedelta(days=3, hours=11)
t1 = 'ISODate(' + d.isoformat() + ')'
t2 = 'ISODate(' + (
d + timedelta(days=0, hours=1, minutes=0)).isoformat() + ')'
print(t1)
# print(db.history.ensure_index({'id':1,'time':1}))
# cursor = db.history.find({'id':{'$gte':1006, '$lte':1018},
# 'time':{'$gte':t1,'$lt':t2}, 'state':1}).sort('time')
cursor = db.history.find({
'id': {
'$gte': 1006,
'$lte': 1018
},
'time': {
'$gte': t1,
'$lt': t2
},
'state': 1
})
for doc in cursor:
# if not rospy.is_shutdown():
# print(doc)
del doc['_id']
temp_dbdata = db_util.document_to_msg(doc, Tmsdb)
object_information.tmsdb.append(temp_dbdata)
self.data_pub.publish(object_information)
rate.sleep()
def dbPublisherSrvCallback(self):
threading.Timer(0.01, self.dbPublisherSrvCallback).start()
temp_dbdata = Tmsdb()
current_environment_information = TmsdbStamped()
cursor = db.now.find()
for doc in cursor:
del doc['_id']
temp_dbdata = db_util.document_to_msg(doc, Tmsdb)
current_environment_information.tmsdb.append(temp_dbdata)
self.data_pub.publish(current_environment_information)
def control_refrigerator(req):
msg = TmsdbStamped()
db = Tmsdb()
msg.header.stamp = rospy.get_rostime() + rospy.Duration(9*60*60)
ms_time = datetime.datetime.fromtimestamp(time.time()).strftime("%Y%m%dT%H%M%S")
# print ms_time
db.time = str(ms_time)
db.id = 2009
db.sensor = 2009
if req.service == 1:
db.state = 2
elif req.service == 0:
db.state = 1
msg.tmsdb.append(db)
if req.service == 1:
clearpin.write(0)
time.sleep(0.1)
clearpin.write(1)
time.sleep(0.1)
startpin.write(0)
time.sleep(0.1)
closepin.write(0)
time.sleep(0.1)
openpin.write(1)
time.sleep(0.1)
startpin.write(1)
pub.publish(msg)
elif req.service == 0:
clearpin.write(0)
time.sleep(0.1)
clearpin.write(1)
time.sleep(0.1)
startpin.write(0)
time.sleep(0.1)
closepin.write(1)
time.sleep(0.1)
openpin.write(0)
time.sleep(0.1)
startpin.write(1)
pub.publish(msg)
else:
return rs_home_appliancesResponse(0)
return rs_home_appliancesResponse(1)
def main():
print "Hello World"
###init mind wave mobile
cmd_release = "sudo rfcomm release " + RFCOMM_NUM
cmd_bind = "sudo rfcomm bind " + RFCOMM_NUM + " " + BT_ADDR
cmd_chmod = "sudo chmod a+rw /dev/rfcomm" + RFCOMM_NUM
print cmd_release + "\n", subprocess.check_output(cmd_release.split(" "))
print cmd_bind + "\n", subprocess.check_output(cmd_bind.split(" "))
print cmd_chmod + "\n", subprocess.check_output(cmd_chmod.split(" "))
dev = MindWaveMobile(DEV_PORT)
###init ROS
rospy.init_node('tms_ss_vs_mindwave')
db_pub = rospy.Publisher('tms_db_data', TmsdbStamped, queue_size=10)
r = rospy.Rate(1)
while not rospy.is_shutdown():
dev.update(
) # must update() many times in a loop because most of buffer data is stuff data
if dev.is_updated:
r.sleep()
dev.is_updated = False
###make json text
note_d = {
"meditation": str(dev.meditation),
"attention": str(dev.attention),
"poor_signal": str(dev.poor_signal)
}
note_j = json.dumps(note_d)
###regist to DB
msg = TmsdbStamped()
db = Tmsdb()
db.time = datetime.datetime.now().strftime("%Y-%m-%dT%H:%M:%S.%f")
db.name = "mindwavemobile"
db.id = 3017
db.sensor = 3017
db.place = 1001
db.note = note_j
msg.tmsdb.append(db)
msg.header.stamp = rospy.get_rostime() + rospy.Duration(
9 * 60 * 60)
db_pub.publish(msg)
###show messege
print "Med:",
print dev.meditation,
print " Att:",
print dev.attention,
print " signal:",
print dev.poor_signal
def call_dbwriter(self, msg):
print 'SleepState -> %s' % (msg)
note_d = {"sleepstate": str(msg)}
note_j = json.dumps(note_d)
msg = TmsdbStamped()
db = Tmsdb()
db.time = datetime.datetime.now().strftime("%Y-%m-%dT%H:%M:%S.%f")
db.name = "nursingcare_bed"
db.type = "sensor"
db.id = 3020
db.sensor = 3020
db.place = 5001
db.note = note_j
msg.tmsdb.append(db)
msg.header.stamp = rp.get_rostime() + rp.Duration(9 * 60 * 60)
self.db_pub.publish(msg)
return True
r = rospy.Rate(1)
while not rospy.is_shutdown():
r.sleep()
# ##get heartrate value
dev.write("G5\r") # get heartrate buffer[0~4]
time.sleep(0.5)
ret = dev.read(dev.inWaiting())
rate = ret.split(" ")[2]
print "heartrate:" + rate + " raw:" + ret
if int(rate) == 0:
print " failed to get heartrate value"
continue
# ##make json text
note_d = {"heartrate": rate}
# print json.dumps(note_d, indent=4)
note_j = json.dumps(note_d)
# ##regist to DB
msg = TmsdbStamped()
db = Tmsdb()
db.time = datetime.datetime.now().strftime("%Y-%m-%dT%H:%M:%S.%f")
db.name = "heartrate_sensor"
db.id = 3018
db.sensor = 3018
db.place = 1001
db.note = note_j
msg.tmsdb.append(db)
msg.header.stamp = rospy.get_rostime() + rospy.Duration(9 * 60 * 60)
db_pub.publish(msg)
def main():
print "Hello World"
rfidValue = dict()
rfidValue["E00401004E17F97A"] = 7001
rfidValue["E00401004E180E50"] = 7002
rfidValue["E00401004E180E58"] = 7003
rfidValue["E00401004E180E60"] = 7004
rfidValue["E00401004E180E68"] = 7005
rfidValue["E00401004E180EA0"] = 7006
rfidValue["E00401004E180EA8"] = 7007
rfidValue["E00401004E181C88"] = 7008
rfidValue["E00401004E181C87"] = 7009
rfidValue["E00401004E181C7F"] = 7010
rfidValue["E00401004E181C77"] = 7011
rfidValue["E00401004E181C3F"] = 7012
rfidValue["E00401004E181C37"] = 7013
rfidValue["E00401004E180E47"] = 7014
rfidValue["E00401004E180E3F"] = 7015
rfidValue["E00401004E180E37"] = 7016
rfidValue["E00401004E1805BD"] = 7017
rfidValue["E00401004E180585"] = 7018
rfidValue["E00401004E18057D"] = 7019
rfidValue["E00401004E17EF3F"] = 7020
rfidValue["E00401004E17EF37"] = 7021
rfidValue["E00401004E17EF2F"] = 7022
rfidValue["E00401004E17EF27"] = 7023
rfidValue["E00401004E17EEEF"] = 7024
rfidValue["E00401004E17EEE7"] = 7025
# init ROS
rospy.init_node('ibs', anonymous=True)
db_pub = rospy.Publisher('tms_db_data', TmsdbStamped, queue_size=10)
if not rospy.has_param('~idSensor'):
print "ros param 'idSensor' isn't exist"
if not rospy.has_param('~idPlace'):
print "ros param 'idPlace' isn't exist"
if not rospy.has_param('~z'):
print "ros param 'z' isn't exist"
if not rospy.has_param('~frame_id'):
print "ros param 'frame_id' isn't exist"
if not rospy.has_param('~loadcell_points/x'):
print "ros param 'loadcell_points/x' isn't exist"
if not rospy.has_param('~loadcell_points/y'):
print "ros param 'loadcell_points/y' isn't exist"
idSensor = rospy.get_param('~idSensor')
idPlace = rospy.get_param('~idPlace')
z = rospy.get_param('~z')
frame_id = rospy.get_param('~frame_id')
PORT_LC0 = rospy.get_param("~PORT_LC0", "/dev/ttyACM0")
PORT_TR = rospy.get_param("~PORT_TR", "/dev/ttyUSB0")
xpos0 = rospy.get_param('~loadcell_points/x', (0.0, 1000.0, 0.0, 1000.0))
ypos0 = rospy.get_param('~loadcell_points/y', (0.0, 0.0, 1000.0, 1000.0))
cmd_chmod = "sudo -S chmod a+rw " + PORT_LC0
print cmd_chmod + "\n", subprocess.check_output(cmd_chmod.split(" "))
cmd_chmod = "sudo -S chmod a+rw " + PORT_TR
print cmd_chmod + "\n", subprocess.check_output(cmd_chmod.split(" "))
# xpos0 = (16.0, 407.0, 16.0, 407.0)
# ypos0 = (16.0, 16.0, 244.0, 244.0)
cIntelCab = CIntelCab(
lc_port=PORT_LC0,
lc_xpos=xpos0,
lc_ypos=ypos0,
tr_port=PORT_TR,
tr_antenna=TR3_ANT1,
)
# 初回時の起動は多少時間がかかるためここで一回実行しておく
cIntelCab.UpdateObj()
# 計測開始
change_flag = False
index = 0
print "\nSTART"
r = rospy.Rate(10)
while not rospy.is_shutdown(): # vector 初期化
r.sleep()
# D_COUT(datetime.datetime.now().strftime("%Y-%m-%dT%H:%M:%S.%f")+"\n")
# cObj: CTagOBJ type
state, cObj = cIntelCab.UpdateObj()
# print "state:", state
if state == IC_OBJECT_STAY:
change_flag = False
elif state == IC_OBJECT_IN:
# Beep(2500,50)
print "\n\n IN : ",
index = int(len(cIntelCab.TagObjList) - 1)
cIntelCab.TagObjList[index].mName = cObj.mName
cIntelCab.TagObjList[index].mComment = cObj.mComment
change_flag = True
elif state == IC_OBJECT_MOVE:
# Beep(2500,50)
print "\n\nMOVE: ",
change_flag = True
elif state == IC_OBJECT_OUT:
# Beep(2500,50); Sleep(50); Beep(2500,50)
print "\n\n OUT: ",
change_flag = True
else:
change_flag = False
if change_flag:
change_flag = False
# 毎回初期化し，庫内にある物品だけ値を更新して送信する
msg = TmsdbStamped()
msg.header.frame_id = frame_id
msg.header.stamp = rospy.get_rostime() + rospy.Duration(
9 * 60 * 60)
for i in xrange(MAX_OBJECT_NUM):
time.sleep(0.001)
tmp_db = Tmsdb()
tmp_db.time = datetime.datetime.now().strftime(
"%Y-%m-%dT%H:%M:%S.%f")
tmp_db.id = i + 7001 # 物品IDは 7001 から
tmp_db.x = -1.0
tmp_db.y = -1.0
tmp_db.z = -1.0
tmp_db.place = idPlace
tmp_db.sensor = idSensor
tmp_db.state = NONE # 知的収納庫内に 0:存在しない, 1:存在する
msg.tmsdb.append(tmp_db)
for j in xrange(len(cIntelCab.TagObjList)):
cObj = cIntelCab.TagObjList[j]
vi = rfidValue[cObj.mUID] - 7001
time.sleep(0.001) # 1ms
msg.tmsdb[vi].time = datetime.datetime.now().strftime(
"%Y-%m-%dT%H:%M:%S.%f")
msg.tmsdb[vi].id = rfidValue[cObj.mUID]
msg.tmsdb[vi].state = EXIST
msg.tmsdb[vi].x = cObj.mX
msg.tmsdb[vi].y = cObj.mY
msg.tmsdb[vi].z = z
msg.tmsdb[vi].weight = cObj.mWeight
cIntelCab.PrintObjInfo()
db_pub.publish(msg)
return 0
raise Exception("Problem of connection")
self.data_pub = rospy.Publisher('tms_db_publisher', TmsdbStamped, queue_size=10)
self.sendDbCurrentInformation()
def sendDbCurrentInformation(self):
<<<<<<< HEAD
rate = rospy.Rate(100) # 100hz
=======
rate = rospy.Rate(100) # 100hz
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
while not rospy.is_shutdown():
temp_dbdata = Tmsdb()
current_environment_information = TmsdbStamped()
<<<<<<< HEAD
# cursor = db.now.find({'$or': [{'state': 1}, {'state': 2}]})
cursor = db.now.find()
=======
cursor = db.now.find({'state':1})
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
# print(cursor.count())
for doc in cursor:
del doc['_id']
temp_dbdata = db_util.document_to_msg(doc, Tmsdb)
current_environment_information.tmsdb.append(temp_dbdata)
# rospy.loginfo("send db data!")
self.data_pub.publish(current_environment_information)