def callback_input(self, gpio_pin):
"""
スイッチ押下のコールバック関数
Parameters
----------
gpio_pin : int
ピン番号
"""
# 送信するメッセージの作成
msg = gpio_mes()
# BCMポート番号をX端子番号に読み替え
msg.port = self.__GpioPin.index(gpio_pin)
msg.value = 1
# ボタンが押された情報を送信
self.pub.publish(msg)
# ログの表示
self.__s.message("Port (IN): %s, Val: %s" % (msg.port, msg.value))
def StatusReset(self):
"""
ステータスを全てリセット（上手く動いてない）
"""
# 値をリセット
for i in range(len(self.__worker_stat)):
self.__worker_stat[i] = self.STAT_NONE
# ボタン点灯もリセット
for i in c.GPIO_OUT_WORKERSTAT:
# 指定のトピックへメッセージを送信：ボタンランプ
pub = rospy.Publisher('mes_gpio_out', gpio_mes, queue_size=10)
# 送信するメッセージの作成
msg = gpio_mes()
msg.port = i
msg.value = self.STAT_OFF
# メッセージ送信
pub.publish(msg)
# 有効なボタンだけゼロリセット
self.__worker_stat[i] = self.STAT_OFF
def update_signaltower(arg_Color):
"""
シグナルタワーの色変更
Parameters
----------
arg_Color:int
シグナルタワーの色番号
"""
# 指定のトピックへメッセージを送信
pub = rospy.Publisher('mes_gpio_out', gpio_mes, queue_size=10)
# 送信するメッセージの作成
msg = gpio_mes()
for i in c.GPIO_OUT_SIGTOWER:
# 点灯状態を変更するポート番号(X番号)を指定
msg.port = i
if i == arg_Color:
# 引数に受け取ったポート番号(X番号)と一致していれば
# 指定のランプをON
msg.value = 1
else:
# それ以外のランプをOFF
msg.value = 0
# メッセージ送信
pub.publish(msg)
def callback_button(self, arg_mes):
"""
ボタン入力を受け取るコールバック関数
Parameters
----------
message : gpio_mes
メッセージ
"""
# 無効化されているときは、以降の処理をしない
if not self.__enabled:
return
# ボタン受付処理
try:
#
# WORKSTATボタンの点灯を更新
#
# 指定のトピックへメッセージを送信：ボタンランプ
pub = rospy.Publisher('mes_gpio_out', gpio_mes, queue_size=10)
# 送信するメッセージの作成
msg = gpio_mes()
stat = []
logmes = "<WorkStat PB P:V, PL P:V> "
# GPIO_OUT_WORKERSTATをスキャン
for i in c.GPIO_OUT_WORKERSTAT:
# 点灯状態を変更するポート番号(X番号)を指定
msg.port = i
if i == arg_mes.port:
# callbackで受け取ったポート番号(X番号)と一致していれば
# 指定のランプをON
msg.value = self.STAT_ON
else:
# それ以外のランプをOFF
msg.value = self.STAT_OFF
# メッセージ送信
pub.publish(msg)
# ステータスバッファに追加
stat.append(msg.value)
logmes += ("[ %s:%s,%s:%s ], " %
(arg_mes.port, arg_mes.value, msg.port, msg.value))
# ログの表示
self.__s.message(logmes)
except:
# 例外発生時
self.__s.message(
"Grove AD Measure Error: %s" % (sys.exc_info()[0]), s.WARN)
finally:
pass
#
# 色ボタンごとのステータスの更新
# 0:RED 1:GREEN 2:YELLOW 3:UMBER 4:BLUE 5:WHITE
#
i = 0
for j in c.GPIO_WORKERSTATORDER:
if j == "RED":
self.__worker_stat[0] = stat[i]
if j == "YELLOW":
self.__worker_stat[1] = stat[i]
if j == "GREEN":
self.__worker_stat[2] = stat[i]
if j == "BLUE":
self.__worker_stat[3] = stat[i]
if j == "WHITE":
self.__worker_stat[4] = stat[i]
if j == "UMBER":
self.__worker_stat[5] = stat[i]
else:
pass
i += 1
def do():
"""
publish関数
Parameters
----------
"""
# ノードの初期化と名称設定
# anonymousをTrueにして、ノード名を自動で変更して複数接続OKとする
rospy.init_node('gpio_test', anonymous=True)
# 指定のトピックへメッセージを送信
pub_out = rospy.Publisher('mes_gpio_out', gpio_mes, queue_size=10)
# 指定のトピックからメッセージを受信
rospy.Subscriber("mes_gpio_in", gpio_mes, callback)
# ローカルプロキシにサービス名(srv_grove_ad)と型(grove_ad_srv)を設定
grove_ad = rospy.ServiceProxy('srv_grove_ad', grove_ad_srv)
# 送信周期(Hz)を設定
rate = rospy.Rate(1)
count = 0
port = 0
while not rospy.is_shutdown():
# ------------------------------
# テスト：GPIO
# ------------------------------
# 送信するメッセージの作成
msg = gpio_mes()
msg.port = port
msg.value = 1
rospy.loginfo("[%s]Lamp Port: %s, Val: %s" %
(count, msg.port, msg.value))
# 送信
pub_out.publish(msg)
rate.sleep()
count += 1
# 送信するメッセージの作成
msg = gpio_mes()
msg.port = port
msg.value = 0
rospy.loginfo("[%s]Lamp Port: %s, Val: %s" %
(count, msg.port, msg.value))
# 送信
pub_out.publish(msg)
count += 1
port += 1
if port > 7:
port = 0
# ------------------------------
# テスト：GROVE AD
# ------------------------------
# word_counterが呼び出されたときにサービスコールが行われる
try:
ad_0 = grove_ad(0)
ad_1 = grove_ad(1)
ad_2 = grove_ad(2)
ad_3 = grove_ad(3)
ad_4 = grove_ad(4)
ad_5 = grove_ad(5)
rospy.loginfo("Grove AD [%sch]:%s [%sch]:%s [%sch]:%s [%sch]:%s" %
(0, ad_0.Val_VOLT, 2, ad_2.Val_VOLT, 3,
ad_3.Val_VOLT, 4, ad_4.Val_VOLT))
except:
pass
finally:
pass