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)
async def get_whs1_heartrate(self):
"""Whs1の心拍数を取得する
tms_ss_whs1/src/main.cppは、tmsdbのnoteにjsonとして各種データを保存しているので、
noteをjson.loadsで辞書型に変更して読みだす。
"""
rate = -1 # Error Number
data_tmsdb = Tmsdb()
data_tmsdb.id = TMSDB_ID
data_tmsdb.sensor = TMSDB_SENSOR
res = await self.call_dbreader(data_tmsdb)
if len(res) == 0:
return rate
note = res[0].note
whs1_params = json.loads(note)
rate = whs1_params["rate"]
wave = whs1_params["wave"]
# draw graph
plt.cla()
plt.title("tms_ss_whs1 Monitor")
plt.ylim(0,1023)
fig = plt.plot(wave)
plt.text(0, 850, "{0:.2f} bpm".format(rate), fontsize=32, color="green")
plt.pause(0.01)
return rate
def CallTmddbReader():
rospy.wait_for_service('tms_db_reader')
try:
read = rospy.ServiceProxy('tms_db_reader', TmsdbGetData)
r_req = Tmsdb()
r_req.id = 2003
r_req.sensor = 3001
result = read(r_req)
return result
except rospy.ServiceException, e:
print "Service call failed: %s" % e
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 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
async def get_whs1_heartrate(self):
"""Whs1の心拍数を取得する
tms_ss_whs1/src/main.cppは、tmsdbのnoteにjsonとして各種データを保存しているので、
noteをjson.loadsで辞書型に変更して読みだす。
"""
rate = -1 # Error Number
data_tmsdb = Tmsdb()
data_tmsdb.id = TMSDB_ID
data_tmsdb.sensor = TMSDB_SENSOR
res = await self.call_dbreader(data_tmsdb)
if len(res) == 0:
return rate
note = res[0].note
whs1_params = json.loads(note)
rate = whs1_params["rate"]
return rate
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
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
