diff --git a/CMakeLists.txt b/CMakeLists.txt
index c34b0070f919fe2751f4c8f3b59bbfad33087b5c..b0b539b82f761f1a73c941751a885edd7a721dd2 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 2.8.3)
-project(board_status)
+project(board_state)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
@@ -21,7 +21,7 @@ find_package(catkin REQUIRED COMPONENTS
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
+catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
@@ -50,8 +50,8 @@ find_package(catkin REQUIRED COMPONENTS
## Generate messages in the 'msg' folder
add_message_files(
FILES
- StrList.msg
-# Message2.msg
+ TokenMsg.msg
+ BoardMsg.msg
)
## Generate services in the 'srv' folder
diff --git a/launch/board_state.launch b/launch/board_state.launch
new file mode 100644
index 0000000000000000000000000000000000000000..0acf3fa399146af023a36f68fe6b2c5fa9bc7b9c
--- /dev/null
+++ b/launch/board_state.launch
@@ -0,0 +1,24 @@
+<launch>
+ <node pkg="board_state" type="main.py" name="board_state" output="screen"/>
+
+ <!-- Initial tokens locations -->
+ <rosparam param="initial_board">
+ {'1': '0', "2": '0', "3": '0', "4": '0', "5": '0',
+ "6": '0', "7": '0', "8": '0', "9": '0', "10": '0',
+ "11": '1', "12": '2', "13": '3', "14": '4', "15": '5',
+ "16": '6', "17": '7', "18": '8', "19": '9', "20": '10'}
+ </rosparam>
+
+
+
+ <rosparam param="tokens_number">
+ [ '111', '158', '183','192', '253','328', '376', '394', '449','463', '475', '486','517', '744', '819']
+ </rosparam>
+
+ <rosparam param="rfid_id">
+ 3581181951 3306978559 894864895 2247134207 1439793151 3315042303 1972340735 2768466943 2509992703 2777051135 4117136895 3852242175 88436991 2504166399 356806911
+</rosparam>
+
+
+
+</launch>
diff --git a/msg/StrList.msg b/msg/BoardMsg.msg
similarity index 100%
rename from msg/StrList.msg
rename to msg/BoardMsg.msg
diff --git a/msg/TokenMsg.msg b/msg/TokenMsg.msg
new file mode 100644
index 0000000000000000000000000000000000000000..2daae82e1925c9e0db94d0c4428d0152622c4f8c
--- /dev/null
+++ b/msg/TokenMsg.msg
@@ -0,0 +1,4 @@
+string[] detected_token
+bool picked
+bool placed
+bool moved_back
diff --git a/package.xml b/package.xml
index a7288bbfb9cc223c70af54a205b00a6532d0ecaf..d5735f180c6f7009d3cff6b25f2ec0ed8c43c3b4 100644
--- a/package.xml
+++ b/package.xml
@@ -1,8 +1,8 @@
<?xml version="1.0"?>
<package format="2">
- <name>board_status</name>
+ <name>board_state</name>
<version>0.0.0</version>
- <description>The board_status package</description>
+ <description>The board_state package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
diff --git a/setup.py b/setup.py
new file mode 100644
index 0000000000000000000000000000000000000000..95d8db3d6344a52cb91dc8f8ef3d703cb974b063
--- /dev/null
+++ b/setup.py
@@ -0,0 +1,6 @@
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(packages=['board_state'], package_dir={'':'src'})
+
+setup(**d)
diff --git a/sounds/beep.mp3 b/sounds/beep.mp3
new file mode 100644
index 0000000000000000000000000000000000000000..51e0670aeaa0c0cc200a66abe99ce6241125dac6
Binary files /dev/null and b/sounds/beep.mp3 differ
diff --git a/sounds/pair.ogg b/sounds/pair.ogg
new file mode 100644
index 0000000000000000000000000000000000000000..4a592345fe829e49ce97137424b1ae7eb8f755cb
Binary files /dev/null and b/sounds/pair.ogg differ
diff --git a/src/board_state/__init__.py b/src/board_state/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/board_state/main.py b/src/board_state/main.py
new file mode 100755
index 0000000000000000000000000000000000000000..f616afb73743b91e9d08ad249071841675f2a938
--- /dev/null
+++ b/src/board_state/main.py
@@ -0,0 +1,318 @@
+#!/usr/bin/python
+import os
+#This is necessary to import code that is not rosbased in the main.py
+path = os.path.abspath(__file__)
+dir_path = os.path.dirname(path)
+parent_dir_of_file = os.path.dirname(dir_path)
+parent_parent_dir_of_file = os.path.dirname(parent_dir_of_file)
+
+
+#ros import
+import time
+import rospy
+from std_msgs.msg import String, Int8
+import pygame
+
+
+from iri_rfid_board_scanner.msg import BoardIds
+from board_state.msg import TokenMsg
+from board_state.msg import BoardMsg
+
+class BoardGame():
+ def __init__(self):
+ rospy.init_node('big_hero', anonymous=True)
+ # subscriber for getting info from the board
+ rospy.Subscriber("/rfid_board_scanner/board_ids", BoardIds, self.get_electro_board_callback)
+
+ self.electro_board = list()
+ self.electro_board_with_tokens = dict()
+ self.current_board = dict()
+ self.initial_board = dict()
+ self.placed_token = (0,0,0)
+ self.picked_token = (0,0)
+
+ #getting the ids from the rfids sensors
+ self.ids = list(map(int, rospy.get_param('/rfid_id').split()))
+ self.ids = [ int(self.ids[i]) for i in range(len(self.ids))]
+
+ #get the numbers for playing the game
+ self.tokens = rospy.get_param('tokens_number')
+ #coupling ids with numbers
+ self.electro_board_from_ids_to_tokens = self.convert_electro_board_ids_to_tokens(self.ids, self.tokens)
+
+ def play_sound(self, file_path, sleep):
+ '''
+ play a sound using pygame
+ '''
+ pygame.mixer.init()
+ # pygame.mixer.music.load(parent_dir_of_file+'/sounds/wrong_move_trim.mp3')
+ pygame.mixer.music.load(parent_parent_dir_of_file + file_path)
+ pygame.mixer.music.play(0)
+ time.sleep(sleep)
+
+ def get_tokens_id(self):
+ '''
+ get the tokens id from the board
+ '''
+ return self.tokens
+
+ def get_initial_board(self):
+ '''
+ get initial board status
+ '''
+ return self.initial_board.copy()
+
+ #get value from the topic
+ def get_electro_board_ids(self):
+ '''
+ get the ids from the board, these ids need to be converted to the id on the top of the token
+ '''
+ return self.electro_board
+
+ #topic subscriber
+ def get_electro_board_callback(self, msg):
+ '''callback from the topic subscriber'''
+ self.electro_board = (msg.id)
+
+ def get_electro_board(self):
+ '''get the board with the correct ids on the top'''
+ return self.get_tokens_by_ids(self.electro_board_from_ids_to_tokens, self.electro_board)
+
+ def convert_electro_board_ids_to_tokens(self, ids, tokens):
+ '''
+ :param ids:
+ :param tokens:
+ :return: this method creates a dic where the ids are the keys and the values are the tokens
+ '''
+ from_ids_to_tokens = dict()
+ for i in range(len(ids)):
+ from_ids_to_tokens[ids[i]] = tokens[i]
+ return from_ids_to_tokens
+
+ def get_tokens_by_ids(self, from_ids_to_tokens_dict, msg):
+ '''
+ :param electro_board:
+ :param msg:
+ :return: given a dictionary where key is the id and value is the token change the msg according (0 empty !0 a token id)
+ '''
+ for i in range(len(msg)):
+ if msg[i] == 0:
+ # i+1 because the id start
+ self.electro_board_with_tokens[i+1] = '0'
+ else:
+ self.electro_board_with_tokens[i+1] = from_ids_to_tokens_dict.get(msg[i])
+ return self.electro_board_with_tokens
+
+ def get_token_location_from_board_state(self, previous_board, token):
+ '''
+ :param: token
+ :return: the location of the token given as input
+ '''
+ for key, val in previous_board.items():
+ if val == token:
+ return key
+
+ def detect_grasped_token(self, previous_board, current_board):
+ '''
+ this function detects if a token has been picked
+ Args:
+ previous_board: the status of the board at time t-1
+ current_board: the status of the board at time t
+ '''
+
+ picked_token = None
+ for k in current_board.keys():
+ if current_board.get(k) == '0' and previous_board.get(k) != current_board.get(k):
+ # if the location now is empty check where it was before
+ token_id = previous_board.get(k)
+ location = k
+ picked_token = (str(token_id), str(location))
+ return picked_token
+
+ def detect_placed_token(self, picked_token, current_board):
+ '''
+ this function detects when token has been placed on the board
+ Args:
+ picked_token: the token the user has picked
+ current_board: the status of the board at time t
+ '''
+ token_id, loc_from = picked_token
+ move_it_back = False
+ placed_token = ()
+ for key, val in current_board.items():
+ if token_id == val and str(key) != loc_from:
+ loc_to = key
+ print("placed in a different location")
+ placed_token = (str(token_id), str(loc_from), str(loc_to))
+ elif token_id == val and str(key) == loc_from:
+ loc_to = key
+ print("placed in the same location")
+ placed_token = (str(token_id), str(loc_from), str(loc_to))
+ move_it_back = True
+ return placed_token, move_it_back
+
+ def detect_move(self, previous_board, current_board):
+ '''
+ this function detects if a token has moved from loc_from to loc_to
+ Args:
+ previous_board: the status of the board at time t-1
+ current_board: the status of the board at time t
+ '''
+
+ move = ()
+ new_location = 0
+ for x1 in previous_board.keys():
+ z = current_board.get(x1) == previous_board.get(x1)
+ if not z:
+ #print('location', x1)
+ if (current_board.get(x1))!='0':
+ token = (current_board.get(x1))
+ #check where the token is in the new configuration
+ prev_location = self.get_token_location_from_board_state(previous_board, token)
+ for key, value in current_board.items():
+ if token == value and prev_location !=key:
+ new_location = key
+ move = (token, prev_location, new_location)
+ return move
+
+ def get_current_board(self):
+ '''get the current board status'''
+ self.current_board = self.get_electro_board()
+ return self.current_board.copy()
+
+ def update_board(self, token_id, location):
+ '''
+ update the current board given a token and a location
+ Args
+ token_id: the number
+ location: its location
+ Return:
+ a board with the token token_id in location location
+ '''
+ if type(token_id)!=str:
+ token_id = str(token_id)
+
+
+ for loc, token in self.current_board.items():
+ if token == (token_id):
+ self.current_board[loc] = '0'
+
+ for key in self.current_board.keys():
+ if location == key:
+ self.current_board[key] = token_id
+
+
+
+def wait_for_subscribers(publisher):
+ wait_rate = rospy.Rate(20)
+ while not publisher.get_num_connections():
+ wait_rate.sleep()
+
+
+def main():
+ board_game = BoardGame()
+ ###############################################################
+ rospy.init_node('big_hero', anonymous=True)
+ pub_move = rospy.Publisher('detected_move', TokenMsg, queue_size=10)
+ pub_board_status = rospy.Publisher('board_status', BoardMsg, queue_size=10)
+ rate = rospy.Rate(10) # 10hz
+ event_counter = 0
+ #this is necessary in order to give the time to the board to start publishing
+ rospy.sleep(2)
+
+ #initialise the publisher and the board
+ wait_for_subscribers(pub_move)
+ msg_move = TokenMsg()
+ msg_move.detected_token = []
+ msg_move.picked = False
+ msg_move.placed = False
+ msg_move.moved_back = False
+ pub_move.publish(msg_move)
+
+ wait_for_subscribers(pub_board_status)
+
+ initial_board = board_game.get_current_board()
+ current_board = initial_board
+ msg_board = BoardMsg()
+ msg_board.data = current_board.values()
+ pub_board_status.publish(msg_board)
+
+ token_picked = False
+ while not rospy.is_shutdown():
+ detected_board = board_game.get_current_board()
+ msg_board.data = detected_board.values()
+ pub_board_status.publish(msg_board)
+
+
+ #check if a token has been picked
+ picked_token = board_game.detect_grasped_token(current_board, detected_board)
+
+ #if a token has been picked
+ if (picked_token)!=None:
+ print("current ", current_board)
+ print("detected ", detected_board)
+ print("grasped token:", picked_token)
+
+ #prepare the message and publish it
+ msg_move.detected_token = picked_token
+ msg_move.picked = True
+ #detected_board.values()
+ board_game.play_sound("/sounds/pull-out.ogg", 0.2)
+ pub_move.publish(msg_move)
+ event_counter += 1
+ rate.sleep()
+ current_board = board_game.get_current_board()
+ msg_board.data = current_board.values()
+ pub_board_status.publish(msg_board)
+ board_game.picked_token = picked_token
+ #print("token {}, from {}, to {}".format(detected_action[1], detected_action[2], detected_action[3]))
+ print("***********************************")
+ print("Picked token " + str(msg_move.detected_token))
+ print("Picked "+ str(msg_move.picked))
+ print("Placed " + str(msg_move.placed))
+ print("Moved back " + str(msg_move.moved_back))
+ print("***********************************")
+ token_picked = True
+
+ #check when a picked token is going to be placed by the user
+ elif (token_picked):
+ placed_token, moved_back = board_game.detect_placed_token(board_game.picked_token, detected_board)
+ if len(placed_token)>0:
+ board_game.play_sound("/sounds/pull-out.ogg", 0.2)
+ msg_move.detected_token = placed_token
+ msg_move.picked = False
+ msg_move.placed = True
+ msg_move.moved_back = moved_back
+ pub_move.publish(msg_move)
+ event_counter += 1
+ rate.sleep()
+ current_board = board_game.get_current_board()
+ msg_board.data = current_board.values()
+ pub_board_status.publish(msg_board)
+ board_game.placed_token = placed_token
+ token_picked = False
+ print("***********************************")
+ print("Picked token " + str(msg_move.detected_token))
+ print("Picked " + str(msg_move.picked))
+ print("Placed " + str(msg_move.placed))
+ print("Moved back " + str(msg_move.moved_back))
+ print("***********************************")
+ else:
+ msg_move.detected_token = []
+ msg_move.picked = False
+ msg_move.placed = False
+ msg_move.moved_back = False
+
+
+if __name__ == '__main__':
+ try:
+ main()
+ except rospy.ROSInterruptException:
+ pass
+
+
+
+
+
+
+