diff --git a/interface_rajant/CMakeLists.txt b/interface_rajant/CMakeLists.txt index df6572d..4fa417e 100644 --- a/interface_rajant/CMakeLists.txt +++ b/interface_rajant/CMakeLists.txt @@ -16,6 +16,8 @@ ament_python_install_package(${PROJECT_NAME}) # Install executables install(PROGRAMS interface_rajant/rajant_peer_rssi.py + interface_rajant/rajant_query.py + interface_rajant/rajant_parser.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/interface_rajant/interface_rajant/rajant_parser.py b/interface_rajant/interface_rajant/rajant_parser.py new file mode 100755 index 0000000..cb52a11 --- /dev/null +++ b/interface_rajant/interface_rajant/rajant_parser.py @@ -0,0 +1,161 @@ +#!/usr/bin/env python3 + +import ast +import yaml +import os +import re +import threading +import pprint +import sys +import pdb + +import rclpy +from rclpy.node import Node +from std_msgs.msg import String +from std_msgs.msg import Int32 +from ament_index_python.packages import get_package_share_directory + +class RajantParser(Node): + def __init__(self, this_robot, robot_configs, radio_configs): + super().__init__('rajant_parser') + + # Check input args + assert isinstance(this_robot, str) + assert isinstance(robot_configs, dict) + assert isinstance(radio_configs, dict) + self.MAC_DICT = {} + + self.this_robot = this_robot + self.robot_cfg = robot_configs + self.radio_cfg = radio_configs + + self.get_logger().info(f"{self.this_robot} - Rajant API Parser - Starting") + + # Generate a standard configuration with a RSSI of -1 + for radio in self.radio_cfg.keys(): + for address in self.radio_cfg[radio]['MAC-address']: + self.MAC_DICT[address] = {} + self.MAC_DICT[address]['rssi'] = -20 + self.MAC_DICT[address]['timestamp'] = self.get_clock().now() + self.MAC_DICT[address]['radio'] = radio + self.MAC_DICT[address]['publisher'] = None + + # Generate publishers for each item in the dict + for mac in self.MAC_DICT.keys(): + for robot in self.robot_cfg.keys(): + if self.MAC_DICT[mac]['radio'] == self.robot_cfg[robot]['using-radio'] and robot != self.this_robot: + self.MAC_DICT[mac]['publisher'] = self.create_publisher(Int32, 'mocha/rajant/rssi/' + robot, 10) + + # Create subscriber + self.subscription = self.create_subscription( + String, + 'mocha/rajant/log', + self.update_dict, + 10 + ) + + + def update_dict(self, data): + # If we did not receive an update after dt, drop the RSSI to -1 + no_rssi = -1 + dt = rclpy.duration.Duration(seconds=20.0) + + # Evaluate the input data as a dictionary + alldata = data.data + data_dict = ast.literal_eval(data.data) + + state = data_dict['watchResponse']['state'] + + # Update the RSSI + for wireless_channel in state.keys(): + for wireless_keys in state[wireless_channel].keys(): + if wireless_keys[0:4] == 'peer': + peer = wireless_keys + if 'rssi' in state[wireless_channel][peer].keys(): + mac = state[wireless_channel][peer]['mac'] + if mac not in self.MAC_DICT.keys(): + self.get_logger().error(f"MAC: {mac} is not in the list of knowns MACs. Is your radio_configs.yaml file correct?") + continue + rssi = state[wireless_channel][peer]['rssi'] + self.MAC_DICT[mac]['rssi'] = rssi + self.MAC_DICT[mac]['timestamp'] = self.get_clock().now() + # Only publish if the publisher is not None + # This avoids an error for a radio that is connected but that is not + # actively used by any robot + if self.MAC_DICT[mac]['publisher'] is not None: + msg = Int32() + msg.data = rssi + self.MAC_DICT[mac]['publisher'].publish(msg) + else: + self.get_logger().debug(f"{self.this_robot} - Rajant API Parser - " + + f"active radio {self.MAC_DICT[mac]['radio']} not assigned to any robot") + elif 'mac' in state[wireless_channel][peer].keys() and 'rssi' not in state[wireless_channel][peer].keys(): + mac = state[wireless_channel][peer]['mac'] + if mac not in self.MAC_DICT.keys(): + self.get_logger().error(f"MAC: {mac} is not in the list of knowns MACs. Is your radio_configs.yaml file correct?") + continue + if self.get_clock().now() - self.MAC_DICT[mac]['timestamp'] > dt: + self.MAC_DICT[mac]['rssi'] = no_rssi + # Only publish if the publisher is not None + # See comment above + if self.MAC_DICT[mac]['publisher'] is not None: + msg = Int32() + msg.data = no_rssi + self.MAC_DICT[mac]['publisher'].publish(msg) + else: + self.get_logger().debug(f"{self.this_robot} - Rajant API Parser - " + + f"active radio {self.MAC_DICT[mac]['radio']} not assigned to any robot") + + +def main(args=None): + rclpy.init(args=args) + + # Create a temporary node to get parameters + temp_node = Node('temp_rajant_parser') + + # Declare parameters + temp_node.declare_parameter('robot_name', 'charon') + temp_node.declare_parameter('robot_configs', '') + temp_node.declare_parameter('radio_configs', '') + + # Get parameters + robot_name = temp_node.get_parameter('robot_name').get_parameter_value().string_value + robot_configs_file = temp_node.get_parameter('robot_configs').get_parameter_value().string_value + radio_configs_file = temp_node.get_parameter('radio_configs').get_parameter_value().string_value + + # Load robot configs + with open(robot_configs_file, "r") as f: + robot_configs = yaml.load(f, Loader=yaml.FullLoader) + if robot_name not in robot_configs.keys(): + temp_node.get_logger().error("Robot not in config file") + temp_node.destroy_node() + rclpy.shutdown() + return + + # Load radio configs + with open(radio_configs_file, "r") as f: + radio_configs = yaml.load(f, Loader=yaml.FullLoader) + radio = robot_configs[robot_name]["using-radio"] + if radio not in radio_configs.keys(): + temp_node.get_logger().error("Radio not in config file") + temp_node.destroy_node() + rclpy.shutdown() + return + + # Clean up temp node + temp_node.destroy_node() + + # Create the actual parser node + rajant_parser = RajantParser(robot_name, robot_configs, radio_configs) + + try: + rclpy.spin(rajant_parser) + except KeyboardInterrupt: + pass + finally: + rajant_parser.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/interface_rajant/interface_rajant/rajant_query.py b/interface_rajant/interface_rajant/rajant_query.py new file mode 100755 index 0000000..7e3e54b --- /dev/null +++ b/interface_rajant/interface_rajant/rajant_query.py @@ -0,0 +1,203 @@ +#!/usr/bin/env python3 +import sys +import subprocess +from threading import Thread +from queue import Queue, Empty +from pprint import pprint +import sys +import os +import time +import yaml +import re +import pdb +import string +import hashlib +import random + +import rclpy +from rclpy.node import Node +from std_msgs.msg import String +from ament_index_python.packages import get_package_share_directory + +def randomNumber(stringLength=4): + """Generate a random string of fixed length """ + number = random.randint(1000, 9999) + return str(number) + + +def enqueue_output(out, queue): + """ Saves the output of the process in a queue to be parsed + afterwards """ + for line in iter(out.readline, b''): + queue.put(line) + out.close() + + +def ping_ip(ip_address): + try: + # Run the ping command with a single ping packet (-c 1) and a timeout of 1 second (-W 1) + result = subprocess.run(["ping", "-c", "1", "-W", "1", ip_address], + stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True, check=True) + return result.returncode == 0 + except subprocess.CalledProcessError: + # An error occurred (ping failed) + return False + + +def line_parser(line_bytes): + """ Returns parsed str version of bytes input line + This is quite magic: rajant output is not yaml but it is very + yamlish. If we replace the { with :, we remove }, and we do some + minor modifications everything works out of the box!""" + line_str = line_bytes.decode('unicode-escape') + line_str = line_str.replace("{", ":") + line_str = line_str.replace("}", "") + # random numbers are added to avoid overwriting the key on the yaml + line_str = re.sub("wireless", + "wireless-" + randomNumber(), line_str) + line_str = re.sub("peer", + "peer-" + randomNumber(), line_str) + # MACs are a little bit more tricky + if line_str.replace(" ", "")[:4] == "mac:": + separator = line_str.find(":") + 2 + mac_str = line_str[separator:] + mac_bytes = bytes(mac_str, 'raw_unicode_escape') + mac_decoded = ":".join(["%02x" % c for c in mac_bytes[1:-2]]) + line_str = line_str[:separator] + mac_decoded + "\n" + return line_str + + +ON_POSIX = 'posix' in sys.builtin_module_names + + +class RajantQueryNode(Node): + def __init__(self): + super().__init__('rajant_query') + + # Declare parameters + self.declare_parameter('robot_name', 'charon') + self.declare_parameter('robot_configs', '') + self.declare_parameter('radio_configs', '') + + # Get parameters + self.robot_name = self.get_parameter('robot_name').get_parameter_value().string_value + robot_configs_file = self.get_parameter('robot_configs').get_parameter_value().string_value + radio_configs_file = self.get_parameter('radio_configs').get_parameter_value().string_value + + # Load robot configs + with open(robot_configs_file, "r") as f: + robot_configs = yaml.load(f, Loader=yaml.FullLoader) + if self.robot_name not in robot_configs.keys(): + self.get_logger().error("Robot not in config file") + return + + # Load radio configs + with open(radio_configs_file, "r") as f: + radio_configs = yaml.load(f, Loader=yaml.FullLoader) + radio = robot_configs[self.robot_name]["using-radio"] + if radio not in radio_configs.keys(): + self.get_logger().error("Radio not in config file") + return + + # Get target IP + rajant_name = robot_configs[self.robot_name]['using-radio'] + if rajant_name in radio_configs.keys(): + self.target_ip = radio_configs[rajant_name]['computed-IP-address'] + else: + self.get_logger().error(f"Radio {rajant_name} for robot {self.robot_name} not found in configs") + return + + # Create ROS publisher + self.pub = self.create_publisher(String, 'mocha/rajant/log', 10) + + # Get package path + try: + ros_path = get_package_share_directory('interface_rajant') + except: + self.get_logger().error("Could not find interface_rajant package") + return + + # Java binary path + self.java_bin = os.path.join(ros_path, + 'thirdParty/watchstate/bcapi-watchstate-11.19.0-SNAPSHOT-jar-with-dependencies.jar') + + # Initialize subprocess variables + self.p = None + self.q = None + self.t = None + + # Start the Java process + self.start_java_process() + + # Go + self.get_logger().info(f"{self.robot_name} - Rajant API Query - Starting on {rajant_name}") + + # Ping the assigned radio + if ping_ip(self.target_ip): + self.get_logger().info(f"{self.robot_name} - Rajant API Query - ping success") + else: + self.get_logger().error(f"{self.robot_name} - Rajant API Query - Rajant ping failed") + return + + # Create timer for main processing loop + self.timer = self.create_timer(1.0, self.process_rajant_data) + + def start_java_process(self): + """Start or restart the Java process""" + self.p = subprocess.Popen(['java', + '-jar', + self.java_bin, + self.target_ip], stdout=subprocess.PIPE, close_fds=ON_POSIX) + self.q = Queue() + self.t = Thread(target=enqueue_output, args=(self.p.stdout, self.q)) + self.t.daemon = True # thread dies with the program + self.t.start() + + def process_rajant_data(self): + """Main processing loop - called by timer""" + if self.t is not None and not self.t.is_alive(): + self.get_logger().error(f'{self.robot_name}: Rajant Java process died! Restarting...') + self.start_java_process() + + try: + line = self.q.get_nowait() + except Empty: + # No output yet + return + else: # got line + answ_array = line_parser(line) + while True: + try: + newline = self.q.get_nowait() + except Empty: + break + else: + answ_array += line_parser(newline) + try: + yaml_res = yaml.load(answ_array, Loader=yaml.Loader) + if type(yaml_res) == type({}): + msg = String() + msg.data = str(yaml_res) + self.pub.publish(msg) + else: + self.get_logger().error(f"{self.robot_name}: YAML from Rajant did not look like an object!") + except yaml.scanner.ScannerError: + self.get_logger().error(f"{self.robot_name}: Could not parse YAML from Rajant!") + + +def main(args=None): + rclpy.init(args=args) + + rajant_query_node = RajantQueryNode() + + try: + rclpy.spin(rajant_query_node) + except KeyboardInterrupt: + pass + finally: + rajant_query_node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/interface_rajant/setup.py b/interface_rajant/setup.py index 5b4cb26..b92d096 100644 --- a/interface_rajant/setup.py +++ b/interface_rajant/setup.py @@ -22,6 +22,8 @@ entry_points={ 'console_scripts': [ 'rajant_peer_rssi = interface_rajant.rajant_peer_rssi:main', + 'rajant_query = interface_rajant.rajant_query:main', + 'rajant_parser = interface_rajant.rajant_parser:main' ], }, )