From e3ff5ae6ea4372ae86f06e32237d8c14e2b3915b Mon Sep 17 00:00:00 2001 From: tilt-silvie Date: Sat, 3 Oct 2020 00:29:20 +0900 Subject: [PATCH 1/2] =?UTF-8?q?kazafoo=E7=B5=84=E3=81=BF=E8=BE=BC=E3=81=BF?= =?UTF-8?q?=E3=81=AE=E3=83=95=E3=83=83=E3=83=88=E3=82=B9=E3=82=A4=E3=83=83?= =?UTF-8?q?=E3=83=81=E3=81=AE=E5=9F=BA=E6=9C=AC=E8=AA=AD=E3=81=BF=E8=BE=BC?= =?UTF-8?q?=E3=81=BF=E3=82=B3=E3=83=BC=E3=83=89=E5=AE=9F=E8=A3=85?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- consai2_kazafoo/scripts/example/kazafoo.py | 33 ++++++++++++++++++++-- 1 file changed, 30 insertions(+), 3 deletions(-) diff --git a/consai2_kazafoo/scripts/example/kazafoo.py b/consai2_kazafoo/scripts/example/kazafoo.py index d3e25dd..4bf185c 100755 --- a/consai2_kazafoo/scripts/example/kazafoo.py +++ b/consai2_kazafoo/scripts/example/kazafoo.py @@ -14,6 +14,9 @@ NUM_OF_LED = 30 +FOOT_SWITCH_MODE_NORMAL = 0 +FOOT_SWITCH_MODE_KEYBOARD = 1 + led_data = { "left": { "r": 0.0, @@ -73,6 +76,17 @@ def getSensorValues(self): return (left_value, right_value) + def getFootSwtichState(self): + self._serial.write('GF\n'.encode('ascii')) + data = self._serial.readline() + + if len(data) == 0: + return False + + if int(data[0]) == 0: + return True + return False + def setLedLeft(self, r, g, b, n): self._setLed(True, r, g, b, n) @@ -135,6 +149,11 @@ def getKey(self, key_timeout): if __name__=="__main__": rospy.init_node('kazafoo_node') + # Foot switch mode + # 0: embedded to kazafoo + # 1: keyboard['b'] + footswitch_mode = rospy.get_param('consai2_description/kazafoo_footswitch', 0) + publisher = rospy.Publisher('joy_kazafoo', Joy, queue_size = 1) sub_led_left = rospy.Subscriber('led_left', ColorRGBA, callback_led_left) @@ -142,8 +161,9 @@ def getKey(self, key_timeout): kazafoo_com = KazafooCom() - keyboard_thread = KeyboardThread() - keyboard_thread.start() + if footswitch_mode == FOOT_SWITCH_MODE_KEYBOARD: + keyboard_thread = KeyboardThread() + keyboard_thread.start() r = rospy.Rate(20) while not rospy.is_shutdown(): @@ -154,10 +174,17 @@ def getKey(self, key_timeout): # get sensor values (sensor_left, sensor_right) = kazafoo_com.getSensorValues() + # get footswitch state + footswitch_state = 0 + if footswitch_mode == FOOT_SWITCH_MODE_KEYBOARD: + footswitch_state = keyboard_thread.kick_flag + else: + footswitch_state = kazafoo_com.getFootSwtichState() + # publish sensor values joy = Joy() joy.buttons = [0.0] * 3 - joy.buttons[0] = 1 if keyboard_thread.kick_flag else 0 + joy.buttons[0] = 1 if footswitch_state else 0 joy.buttons[1] = 1 if sensor_left > 0.5 else 0 joy.buttons[2] = 1 if sensor_right > 0.5 else 0 joy.axes = [0.0] * 3 From be1d7674e59044779c0399c53180043ba9a9abb6 Mon Sep 17 00:00:00 2001 From: tilt-silvie Date: Sat, 3 Oct 2020 00:39:10 +0900 Subject: [PATCH 2/2] =?UTF-8?q?1.0=E7=A7=92=E9=96=93=E3=81=AF=E3=83=95?= =?UTF-8?q?=E3=83=83=E3=83=88=E3=82=B9=E3=82=A4=E3=83=83=E3=83=81=E3=81=AE?= =?UTF-8?q?ON=E7=8A=B6=E6=85=8B=E3=82=92=E4=BF=9D=E6=8C=81=E3=81=99?= =?UTF-8?q?=E3=82=8B=E3=82=88=E3=81=86=E3=81=AB=E3=81=97=E3=81=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- consai2_kazafoo/scripts/example/kazafoo.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/consai2_kazafoo/scripts/example/kazafoo.py b/consai2_kazafoo/scripts/example/kazafoo.py index 4bf185c..ca21b3f 100755 --- a/consai2_kazafoo/scripts/example/kazafoo.py +++ b/consai2_kazafoo/scripts/example/kazafoo.py @@ -54,6 +54,7 @@ def __init__(self): self._BAUDRATE = rospy.get_param('consai2_description/kazafoo_device', 9600) self._serial = serial.Serial(self._DEVICE, self._BAUDRATE, timeout=1.0) + self._latest_active_foot_switch = rospy.Time() def getSensorValues(self): self._serial.write('GS\n'.encode('ascii')) @@ -84,7 +85,15 @@ def getFootSwtichState(self): return False if int(data[0]) == 0: + self._latest_active_foot_switch = rospy.Time.now() return True + + # Hold True state for 1.0 sec + now = rospy.Time.now() + last_active_foot_switch_elapsed = now - self._latest_active_foot_switch + if last_active_foot_switch_elapsed.to_sec() < 1.0: + return True + return False def setLedLeft(self, r, g, b, n):