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 getPose(self):
user_dbdata = Tmsdb()
user_dbdata.id = 1100
user_dbdata.state = 1
user_data = self.db_reader(user_dbdata)
if user_data is None:
print "failed to get user data"
# robot_dbdata = Tmsdb()
# robot_dbdata.id = 2012
# robot_dbdata.sensor = 3001
# robot_dbdata.state = 1
# robot_data = self.db_reader(robot_dbdata)
# if robot_data is None:
# print "failed to get robot data"
# print user_data.tmsdb[0].x
# distance = np.sqrt(np.square(self.user_place_x - user_data.tmsdb[0].x) + np.square(self.user_place_y - user_data.tmsdb[0].y))
# user_place_x = user_data.tmsdb[0].x
# user_place_y = user_data.tmsdb[0].y
# if distance > 2:
# return True
# else:
# return False
return user_data
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 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 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 alarm(self):
while True:
print "alarm"
self.speaker('\sound4')
time.sleep(1.5)
temp_dbdata = Tmsdb()
temp_dbdata.id = 1100
temp_dbdata.state = 1
target = self.db_reader(temp_dbdata)
if target is None:
self.announce(error_msg2)
return
print 'rp:' + str(target.rp)
if target.rp > -0.2:
break
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 graspSrvCallback(self, req):
rospy.loginfo("Received the service call!")
rospy.loginfo(req)
temp_dbdata = Tmsdb()
target = Tmsdb()
temp_dbdata.id = req.object_id
rospy.wait_for_service('tms_db_reader')
try:
tms_db_reader = rospy.ServiceProxy('tms_db_reader', TmsdbGetData)
res = tms_db_reader(temp_dbdata)
target = res.tmsdb[0]
except rospy.ServiceException, e:
print "Service call failed: %s" % e
self.shutdown()
def graspSrvCallback(self, req):
rospy.loginfo("Received the service call!")
rospy.loginfo(req)
temp_dbdata = Tmsdb()
target = Tmsdb()
temp_dbdata.id = req.object_id
rospy.wait_for_service('tms_db_reader')
try:
tms_db_reader = rospy.ServiceProxy('tms_db_reader', TmsdbGetData)
res = tms_db_reader(temp_dbdata)
target = res.tmsdb[0]
except rospy.ServiceException, e:
print "Service call failed: %s"%e
self.shutdown()
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
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
note_d = {"meditation": str(dev.meditation),
"attention": str(dev.attention),
"poor_signal": str(dev.poor_signal)}
note_j = json.dumps(note_d)
<<<<<<< HEAD
# regist to DB
=======
###regist to DB
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
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)
<<<<<<< HEAD
msg.header.stamp = rospy.get_rostime() + rospy.Duration(9 * 60 * 60)
db_pub.publish(msg)
# show messege
=======
msg.header.stamp = rospy.get_rostime()+rospy.Duration(9*60*60)
db_pub.publish(msg)
###show messege
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
def callback(self, data, id):
rospy.loginfo(str(id))
rospy.loginfo(data)
if id < 100:
if data.data not in trigger:
return
if self.gSpeech_launched == True:
return
self.gSpeech_launched = True
rospy.loginfo("call trigger on raspi:%d", id)
rospy.loginfo("kill julius!!")
self.julius_power(False)
self.speaker("\sound1")
time.sleep(0.5)
data = self.launch_gSpeech(id)
self.gSpeech_launched = False
if data.data == "":
tim = self.announce(error_msg0)
self.julius_power(True, tim.sec)
return
rospy.loginfo("get command!")
tokens = self.tok.tokenize(data.data.decode('utf-8'))
words = []
verb = ''
for token in tokens:
print token
if token.part_of_speech.split(',')[0] == u'動詞':
verb += token.base_form.encode('utf-8')
elif token.part_of_speech.split(',')[0] == u'名詞':
if token.base_form.encode('utf-8') != "*":
words.append(token.base_form.encode('utf-8'))
else:
words.append(token.surface.encode('utf-8'))
if verb != '':
words.append(verb)
if "言う" in words: #「〇〇に行って」が「〇〇に言って」と認識される
words.append("行く")
if "入る" in words: #同上
words.append("行く")
print str(words).decode('string-escape')
task_id = 0
robot_id = 0
object_id = 0
user_id = 1100
place_id = 0
announce = ""
robot_name = ""
object_name = ""
user_name = "太郎さん"
place_name = ""
task_dic = {}
robot_dic = {}
object_dic = {}
user_dic = {1100: "太郎さん"}
place_dic = {}
other_words = []
for word in words:
res = self.tag_reader(word)
if res is None:
tim = self.announce(error_msg2)
self.julius_power(True, tim.sec)
return
for target in res.tmsdb:
if target.type == 'task':
task_dic[target.id] = target.announce
elif target.type == 'robot':
robot_dic[target.id] = target.announce
elif target.type == 'object':
object_dic[target.id] = target.announce
elif target.type == 'person':
user_dic[target.id] = target.announce
elif target.type == 'furniture':
place_dic[target.id] = target.announce
else:
other_words.append(word)
print "task:" + str(task_dic)
print "robot:" + str(robot_dic)
print "object:" + str(object_dic)
print "user:"******"place:" + str(place_dic)
if len(task_dic) == 1:
task_id = task_dic.keys()[0]
announce = task_dic[task_id]
elif len(task_dic) > 1:
print "len(task_dic) > 1"
#未実装
task_id = task_dic.keys()[0]
announce = task_dic[task_id]
if task_id == 0:
print 'ask docomo Q&A api'
print data.data
urlenc = urllib.quote(data.data)
args = "curl -s 'https://api.apigw.smt.docomo.ne.jp/knowledgeQA/v1/ask?APIKEY=" + self.apikey + "&q=" + urlenc + "'"
ret = subprocess.check_output(shlex.split(args))
json_dict = json.loads(ret, "utf-8")
announce = "すみません、わかりませんでした。"
if "message" in json_dict:
print json_dict["message"]["textForDisplay"]
announce = json_dict["message"]["textForSpeech"]
tim = self.announce(announce)
self.julius_power(True, tim.sec)
return
elif task_id == 8100: #search_object
if len(object_dic) == 1:
object_id = object_dic.keys()[0]
object_name = object_dic[object_id]
elif len(object_dic) > 1:
print "len(object_dic) > 1"
#未実装
place_id = 0
place_name = ""
temp_dbdata = Tmsdb()
temp_dbdata.id = object_id
temp_dbdata.state = 1
target = self.db_reader(temp_dbdata)
if target is None:
tim = self.announce(error_msg2)
self.julius_power(True, tim.sec)
return
place_id = target.place
temp_dbdata = Tmsdb()
temp_dbdata.id = place_id + sid
target = self.db_reader(temp_dbdata)
if target is None:
tim = self.announce(error_msg2)
self.julius_power(True, tim.sec)
return
place_name = target.announce
if object_name == "" or place_name == "":
tim = self.announce(error_msg1)
self.julius_power(True, tim.sec)
return
anc_list = announce.split("$")
announce = ""
for anc in anc_list:
if anc == "object":
announce += object_name
elif anc == "place":
announce += place_name
else:
announce += anc
tim = self.announce(announce)
self.julius_power(True, tim.sec)
elif task_id == 8101: #weather_forecast
place = "福岡市"
date = ""
weather = ""
for word in other_words:
if word in ['今日', '明日', '明後日', 'あさって']:
date = word
if date == "":
tim = self.announce(error_msg1)
self.julius_power(True, tim.sec)
return
args = "curl -s http://weather.livedoor.com/forecast/webservice/json/v1\?city\=400010"
ret = subprocess.check_output(shlex.split(args))
json_dict = json.loads(ret, "utf-8")
if "forecasts" in json_dict:
if date == '今日':
weather = json_dict["forecasts"][0]["telop"].encode(
'utf-8')
elif date == '明日':
weather = json_dict["forecasts"][1]["telop"].encode(
'utf-8')
elif date == '明後日' or date == 'あさって':
weather = json_dict["forecasts"][2]["telop"].encode(
'utf-8')
if weather == "":
tim = self.announce(error_msg1)
self.julius_power(True, tim.sec)
return
anc_list = announce.split("$")
announce = ""
for anc in anc_list:
if anc == "place":
announce += place
elif anc == "date":
announce += date
elif anc == "weather":
announce += weather
else:
announce += anc
tim = self.announce(announce)
self.julius_power(True, tim.sec)
elif task_id == 8102: #set_alarm
today = datetime.datetime.today()
print 'now:' + today.strftime("%Y/%m/%d %H:%M:%S")
if today.hour < 6:
date = 0
else:
date = 1
hour = -1
minute = 0
for i, word in enumerate(other_words):
if word == "今日":
date = 0
elif word == "明日" and today.hour > 6:
date = 1
elif word in ["時", "時半"] and i > 0:
if words[i - 1].isdigit():
hour = int(words[i - 1])
if word == "時半":
minute = 30
if i > 1 and words[i - 2] == "午後" and hour <= 12:
hour += 12
elif i > 1 and words[
i - 2] == "夜" and hour <= 12 and hour >= 6:
hour += 12
elif word == "分":
if words[i - 1].isdigit():
minute = int(words[i - 1])
print "d:" + str(date) + " h:" + str(hour) + " m:" + str(minute)
if hour == -1:
tim = self.announce(error_msg1)
self.julius_power(True, tim.sec)
return
tgt_tim = datetime.datetime(today.year, today.month, today.day,
hour, minute, 0, 0)
tgt_time += datetime.timedelta(1)
print 'tgt_time:' + tgt_time.strftime("%Y/%m/%d %H:%M:%S")
offset = tgt_time - today
print 'offset_sec:' + str(offset.seconds)
if offset.seconds < 0:
tim = self.announce(error_msg1)
self.julius_power(True, tim.sec)
return
self.timer = threading.Timer(
15, self.alarm) #(offset.seconds,self.alarm)
self.timer.start()
anc_list = announce.split("$")
announce = ""
for anc in anc_list:
if anc == "date":
announce += str(tgt_time.month) + "月" + str(
tgt_time.day) + "日"
elif anc == "time":
announce += str(tgt_time.hour) + "時" + str(
tgt_time.minute) + "分"
else:
announce += anc
tim = self.announce(announce)
self.julius_power(True, tim.sec)
elif task_id == 8103:
url = "http://192.168.100.101/codemari_kyudai/CodemariServlet?deviceID=9999&locale=ja&cmd=%251CFLP%"
onoff = ""
if "つける" in other_words:
print "light on"
onoff = "付け"
url += "2003"
elif "消す" in other_words:
print "light off"
onoff = "消し"
url += "2005"
else:
tim = self.announce(error_msg1)
self.julius_power(True, tim.sec)
return
anc_list = announce.split("$")
announce = ""
for anc in anc_list:
if anc == "onoff":
announce += onoff
else:
announce += anc
tim = self.announce(announce)
self.julius_power(True, tim.sec)
res = requests.get(url)
print res.text
elif task_id == 8104:
msg = Int32()
cmd = ""
if "起こす" in words:
msg.data = 1
cmd = "を起こし"
elif "寝かせる" in words:
msg.data = 2
cmd = "を寝かせ"
elif "立てる" in words:
msg.data = 3
cmd = "を立て"
elif "倒す" in words:
msg.data = 4
cmd = "を倒し"
elif "上げる" in words:
msg.data = 7
cmd = "の高さを上げ"
elif "下げる" in words:
msg.data = 8
cmd = "の高さを下げ"
else:
tim = self.announce(error_msg1)
self.julius_power(True, tim.sec)
return
anc_list = announce.split("$")
announce = ""
for anc in anc_list:
if anc == "cmd":
announce += cmd
else:
announce += anc
tim = self.announce(announce)
self.julius_power(True, tim.sec)
self.bed_pub.publish(msg)
else: #robot_task
anc_list = announce.split("$")
announce = ""
for anc in anc_list:
if anc == "robot":
if len(robot_dic) == 1:
robot_id = robot_dic.keys()[0]
robot_name = robot_dic[robot_id]
elif len(robot_dic) > 1:
print "len(robot_dic) > 1"
#未実装
if robot_id == 0:
tim = self.announce(error_msg1)
self.julius_power(True, tim.sec)
return
announce += robot_name
elif anc == "object":
if len(object_dic) == 1:
object_id = object_dic.keys()[0]
object_name = object_dic[object_id]
elif len(object_dic) > 1:
print "len(object_dic) > 1"
#未実装
if object_id == 0:
tim = self.announce(error_msg1)
self.julius_power(True, tim.sec)
return
announce += object_name
elif anc == "user":
if len(user_dic) == 1:
user_id = user_dic.keys()[0]
user_name = user_dic[user_id]
elif len(user_dic) > 1:
print "len(user_dic) > 1"
#未実装
if user_id == 0:
tim = self.announce(error_msg1)
self.julius_power(True, tim.sec)
return
announce += user_name
elif anc == "place":
if len(place_dic) == 1:
place_id = place_dic.keys()[0]
place_name = place_dic[place_id]
elif len(place_dic) > 1:
print "len(place_dic) > 1"
#未実装
if place_id == 0:
tim = self.announce(error_msg1)
self.julius_power(True, tim.sec)
return
announce += place_name
else:
announce += anc
print 'send command'
try:
rospy.wait_for_service('tms_ts_master', timeout=1.0)
except rospy.ROSException:
print "tms_ts_master is not work"
try:
tms_ts_master = rospy.ServiceProxy('tms_ts_master', ts_req)
res = tms_ts_master(0, task_id, robot_id, object_id, user_id,
place_id, 0)
print res
except rospy.ServiceException as e:
print "Service call failed: %s" % e
tim = self.announce(announce)
self.julius_power(True, tim.sec)
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)
async def callback(self, data, id=0):
"""[tms_db_reader] マイクに向かって何か発音したときのコールバック関数
"""
self.get_logger().info(data.data)
if id < 100:
if data.data not in trigger:
return
if self.gSpeech_launched == True:
return
self.gSpeech_launched = True
# self.get_logger().info("call trigger on raspi:%d" % id)
self.get_logger().info("[To tms_ur_listener_client] kill julius.")
self.julius_power(False)
self.speaker("\sound1")
time.sleep(0.5)
data = await self.call_gspeech(id)
self.gSpeech_launched = False
if data.data == "":
tim = await self.call_speaker(error_msg0)
self.julius_power(True, tim)
return
self.get_logger().info("[From tms_ur_listener_client] get command.")
tokens = self.tok.tokenize(data.data)
words = []
verb = ''
for token in tokens:
print('token: ' + str(token))
if token.part_of_speech.split(',')[0] == u'動詞':
verb += token.base_form
elif token.part_of_speech.split(',')[0] == u'名詞':
if token.base_form != "*":
words.append(token.base_form)
else:
words.append(token.surface)
if verb != '':
words.append(verb)
if "言う" in words: #「〇〇に行って」が「〇〇に言って」と認識される
words.append("行く")
if "入る" in words: #同上
words.append("行く")
print("words:")
print(words)
task_id = 0
robot_id = 0
object_id = 0
user_id = 0 #1100
place_id = 0
announce = ""
task_name = "" #for remote task
robot_name = ""
object_name = ""
user_name = "" #"太郎さん"
place_name = ""
task_dic = {}
robot_dic = {}
object_dic = {}
user_dic = {} #{1100:"太郎さん"}
place_dic = {}
other_words = []
for word in words:
res = await self.tag_reader(word)
if res is None:
tim = await self.call_speaker(error_msg2)
self.julius_power(True, tim)
return
for target in res:
print(target)
if target.type == 'task':
if target.id == 8102: # 「起こす」でタイマーが起動してしまい、ベッドを起こしてが聞かない
continue
task_dic[target.id] = target.announce
elif target.type == 'robot':
robot_dic[target.id] = target.announce
elif target.type == 'object':
object_dic[target.id] = target.announce
elif target.type == 'person':
user_dic[target.id] = target.announce
elif target.type == 'furniture':
place_dic[target.id] = target.announce
else:
other_words.append(word)
print("task:" + str(task_dic))
print("robot:" + str(robot_dic))
print("object:" + str(object_dic))
print("user:"******"place:" + str(place_dic))
if len(task_dic) == 1:
task_id = list(task_dic.keys())[0]
announce = task_dic[task_id]
elif len(task_dic) > 1:
print("len(task_dic) > 1")
#「ベッド」がタスクとして認識されてしまい、「ベッドに行って」が失敗してしまう
# => ロボットによるタスクを優先するため、task_id が小さい方を優先(要検討)
task_id = min(task_dic.keys())
announce = task_dic[task_id]
print("task_id : " + str(task_id))
# task_id = 8101 ##################################DEBUG####################
for word in words:
other_words.append(word)
if task_id == 0:
print('ask docomo Q&A api')
print(data.data)
announce = "jsonが帰ってきません"
urlenc = urllib.parse.quote(data.data)
args = "curl -s 'https://api.apigw.smt.docomo.ne.jp/knowledgeQA/v1/ask?APIKEY=" + self.apikey + "&q=" + urlenc + "'"
ret = subprocess.check_output(shlex.split(args))
print(ret)
if ret != b'': # json return
json_dict = json.loads(ret)
print(json_dict)
if "message" in json_dict: # docomo api return message
print(json_dict["message"]["textForDisplay"])
announce = json_dict["message"]["textForSpeech"]
else:
announce = "apiキーが違うようです"
tim = await self.call_speaker(announce)
self.julius_power(True, tim)
return
elif task_id == 8100: #search_object
if len(object_dic) == 1:
object_id = object_dic.keys()[0]
object_name = object_dic[object_id]
elif len(object_dic) > 1:
print("len(object_dic) > 1")
#未実装
else:
# self.ask_remote(words, "search_object")
return
place_id = 0
place_name = ""
temp_dbdata = Tmsdb()
temp_dbdata.id = object_id
temp_dbdata.state = 1
#target = await self.call_dbreader(temp_dbdata)
db_target = await self.call_dbreader(temp_dbdata)
target = db_target[0]
if target is None:
tim = await self.call_speaker(error_msg2)
self.julius_power(True, tim)
return
place_id = target.place
temp_dbdata = Tmsdb()
temp_dbdata.id = place_id + sid
db_target = await self.call_dbreader(temp_dbdata)
target = db_target[0]
#target = await self.call_dbreader(temp_dbdata)
if target is None:
tim = await self.call_speaker(error_msg2)
self.julius_power(True, tim)
return
place_name = target.announce
if object_name == "" or place_name == "":
tim = await self.call_speaker(error_msg1)
self.julius_power(True, tim)
return
anc_list = announce.split("$")
announce = ""
for anc in anc_list:
if anc == "object":
announce += object_name
elif anc == "place":
announce += place_name
else:
announce += anc
tim = await self.call_speaker(announce)
self.julius_power(True, tim)
elif task_id == 8101: #weather_forecast
place = "福岡市"
date = ""
weather = ""
for word in other_words:
if word in ['今日', '明日', '明後日', 'あさって']:
date = word
if date == "":
tim = await self.call_speaker(error_msg1)
self.julius_power(True, tim)
return
args = "curl -s http://weather.livedoor.com/forecast/webservice/json/v1\?city\=400010"
ret = subprocess.check_output(shlex.split(args))
json_dict = json.loads(ret)
pprint.pprint(json_dict)
if "forecasts" in json_dict:
if date == '今日':
weather = json_dict["forecasts"][0]["telop"]
elif date == '明日':
weather = json_dict["forecasts"][1]["telop"]
elif date == '明後日' or date == 'あさって':
weather = json_dict["forecasts"][2]["telop"]
if weather == "":
tim = await self.call_speaker(error_msg1)
self.julius_power(True, tim)
return
# anc_list = announce.split("$")
# announce = ""
# for anc in anc_list:
# if anc == "place":
# announce += place
# elif anc == "date":
# announce += date
# elif anc == "weather":
# announce += weather
# else:
# announce += anc
# tim = await self.call_speaker(announce)
tim = await self.call_speaker(place + "の" + date + "の天気は" +
weather + "です")
self.julius_power(True, tim)
elif task_id == 8102: #set_alarm
today = datetime.datetime.today()
print('now:' + today.strftime("%Y/%m/%d %H:%M:%S"))
if today.hour < 6:
date = 0
else:
date = 1
hour = -1
minute = 0
for i, word in enumerate(other_words):
if word == "今日":
date = 0
elif word == "明日" and today.hour > 6:
date = 1
elif word in ["時", "時半"] and i > 0:
if words[i - 1].isdigit():
hour = int(words[i - 1])
if word == "時半":
minute = 30
if i > 1 and words[i - 2] == "午後" and hour <= 12:
hour += 12
elif i > 1 and words[
i - 2] == "夜" and hour <= 12 and hour >= 6:
hour += 12
elif word == "分":
if words[i - 1].isdigit():
minute = int(words[i - 1])
print("d:" + str(date) + " h:" + str(hour) + " m:" + str(minute))
if hour == -1:
tim = self.announce(error_msg1)
self.julius_power(True, tim)
return
tgt_tim = datetime.datetime(today.year, today.month, today.day,
hour, minute, 0, 0)
tgt_time += datetime.timedelta(1)
print('tgt_time:' + tgt_time.strftime("%Y/%m/%d %H:%M:%S"))
offset = tgt_time - today
print('offset_sec:' + str(offset.seconds))
if offset.seconds < 0:
tim = await self.call_speaker(error_msg1)
self.julius_power(True, tim)
return
self.timer = threading.Timer(
15, self.alarm) #(offset.seconds,self.alarm)
self.timer.start()
anc_list = announce.split("$")
announce = ""
for anc in anc_list:
if anc == "date":
announce += str(tgt_time.month) + "月" + str(
tgt_time.day) + "日"
elif anc == "time":
announce += str(tgt_time.hour) + "時" + str(
tgt_time.minute) + "分"
else:
announce += anc
tim = await self.call_speaker(announce)
self.julius_power(True, tim)
# 電気をつけて / 消して ----------
elif task_id == 8103:
url = "http://192.168.100.101/codemari_kyudai/CodemariServlet?deviceID=9999&locale=ja&cmd=%251CFLP%"
onoff = ""
if "つける" in other_words:
print("light on")
onoff = "付け"
url += "2003"
elif "消す" in other_words:
print("light off")
onoff = "消し"
url += "2005"
else:
tim = await self.call_speaker(error_msg1)
self.julius_power(True, tim)
return
anc_list = announce.split("$")
announce = ""
for anc in anc_list:
if anc == "onoff":
announce += onoff
else:
announce += anc
tim = await self.call_speaker(announce)
self.julius_power(True, tim)
res = requests.get(url)
print(res.text)
# tms_rc_bed ベッドを起こして / 寝かせて ----------
elif task_id == 8104:
control_number = 0
cmd = ""
if "起こす" in words:
control_number = 1
cmd = "を起こし"
elif "寝かせる" in words:
control_number = 2
cmd = "を寝かせ"
elif "立てる" in words:
control_number = 3
cmd = "を立て"
elif "倒す" in words:
control_number = 4
cmd = "を倒し"
elif "上げる" in words:
control_number = 7
cmd = "の高さを上げ"
elif "下げる" in words:
control_number = 8
cmd = "の高さを下げ"
else:
tim = await self.call_speaker(error_msg1)
self.julius_power(True, tim)
return
anc_list = announce.split("$")
announce = ""
for anc in anc_list:
if anc == "cmd":
announce += cmd
else:
announce += anc
tim = await self.call_speaker(announce)
self.julius_power(True, tim)
ws = create_connection('ws://192.168.4.131:9989') # open socket
ws.send(str(control_number)) # send to socket
ws.close() # close socket
elif task_id == 8105:
print(user_dic)
if len(user_dic) == 1:
user_id = user_dic.keys()[0]
user_name = user_dic[user_id]
elif len(user_dic) > 1:
print("len(user_dic) > 1")
#未実装
else:
# self.ask_remote(words, "get_health_condition")
return
place_id = 0
place_name = ""
temp_dbdata = Tmsdb()
temp_dbdata.id = user_id
temp_dbdata.state = 1
#target = await self.call_dbreader(temp_dbdata)
db_target = await self.call_dbreader(temp_dbdata)
target = db_target[0]
if target is None:
tim = await self.call_speaker(error_msg2)
self.julius_power(True, tim)
return
note = json.loads(target.note)
rate = note["rate"]
anc_list = announce.split("$")
announce = ""
for anc in anc_list:
if anc == "user":
announce += user_name
elif anc == "data":
announce += str(rate)
else:
announce += anc
tim = await self.call_speaker(announce)
self.julius_power(True, tim)
else: #robot_task
if task_id == 8009:
talk = True
else:
talk = False
task_announce_list = announce.split(";")
for i in range(len(task_announce_list)):
anc_list = task_announce_list[i].split("$")
announce = ""
task_flag = 0
for anc in anc_list:
if anc == "robot":
if len(robot_dic) == 1:
robot_id = robot_dic.keys()[0]
robot_name = robot_dic[robot_id]
elif len(robot_dic) > 1:
print("len(robot_dic) > 1")
#未実装
if robot_id == 0:
if i == len(task_announce_list) - 1:
# self.ask_remote(words, "robot_task",talk)
return
else:
task_flag = 1
announce += robot_name
elif anc == "object":
if len(object_dic) == 1:
object_id = object_dic.keys()[0]
object_name = object_dic[object_id]
elif len(object_dic) > 1:
print("len(object_dic) > 1")
#未実装
if object_id == 0:
if i == len(task_announce_list) - 1:
# self.ask_remote(words, "robot_task",talk)
return
else:
task_flag = 1
announce += object_name
elif anc == "user":
if len(user_dic) == 1:
user_id = user_dic.keys()[0]
user_name = user_dic[user_id]
elif len(user_dic) > 1:
print("len(user_dic) > 1")
#未実装
if user_id == 0:
if i == len(task_announce_list) - 1:
# self.ask_remote(words, "robot_task",talk)
return
else:
task_flag = 1
announce += user_name
elif anc == "place":
if len(place_dic) == 1:
place_id = place_dic.keys()[0]
place_name = place_dic[place_id]
elif len(place_dic) > 1:
print("len(place_dic) > 1")
#未実装
if place_id == 0:
if i == len(task_announce_list) - 1:
# self.ask_remote(words, "robot_task",talk)
return
else:
task_flag = 1
announce += place_name
else:
announce += anc
if task_flag == 1:
continue
print('send command')
try:
rospy.wait_for_service('tms_ts_master', timeout=1.0)
except rospy.ROSException:
print("tms_ts_master is not work")
try:
tms_ts_master = rospy.ServiceProxy(
'tms_ts_master', ts_req)
res = tms_ts_master(0, task_id, robot_id, object_id,
user_id, place_id, 0)
print(res)
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
tim = await self.call_speaker(announce)
self.julius_power(True, tim)
return
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)
async def julius_callback(self, julius_data,id=0):
"""[tms_ur_listener_client] マイクに向かって何か発音したときのコールバック関数
"""
self.get_logger().info(julius_data.data)
### 1. 「ROS-TMS」とマイクに向かって発音していたら、子機上でGoogle Speech APIを立ち上げる
if julius_data.data not in trigger: # ROS-TMSと発音してなかったら終了
return
if self.gSpeech_launched == True: # すでに端末がGSpeechを起動していたら終了
return
self.gSpeech_launched = True
self.get_logger().info("[tms_ur_listener_client] << kill julius.")
self.julius_power(False)
self.speaker("\sound1") # 起動音
time.sleep(0.5)
gspeech_data = await self.call_gspeech(id)
self.gSpeech_launched = False
### 2-1. Google Speech APIが認識に失敗した場合、失敗音を出して終了
if gspeech_data.data == "":
tim = await self.call_speaker(error_msg0)
self.julius_power(True,tim)
return
### 2-2. Google Speech APIが認識に成功した場合、単語ごとに分解しwordsに格納
self.get_logger().info("[tms_ur_listener_client] >> get command.")
words = self.command_to_words(gspeech_data.data)
print(f'words: {words}')
### 3. wordsのwordをタグとしてDBを検索し、各辞書に格納
task_dic = {} # type == "task"
robot_dic = {} # type == "robot"
object_dic = {} # type == "object"
person_dic = {} # type == "person"
furniture_dic = {} # type == "furniture"
other_words = {} # type != ALL_TYPES
for word in words:
responses = await self.tag_reader(word)
for target in responses:
print(target)
if target.type == 'task':
if target.id == 8102: # 「起こす」でタイマーが起動してしまい、ベッドを起こしてが聞かない
continue
task_dic[target.id] = target.announce
elif target.type == 'robot':
robot_dic[target.id] = target.announce
elif target.type == 'object':
object_dic[target.id] = target.announce
elif target.type == 'person':
person_dic[target.id] = target.announce
elif target.type == 'furniture':
furniture_dic[target.id] = target.announce
else:
other_words.append(word)
print("task:" + str(task_dic))
print("robot:" + str(robot_dic))
print("object:" + str(object_dic))
print("person:" + str(person_dic))
print("place:" + str(furniture_dic))
### 4. id == "task"のものがあった場合,task_idを決定
task_id = 0
if len(task_dic) == 1:
task_id = list(task_dic.keys())[0]
announce = task_dic[task_id]
elif len(task_dic) > 1:
print("len(task_dic) > 1")
#「ベッド」がタスクとして認識されてしまい、「ベッドに行って」が失敗してしまう
# => ロボットによるタスクを優先するため、task_id が小さい方を優先(要検討)
task_id = min(task_dic.keys())
announce = task_dic[task_id]
print("task_id : " + str(task_id))
# task_id = 8101 ##################################DEBUG####################
for word in words:
other_words.append(word)
if task_id == 0:
print('ask docomo Q&A api')
print(data.data)
announce = "すみません、よくわかりませんでした"
urlenc = urllib.parse.quote(data.data)
args = "curl -s 'https://api.apigw.smt.docomo.ne.jp/knowledgeQA/v1/ask?APIKEY=" + self.apikey + "&q=" + urlenc + "'"
ret = subprocess.check_output(shlex.split(args))
print(ret)
if ret != b'': # json return
json_dict = json.loads(ret)
print(json_dict)
if "message" in json_dict: # docomo api return message
print(json_dict["message"]["textForDisplay"])
announce = json_dict["message"]["textForSpeech"]
else :
announce = "apiキーが違うようです"
tim = await self.call_speaker(announce)
self.julius_power(True,tim)
return
# 「今日の天気」、「明日の天気」、「明後日の天気」----------
elif task_id == 8101:
await self.ask_to_weather_api(words)
# アラーム ------------
elif task_id == 8102:
pass # 未実装
self.julius_power(True)
# 電気をつけて / 消して ----------
elif task_id == 8103:
await self.control_roomlights(words)
# tms_rc_bed ベッドを起こして / 寝かせて ----------
elif task_id == 8104:
await self.control_tms_rc_bed(words)
# 物体探索「どこ」、「ある」 ----------
elif task_id == 8100:
if len(object_dic) == 1:
object_id = object_dic.keys()[0]
object_name = object_dic[object_id]
elif len(object_dic) > 1:
print("len(object_dic) > 1")
#未実装
else:
# self.ask_remote(words, "search_object")
return
place_id = 0
place_name = ""
temp_dbdata = Tmsdb()
temp_dbdata.id = object_id
temp_dbdata.state = 1
#target = await self.call_dbreader(temp_dbdata)
db_target = await self.call_dbreader(temp_dbdata)
target = db_target[0]
if target is None:
tim = await self.call_speaker(error_msg2)
self.julius_power(True,tim)
return
place_id = target.place
temp_dbdata = Tmsdb()
temp_dbdata.id = place_id + sid
db_target =await self.call_dbreader(temp_dbdata)
target = db_target[0]
#target = await self.call_dbreader(temp_dbdata)
if target is None:
tim = await self.call_speaker(error_msg2)
self.julius_power(True,tim)
return
place_name = target.announce
if object_name == "" or place_name == "":
tim = await self.call_speaker(error_msg1)
self.julius_power(True,tim)
return
anc_list = announce.split("$")
announce = ""
for anc in anc_list:
if anc == "object":
announce += object_name
elif anc == "place":
announce += place_name
else:
announce += anc
tim = await self.call_speaker(announce)
self.julius_power(True,tim)
elif task_id == 8101: #weather_forecast
place = "福岡市"
date = ""
weather = ""
for word in other_words:
if word in ['今日','明日','明後日','あさって']:
date = word
if date == "":
tim = await self.call_speaker(error_msg1)
self.julius_power(True,tim)
return
args = "curl -s http://weather.livedoor.com/forecast/webservice/json/v1\?city\=400010"
ret = subprocess.check_output(shlex.split(args))
json_dict = json.loads(ret)
pprint.pprint(json_dict)
if "forecasts" in json_dict:
if date == '今日':
weather = json_dict["forecasts"][0]["telop"]
elif date == '明日':
weather = json_dict["forecasts"][1]["telop"]
elif date == '明後日' or date == 'あさって':
weather = json_dict["forecasts"][2]["telop"]
if weather == "":
tim = await self.call_speaker(error_msg1)
self.julius_power(True, 5.0) #tim)
return
# anc_list = announce.split("$")
# announce = ""
# for anc in anc_list:
# if anc == "place":
# announce += place
# elif anc == "date":
# announce += date
# elif anc == "weather":
# announce += weather
# else:
# announce += anc
# tim = await self.call_speaker(announce)
tim = await self.call_speaker(place + "の" +date + "の天気は" + weather + "です")
self.julius_power(True,tim)
elif task_id == 8102: #set_alarm
today = datetime.datetime.today()
print('now:' + today.strftime("%Y/%m/%d %H:%M:%S"))
if today.hour < 6:
date = 0
else:
date = 1
hour = -1
minute = 0
for i,word in enumerate(other_words):
if word == "今日":
date = 0
elif word == "明日" and today.hour > 6:
date = 1
elif word in ["時","時半"] and i>0:
if words[i-1].isdigit():
hour = int(words[i-1])
if word == "時半":
minute = 30
if i>1 and words[i-2] == "午後" and hour <=12:
hour += 12
elif i>1 and words[i-2] == "夜" and hour <=12 and hour>=6:
hour += 12
elif word == "分":
if words[i-1].isdigit():
minute = int(words[i-1])
print("d:"+str(date)+" h:"+str(hour)+" m:"+str(minute))
if hour == -1:
tim = self.announce(error_msg1)
self.julius_power(True,tim)
return
tgt_tim = datetime.datetime(today.year,today.month,today.day,hour,minute,0,0)
tgt_time += datetime.timedelta(1)
print('tgt_time:' + tgt_time.strftime("%Y/%m/%d %H:%M:%S"))
offset = tgt_time - today
print('offset_sec:' + str(offset.seconds))
if offset.seconds < 0:
tim = await self.call_speaker(error_msg1)
self.julius_power(True,tim)
return
self.timer = threading.Timer(15,self.alarm)#(offset.seconds,self.alarm)
self.timer.start()
anc_list = announce.split("$")
announce = ""
for anc in anc_list:
if anc == "date":
announce += str(tgt_time.month)+"月"+str(tgt_time.day)+"日"
elif anc == "time":
announce += str(tgt_time.hour)+"時"+str(tgt_time.minute)+"分"
else:
announce += anc
tim = await self.call_speaker(announce)
self.julius_power(True,tim)
elif task_id == 8103:
url = "http://192.168.100.101/codemari_kyudai/CodemariServlet?deviceID=9999&locale=ja&cmd=%251CFLP%"
onoff = ""
if "つける" in other_words:
print("light on")
onoff = "付け"
url += "2003"
elif "消す" in other_words:
print("light off")
onoff = "消し"
url += "2005"
else:
tim = await self.call_speaker(error_msg1)
self.julius_power(True,tim)
return
anc_list = announce.split("$")
announce = ""
for anc in anc_list:
if anc == "onoff":
announce += onoff
else:
announce += anc
tim = await self.call_speaker(announce)
self.julius_power(True,tim)
res = requests.get(url)
print(res.text)
elif task_id == 8104:
control_number = 0
cmd = ""
if "起こす" in words:
control_number = 1
cmd = "を起こし"
elif "寝かせる" in words:
control_number = 2
cmd = "を寝かせ"
elif "立てる" in words:
control_number = 3
cmd = "を立て"
elif "倒す" in words:
control_number = 4
cmd = "を倒し"
elif "上げる" in words:
control_number = 7
cmd = "の高さを上げ"
elif "下げる" in words:
control_number = 8
cmd = "の高さを下げ"
else:
tim = await self.call_speaker(error_msg1)
self.julius_power(True,tim)
return
anc_list = announce.split("$")
announce = ""
for anc in anc_list:
if anc == "cmd":
announce += cmd
else:
announce += anc
tim = await self.call_speaker(announce)
self.julius_power(True,tim)
ws = create_connection('ws://192.168.4.131:9989') # open socket
ws.send(str(control_number)) # send to socket
ws.close() # close socket
elif task_id == 8105:
print(user_dic)
if len(user_dic) == 1:
user_id = user_dic.keys()[0]
user_name = user_dic[user_id]
elif len(user_dic) > 1:
print("len(user_dic) > 1")
#未実装
else:
# self.ask_remote(words, "get_health_condition")
return
place_id = 0
place_name = ""
temp_dbdata = Tmsdb()
temp_dbdata.id = user_id
temp_dbdata.state = 1
#target = await self.call_dbreader(temp_dbdata)
db_target = await self.call_dbreader(temp_dbdata)
target = db_target[0]
if target is None:
tim = await self.call_speaker(error_msg2)
self.julius_power(True,tim)
return
note = json.loads(target.note)
rate = note["rate"]
anc_list = announce.split("$")
announce = ""
for anc in anc_list:
if anc == "user":
announce += user_name
elif anc == "data":
announce += str(rate)
else:
announce += anc
tim = await self.call_speaker(announce)
self.julius_power(True,tim)
else: #robot_task
if task_id ==8009:
talk = True
else:
talk = False
task_announce_list = announce.split(";")
for i in range(len(task_announce_list)):
anc_list = task_announce_list[i].split("$")
announce = ""
task_flag = 0
for anc in anc_list:
if anc == "robot":
if len(robot_dic) == 1:
robot_id = robot_dic.keys()[0]
robot_name = robot_dic[robot_id]
elif len(robot_dic) > 1:
print("len(robot_dic) > 1")
#未実装
if robot_id==0:
if i == len(task_announce_list) - 1:
# self.ask_remote(words, "robot_task",talk)
return
else:
task_flag = 1
announce += robot_name
elif anc == "object":
if len(object_dic) == 1:
object_id = object_dic.keys()[0]
object_name = object_dic[object_id]
elif len(object_dic) > 1:
print("len(object_dic) > 1")
#未実装
if object_id==0:
if i == len(task_announce_list) - 1:
# self.ask_remote(words, "robot_task",talk)
return
else:
task_flag = 1
announce += object_name
elif anc == "user":
if len(person_dic) == 1:
user_id = person_dic.keys()[0]
user_name = person_dic[user_id]
elif len(person_dic) > 1:
print("len(person_dic) > 1")
#未実装
if user_id==0:
if i == len(task_announce_list) - 1:
# self.ask_remote(words, "robot_task",talk)
return
else:
task_flag = 1
announce += user_name
elif anc == "place":
if len(furniture_dic) == 1:
place_id = furniture_dic.keys()[0]
place_name = furniture_dic[place_id]
elif len(furniture_dic) > 1:
print("len(furniture_dic) > 1")
#未実装
if place_id==0:
if i == len(task_announce_list) - 1:
# self.ask_remote(words, "robot_task",talk)
return
else:
task_flag = 1
announce += place_name
else:
announce += anc
if task_flag == 1:
continue
print('send command')
try:
rospy.wait_for_service('tms_ts_master', timeout=1.0)
except rospy.ROSException:
print("tms_ts_master is not work")
try:
tms_ts_master = rospy.ServiceProxy('tms_ts_master',ts_req)
res = tms_ts_master(0,task_id,robot_id,object_id,user_id,place_id,0)
print(res)
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
tim = await self.call_speaker(announce)
self.julius_power(True,tim)
return
def pickSrvCallback(self, req):
rospy.loginfo("Received the service call!")
rospy.loginfo(req)
temp_dbdata = Tmsdb()
target = Tmsdb()
temp_dbdata.id = req.object_id
temp_dbdata.state = 1
rospy.wait_for_service('tms_db_reader')
try:
tms_db_reader = rospy.ServiceProxy('tms_db_reader', TmsdbGetData)
res = tms_db_reader(temp_dbdata)
target = res.tmsdb[0]
except rospy.ServiceException as e:
print "Service call failed: %s" % e
self.shutdown()
print(target.name)
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
#self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, 10)
# Create a publisher for displaying gripper poses
#self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped,
10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
print("***** set pose reference frame")
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
arm.set_planning_time(5)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(0.05)
target_id = str(req.object_id)
scene.remove_world_object(target_id)
scene.remove_attached_object(GRIPPER_FRAME, target_id)
rospy.sleep(0.05)
arm.set_named_target('l_arm_init')
arm.go()
print("***** Set initial pose defined SRDF")
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(0.05)
if target.offset_x < 0.01 and target.offset_y < 0.01 and target.offset_x < 0.01:
target.offset_x = 0.033
target.offset_y = 0.033
target.offset_z = 0.083
target_size = [(target.offset_x * 2), (target.offset_y * 2),
(target.offset_z * 2)]
# target_size = [0.03, 0.03, 0.12]
# target_size = [0.07, 0.07, 0.14]
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = target.x
target_pose.pose.position.y = target.y
target_pose.pose.position.z = target.z + target.offset_z + 0.01
# q = quaternion_from_euler(target.rr, target.rp, target.ry)
q = quaternion_from_euler(0, 0, 0)
target_pose.pose.orientation.x = q[0]
target_pose.pose.orientation.y = q[1]
target_pose.pose.orientation.z = q[2]
target_pose.pose.orientation.w = q[3]
scene.add_box(target_id, target_pose, target_size)
rospy.sleep(0.05)
print(target_pose.pose.position.x)
print(target_pose.pose.position.y)
print(target_pose.pose.position.z)
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose by half the width of the target to center it
grasp_pose.pose.position.x -= target_size[0] / 2.0 + 0.01
grasp_pose.pose.position.y -= target_size[1] / 2.0
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.05)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = arm.pick(target_id, grasps)
print(result)
rospy.sleep(0.02)
if result != MoveItErrorCodes.SUCCESS:
scene.remove_attached_object(GRIPPER_FRAME, target_id)
ret = rp_pickResponse()
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Success the pick operation")
ret.result = True
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) +
" attempts.")
ret.result = False
return ret
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
def placeSrvCallback(self, req):
rospy.loginfo("Received the service call!")
rospy.loginfo(req)
temp_dbdata = Tmsdb()
target = Tmsdb()
temp_dbdata.id = req.object_id
rospy.wait_for_service('tms_db_reader')
try:
tms_db_reader = rospy.ServiceProxy('tms_db_reader', TmsdbGetData)
res = tms_db_reader(temp_dbdata)
target = res.tmsdb[0]
except rospy.ServiceException as e:
print "Service call failed: %s" % e
self.shutdown()
print(target.name)
scene = PlanningSceneInterface()
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
arm.set_goal_position_tolerance(0.1)
arm.set_goal_orientation_tolerance(0.3)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
arm.set_planning_time(5)
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(0.05)
target_id = str(req.object_id)
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = req.x
target_pose.pose.position.y = req.y
target_pose.pose.position.z = req.z
# q = quaternion_from_euler(target.rr, target.rp, target.ry)
q = quaternion_from_euler(req.roll, req.pitch, req.yaw)
target_pose.pose.orientation.x = q[0]
target_pose.pose.orientation.y = q[1]
target_pose.pose.orientation.z = q[2]
target_pose.pose.orientation.w = q[3]
print(target_pose.pose.position.x)
print(target_pose.pose.position.y)
print(target_pose.pose.position.z)
print(target_pose.pose.orientation.x)
print(target_pose.pose.orientation.y)
print(target_pose.pose.orientation.z)
print(target_pose.pose.orientation.w)
# Initialize the grasp pose to the target pose
place_pose = target_pose
# Generate a list of grasps
places = self.make_places(place_pose)
result = None
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = arm.place(target_id, place)
print(result)
if result == MoveItErrorCodes.SUCCESS:
break
# rospy.sleep(0.2)
scene.remove_world_object(str(req.object_id))
ret = rp_placeResponse()
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Success the place operation")
ret.result = True
else:
rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.")
ret.result = False
return ret