diff --git a/.komment/00000.json b/.komment/00000.json new file mode 100644 index 00000000..bd81dfec --- /dev/null +++ b/.komment/00000.json @@ -0,0 +1,3057 @@ +[ + { + "name": "long_term_graph.py", + "path": "agents/long_term_spatial_memory_agent/src/long_term_graph.py", + "content": { + "structured": { + "description": "A class `Room` that represents a 3D room in a graph, with nodes representing corners and edges representing walls. The class provides methods for calculating the projective coordinates (4x1) of the corners and objects in the room frame, as well as for drawing the room and its objects on a plot. The code uses the `networkx` and `scipy.spatial` libraries to manipulate and analyze the graph data.", + "items": [ + { + "id": "e77351db-7a15-56b3-3644-4399c887d62b", + "ancestors": [], + "description": "Provides a user interface for visualizing and exploring a graph, including rooms and doors. It allows for adding nodes, edges, and doors, as well as displaying room names and distances between nodes.", + "attributes": [ + { + "name": "g", + "type_name": "str|int", + "description": "Used to specify the color of the graph's edges. It can be set to a valid matplotlib color name or an integer value between 0 and 1, representing the transparency level of the edge." + }, + { + "name": "read_graph", + "type_name": "Callable[[str],Dict[str,float]]", + "description": "Used to read a graph from a file specified by the filename parameter. It returns a dictionary containing the graph data as key-value pairs where keys are node or edge indices and values are the corresponding coordinates or weights." + }, + { + "name": "fig", + "type_name": "matplotlibfigureFigure", + "description": "Used to represent the figure object that will be drawn with the graph. It contains information about the figure, such as its size, layout, and any additional elements that will be displayed in it." + }, + { + "name": "ax", + "type_name": "matplotlibpyplotAxes", + "description": "Used to represent a 2D axes object that displays the graphical representation of the long-term graph." + }, + { + "name": "fig_2", + "type_name": "matplotlibfigureFigure", + "description": "Used to store the figure object for the second graph." + }, + { + "name": "ax_2", + "type_name": "AxesSubplot|MatplotlibFigure", + "description": "Used to store a secondary axes object, which can be used to display additional visualizations or metrics alongside the primary graph." + } + ], + "name": "LongTermGraph", + "location": { + "start": 15, + "insert": 16, + "offset": " ", + "indent": 4, + "comment": null + }, + "item_type": "class", + "length": 269, + "docLength": null + }, + { + "id": "35f455db-0eef-1ba5-f04f-cb6cacf60abb", + "ancestors": [ + "e77351db-7a15-56b3-3644-4399c887d62b" + ], + "description": "Reads a graph from a file, creates an igraph object, and displays both the original graph and its metric reconstruction using matplotlib.", + "params": [ + { + "name": "file_name", + "type_name": "str", + "description": "Used to specify the name of the graph file that contains the LTSM data to be reconstructed." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "from LongTermGraph import LongTermGraph\n\n# create an instance of the class and set its name\nltg = LongTermGraph(\"my_graph.pickle\")\n\n# print the summary of the graph\nprint(ltg.summary())\n\n# plot the graph on a matplotlib figure\nltg.plot()\n", + "description": "" + }, + "name": "__init__", + "location": { + "start": 16, + "insert": 17, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "constructor", + "length": 22, + "docLength": null + }, + { + "id": "252c67c9-d429-faa7-ef4a-8d465711c8a0", + "ancestors": [ + "e77351db-7a15-56b3-3644-4399c887d62b" + ], + "description": "Generates a graph based on a subgraph of the original graph, with only certain types of nodes and edges visible.", + "params": [ + { + "name": "only_rooms", + "type_name": "bool", + "description": "Used to filter out nodes that are not rooms or doors, and edges that do not connect rooms or doors." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "# create an instance of LongTermGraph class and read graph from file\nmy_graph = LongTermGraph(\"myfile.pkl\")\n\n# draw graph with only rooms\nmy_graph.draw_graph(only_rooms=True)\n\n# draw graph with all nodes including walls, doors, and rooms\nmy_graph.draw_graph()\n", + "description": "" + }, + "name": "draw_graph", + "location": { + "start": 252, + "insert": 253, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 33, + "docLength": null + } + ] + } + } + }, + { + "name": "genericworker.py", + "path": "agents/g2o_agent/src/genericworker.py", + "content": { + "structured": { + "description": "A worker class that inherits from QtWidgets.QWidget and provides a kill signal for shutting down the worker. The code also sets up an ice slice and imports a RoboCompCommonBehavior module. Additionally, the code creates a Ui_guiDlg class and initializes a timer to call a method every 30 milliseconds.", + "items": [ + { + "id": "cb6f42fc-bcd1-7fb5-eb49-91afccba59e8", + "ancestors": [], + "description": "Manages a worker process with a periodic timer and provides a signal for termination. It also has a method to set the period of the timer.", + "attributes": [ + { + "name": "kill", + "type_name": "QtCoreSignal", + "description": "Used to emit a signal when the object needs to be killed." + }, + { + "name": "ui", + "type_name": "Ui_guiDlg", + "description": "Used to initialize and access the user interface of the widget." + }, + { + "name": "mutex", + "type_name": "QMutex", + "description": "Used to protect access to the internal state of the worker object, particularly the timer and kill signal." + }, + { + "name": "Period", + "type_name": "int", + "description": "30 milliseconds by default, which represents the time interval for the timer to run." + }, + { + "name": "timer", + "type_name": "QTimer", + "description": "Used to schedule a call to the `killYourSelf` slot after a specified period of time." + } + ], + "name": "GenericWorker", + "location": { + "start": 43, + "insert": 45, + "offset": " ", + "indent": 4, + "comment": null + }, + "item_type": "class", + "length": 26, + "docLength": null + }, + { + "id": "795c292f-f253-f1aa-a748-23953c9013be", + "ancestors": [ + "cb6f42fc-bcd1-7fb5-eb49-91afccba59e8" + ], + "description": "Initializes an instance of the `GenericWorker` class, setting up a GUI dialog and creating a mutex for managing access to the timer. It also sets the period of the timer to 30 seconds.", + "params": [ + { + "name": "mprx", + "type_name": "Ui_guiDlg", + "description": "Used as an argument for the setupUi method." + } + ], + "returns": null, + "name": "__init__", + "location": { + "start": 47, + "insert": 48, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 9, + "docLength": null + }, + { + "id": "ea51cd3f-4286-88a2-5c4e-a765280c1202", + "ancestors": [ + "cb6f42fc-bcd1-7fb5-eb49-91afccba59e8" + ], + "description": "Emits the `kill` signal, indicating that the instance should be destroyed.", + "params": [], + "returns": null, + "name": "killYourSelf", + "location": { + "start": 60, + "insert": 62, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 4, + "docLength": null + }, + { + "id": "20082581-a4f0-66af-9a45-78e2e70b070f", + "ancestors": [ + "cb6f42fc-bcd1-7fb5-eb49-91afccba59e8" + ], + "description": "Updates the `Period` attribute and starts a timer with the new period value using the `timer.start()` method.", + "params": [ + { + "name": "p", + "type_name": "int", + "description": "Used to set the new period for the timer." + } + ], + "returns": null, + "name": "setPeriod", + "location": { + "start": 67, + "insert": 69, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 5, + "docLength": null + } + ] + } + } + }, + { + "name": "specificworker.py", + "path": "agents/g2o_agent/src/specificworker.py", + "content": { + "structured": { + "description": "A specific worker class that inherits from a generic worker class and is part of a robotic system using DSR (Data Stream Representation) framework. It handles robot odometry updates, generates an information matrix considering noise, optimizes pose estimation, and visualizes the results in 3D space using matplotlib. The worker initializes a G2O graph based on room nodes, corners, and edges, which is then used to optimize robot poses and predict future positions.", + "items": [ + { + "id": "609f6edc-14d7-26a1-1c4c-62744b75a643", + "ancestors": [], + "description": "Is a worker thread responsible for processing robot odometry data and updating the graph representation using G2O library. It initializes the graph, adds edges and vertices, computes pose and covariance matrix, and visualizes the graph in real-time.", + "attributes": [ + { + "name": "Period", + "type_name": "int", + "description": "50 by default. It represents the time period for which the worker's timer should wait before calling the `compute` method." + }, + { + "name": "agent_id", + "type_name": "int", + "description": "20 by default. It seems to be used as a unique identifier for the agent, possibly in conjunction with other nodes or edges in the graph." + }, + { + "name": "g", + "type_name": "DSRGraph", + "description": "Initialized with a node named \"G2O_agent\" and agent_id 20. It represents a graph data structure used to store nodes, edges, and their attributes." + }, + { + "name": "startup_check", + "type_name": "NoneNone", + "description": "Used as a parameter in the method with the same name. It seems to be a placeholder or a flag for checking whether some initialization is done, but its exact purpose is not clear from the provided code." + }, + { + "name": "rt_api", + "type_name": "object", + "description": "Used to get edge information from a Room-Terrain model (RT) API, which seems to provide information about robot edges in a room." + }, + { + "name": "inner_api", + "type_name": "object", + "description": "Not fully defined in this code snippet. Its purpose appears to be related to transforming room node names to robot pose measurements, but its implementation is unclear without more context or additional code." + }, + { + "name": "odometry_node_id", + "type_name": "int", + "description": "20 in this case. It seems to be used as a reference to the robot's node in the graph, likely representing its current position or pose." + }, + { + "name": "odometry_queue", + "type_name": "deque[int,float,int]", + "description": "15 elements long. It stores odometry data, where each element contains robot's current advance speed, side speed, angular speed, and timestamp in milliseconds." + }, + { + "name": "last_odometry", + "type_name": "Tuple[float,float,float,int]", + "description": "4-element tuple that represents the last recorded odometry values of the robot, including its advancement speed, lateral speed, angular speed, and timestamp." + }, + { + "name": "g2o", + "type_name": "G2OGraph", + "description": "Used for graph optimization with g2o library. It represents a graph where nodes are poses and edges are measurements between them, allowing to compute the most likely pose given the measurements." + }, + { + "name": "odometry_noise_std_dev", + "type_name": "float", + "description": "1 by default. It seems to be related to noise added to odometry measurements, representing the standard deviation of the noise." + }, + { + "name": "odometry_noise_angle_std_dev", + "type_name": "float", + "description": "1 by default. It represents the standard deviation of the noise added to odometry angles during the estimation process using G2O." + }, + { + "name": "measurement_noise_std_dev", + "type_name": "float", + "description": "1 by default. It seems to be used as a standard deviation for noise in measurements, possibly related to odometry or landmark estimation in SLAM (Simultaneous Localization and Mapping)." + }, + { + "name": "last_room_id", + "type_name": "None|str", + "description": "Used to store the previously visited room ID. It is initialized as None, but gets updated when the current room ID changes." + }, + { + "name": "actual_room_id", + "type_name": "int|None", + "description": "Set as follows:\n\n- Initially, it is None.\n- When a room node is found with its id matching `actual_room_id`, it becomes equal to that room's id.\n- When a new room node is found, `last_room_id` is set to the current `actual_room_id`, and `actual_room_id` is updated with the new room's id." + }, + { + "name": "elapsed", + "type_name": "float", + "description": "Initialized as `time.time()`. It seems to be used to track the time elapsed since the last update of some variables or data, possibly related to the robot's odometry." + }, + { + "name": "room_initialized", + "type_name": "bool", + "description": "Used to indicate whether the g2o graph has been successfully initialized for a specific room or not." + }, + { + "name": "iterations", + "type_name": "int", + "description": "Initialized as 0 in the constructor (`__init__`). It keeps track of the number of iterations since the last frame was processed." + }, + { + "name": "hide", + "type_name": "NoneType", + "description": "Set to True or False. However, there is no clear description or usage of this attribute within the provided code." + }, + { + "name": "init_graph", + "type_name": "bool", + "description": "Initialized as `False`. It seems to be used to track whether the g2o graph has been initialized or not, and its value changes after certain events such as a room change." + }, + { + "name": "current_edge_set", + "type_name": "bool", + "description": "Set to True when a current edge is detected between a room node and the Shadow node, indicating that the robot has moved into a new room." + }, + { + "name": "first_rt_set", + "type_name": "bool", + "description": "Set to True when a first RT edge (Robot Translation) is detected between the \"room\" node and the \"Shadow\" node, indicating that the robot has moved into a new room." + }, + { + "name": "translation_to_set", + "type_name": "Tuple[float,float,float]", + "description": "Set when a new RT edge is detected, storing the translation values (tx, ty, tz) for further processing in the G2O graph." + }, + { + "name": "rotation_to_set", + "type_name": "Tuple[float,float,float]", + "description": "Set when a \"RT\" edge (robot translation) from a room node to the robot node is detected, indicating that the robot's rotation needs to be updated." + }, + { + "name": "room_polygon", + "type_name": "QPolygonF", + "description": "Used to store a polygon representing the boundaries of the current room being explored by the robot." + }, + { + "name": "security_polygon", + "type_name": "QPolygonF", + "description": "Used to store a polygon that defines a region within which the robot must remain to ensure safety." + }, + { + "name": "initialize_g2o_graph", + "type_name": "bool", + "description": "Used to initialize a G2O graph object." + }, + { + "name": "rt_set_last_time", + "type_name": "float", + "description": "Set to the current time when a \"RT\" edge is updated between a room node and the robot node named \"Shadow\". This is used to track the last update time for RT edges." + }, + { + "name": "rt_time_min", + "type_name": "float", + "description": "1 second. It is used to determine when to update the translation and rotation values after setting a new RT edge." + }, + { + "name": "last_update_with_corners", + "type_name": "float", + "description": "Updated with the current time when the robot's corners are updated. It keeps track of the last update time for the corners." + }, + { + "name": "timer", + "type_name": "QtCoreQTimer", + "description": "Used to schedule a method (in this case, `compute`) at regular intervals (`self.Period`). It starts the timer after initializing the g2o graph." + }, + { + "name": "compute", + "type_name": "QtCoreSlot", + "description": "Used to compute and update the robot's position in real-time based on its odometry data and room information. It also checks for valid corners and doors, and updates the g2o graph accordingly." + }, + { + "name": "update_node_att", + "type_name": "NoneNone", + "description": "Not defined within the given code snippet." + }, + { + "name": "update_edge", + "type_name": "defupdate_edgeself,frint,toint,typestrNone", + "description": "Called when an edge in the graph changes. It handles different types of edges, such as \"current\" or \"RT\", and updates internal state accordingly." + }, + { + "name": "update_edge_att", + "type_name": "None", + "description": "Passed as a parameter to the `update_edge_att` method. It is not used anywhere in the code." + } + ], + "name": "SpecificWorker", + "location": { + "start": 50, + "insert": 51, + "offset": " ", + "indent": 4, + "comment": null + }, + "item_type": "class", + "length": 423, + "docLength": null + }, + { + "id": "d4c86ec3-d2a9-ebbb-6b41-649a5fecc346", + "ancestors": [ + "609f6edc-14d7-26a1-1c4c-62744b75a643" + ], + "description": "Initializes various attributes and performs startup checks or sets up internal APIs, graph structures, and timers based on input parameters. It connects signals to methods for handling node updates, edge updates, and other events.", + "params": [ + { + "name": "proxy_map", + "type_name": "object", + "description": "Passed to the superclass using `super(SpecificWorker, self).__init__(proxy_map)`. Its purpose is not explicitly mentioned in this snippet, but it might be related to initialization or setup of some sort." + }, + { + "name": "startup_check", + "type_name": "bool", + "description": "Used to decide whether to perform a startup check or not. If it is True, the `startup_check` method is called; otherwise, other initialization steps are performed." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker({\"proxy1\": \"localhost:9000\", \"proxy2\": \"localhost:9001\"}, startup_check=True)\n", + "description": "\n\nThis example demonstrates how to create a `SpecificWorker` instance with a dictionary containing proxy map and a boolean flag for the startup check." + }, + "name": "__init__", + "location": { + "start": 51, + "insert": 52, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "constructor", + "length": 54, + "docLength": null + }, + { + "id": "89daf63d-e190-4bac-1b45-3b07fbc03640", + "ancestors": [ + "609f6edc-14d7-26a1-1c4c-62744b75a643" + ], + "description": "Computes and updates the robot's pose, odometry, and edge information by analyzing the room environment and integrating new data from sensors. It also optimizes the pose estimation using the G2O library and updates the robot's position in the graph.", + "params": [], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map, startup_check=False)\nwhile True:\n worker.compute()\n time.sleep(0.02)", + "description": "" + }, + "name": "compute", + "location": { + "start": 123, + "insert": 125, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 103, + "docLength": null + }, + { + "id": "e8b6c91f-187a-74af-dc4e-6c91a9381907", + "ancestors": [ + "609f6edc-14d7-26a1-1c4c-62744b75a643" + ], + "description": "Initializes the G2O graph by setting fixed poses and adding nominal corners based on the robot's pose, room geometry, and door nodes. It also updates the actual room ID and checks for valid node attributes.", + "params": [], + "returns": { + "type_name": "bool|None", + "description": "True if successful and initializes a G2O graph, otherwise it returns None or False indicating failure." + }, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map)\nworker.initialize_g2o_graph()\n", + "description": "\nThis code instantiates a SpecificWorker object with proxy_map as its parameter, then calls the initialize_g2o_graph method." + }, + "name": "initialize_g2o_graph", + "location": { + "start": 253, + "insert": 256, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 115, + "docLength": null + }, + { + "id": "7897dc05-497c-c28e-3b4f-9ed0a337ea5c", + "ancestors": [ + "609f6edc-14d7-26a1-1c4c-62744b75a643" + ], + "description": "Calculates displacement values (lateral, advance, and angular) based on odometry data stored in a queue, using timestamps to match consecutive readings and applying a filtering factor (0.8).", + "params": [ + { + "name": "odometry", + "type_name": "Tuple[float, float, float, float]", + "description": "Assumed to represent the current odometry data." + } + ], + "returns": { + "type_name": "Tuple[float,float,float]", + "description": "A tuple containing three float values representing the lateral displacement, advance displacement, and angular displacement respectively." + }, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map)\nodometry = [(1, 2, 3, 4), (5, 6, 7, 8)]\nlateral, advance, angular = worker.get_displacement(odometry)", + "description": "" + }, + "name": "get_displacement", + "location": { + "start": 458, + "insert": 459, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 14, + "docLength": null + }, + { + "id": "5175f913-8e97-5989-554b-9ba4188aead9", + "ancestors": [ + "609f6edc-14d7-26a1-1c4c-62744b75a643" + ], + "description": "Computes and returns the covariance matrix for a given vertex using the g2o optimizer's compute_marginals function, and prints relevant messages indicating whether computation was successful or not.", + "params": [ + { + "name": "vertex", + "type_name": "object", + "description": "Expected to have attributes such as `hessian_index`." + } + ], + "returns": { + "type_name": "Tuple[Optional[str],Optional[Dict[int,float]]|None", + "description": "A tuple containing two values: the first one is an optional string (\"Covariance computed\" or \"Covariance not computed\") and the second one is an optional covariance matrix (or None)." + }, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map, startup_check=False)\nvertex = worker.g.get_node(\"Shadow\")\ncovariances_result, covariances = worker.get_covariance_matrix(vertex)", + "description": "" + }, + "name": "get_covariance_matrix", + "location": { + "start": 477, + "insert": 478, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 11, + "docLength": null + }, + { + "id": "0845024e-0656-4daa-114c-2c7c98f50930", + "ancestors": [ + "609f6edc-14d7-26a1-1c4c-62744b75a643" + ], + "description": "Visualizes the estimated positions of vertices and edges from a G2O optimization process in real-time using matplotlib's 3D plotting functionality.", + "params": [ + { + "name": "optimizer", + "type_name": "g2o::OptimizationAlgorithm", + "description": "Responsible for optimizing the graph represented by vertices and edges. It contains methods to load, iterate over vertices and edges, as well as other operations." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map)\nwhile True:\n worker.visualize_g2o_realtime(worker.optimizer)", + "description": "" + }, + "name": "visualize_g2o_realtime", + "location": { + "start": 490, + "insert": 491, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 29, + "docLength": null + }, + { + "id": "86df470e-4c72-b08b-6b41-4ed9860faf95", + "ancestors": [ + "609f6edc-14d7-26a1-1c4c-62744b75a643" + ], + "description": "Updates node attributes and appends data to a queue when the specified ID matches the odometry node ID, processing relevant attribute values from the Shadow node.", + "params": [ + { + "name": "id", + "type_name": "int", + "description": "Required when calling this function. It represents an identifier that determines which node attributes will be updated, specifically the odometry node if its value matches the self.odometry_node_id." + }, + { + "name": "attribute_names", + "type_name": "[str]", + "description": "Not used within the provided code snippet. Its presence suggests that it may be intended for future use or as a placeholder for some functionality, but its purpose remains unclear." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map, startup_check=True)\nworker.update_node_att(1, [\"attribute1\", \"attribute2\"])", + "description": "" + }, + "name": "update_node_att", + "location": { + "start": 527, + "insert": 536, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 7, + "docLength": null + }, + { + "id": "110e6d65-7753-3797-b145-9550aa15c08c", + "ancestors": [ + "609f6edc-14d7-26a1-1c4c-62744b75a643" + ], + "description": "Updates a node in a graph based on its ID and type, possibly initializing the graph if necessary for certain types of nodes, such as room corners.", + "params": [ + { + "name": "id", + "type_name": "int", + "description": "Required. It specifies the unique identifier for the node to be updated in the graph." + }, + { + "name": "type", + "type_name": "str", + "description": "Expected to be either \"corner\" or any other string value. This information will help determine how the node with given `id` should be updated." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map)\nworker.update_node(1, \"room\")", + "description": "" + }, + "name": "update_node", + "location": { + "start": 543, + "insert": 553, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 6, + "docLength": null + }, + { + "id": "e9952f42-2b9e-63ab-1849-1cb653ec4423", + "ancestors": [ + "609f6edc-14d7-26a1-1c4c-62744b75a643" + ], + "description": "Deletes a node based on its id and resets the `room_initialized` flag to False after deletion, implying that the room's initialization is undone as a result of deleting a node.", + "params": [ + { + "name": "id", + "type_name": "int", + "description": "Expected to receive an integer value representing the ID of a node to be deleted." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map, startup_check=True)\nnode = self.g.get_node(id)\nworker.delete_node(node.id)", + "description": "" + }, + "name": "delete_node", + "location": { + "start": 555, + "insert": 556, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 6, + "docLength": null + }, + { + "id": "984f9f40-6985-028f-ea43-2b448d8b12be", + "ancestors": [ + "609f6edc-14d7-26a1-1c4c-62744b75a643" + ], + "description": "Updates the current room ID and initializes/updates RT (Rotation Translation) settings based on the edge type (\"current\", \"RT\") and node attributes. It also prints status messages for debugging purposes.", + "params": [ + { + "name": "fr", + "type_name": "int", + "description": "Referred to as \"from\" or source node. It represents the node from which an edge originates in a graph, specifically a room in the context of this function." + }, + { + "name": "to", + "type_name": "int", + "description": "Used as an edge destination node ID when updating edges of graph `g`." + }, + { + "name": "type", + "type_name": "str", + "description": "Used to determine the action to be taken when updating an edge in the graph. It can have one of two values: \"current\" or \"RT\", depending on the type of update being performed." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map, startup_check=False)\nworker.update_edge(1, 2, \"current\")\nworker.update_edge(3, 4, \"RT\")", + "description": "" + }, + "name": "update_edge", + "location": { + "start": 564, + "insert": 565, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 22, + "docLength": null + }, + { + "id": "6e6d2faf-e905-d2a0-df4b-fc5568d9863a", + "ancestors": [ + "609f6edc-14d7-26a1-1c4c-62744b75a643" + ], + "description": "Removes an edge from the graph. The method takes three parameters: fr (from), to (to) and type, representing the edge's endpoints and type respectively.", + "params": [ + { + "name": "fr", + "type_name": "int", + "description": "Expected to represent the starting vertex of an edge in a graph. It specifies the node from which the edge originates." + }, + { + "name": "to", + "type_name": "int", + "description": "Likely an identifier for a node or vertex in a graph." + }, + { + "name": "type", + "type_name": "str", + "description": "Intended to specify the type or nature of the edge being deleted between nodes fr and to." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map, startup_check=False)\nworker.delete_edge(1, 2, \"edge_type\")", + "description": "" + }, + "name": "delete_edge", + "location": { + "start": 592, + "insert": 593, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 3, + "docLength": null + } + ] + } + } + }, + { + "name": "long_term_graph.py", + "path": "agents/long_term_spatial_memory_agent/scripts/long_term_graph.py", + "content": { + "structured": { + "description": "a function called draw_metric_map that takes in a metric map object and plots the map using Matplotlib. It first loads the graph from a text file and defines various functions to work with the graph, such as get_room_corners, get_room_objects, and get_room_objects_coordinates. These functions recursively traverse the graph to find the corners or objects in a room, respectively. The draw_metric_map function then uses these functions to plot the rooms and objects in the map. Specifically, it plots each room as a rectangle and adds names to each room, and it plots each object as a point and adds a text label with its name.", + "items": [ + { + "id": "5176435a-f92b-e4bd-1b4e-e1e310ab5ae8", + "ancestors": [], + "description": "Draws a graph of long-term spatial mobility data using PyQt and Matplotlib. It provides methods to visualize rooms, doors, walls, and edges in the graph.", + "attributes": [ + { + "name": "g", + "type_name": "Graph", + "description": "Used to represent the graph object that contains the rooms, doors, and walls to be visualized." + }, + { + "name": "read_graph", + "type_name": "instance", + "description": "Used to read a graph from a file specified by the user. It takes a string path as input and reads the graph data from it." + }, + { + "name": "fig", + "type_name": "instance", + "description": "A reference to the figure object that will be used to draw the graph." + }, + { + "name": "ax", + "type_name": "MatplotlibFigure", + "description": "Used to represent the axis object for the graph. It provides methods for adding patches, lines, and other visual elements to the graph." + } + ], + "name": "LongTermGraph", + "location": { + "start": 15, + "insert": 16, + "offset": " ", + "indent": 4, + "comment": null + }, + "item_type": "class", + "length": 198, + "docLength": null + }, + { + "id": "411ae665-2c3f-efb9-7546-d76e93280634", + "ancestors": [ + "5176435a-f92b-e4bd-1b4e-e1e310ab5ae8" + ], + "description": "Initializes an object of `LongTermGraph` class, loading a graph from a file using the `read_graph` method and displaying its summary.", + "params": [ + { + "name": "file_name", + "type_name": "str", + "description": "Used to specify the name of a file containing a graph represented as an adjacency matrix." + } + ], + "returns": null, + "name": "__init__", + "location": { + "start": 16, + "insert": 17, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 8, + "docLength": null + }, + { + "id": "fbfe681a-6ac8-9cb2-ad48-9c2d64fec643", + "ancestors": [ + "5176435a-f92b-e4bd-1b4e-e1e310ab5ae8" + ], + "description": "Generates a graphical representation of a subgraph within a larger graph, based on node and edge properties. It creates a figure and axis object, sets the title, and draws the nodes and edges using different colors for each type of node or edge.", + "params": [ + { + "name": "only_rooms", + "type_name": "bool", + "description": "Used to filter the nodes in the graph based on their types, only showing rooms and doors." + } + ], + "returns": null, + "name": "draw_graph", + "location": { + "start": 191, + "insert": 192, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 34, + "docLength": null + } + ] + } + } + }, + { + "name": "main.py", + "path": "agents/long_term_spatial_memory_agent/scripts/main.py", + "content": { + "structured": { + "description": "Three functions: draw_graph, find_edge_with_attribute, and get_connected_door_nodes. The draw_graph function takes a graph object as input and uses PySide2's QtCore module to create a subplot and plot points on the graph. The find_edge_with_attribute function searches for an edge in a given graph based on a specific attribute, and the get_connected_door_nodes function recursively traverses a graph to find all connected door nodes in a given room. The code also loads a graph from a pickled file using the LongTermGraph class, and uses the compute_metric_map and draw_metric_map functions to display the graph's metric map and point locations.", + "items": [ + { + "id": "d3824e62-3401-2c9e-394e-2ee9ee755e69", + "ancestors": [], + "description": "Generates a graph based on a provided adjacency matrix using Kamada-Kawai layout algorithm, and adds node names and edges with arrowheads.", + "params": [ + { + "name": "graph", + "type_name": "AbstractGraph", + "description": "Used to represent a graph object that contains vertices and edges." + } + ], + "returns": null, + "name": "draw_graph", + "location": { + "start": 6, + "insert": 7, + "offset": " ", + "indent": 4, + "comment": null + }, + "item_type": "function", + "length": 24, + "docLength": null + }, + { + "id": "d7906eb3-709a-3c95-7c40-18ce0ab8dcfd", + "ancestors": [], + "description": "Searches through a graph's edges for an edge with a specific attribute equal to a given value. If such an edge is found, it returns it; otherwise, it returns `None`.", + "params": [ + { + "name": "graph", + "type_name": "Graph", + "description": "Represented as an object that contains a collection of edges, where each edge represents a connection between two nodes in the graph." + }, + { + "name": "attribute", + "type_name": "attribute", + "description": "Used to specify the attribute of interest for finding an edge in a graph." + }, + { + "name": "value", + "type_name": "object", + "description": "Used to search for an edge in a graph based on a specific attribute." + } + ], + "returns": { + "type_name": "edge", + "description": "An untyped reference to a graph edge that has the specified attribute equal to the provided value." + }, + "name": "find_edge_with_attribute", + "location": { + "start": 41, + "insert": 42, + "offset": " ", + "indent": 4, + "comment": null + }, + "item_type": "function", + "length": 5, + "docLength": null + }, + { + "id": "fe2cae8a-54a8-32a7-014a-24ebf3a8e0f2", + "ancestors": [], + "description": "Iterates over the edges in a graph and adds to an output list any edge connecting nodes with \"door\" in their names.", + "params": [ + { + "name": "graph", + "type_name": "Graph", + "description": "Represented as g, which contains a collection of nodes and edges that define a graph structure." + } + ], + "returns": { + "type_name": "list", + "description": "A collection of edges from the given graph." + }, + "name": "get_room_edges", + "location": { + "start": 47, + "insert": 48, + "offset": " ", + "indent": 4, + "comment": null + }, + "item_type": "function", + "length": 10, + "docLength": null + }, + { + "id": "e9acd19c-513f-f78e-4949-cd353d1b9b16", + "ancestors": [], + "description": "In Java code recursively queries the graph for all nodes connected to a given node via doors, returning a list of such nodes.", + "params": [ + { + "name": "graph", + "type_name": "Graph", + "description": "Used to represent a graph structure." + }, + { + "name": "node", + "type_name": "GraphNode", + "description": "Referred to as a node in the graph." + } + ], + "returns": { + "type_name": "list", + "description": "A collection of nodes that are connected to a specific node through doors." + }, + "name": "get_connected_door_nodes", + "location": { + "start": 58, + "insert": 60, + "offset": " ", + "indent": 4, + "comment": null + }, + "item_type": "function", + "length": 10, + "docLength": null + }, + { + "id": "9a2a2e72-8415-229d-6c4a-657476ccda61", + "ancestors": [], + "description": "Navigates through a graph by starting from a given room and visiting all other rooms reachable through doors. It keeps track of visited rooms using a list and prints information about each room it visits.", + "params": [ + { + "name": "graph", + "type_name": "Graph", + "description": "Used to represent a graph with nodes and edges." + }, + { + "name": "current_room", + "type_name": "dict", + "description": "Represents the current room to be traversed in the graph." + }, + { + "name": "visited", + "type_name": "list", + "description": "Used to keep track of the rooms that have been visited during the traversal process, initialized to an empty list if None." + } + ], + "returns": { + "type_name": "list", + "description": "A collection of strings representing the rooms that have been visited." + }, + "name": "traverse_graph", + "location": { + "start": 74, + "insert": 75, + "offset": " ", + "indent": 4, + "comment": null + }, + "item_type": "function", + "length": 17, + "docLength": null + } + ] + } + } + }, + { + "name": "genericworker.py", + "path": "agents/long_term_spatial_memory_agent/src/genericworker.py", + "content": { + "structured": { + "description": "A worker class that inherits from QtWidgets.QWidget and implements a timer-based mechanism for killing itself after a specified period. It also provides a signal kill to allow for handling of the termination from outside the worker. The code uses PySide2, Ice, and the RoboCompCommonBehavior module.", + "items": [ + { + "id": "8a50128c-009f-a1ac-2c45-674f0e62438e", + "ancestors": [], + "description": "Manages a timer and a signal to stop its own execution. It has methods to change the timer period and to emit the signal to stop itself.", + "attributes": [ + { + "name": "kill", + "type_name": "QtCoreQObjectSlot", + "description": "Used to emit a signal that can be caught by any connected slots to stop the worker's execution." + }, + { + "name": "ui", + "type_name": "Ui_guiDlg", + "description": "Used to setup the user interface of the class." + }, + { + "name": "mutex", + "type_name": "QMutex", + "description": "Used to protect the worker's state from concurrent access." + }, + { + "name": "Period", + "type_name": "int", + "description": "Used to set the time interval for the timer signal emitted by the `setPeriod()` method, which changes its value on each call." + }, + { + "name": "timer", + "type_name": "QtCoreQTimer", + "description": "Used to start a timer that emits the `kill` signal after a specified period." + } + ], + "name": "GenericWorker", + "location": { + "start": 43, + "insert": 45, + "offset": " ", + "indent": 4, + "comment": null + }, + "item_type": "class", + "length": 26, + "docLength": null + }, + { + "id": "dc756aa5-ebc6-5490-d649-854bb2ac26a4", + "ancestors": [ + "8a50128c-009f-a1ac-2c45-674f0e62438e" + ], + "description": "Initializes an object of the `GenericWorker` class, setting up a UI widget, creating a mutex for synchronization, and defining a timer with a period of 500 milliseconds.", + "params": [ + { + "name": "mprx", + "type_name": "Ui_guiDlg", + "description": "Used as the parent widget for the GenericWorker object's UI." + } + ], + "returns": null, + "name": "__init__", + "location": { + "start": 47, + "insert": 48, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 9, + "docLength": null + }, + { + "id": "17fccd3d-bcc7-a9a3-5f45-73125f7d1f78", + "ancestors": [ + "8a50128c-009f-a1ac-2c45-674f0e62438e" + ], + "description": "Emits the `kill` signal, indicating that the object should be terminated.", + "params": [], + "returns": null, + "name": "killYourSelf", + "location": { + "start": 60, + "insert": 62, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 4, + "docLength": null + }, + { + "id": "a2ebb705-7932-fb9b-3e45-f965c61ebf4d", + "ancestors": [ + "8a50128c-009f-a1ac-2c45-674f0e62438e" + ], + "description": "Sets the period of a timer and updates the internal variable `Period`.", + "params": [ + { + "name": "p", + "type_name": "int", + "description": "Used to set the new period for the timer." + } + ], + "returns": null, + "name": "setPeriod", + "location": { + "start": 67, + "insert": 69, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 5, + "docLength": null + } + ] + } + } + }, + { + "name": "specificworker.py", + "path": "agents/long_term_spatial_memory_agent/src/specificworker.py", + "content": { + "structured": { + "description": "A class that represents an agent in a robot's environment. The agent uses graph theory and spatial reasoning to navigate through rooms, identify doors, and recognize patterns. The code utilizes igraph for graph manipulation and visualization, and NumPy for numerical computations. It also incorporates various functions for node and edge creation, updating, and deletion, as well as edge attributes modification and room image generation.", + "items": [ + { + "id": "895186b9-f902-39ab-ec46-de7001893323", + "ancestors": [], + "description": "Simulates a robot navigating through a virtual environment by traversing a graph, updating node and edge attributes, and performing tasks such as door associations, room creation, and path planning.", + "attributes": [ + { + "name": "Period", + "type_name": "None", + "description": "100 by default, indicating a period of time (in milliseconds) for computing tasks. It is used to set the timer interval for periodic computations." + }, + { + "name": "agent_id", + "type_name": "int", + "description": "13, which represents the ID of a specific agent in the robotic environment. It is used as an identifier for the agent's internal graph." + }, + { + "name": "g", + "type_name": "igraphGraph", + "description": "Used to interact with the graph library igraph. It contains methods to insert, delete, update nodes and edges in the graph, as well as traverse the graph." + }, + { + "name": "startup_check", + "type_name": "None|bool", + "description": "Set by default to None. It is a simple flag that is used for checking if the startup process has completed successfully." + }, + { + "name": "rt_api", + "type_name": "object", + "description": "Used to get edge RT (Rotation and Translation) from the `room_exit_door_id` to the robot node." + }, + { + "name": "inner_api", + "type_name": "object", + "description": "Used for internal API operations, likely related to robot pose transformations, door connections, or other internal mechanisms. The exact functionality depends on the implementation details." + }, + { + "name": "testing", + "type_name": "Attribute", + "description": "2. It seems to be used as a flag for testing mode, controlling certain behavior or actions within the class." + }, + { + "name": "robot_name", + "type_name": "str", + "description": "Used as a name for the robot node in the graph. It appears to be used to identify the robot's location within the graph." + }, + { + "name": "robot_id", + "type_name": "int", + "description": "13, as initialized in the `__init__` method. It appears to be a unique identifier for the robot node in the graph." + }, + { + "name": "last_robot_pose", + "type_name": "npfloat64", + "description": "3-dimensional, representing the last known pose (position and orientation) of the robot in the room. It is used to store the previous position of the robot before updating it with new data." + }, + { + "name": "robot_exit_pose", + "type_name": "npndarray[npfloat64,1D]", + "description": "3-dimensional representing a pose (x, y, z) of the robot when exiting a room. It stores the final affordance pose transformed to the global reference system." + }, + { + "name": "state", + "type_name": "str|None", + "description": "Used to keep track of the current state of the worker, such as \"idle\", \"crossing\", \"known_room\", etc., which determines how it should behave in different situations." + }, + { + "name": "affordance_node_active_id", + "type_name": "int", + "description": "13 by default, which corresponds to the agent's ID. This variable seems to keep track of the active affordance node in the graph." + }, + { + "name": "exit_door_id", + "type_name": "int", + "description": "Used as a reference to identify the ID of the door that represents the exit from a room." + }, + { + "name": "room_exit_door_id", + "type_name": "int", + "description": "13, as specified in the class initialization method (`__init__`). It represents the ID of the exit door node in the graph." + }, + { + "name": "enter_room_node_id", + "type_name": "int", + "description": "Set when a new room is entered. It holds the ID of the node that represents the current room being explored by the robot." + }, + { + "name": "vertex_size", + "type_name": "int", + "description": "0 initially. It increments by 1 each time a new vertex (node) is inserted into the graph, keeping track of the number of vertices created so far." + }, + { + "name": "not_required_attrs", + "type_name": "List[str]", + "description": "Used to store the names of attributes that are not required for vertex objects in the graph, such as \"parent\", \"timestamp_alivetime\", etc. These attributes are ignored when inserting vertices into the graph." + }, + { + "name": "last_save_time", + "type_name": "float", + "description": "Used to store the timestamp when the worker's state was last saved or updated." + }, + { + "name": "long_term_graph", + "type_name": "igraphGraph", + "description": "Used to store and manipulate the long-term graph data structure, which represents the agent's internal model of its environment." + }, + { + "name": "room_number", + "type_name": "int", + "description": "1-based indexing for the room ID. It represents the unique identifier of a room node in the graph data structure." + }, + { + "name": "room_number_limit", + "type_name": "None|int", + "description": "0 by default. It seems to be related to the limit of rooms in a generated apartment, used for procedural room generation." + }, + { + "name": "insert_current_edge", + "type_name": "None", + "description": "Used to set a current edge between two nodes in the graph, updating the room state to idle." + }, + { + "name": "pending_doors_to_stabilize", + "type_name": "List[tuple[str,str]]", + "description": "Initialized with a deque(maxlen=10) to store door names to be stabilized later. It keeps track of doors that need to be connected in the graph based on certain conditions." + }, + { + "name": "timer", + "type_name": "QTimer", + "description": "Used for scheduling the execution of a method after a certain time delay or interval (set by the `start` method)." + }, + { + "name": "compute", + "type_name": "None|Callable[[Any],Any]", + "description": "Used as a slot for handling timer events. It contains a method that gets called at regular intervals, performing specific tasks depending on the state of the worker." + }, + { + "name": "update_node", + "type_name": "None|int,str", + "description": "Used to update a node with the given id and type." + }, + { + "name": "update_edge", + "type_name": "NoneNonefrint,toint,typestr", + "description": "Used to update edges in the graph, specifically when the edge's destination is the robot ID, source node is not the room exit door, type is \"RT\" and there are no current edges." + } + ], + "name": "SpecificWorker", + "location": { + "start": 52, + "insert": 53, + "offset": " ", + "indent": 4, + "comment": null + }, + "item_type": "class", + "length": 699, + "docLength": null + }, + { + "id": "f43929f0-c77b-5d99-7d40-e09555e1a3ab", + "ancestors": [ + "895186b9-f902-39ab-ec46-de7001893323" + ], + "description": "Initializes various attributes, sets up graph data structures, and connects signals to methods for updating nodes and edges. It also starts a timer that triggers the compute method at regular intervals.", + "params": [ + { + "name": "proxy_map", + "type_name": "object", + "description": "Passed to the superclass constructor using the `super()` function, indicating that it should be used for initialization. Its purpose and content are not specified within this code block." + }, + { + "name": "startup_check", + "type_name": "bool", + "description": "Set to False by default. It determines whether a startup check should be performed or not. If set to True, it calls the `startup_check` method; otherwise, it initializes the other parts of the class." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "specific_worker = SpecificWorker(proxy_map, startup_check=True)\n", + "description": "\nNote that this example only shows a basic usage of the class without any specific parameters." + }, + "name": "__init__", + "location": { + "start": 53, + "insert": 54, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "constructor", + "length": 65, + "docLength": null + }, + { + "id": "49b3ce54-b2b8-e8ac-844b-ec91e99feeab", + "ancestors": [ + "895186b9-f902-39ab-ec46-de7001893323" + ], + "description": "Sets parameters for a scenario and performs subsequent actions on doors, including removing self-edges, storing door IDs, updating door attributes, and adding an attribute to an entrance door node.", + "params": [ + { + "name": "params", + "type_name": "Dict[any, any]", + "description": "Expected to contain various parameters used for setting up the current room state in the game." + } + ], + "returns": { + "type_name": "bool", + "description": "True." + }, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map, startup_check=False)\nparams = {\"agent_id\": 13, \"robot_name\": \"Shadow\"}\nresult = worker.setParams(params)", + "description": "" + }, + "name": "setParams", + "location": { + "start": 147, + "insert": 148, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 9, + "docLength": null + }, + { + "id": "565d28ea-34ad-14ae-2d4a-eea4ebaac461", + "ancestors": [ + "895186b9-f902-39ab-ec46-de7001893323" + ], + "description": "Computes the position and orientation of rooms and doors, generates JSON data for each room, saves it to a file, and then terminates the process if testing mode is enabled.", + "params": [], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map)\nwhile True:\n worker.compute()\n time.sleep(worker.Period)", + "description": "" + }, + "name": "compute", + "location": { + "start": 163, + "insert": 169, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 60, + "docLength": null + }, + { + "id": "b693ac11-a005-33bb-6c45-152db71efd4e", + "ancestors": [ + "895186b9-f902-39ab-ec46-de7001893323" + ], + "description": "Initializes a room structure from an igraph object, creates a root node and edges, updates node attributes, and inserts or assigns new edges into the graph.", + "params": [], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map, startup_check=True)\nworker.initialize_room_from_igraph()\n", + "description": "" + }, + "name": "initialize_room_from_igraph", + "location": { + "start": 237, + "insert": 239, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 22, + "docLength": null + }, + { + "id": "b242f7eb-eb84-9880-b745-8ba8bec9e950", + "ancestors": [ + "895186b9-f902-39ab-ec46-de7001893323" + ], + "description": "Updates the robot's pose in an igraph graph by reflecting changes from a long-term graph to the current graph and saving the updated graph. It handles cases where the robot moves between rooms or when it is not found in the igraph.", + "params": [], + "returns": null, + "usage": { + "language": "python", + "code": "sw = SpecificWorker(proxy_map, startup_check=True)\nsw.update_robot_pose_in_igraph()\n", + "description": "\n\nThis code initializes a SpecificWorker object with proxy_map and sets the startup_check to True. It then calls the update_robot_pose_in_igraph method on this object." + }, + "name": "update_robot_pose_in_igraph", + "location": { + "start": 270, + "insert": 272, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 41, + "docLength": null + }, + { + "id": "bced392b-adf3-4a9f-ed49-12d2abc2f0d9", + "ancestors": [ + "895186b9-f902-39ab-ec46-de7001893323" + ], + "description": "Handles affordance nodes, stabilizes doors, and computes robot poses for crossing between rooms. It updates the graph state and prints relevant information during its execution.", + "params": [], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map, startup_check=True)\nworker.idle()\n", + "description": "\nThis code snippet creates a specific worker object and then invokes its idle method. The idle method performs some checks based on the worker's state and graph data, and if certain conditions are met, it may update the worker's state to \"crossing\"." + }, + "name": "idle", + "location": { + "start": 323, + "insert": 325, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 86, + "docLength": null + }, + { + "id": "4b50aa68-287c-e0a6-094a-b49300e80dd5", + "ancestors": [ + "895186b9-f902-39ab-ec46-de7001893323" + ], + "description": "Updates the room state and door connections based on the current affordance node and exit door node in the graph.", + "params": [], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map)\nworker.crossed()\n", + "description": "\nThis code creates an instance of the class SpecificWorker, and then calls its method crossed." + }, + "name": "crossed", + "location": { + "start": 436, + "insert": 438, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 16, + "docLength": null + }, + { + "id": "f0eea51e-80ec-3382-9845-e7ae11931b76", + "ancestors": [ + "895186b9-f902-39ab-ec46-de7001893323" + ], + "description": "Retrieves a list of room nodes, selects the first node, sets it as the current room, and inserts an edge representing this change into a graph. The method then changes its internal state to \"initializing_doors\" and prints a status message.", + "params": [], + "returns": null, + "usage": { + "language": "python", + "code": "specific_worker = SpecificWorker(proxy_map, startup_check=True)\nspecific_worker.initializing_room()\n", + "description": "" + }, + "name": "initializing_room", + "location": { + "start": 456, + "insert": 459, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 10, + "docLength": null + }, + { + "id": "b33bb58f-ff15-99bd-bf41-461d97919761", + "ancestors": [ + "895186b9-f902-39ab-ec46-de7001893323" + ], + "description": "Navigates from the current room to another connected room through an exit door, updates the graph representation of the new room and door, and adjusts the robot's position and orientation accordingly.", + "params": [], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map)\nworker.known_room()\n", + "description": "\nThis will start executing the method known_room, passing no arguments. The worker class object must have been previously instantiated and initialized properly before calling this function." + }, + "name": "known_room", + "location": { + "start": 470, + "insert": 472, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 73, + "docLength": null + }, + { + "id": "09302d92-1220-46a5-564c-abb4ed5068ba", + "ancestors": [ + "895186b9-f902-39ab-ec46-de7001893323" + ], + "description": "Initializes and associates exit doors with their corresponding rooms, storing the door names and connected room names as attributes. It also updates the graph and sets a state to \"removing\".", + "params": [], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(None, startup_check=False)\nworker.exit_door_id = 123\nworker.initializing_doors()", + "description": "" + }, + "name": "initializing_doors", + "location": { + "start": 564, + "insert": 566, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 26, + "docLength": null + }, + { + "id": "860d158e-bcd7-bbb3-ff4f-c34b26f5454f", + "ancestors": [ + "895186b9-f902-39ab-ec46-de7001893323" + ], + "description": "Associates two doors by adding an edge between their corresponding nodes in the long-term graph, and sets additional attributes on each node to store information about the other door and connected room.", + "params": [ + { + "name": "door_1", + "type_name": "Tuple[str, str]", + "description": "Expected to represent a pair of door names with connected rooms' names as its elements." + }, + { + "name": "door_2", + "type_name": "Tuple[str, str]", + "description": "Expected to contain the name of the second door and its connected room. It is used to find the node corresponding to this door in the graph and associate it with the first door." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(None)\ndoor_1 = (\"Door 1\", \"Room A\", 123)\ndoor_2 = (\"Door 2\", \"Room B\", 456)\nworker.associate_doors(door_1, door_2)", + "description": "" + }, + "name": "associate_doors", + "location": { + "start": 601, + "insert": 603, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 20, + "docLength": null + }, + { + "id": "7d9e23ea-fe30-b6b7-f846-d999e5057864", + "ancestors": [ + "895186b9-f902-39ab-ec46-de7001893323" + ], + "description": "Stores the current state of a graph, represented by an igraph object, into a pickle file named \"graph.pkl\". It first retrieves and verifies the existence of a room node in the graph before storing it.", + "params": [], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map, startup_check=True)\nworker.store_graph()", + "description": "" + }, + "name": "store_graph", + "location": { + "start": 624, + "insert": 625, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 11, + "docLength": null + }, + { + "id": "242f4fee-7403-e2be-6f4d-a278ec8a96c9", + "ancestors": [ + "895186b9-f902-39ab-ec46-de7001893323" + ], + "description": "Deletes nodes and edges from the graph based on certain conditions. It first identifies nodes connected to a specified room exit door, then removes nodes with no outgoing edges and finally removes destinations of remaining edges sorted by their values in descending order.", + "params": [], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map, startup_check=True)\nworker.removing()", + "description": "" + }, + "name": "removing", + "location": { + "start": 638, + "insert": 640, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 19, + "docLength": null + }, + { + "id": "77fff487-b57f-4fa4-1d44-3789f5a13b53", + "ancestors": [ + "895186b9-f902-39ab-ec46-de7001893323" + ], + "description": "Recursively traverses a graph, starting from a specified node, and inserts nodes and edges into an igraph object. It identifies RT-type edges with destinations other than the robot ID and explores them further.", + "params": [ + { + "name": "node_id", + "type_name": "int", + "description": "Expected to be an identifier for a node within the graph data structure (`self.g`)." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map)\nnode_id = 5\nworker.traverse_graph(node_id)", + "description": "" + }, + "name": "traverse_graph", + "location": { + "start": 664, + "insert": 666, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 7, + "docLength": null + }, + { + "id": "edceaa7c-333f-dd99-9e44-b9bdbae031be", + "ancestors": [ + "895186b9-f902-39ab-ec46-de7001893323" + ], + "description": "Traverses the long-term graph, starting from a given node, and recursively explores its successors, inserting new vertices and edges into a data structure whenever it finds a suitable successor with higher level and same room_id as the current node.", + "params": [ + { + "name": "node", + "type_name": "igraph.vs", + "description": "Passed to this function. It represents an individual vertex (node) within the graph and contains information about the node, such as its index, name, room_id, and level." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(None, startup_check=False)\nnode = worker.long_term_graph.get_node(\"Initial Room\")\nworker.traverse_igraph(node)", + "description": "" + }, + "name": "traverse_igraph", + "location": { + "start": 674, + "insert": 675, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 10, + "docLength": null + }, + { + "id": "a71b8f95-8dea-f190-3143-5b6d1f0a1dfd", + "ancestors": [ + "895186b9-f902-39ab-ec46-de7001893323" + ], + "description": "Adds a vertex to an igraph graph object, long_term_graph.g, with attributes from a given node object. It also checks for specific attributes and adds edges between vertices if necessary.", + "params": [ + { + "name": "node", + "type_name": "object", + "description": "Assumed to have attributes such as `name`, `id`, `type`, and possibly others like `attrs`. The `node` parameter is used to add vertices to an igraph graph and also potentially establish edges between them." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "specific_worker = SpecificWorker(proxy_map, startup_check=True)\nnode = Node(name='room1', id=1, type='room')\nnode.attrs['connected_room_name'] = ConnectedRoomName('other_room', 'door1')\nnode.attrs['other_side_door_name'] = OtherSideDoorName('door2', 'other_side_door_name')\nspecific_worker.insert_igraph_vertex(node)", + "description": "" + }, + "name": "insert_igraph_vertex", + "location": { + "start": 687, + "insert": 688, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 29, + "docLength": null + }, + { + "id": "021e56e0-815f-d9a6-5343-d8f2a6d2e7bd", + "ancestors": [ + "895186b9-f902-39ab-ec46-de7001893323" + ], + "description": "Inserts a new node into a graph, creating it from a given dictionary and linking it to an existing parent node with the provided name. It also copies over some attributes from the original node.", + "params": [ + { + "name": "parent_name", + "type_name": "str", + "description": "Used to get an existing node from the graph using its name, which is then used as the parent for the new vertex being inserted into the graph." + }, + { + "name": "node", + "type_name": "Dict", + "description": "Expected to contain attributes such as \"type\", \"name\" that are used to create a new Node object. The values in this dictionary are used to set the properties of the newly created node." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "specific_worker = SpecificWorker(proxy_map, startup_check=False)\ndsr_node = {\"type\": \"vertex\", \"name\": \"new_vertex\", \"attributes\": {\"color\": \"red\"}}\nspecific_worker.insert_dsr_vertex(\"parent_name\", dsr_node)", + "description": "" + }, + "name": "insert_dsr_vertex", + "location": { + "start": 722, + "insert": 724, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 9, + "docLength": null + }, + { + "id": "1c8acec2-134d-5faf-3c43-92947a256819", + "ancestors": [ + "895186b9-f902-39ab-ec46-de7001893323" + ], + "description": "Inserts an edge into the long-term graph based on the origin and destination nodes, considering translation and rotation attributes from the provided edge. It also applies special handling for door destinations.", + "params": [ + { + "name": "edge", + "type_name": "object", + "description": "Likely an instance of a class representing an edge in a graph, possibly containing information about its origin, destination, translation, rotation, and other attributes." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map, startup_check=False)\nedge = Edge(origin=\"room1\", destination=\"door1\")\nworker.insert_igraph_edge(edge)", + "description": "" + }, + "name": "insert_igraph_edge", + "location": { + "start": 736, + "insert": 737, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 15, + "docLength": null + }, + { + "id": "30527036-60e3-8886-a946-2f1b41d32ef6", + "ancestors": [ + "895186b9-f902-39ab-ec46-de7001893323" + ], + "description": "Inserts or updates an edge between two nodes in a graph based on whether one node (org) exists or not. It retrieves relevant data, creates a new edge with attributes and inserts/assigns it to the graph.", + "params": [ + { + "name": "org", + "type_name": "object | None", + "description": "Used to specify whether a root node should be created or an existing node from which to create a new edge." + }, + { + "name": "dest", + "type_name": "Dict[str, int]", + "description": "Used to represent a destination node in the graph. It contains a \"name\" key with a string value representing the name of the node, and an \"index\" key with an integer value representing the index of the node." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "specific_worker = SpecificWorker(proxy_map, startup_check=True)\norg = Node(\"room1\", \"room\")\ndest = Node(\"door2\", \"door\")\nspecific_worker.insert_dsr_edge(org, dest)", + "description": "" + }, + "name": "insert_dsr_edge", + "location": { + "start": 757, + "insert": 760, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 22, + "docLength": null + }, + { + "id": "b71963ca-d296-d386-3542-ca6aca921929", + "ancestors": [ + "895186b9-f902-39ab-ec46-de7001893323" + ], + "description": "Retrieves the room ID associated with a node identified by its node ID from a graph, and returns it if found; otherwise, it returns -1. The method handles potential exceptions that may occur during the retrieval process.", + "params": [ + { + "name": "node_id", + "type_name": "int | str", + "description": "Required for this function. It represents an identifier that uniquely identifies a node within a graph data structure." + } + ], + "returns": { + "type_name": "str|int", + "description": "Either a room ID associated with a node or -1 if no room ID is found for that node." + }, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(None, startup_check=False)\nroom_id = worker.check_element_room_number(\"node123\")\n", + "description": "\nThis code creates an instance of the `SpecificWorker` class and then calls its `check_element_room_number` method with a specific node ID (`\"node123\"`). The function checks if the node has a \"room_id\" attribute, returns it if found, or -1 if not." + }, + "name": "check_element_room_number", + "location": { + "start": 787, + "insert": 788, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 7, + "docLength": null + }, + { + "id": "71c5bfb8-3835-7788-9042-be2ab507cbc5", + "ancestors": [ + "895186b9-f902-39ab-ec46-de7001893323" + ], + "description": "Retrieves an element level from the attributes of a node with the given ID in the graph, and returns it if found; otherwise, prints an error message and returns -1.", + "params": [ + { + "name": "node_id", + "type_name": "object", + "description": "Expected to be the ID of a node stored in the graph `self.g`. It is used to retrieve information about the node from the graph." + } + ], + "returns": { + "type_name": "int|str", + "description": "1) the value of \"level\" attribute of node with given id if present, or 2) -1 and a print statement indicating that no such attribute was found." + }, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map, startup_check=False)\nnode_id = \"my_node\"\nelement_level = worker.check_element_level(node_id)\nif element_level == -1:\n # handle node with no element_level attribute\nelse:\n # use the retrieved element_level value", + "description": "" + }, + "name": "check_element_level", + "location": { + "start": 796, + "insert": 797, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 25, + "docLength": null + }, + { + "id": "8d17504d-3d97-6684-5142-8bb1f661e2e4", + "ancestors": [ + "895186b9-f902-39ab-ec46-de7001893323" + ], + "description": "Retrieves information about a given room node, identifies edges of type \"RT\" that are connected to it, and extracts translation data from these edges.", + "params": [ + { + "name": "room_node_id", + "type_name": "int", + "description": "Used to retrieve a specific node from a graph object (`self.g`). This node represents a room, and its id is needed to generate a picture for this room." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map, startup_check=False)\nroom_node_id = 1234\nworker.generate_room_picture(room_node_id)", + "description": "" + }, + "name": "generate_room_picture", + "location": { + "start": 839, + "insert": 841, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 24, + "docLength": null + }, + { + "id": "0e858f9e-90d2-f5ac-9c4d-3bca8d1e9ec8", + "ancestors": [ + "895186b9-f902-39ab-ec46-de7001893323" + ], + "description": "Inserts or updates an edge in a graph (self.g). The edge represents a current connection between two rooms with given IDs, identified by the agent ID. It is created based on room_id and its mirrored version.", + "params": [ + { + "name": "room_id", + "type_name": "int", + "description": "Required for the creation of an `Edge` object. It represents the ID of a room that will be part of the edge being inserted into the graph." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map, startup_check=False)\nroom_id = \"12345\"\nworker.insert_current_edge(room_id)", + "description": "" + }, + "name": "insert_current_edge", + "location": { + "start": 874, + "insert": 876, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 3, + "docLength": null + }, + { + "id": "5d2abf5a-2590-8e8d-3143-a0b3002108a3", + "ancestors": [ + "895186b9-f902-39ab-ec46-de7001893323" + ], + "description": "Executes a shell script, sets its executable permissions, runs it with the argument 'true', and captures any output or errors.", + "params": [], + "returns": null, + "usage": { + "language": "python", + "code": "specific_worker = SpecificWorker(proxy_map)\nspecific_worker.kill_everything()", + "description": "" + }, + "name": "kill_everything", + "location": { + "start": 879, + "insert": 881, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 8, + "docLength": null + }, + { + "id": "af096c0a-4c0c-e28b-6748-b352cae47b40", + "ancestors": [ + "895186b9-f902-39ab-ec46-de7001893323" + ], + "description": "Updates the node with given id and type in the graph. If the type is \"door\", it inserts the door node into an igraph graph and adds edges between nodes. If the id matches the affordance node active id, it checks the state of the affordance node and changes its state if necessary.", + "params": [ + { + "name": "id", + "type_name": "int", + "description": "Used as an identifier for nodes in a graph, specifically to identify either a door node or an affordance node based on the value of the `type` parameter." + }, + { + "name": "type", + "type_name": "str", + "description": "Used to determine what action should be taken when updating a node with the given id. The valid values for this parameter are \"door\" or other types that may be added in the future." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map, startup_check=False)\ndoor_id = 42\nworker.update_node(door_id, \"door\")", + "description": "" + }, + "name": "update_node", + "location": { + "start": 902, + "insert": 903, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 36, + "docLength": null + }, + { + "id": "2f5f1ef6-a378-a8ba-2246-9cb42852f320", + "ancestors": [ + "895186b9-f902-39ab-ec46-de7001893323" + ], + "description": "Updates the current edge when a specific condition is met: if there are no current edges, the update edge originates from the room exit door, and its type is \"RT\". It inserts the edge into the graph and prints a message.", + "params": [ + { + "name": "fr", + "type_name": "int", + "description": "Likely to represent the from ID, indicating the starting point of an edge in the graph." + }, + { + "name": "to", + "type_name": "int", + "description": "Used to specify the destination of an edge in a graph, which represents the ID of another node in the graph." + }, + { + "name": "type", + "type_name": "str", + "description": "Expected to be set with value \"RT\". This indicates that it is an exit door edge, which leads to the robot's current room." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map)\nworker.update_edge(42, 13, \"RT\")", + "description": "" + }, + "name": "update_edge", + "location": { + "start": 957, + "insert": 961, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 5, + "docLength": null + } + ] + } + } + }, + { + "name": "specificworker_sec.py", + "path": "agents/long_term_spatial_memory_agent/src/specificworker_sec.py", + "content": { + "structured": { + "description": "An igraph object `self` and implements various operations on a graph, including inserting nodes and edges, updating edge attributes, and deleting nodes and edges. The code uses the `igraph` package to perform these operations. Specifically, it inserts a node in the graph with a certain name and ID, updates the RT translation and rotation of an edge connecting two nodes, and deletes a node and edge from the graph. Additionally, it checks if an affordance node is active and sets the state to \"crossed\" if it is completed and not active.", + "items": [ + { + "id": "75233bfc-d379-5786-3e4d-6b16aa1599c4", + "ancestors": [], + "description": "Manages a graph representation of a robot navigating through a room, handling various actions such as inserting, updating, and deleting nodes and edges, as well as tracking the current state of the room.", + "attributes": [ + { + "name": "Period", + "type_name": "Union[float,int]", + "description": "Used to represent the time period for which the worker is active." + }, + { + "name": "agent_id", + "type_name": "int|str", + "description": "Used to store the unique identifier of the agent in the simulation." + }, + { + "name": "g", + "type_name": "igraphGraph", + "description": "Used to represent the graph of nodes and edges in the environment. It is used for various operations such as inserting, deleting, updating nodes and edges, and drawing the graph." + }, + { + "name": "update_node", + "type_name": "Dict[str,Any]", + "description": "Used to update the node attributes of a specific node in the graph based on its ID. It takes two arguments: `id` (int) which is the ID of the node to be updated, and `type` (str) which can be either \"door\" or \"room\" indicating whether the node is a door or room node. The function performs different actions depending on the type of node being updated." + }, + { + "name": "update_edge", + "type_name": "Union[int,str]", + "description": "Used to update the edge attributes of a robot-room pair based on certain conditions, such as when there is no current edge and the room node exists." + }, + { + "name": "startup_check", + "type_name": "bool|str", + "description": "Used to check if the worker should perform startup tasks such as inserting a node and edge for a room and robot, and setting the state to \"crossed\"." + }, + { + "name": "rt_api", + "type_name": "str|int", + "description": "Used to store the ID of the robot that the worker is associated with, allowing the worker to perform RT-based actions." + }, + { + "name": "inner_api", + "type_name": "Dict[str,Any]", + "description": "Used to store additional APIs that are specific to this worker." + }, + { + "name": "robot_name", + "type_name": "str|int", + "description": "Used to store the ID of the robot that the worker is associated with." + }, + { + "name": "robot_id", + "type_name": "int", + "description": "Used to identify the robot that the worker is controlling." + }, + { + "name": "last_robot_pose", + "type_name": "Tuple[float,float,float]", + "description": "Used to store the last known pose of the robot in the environment, which can be used for various tasks such as path planning and obstacle avoidance." + }, + { + "name": "robot_exit_pose", + "type_name": "str|int", + "description": "Used to store the exit pose of a robot, which is the position and orientation of the robot when it exits a room." + }, + { + "name": "state", + "type_name": "str|int", + "description": "Used to keep track of the worker's current state (either \"idle\", \"crossed\", or \"completed\")." + }, + { + "name": "affordance_node_active_id", + "type_name": "int|bool", + "description": "Used to keep track of the active affordance node ID in the graph. It is used to determine when the affordance node has been completed and can transition to the crossed state." + }, + { + "name": "exit_door_id", + "type_name": "int|str", + "description": "Used to keep track of the id of the door node that leads from a room to the outside world, which is used for various purposes such as routing, affordance detection, and edge insertion." + }, + { + "name": "room_exit_door_id", + "type_name": "int|str", + "description": "Used to represent the ID of the door node that marks the exit of a room in the graph. It is used for updating edges and inserting current edges." + }, + { + "name": "enter_room_node_id", + "type_name": "int|str", + "description": "Used to store the ID of the room node that the worker enters when it completes its task." + }, + { + "name": "vertex_size", + "type_name": "float|int", + "description": "Used to control the size of the vertices in the graph. It determines the width or height of each vertex in the graph, which can be useful for visualization purposes." + }, + { + "name": "not_required_attrs", + "type_name": "List[str]", + "description": "Used to store a list of attribute names that are not required for the worker's functionality, i.e., they are optional or non-essential attributes." + }, + { + "name": "long_term_graph", + "type_name": "igraphGraph", + "description": "Used to store the long-term graph of the environment, which can be different from the short-term graph represented by the `g` attribute." + }, + { + "name": "graph", + "type_name": "igraphGraph", + "description": "Used to store the current state of the graph, which can be updated and manipulated during the execution of the worker's methods." + }, + { + "name": "insert_current_edge", + "type_name": "List[Edge]", + "description": "Used to insert a new edge into the graph with the specified from and to nodes, and with the \"current\" type." + }, + { + "name": "timer", + "type_name": "int|str", + "description": "Used to store a timer for the worker, indicating how long it has been running." + }, + { + "name": "compute", + "type_name": "str|int", + "description": "Used to store the ID of the node that should be updated or inserted in the graph during computation." + } + ], + "name": "SpecificWorker", + "location": { + "start": 49, + "insert": 50, + "offset": " ", + "indent": 4, + "comment": null + }, + "item_type": "class", + "length": 592, + "docLength": null + }, + { + "id": "1652a875-3a7c-04b8-3944-add50b4df62e", + "ancestors": [ + "75233bfc-d379-5786-3e4d-6b16aa1599c4" + ], + "description": "Initializes the worker's internal state, including its graph, node ID, and affordance node active ID. It also connects signals for updating nodes and edges and sets up a timer to call the `compute` method periodically.", + "params": [ + { + "name": "proxy_map", + "type_name": "Dict[str, Any]", + "description": "Used to store a map of proxy nodes for the specific worker." + }, + { + "name": "startup_check", + "type_name": "bool", + "description": "Used to check if the graph has already been initialized before creating its inner API. It is set to False by default, meaning no check is performed." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "# create an instance of SpecificWorker class\nspecific_worker = SpecificWorker(proxy_map, startup_check=False)\n\n# call the function to initialize the worker\nspecific_worker.__init__(proxy_map, startup_check=False)\n", + "description": "\nIn this example, we create an instance of the `SpecificWorker` class and then call its `__init__` method with the required parameters. The `__init__` method initializes the worker by setting up the necessary connections to the Signals module, creating a new graph if it does not exist, and starting the timer for the computation." + }, + "name": "__init__", + "location": { + "start": 50, + "insert": 51, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "constructor", + "length": 53, + "docLength": null + }, + { + "id": "9e3a3660-ca76-efa0-424c-7384a6dfdac5", + "ancestors": [ + "75233bfc-d379-5786-3e4d-6b16aa1599c4" + ], + "description": "Sets the parameters passed as an argument, then modifies the room by removing a self-edge and adding attributes to doors.", + "params": [ + { + "name": "params", + "type_name": "bool", + "description": "Passed to set the parameters of the Room object." + } + ], + "returns": { + "type_name": "bool", + "description": "True." + }, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map)\nworker.setParams({'Period': 50})\n", + "description": "\nIn this example, we create a new instance of the SpecificWorker class and pass in a proxy map as an argument to its constructor. We then use the setParams method to update the Period attribute of the worker instance to 50." + }, + "name": "setParams", + "location": { + "start": 124, + "insert": 125, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 7, + "docLength": null + }, + { + "id": "13115bf5-e637-5cb7-b343-250c1fd6afbf", + "ancestors": [ + "75233bfc-d379-5786-3e4d-6b16aa1599c4" + ], + "description": "Computes the RT of a robot in a graph, taking into account the current edges and long-term graph information.", + "params": [], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map)\nworker.compute()\n", + "description": "\nIn this example, the user creates an instance of the class SpecificWorker and calls its compute method to execute the code inside it." + }, + "name": "compute", + "location": { + "start": 138, + "insert": 141, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 37, + "docLength": null + }, + { + "id": "592108b6-327e-ba98-1c49-e5b2b4fb8c1f", + "ancestors": [ + "75233bfc-d379-5786-3e4d-6b16aa1599c4" + ], + "description": "Checks if there are any \"current\" edges or affordance nodes in the graph, and performs actions based on their existence.", + "params": [], + "returns": null, + "usage": { + "language": "python", + "code": "# Creating an instance of the SpecificWorker class\nmy_worker = SpecificWorker(proxy_map=None, startup_check=False)\n\n# Calling the idle method with a loop that lasts for 10 seconds\nfor i in range(int(10000 / my_worker.Period)):\n my_worker.idle()\n", + "description": "" + }, + "name": "idle", + "location": { + "start": 182, + "insert": 184, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 76, + "docLength": null + }, + { + "id": "6aed26f6-3473-6faa-7643-8146c8044dbe", + "ancestors": [ + "75233bfc-d379-5786-3e4d-6b16aa1599c4" + ], + "description": "Determines the current room and updates the state of the worker based on whether it is a known or unknown room.", + "params": [], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map)\naffordance_node = worker.g.get_node(affordance_id)\nif affordance_node.attrs[\"parent\"].value:\n exit_door_id = affordance_node.attrs[\"parent\"].value\n if exit_door_id:\n exit_door_id_node = worker.g.get_node(exit_door_id)\n if exit_door_id_node.attrs[\"connected_room_name\"].value:\n worker.state = \"known_room\"\n", + "description": "" + }, + "name": "crossed", + "location": { + "start": 283, + "insert": 285, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 16, + "docLength": null + }, + { + "id": "ab648f62-f59a-0696-1b4f-b5b4de68bc91", + "ancestors": [ + "75233bfc-d379-5786-3e4d-6b16aa1599c4" + ], + "description": "Initializes the room nodes in the graph, identifying and selecting the entrance node based on the exit door ID, and setting the current state to \"initializing doors\".", + "params": [], + "returns": null, + "usage": { + "language": "python", + "code": "# Initiate SpecificWorker class\nworker = SpecificWorker(proxy_map, startup_check=False)\n\n# Run initializing_room method\nworker.initializing_room()\n\n# Do something with the result of initializing_room\nprint(\"Room initialized.\")\n", + "description": "" + }, + "name": "initializing_room", + "location": { + "start": 303, + "insert": 306, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 10, + "docLength": null + }, + { + "id": "c91dcd80-22ac-4a9b-be40-5caba44d2d63", + "ancestors": [ + "75233bfc-d379-5786-3e4d-6b16aa1599c4" + ], + "description": "Determines the room the robot is currently in, based on the global map and the robot's position, and updates the graph with the appropriate edges and nodes.", + "params": [], + "returns": null, + "usage": { + "language": "python", + "code": "import SpecificWorker\n\n# Create a proxy object for the worker\nworker = SpecificWorker.SpecificWorker()\n\n# Set the agent ID for which to perform the task\nagent_id = 13\n\n# Run the function known_room and pass in the agent ID as an argument\nknown_room_output = worker.known_room(agent_id)\n\n# Print the output of the function\nprint(\"Known room:\", known_room_output)\n", + "description": "" + }, + "name": "known_room", + "location": { + "start": 317, + "insert": 319, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 73, + "docLength": null + }, + { + "id": "e7db7e8c-ed5d-4192-e74c-af8b90f950ea", + "ancestors": [ + "75233bfc-d379-5786-3e4d-6b16aa1599c4" + ], + "description": "1) identifies exit edges in the graph, 2) finds matching doors, and 3) associates them to create a connected room hierarchy.", + "params": [], + "returns": null, + "usage": { + "language": "python", + "code": "# Assume that \"worker\" is an instance of SpecificWorker class\nworker.initializing_doors()\n", + "description": "\nIn this example, the initializing_doors method is called on the worker object, which is an instance of the SpecificWorker class. The method then performs various actions such as setting up signals and connecting to a runtime API." + }, + "name": "initializing_doors", + "location": { + "start": 411, + "insert": 413, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 28, + "docLength": null + }, + { + "id": "5cad0a52-9edf-779c-ab40-f409873f2c7f", + "ancestors": [ + "75233bfc-d379-5786-3e4d-6b16aa1599c4" + ], + "description": "Connects two doors in a graph by adding an edge between them and setting their \"other side door name\" and \"connected room name\" attributes.", + "params": [ + { + "name": "door_1", + "type_name": "str", + "description": "A string representing the name of the first door to be associated with another door." + }, + { + "name": "door_2", + "type_name": "str | int", + "description": "Used to represent the name of the second door that needs to be associated with the first door." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "from igraph import Graph\n\n# Create a graph with two nodes and two edges\ng = Graph()\ng.add_vertices([\"A\", \"B\"])\ng.add_edges([(\"A\", \"B\")])\n\n# Use the associate_doors function to add a door between nodes A and B\nspecific_worker = SpecificWorker(proxy_map)\nspecific_worker.associate_doors(door_1=[\"A\", \"C\"], door_2=[\"B\", \"D\"])\n\n# Print the graph\nprint(g)\n", + "description": "\nIn this example, we first create a graph with two nodes and one edge using the `add_vertices` and `add_edges` methods. Then, we create an instance of the `SpecificWorker` class and use the `associate_doors` method to add a door between nodes A and B. Finally, we print the graph to show that the door has been added correctly." + }, + "name": "associate_doors", + "location": { + "start": 450, + "insert": 452, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 16, + "docLength": null + }, + { + "id": "a9f2c249-039d-49a4-e240-853633ecd2f4", + "ancestors": [ + "75233bfc-d379-5786-3e4d-6b16aa1599c4" + ], + "description": "Saves the graph data to a file using pickling.", + "params": [], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map, startup_check=True)\nworker.store_graph()\n", + "description": "" + }, + "name": "store_graph", + "location": { + "start": 469, + "insert": 470, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 11, + "docLength": null + }, + { + "id": "12a2bcfa-ba36-7bb3-404e-aa001af8fb4c", + "ancestors": [ + "75233bfc-d379-5786-3e4d-6b16aa1599c4" + ], + "description": "Removes edges from the long-term graph that have been labeled as RT or has, based on room numbers. It also deletes nodes in the graph that correspond to shadow nodes.", + "params": [], + "returns": null, + "usage": { + "language": "python", + "code": "# Initialize the SpecificWorker class and its associated objects\nsw = SpecificWorker(proxy_map, startup_check=False)\n\n# Define a robot name\nrobot_name = \"Shadow\"\n\n# Call the removing method on the initialized SpecificWorker object\nsw.removing(robot_name)\n", + "description": "" + }, + "name": "removing", + "location": { + "start": 483, + "insert": 485, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 19, + "docLength": null + }, + { + "id": "c94afa2d-08b0-cc91-8a47-bf999d91cbf5", + "ancestors": [ + "75233bfc-d379-5786-3e4d-6b16aa1599c4" + ], + "description": "Traverses a directed graph represented by an igraph object, starting from a given node ID. It recursively visits all reachable nodes and inserts edges from the root node to each visited node.", + "params": [ + { + "name": "node_id", + "type_name": "int", + "description": "Representing the unique identifier of a node in the graph to be traversed." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "# Creating an instance of SpecificWorker class\nmy_worker = SpecificWorker(proxy_map, startup_check=True)\n\n# Traversing the graph starting from the robot's node\nmy_worker.traverse_graph(my_worker.robot_id)\n\n# Printing the traversed nodes and edges\nfor node in my_worker.g.nodes:\n print(node.id, node.label)\nfor edge in my_worker.g.edges:\n print(edge.origin, edge.destination)\n", + "description": "" + }, + "name": "traverse_graph", + "location": { + "start": 509, + "insert": 511, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 7, + "docLength": null + }, + { + "id": "8a30575b-5e73-bbab-b148-8adc360d99a5", + "ancestors": [ + "75233bfc-d379-5786-3e4d-6b16aa1599c4" + ], + "description": "Iterates through the graph's successors of a given vertex, and for each successor, it checks if the successor's level is higher than the current vertex's level, and if so, it inserts a new vertex and edge in the DSR and recursively traverses the graph.", + "params": [ + { + "name": "node", + "type_name": "igraph.Vertex", + "description": "Used to represent a specific vertex in the graph." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "# Example usage of traverse_igraph()\ng = DSRGraph(0, \"LongTermSpatialMemory_agent\", 13)\nnode = g.get_node(\"My_Node\")\ntraverse_igraph(g, node)\n", + "description": "\nThis code creates an instance of the class GenericWorker and a graph object named g. It also gets a node from the graph using its name \"My_Node\". Then it calls traverse_igraph() with g as an argument and the node as an argument, which initiates the recursive traversal through the graph starting at the given node." + }, + "name": "traverse_igraph", + "location": { + "start": 519, + "insert": 520, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 10, + "docLength": null + }, + { + "id": "74851935-7e42-4080-e848-40c81557fc8b", + "ancestors": [ + "75233bfc-d379-5786-3e4d-6b16aa1599c4" + ], + "description": "Adds a vertex to an igraph graph, based on attributes provided by a node object. It also tries to find matching vertices using specific attribute values and adds edges between them if found.", + "params": [ + { + "name": "node", + "type_name": "igraph.Node | dict", + "description": "Used to add a new vertex to an igraph graph with specified attributes." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "from GenericWorker import SpecificWorker\n\n# create a new worker instance with proxy_map and startup_check=False\nworker = SpecificWorker(proxy_map, startup_check=False)\n\n# add a vertex to the graph with name='node1', id=2, type='room'\nworker.insert_igraph_vertex(node=SpecificWorkerNode(name='node1', id=2, type='room'))\n\n# add another vertex to the graph with name='node2', id=3, type='room'\nworker.insert_igraph_vertex(node=SpecificWorkerNode(name='node2', id=3, type='room'))\n\n# add an edge between the two vertices\nworker.insert_igraph_edge(origin_node=worker.graph.vs[1], other_side_door_node=worker.graph.vs[2])\n", + "description": "" + }, + "name": "insert_igraph_vertex", + "location": { + "start": 532, + "insert": 533, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 28, + "docLength": null + }, + { + "id": "6fb56577-9138-aab8-2145-b8d6dbfab0f8", + "ancestors": [ + "75233bfc-d379-5786-3e4d-6b16aa1599c4" + ], + "description": "Inserts a new vertex into a graph, updating the parent node's attribute with the worker's ID and copying over non-optional attributes from the input node to the new vertex.", + "params": [ + { + "name": "parent_name", + "type_name": "str | str", + "description": "Used to specify the name of the parent node to insert the new node as." + }, + { + "name": "node", + "type_name": "Node", + "description": "Passed as an instance of the class `Node`." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "myWorker = SpecificWorker(proxy_map, startup_check=False)\n# create a new node with name \"NewNode\" and type \"Room\":\nnew_node = myWorker.insert_dsr_vertex(\"MyRobot\", {\"type\": \"Room\", \"name\": \"NewNode\"})\n", + "description": "\nIn this example, the user creates an instance of the `SpecificWorker` class, then calls the `insert_dsr_vertex` function with the parent node name \"MyRobot\" and a dictionary containing the node type and name. The function returns a new node with the specified attributes." + }, + "name": "insert_dsr_vertex", + "location": { + "start": 567, + "insert": 569, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 9, + "docLength": null + }, + { + "id": "2addb82a-54dd-dbac-cd4a-bb5786953a0f", + "ancestors": [ + "75233bfc-d379-5786-3e4d-6b16aa1599c4" + ], + "description": "Adds an edge to an existing graph based on information provided by the edge attribute, including translation and rotation values.", + "params": [ + { + "name": "edge", + "type_name": "igraph.Edge | GraphElement", + "description": "An instance representing a single edge to be inserted into the graph." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "# Create an instance of the SpecificWorker class and initialize it with the proxy map\nworker = SpecificWorker(proxy_map)\n\n# Insert a new edge into the DSRGraph object\nworker.insert_igraph_edge(Edge(origin=\"Origin\", destination=\"Destination\", rt_translation=0, rt_rotation_euler_xyz=[0, 0, 0]))\n", + "description": "" + }, + "name": "insert_igraph_edge", + "location": { + "start": 581, + "insert": 582, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 9, + "docLength": null + }, + { + "id": "9645ce4c-8234-ba84-ff45-3574f5a37212", + "ancestors": [ + "75233bfc-d379-5786-3e4d-6b16aa1599c4" + ], + "description": "Inserts or updates an edge in a graph based on the distance-sensitive roadmap (DSR) algorithm, considering the RT translation and rotation of the edge's endpoints.", + "params": [ + { + "name": "org", + "type_name": "Node | None", + "description": "Used to specify the source node of the edge being inserted. If `org` is None, it means the root node of the graph." + }, + { + "name": "dest", + "type_name": "Node | str", + "description": "Used to specify the destination node or name in the graph for which to create a new edge with RT attributes." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "# Creating a new worker class inheriting from GenericWorker\nclass SpecificWorker(GenericWorker):\n def __init__(self, proxy_map):\n super(SpecificWorker, self).__init__(proxy_map)\n # Initializing the DSR graph with an agent ID and robot name\n self.agent_id = 13\n self.robot_name = \"Shadow\"\n self.g = DSRGraph(0, \"LongTermSpatialMemory_agent\", self.agent_id)\n # Connecting to the signals of the DSR graph\n try:\n signals.connect(self.g, signals.UPDATE_NODE, self.update_node)\n signals.connect(self.g, signals.UPDATE_EDGE, self.update_edge)\n console.print(\"signals connected\")\n except RuntimeError as e:\n print(e)\n", + "description": "\nThe user can then use the function insert_dsr_edge to add an edge between two nodes in the DSR graph. Here is an example of how this might be done:\n" + }, + "name": "insert_dsr_edge", + "location": { + "start": 595, + "insert": 598, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 22, + "docLength": null + }, + { + "id": "c91b9329-a134-37b7-f244-aae2be8f76fd", + "ancestors": [ + "75233bfc-d379-5786-3e4d-6b16aa1599c4" + ], + "description": "Generates a graph based on the layout of a Kamada-Kawai graph, and adds node labels and edge annotations.", + "params": [], + "returns": null, + "usage": { + "language": "python", + "code": "# Import necessary libraries and classes\nfrom SpecificWorker import GenericWorker, DSRGraph\nimport signals\nimport console\n\n# Instantiate a new instance of SpecificWorker\nworker = SpecificWorker(proxy_map)\n\n# Define the graph object to be drawn\ngraph = DSRGraph(0, \"LongTermSpatialMemory_agent\", worker.agent_id)\n\n# Connect signals to functions in GenericWorker class\nsignals.connect(graph, signals.UPDATE_NODE, worker.update_node)\nsignals.connect(graph, signals.UPDATE_EDGE, worker.update_edge)\n\n# Draw the graph using the draw_graph function\nworker.draw_graph(graph)\n", + "description": "" + }, + "name": "draw_graph", + "location": { + "start": 625, + "insert": 626, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 16, + "docLength": null + }, + { + "id": "29f979ee-26a0-1ca3-4747-685c4ee28371", + "ancestors": [ + "75233bfc-d379-5786-3e4d-6b16aa1599c4" + ], + "description": "Retrieves the room ID associated with a given node ID using the Graph object's `get_node` method and attribute access, and returns the room ID if found, or -1 otherwise.", + "params": [ + { + "name": "node_id", + "type_name": "int", + "description": "Used to identify the element for which the room number needs to be checked." + } + ], + "returns": { + "type_name": "int", + "description": "The room ID of a given element if the element has a \"room_id\" attribute, or -1 otherwise." + }, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map, startup_check=False)\nroom_id = worker.check_element_room_number(node_id)\nprint(f\"The room ID of the node with ID {node_id} is {room_id}.\")\n", + "description": "" + }, + "name": "check_element_room_number", + "location": { + "start": 650, + "insert": 651, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 7, + "docLength": null + }, + { + "id": "5dddb8ec-4dbc-e387-6e48-64f7ff25662a", + "ancestors": [ + "75233bfc-d379-5786-3e4d-6b16aa1599c4" + ], + "description": "Determines the level of an element with a given ID in the internal graph, handles exceptions, and adjusts door connections based on the room similarity.", + "params": [ + { + "name": "node_id", + "type_name": "int", + "description": "Used to identify the node being checked for its element level attribute value." + } + ], + "returns": { + "type_name": "int", + "description": "The level of an element with the given node ID, or -1 if no such attribute is found." + }, + "usage": { + "language": "python", + "code": "# Example of using the check_element_level function\n\n# Create a new instance of the SpecificWorker class\nmy_worker = SpecificWorker(proxy_map, startup_check=False)\n\n# Call the check_element_level function with node_id as an argument\nelement_level = my_worker.check_element_level(\"node_id\")\n", + "description": "" + }, + "name": "check_element_level", + "location": { + "start": 659, + "insert": 660, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 25, + "docLength": null + }, + { + "id": "d812eadc-e596-7a97-8447-d13ce3895f4d", + "ancestors": [ + "75233bfc-d379-5786-3e4d-6b16aa1599c4" + ], + "description": "Retrieves information about a room and its RT edges, then draws the room polygon and doors on an image.", + "params": [ + { + "name": "room_node_id", + "type_name": "str | int", + "description": "Represented as an integer or string, representing the node ID of the room to be drawn." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "# Importing necessary packages\nfrom SpecificWorker import SpecificWorker\nimport cv2\n\n# Create a new instance of the class SpecificWorker\nworker = SpecificWorker(proxy_map, startup_check=False)\n\n# Call the generate_room_picture method with the desired room node ID as an argument\nroom_node_id = \"Room1\"\nworker.generate_room_picture(room_node_id)\n", + "description": "\nThis code creates a new instance of the SpecificWorker class, imports the necessary packages, and calls the generate_room_picture method with the desired room node ID as an argument. The function will then generate a 2D image of the room based on the information stored in the graph. The resulting image can be viewed using the OpenCV library by calling cv2.imwrite() or displayed directly using the OpenCV library's display() method." + }, + "name": "generate_room_picture", + "location": { + "start": 702, + "insert": 704, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 24, + "docLength": null + }, + { + "id": "a50d6999-6284-35be-614f-deef0dec204d", + "ancestors": [ + "75233bfc-d379-5786-3e4d-6b16aa1599c4" + ], + "description": "Inserts or assigns an edge to the graph representing the current room of the agent, with the source and destination being the same agent ID.", + "params": [ + { + "name": "room_id", + "type_name": "str", + "description": "Passed as an argument to Edge constructor, representing the ID of the current room that the agent is in." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map)\nroom_id = 254\nworker.insert_current_edge(room_id)\n", + "description": "" + }, + "name": "insert_current_edge", + "location": { + "start": 737, + "insert": 739, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 3, + "docLength": null + }, + { + "id": "9bc1d031-9b8b-578b-b14d-26d0bcd00d92", + "ancestors": [ + "75233bfc-d379-5786-3e4d-6b16aa1599c4" + ], + "description": "Updates a node's information based on its type and other factors, such as checking if a door node exists and inserting it into the graph if necessary, or handling an affordance node's state change.", + "params": [ + { + "name": "id", + "type_name": "int", + "description": "Used as an identifier for a node in the graph." + }, + { + "name": "type", + "type_name": "str", + "description": "Used to indicate the type of node being updated." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map, startup_check=True)\nif worker.rt_api.is_running():\n print(\"Robot is running\")\nelse:\n print(\"Robot is not running\")\n return\n\n# Update node with id 123 and type \"door\"\nworker.update_node(123, \"door\")\n", + "description": "" + }, + "name": "update_node", + "location": { + "start": 751, + "insert": 752, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 41, + "docLength": null + }, + { + "id": "0c89aab3-c848-cb96-6046-4854f3cd0019", + "ancestors": [ + "75233bfc-d379-5786-3e4d-6b16aa1599c4" + ], + "description": "Updates an edge in the graph based on the current node, type, and other conditions.", + "params": [ + { + "name": "fr", + "type_name": "int", + "description": "Referred to as \"from room\" indicating that it represents the starting point of an edge in the graph, specifically a room node." + }, + { + "name": "to", + "type_name": "int", + "description": "The id of the target node to which the edge is being updated." + }, + { + "name": "type", + "type_name": "str", + "description": "Used to specify the type of edge being updated (either \"RT\" or \"FT\")." + } + ], + "returns": null, + "usage": { + "language": "python", + "code": "# Instantiate an object of SpecificWorker class\nworker = SpecificWorker()\n\n# Set the robot name and ID\nworker.robot_name = \"Shadow\"\nworker.robot_id = 13\n\n# Update the edge with type 'RT' between node 0 (the robot) and node 1 (a room exit door)\nworker.update_edge(fr=0, to=1, type=\"RT\")\n", + "description": "" + }, + "name": "update_edge", + "location": { + "start": 806, + "insert": 810, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 5, + "docLength": null + } + ] + } + } + } +] \ No newline at end of file diff --git a/.komment/komment.json b/.komment/komment.json new file mode 100644 index 00000000..ee4ecedf --- /dev/null +++ b/.komment/komment.json @@ -0,0 +1,31 @@ +{ + "meta": { + "version": "1", + "updated_at": "2024-07-31T13:11:04.283Z", + "created_at": "2024-07-03T15:48:27.349Z", + "pipelines": [ + "50c96b7c-ad3b-4b93-ae19-7f6fbd5637f7", + "9e4d0253-62f2-4920-b2d3-607cb4cf3291", + "510e9758-a82c-441e-9653-dc071bb409eb", + "1f636eb0-7f1e-4d1d-b462-6d1eff3533cb", + "d6a72e5c-096b-4179-ac39-3343ea5ec3a5", + "395b32ba-01b1-45bb-84e0-96a1b0abb44e", + "fb0c09b8-650c-42fb-a814-0c07d121ec67", + "3216d745-95dc-419d-8524-71b95ba17b45", + "bdcf9dc7-844d-44d2-af0f-6fcd7dfa4a7c", + "2a176ad9-6055-453c-8640-de6390f8f6fd" + ] + }, + "lookup": [ + [ + "agents/long_term_spatial_memory_agent/src/long_term_graph.py", + "agents/g2o_agent/src/genericworker.py", + "agents/g2o_agent/src/specificworker.py", + "agents/long_term_spatial_memory_agent/scripts/long_term_graph.py", + "agents/long_term_spatial_memory_agent/scripts/main.py", + "agents/long_term_spatial_memory_agent/src/genericworker.py", + "agents/long_term_spatial_memory_agent/src/specificworker.py", + "agents/long_term_spatial_memory_agent/src/specificworker_sec.py" + ] + ] +} \ No newline at end of file diff --git a/agents/g2o_agent/src/genericworker.py b/agents/g2o_agent/src/genericworker.py index 9d81d420..a3451bf3 100644 --- a/agents/g2o_agent/src/genericworker.py +++ b/agents/g2o_agent/src/genericworker.py @@ -42,9 +42,33 @@ class GenericWorker(QtWidgets.QWidget): + """ + Manages a worker process with a periodic timer and provides a signal for + termination. It also has a method to set the period of the timer. + + Attributes: + kill (QtCoreSignal): Used to emit a signal when the object needs to be killed. + ui (Ui_guiDlg): Used to initialize and access the user interface of the widget. + mutex (QMutex): Used to protect access to the internal state of the worker + object, particularly the timer and kill signal. + Period (int): 30 milliseconds by default, which represents the time interval + for the timer to run. + timer (QTimer): Used to schedule a call to the `killYourSelf` slot after + a specified period of time. + + """ kill = QtCore.Signal() def __init__(self, mprx): + """ + Initializes an instance of the `GenericWorker` class, setting up a GUI + dialog and creating a mutex for managing access to the timer. It also sets + the period of the timer to 30 seconds. + + Args: + mprx (Ui_guiDlg): Used as an argument for the setupUi method. + + """ super(GenericWorker, self).__init__() @@ -59,6 +83,10 @@ def __init__(self, mprx): @QtCore.Slot() def killYourSelf(self): + """ + Emits the `kill` signal, indicating that the instance should be destroyed. + + """ rDebug("Killing myself") self.kill.emit() @@ -66,6 +94,14 @@ def killYourSelf(self): # @param per Period in ms @QtCore.Slot(int) def setPeriod(self, p): + """ + Updates the `Period` attribute and starts a timer with the new period value + using the `timer.start()` method. + + Args: + p (int): Used to set the new period for the timer. + + """ print("Period changed", p) self.Period = p self.timer.start(self.Period) diff --git a/agents/g2o_agent/src/specificworker.py b/agents/g2o_agent/src/specificworker.py index 8167f8e4..21e87720 100644 --- a/agents/g2o_agent/src/specificworker.py +++ b/agents/g2o_agent/src/specificworker.py @@ -48,7 +48,131 @@ pass class SpecificWorker(GenericWorker): + """ + Is a worker thread responsible for processing robot odometry data and updating + the graph representation using G2O library. It initializes the graph, adds + edges and vertices, computes pose and covariance matrix, and visualizes the + graph in real-time. + + Attributes: + Period (int): 50 by default. It represents the time period for which the + worker's timer should wait before calling the `compute` method. + agent_id (int): 20 by default. It seems to be used as a unique identifier + for the agent, possibly in conjunction with other nodes or edges in + the graph. + g (DSRGraph): Initialized with a node named "G2O_agent" and agent_id 20. + It represents a graph data structure used to store nodes, edges, and + their attributes. + startup_check (NoneNone): Used as a parameter in the method with the same + name. It seems to be a placeholder or a flag for checking whether some + initialization is done, but its exact purpose is not clear from the + provided code. + rt_api (object): Used to get edge information from a Room-Terrain model + (RT) API, which seems to provide information about robot edges in a room. + inner_api (object): Not fully defined in this code snippet. Its purpose + appears to be related to transforming room node names to robot pose + measurements, but its implementation is unclear without more context + or additional code. + odometry_node_id (int): 20 in this case. It seems to be used as a reference + to the robot's node in the graph, likely representing its current + position or pose. + odometry_queue (deque[int,float,int]): 15 elements long. It stores odometry + data, where each element contains robot's current advance speed, side + speed, angular speed, and timestamp in milliseconds. + last_odometry (Tuple[float,float,float,int]): 4-element tuple that represents + the last recorded odometry values of the robot, including its advancement + speed, lateral speed, angular speed, and timestamp. + g2o (G2OGraph): Used for graph optimization with g2o library. It represents + a graph where nodes are poses and edges are measurements between them, + allowing to compute the most likely pose given the measurements. + odometry_noise_std_dev (float): 1 by default. It seems to be related to + noise added to odometry measurements, representing the standard deviation + of the noise. + odometry_noise_angle_std_dev (float): 1 by default. It represents the + standard deviation of the noise added to odometry angles during the + estimation process using G2O. + measurement_noise_std_dev (float): 1 by default. It seems to be used as a + standard deviation for noise in measurements, possibly related to + odometry or landmark estimation in SLAM (Simultaneous Localization and + Mapping). + last_room_id (None|str): Used to store the previously visited room ID. It + is initialized as None, but gets updated when the current room ID changes. + actual_room_id (int|None): Set as follows: + + - Initially, it is None. + - When a room node is found with its id matching `actual_room_id`, it + becomes equal to that room's id. + - When a new room node is found, `last_room_id` is set to the current + `actual_room_id`, and `actual_room_id` is updated with the new room's + id. + elapsed (float): Initialized as `time.time()`. It seems to be used to track + the time elapsed since the last update of some variables or data, + possibly related to the robot's odometry. + room_initialized (bool): Used to indicate whether the g2o graph has been + successfully initialized for a specific room or not. + iterations (int): Initialized as 0 in the constructor (`__init__`). It + keeps track of the number of iterations since the last frame was processed. + hide (NoneType): Set to True or False. However, there is no clear description + or usage of this attribute within the provided code. + init_graph (bool): Initialized as `False`. It seems to be used to track + whether the g2o graph has been initialized or not, and its value changes + after certain events such as a room change. + current_edge_set (bool): Set to True when a current edge is detected between + a room node and the Shadow node, indicating that the robot has moved + into a new room. + first_rt_set (bool): Set to True when a first RT edge (Robot Translation) + is detected between the "room" node and the "Shadow" node, indicating + that the robot has moved into a new room. + translation_to_set (Tuple[float,float,float]): Set when a new RT edge is + detected, storing the translation values (tx, ty, tz) for further + processing in the G2O graph. + rotation_to_set (Tuple[float,float,float]): Set when a "RT" edge (robot + translation) from a room node to the robot node is detected, indicating + that the robot's rotation needs to be updated. + room_polygon (QPolygonF): Used to store a polygon representing the boundaries + of the current room being explored by the robot. + security_polygon (QPolygonF): Used to store a polygon that defines a region + within which the robot must remain to ensure safety. + initialize_g2o_graph (bool): Used to initialize a G2O graph object. + rt_set_last_time (float): Set to the current time when a "RT" edge is + updated between a room node and the robot node named "Shadow". This + is used to track the last update time for RT edges. + rt_time_min (float): 1 second. It is used to determine when to update the + translation and rotation values after setting a new RT edge. + last_update_with_corners (float): Updated with the current time when the + robot's corners are updated. It keeps track of the last update time + for the corners. + timer (QtCoreQTimer): Used to schedule a method (in this case, `compute`) + at regular intervals (`self.Period`). It starts the timer after + initializing the g2o graph. + compute (QtCoreSlot): Used to compute and update the robot's position in + real-time based on its odometry data and room information. It also + checks for valid corners and doors, and updates the g2o graph accordingly. + update_node_att (NoneNone): Not defined within the given code snippet. + update_edge (defupdate_edgeself,frint,toint,typestrNone): Called when an + edge in the graph changes. It handles different types of edges, such + as "current" or "RT", and updates internal state accordingly. + update_edge_att (None): Passed as a parameter to the `update_edge_att` + method. It is not used anywhere in the code. + + """ def __init__(self, proxy_map, startup_check=False): + """ + Initializes various attributes and performs startup checks or sets up + internal APIs, graph structures, and timers based on input parameters. It + connects signals to methods for handling node updates, edge updates, and + other events. + + Args: + proxy_map (object): Passed to the superclass using `super(SpecificWorker, + self).__init__(proxy_map)`. Its purpose is not explicitly mentioned + in this snippet, but it might be related to initialization or setup + of some sort. + startup_check (bool): Used to decide whether to perform a startup check + or not. If it is True, the `startup_check` method is called; + otherwise, other initialization steps are performed. + + """ super(SpecificWorker, self).__init__(proxy_map) self.Period = 50 @@ -91,12 +215,15 @@ def __init__(self, proxy_map, startup_check=False): self.rotation_to_set = None self.room_polygon = None + self.security_polygon = None self.room_initialized = True if self.initialize_g2o_graph() else False self.rt_set_last_time = time.time() self.rt_time_min = 1 + self.last_update_with_corners = time.time() + self.timer.timeout.connect(self.compute) self.timer.start(self.Period) @@ -119,6 +246,13 @@ def setParams(self, params): @QtCore.Slot() def compute(self): + """ + Computes and updates the robot's pose, odometry, and edge information by + analyzing the room environment and integrating new data from sensors. It + also optimizes the pose estimation using the G2O library and updates the + robot's position in the graph. + + """ if time.time() - self.elapsed > 1: print("Frame rate: ", self.iterations, " fps") self.elapsed = time.time() @@ -127,78 +261,114 @@ def compute(self): if self.room_initialized: self.first_rt_set = False - print("Room initialized") + # print("Room initialized") # Get robot odometry if self.odometry_queue: - init_time = time.time() robot_node = self.g.get_node("Shadow") room_nodes = [node for node in self.g.get_nodes_by_type("room") if self.g.get_edge(node.id, node.id, "current")] + if len(room_nodes) > 0: + room_node = room_nodes[0] + else: + # Get last room node + room_node = self.g.get_node("room_" + str(self.actual_room_id)) + + robot_edge_rt = self.rt_api.get_edge_RT(room_node, robot_node.id) + robot_tx, robot_ty, _ = robot_edge_rt.attrs['rt_translation'].value + robot_point = QPointF(robot_tx, robot_ty) + robot_odometry = self.odometry_queue[-1] # print("Robot odometry:", robot_odometry) time_1 = time.time() adv_displacement, side_displacement, ang_displacement = self.get_displacement(robot_odometry) - # print("Time elapsed get_displacement:", time.time() - time_1) + if len(self.g2o.pose_vertex_ids) == self.g2o.queue_max_len: - self.g2o.remove_first_vertex() + if self.security_polygon.containsPoint(robot_point, Qt.OddEvenFill): + print("Robot inside security polygon") + self.g2o.clear_graph() + self.initialize_g2o_graph() + else: + print("Robot outside security polygon") + self.g2o.remove_first_vertex() + # Generate information matrix considering the noise odom_information = np.array([[1, 0.0, 0.0], [0.0, 1, 0.0], [0.0, 0.0, 0.001]]) - self.g2o.add_odometry(adv_displacement, side_displacement, ang_displacement, odom_information) # Check if robot pose is inside room polygon - if len(room_nodes) > 0: - room_node = room_nodes[0] - if self.room_polygon is not None: - robot_edge_rt = self.rt_api.get_edge_RT(room_node, robot_node.id) - robot_tx, robot_ty, _ = robot_edge_rt.attrs['rt_translation'].value - robot_point = QPointF(robot_tx, robot_ty) - if self.room_polygon.containsPoint(robot_point, Qt.OddEvenFill): - for i in range(4): - corner_node = self.g.get_node("corner_"+str(i)+"_measured") + + no_valid_corners_counter = 0 + if self.room_polygon is not None: + if self.room_polygon.containsPoint(robot_point, Qt.OddEvenFill): + for i in range(4): + corner_node = self.g.get_node("corner_"+str(i)+"_measured") + if corner_node is not None: is_corner_valid = corner_node.attrs["valid"].value if is_corner_valid: corner_edge = self.rt_api.get_edge_RT(robot_node, corner_node.id) corner_edge_mat = self.rt_api.get_edge_RT_as_rtmat(corner_edge, robot_odometry[3])[0:3, 3] self.g2o.add_landmark(corner_edge_mat[0], corner_edge_mat[1], 0.05 * np.eye(2), pose_id=self.g2o.vertex_count-1, landmark_id=int(corner_node.name[7])+1) - print("Landmark added:", corner_edge_mat[0], corner_edge_mat[1], "Landmark id:", int(corner_node.name[7]), "Pose id:", self.g2o.vertex_count-1) - else: - # Get last room node - room_node = self.g.get_node("room_" + str(self.actual_room_id)) + # print("Landmark added:", corner_edge_mat[0], corner_edge_mat[1], "Landmark id:", int(corner_node.name[7]), "Pose id:", self.g2o.vertex_count-1) + else: + no_valid_corners_counter += 1 + + door_nodes = [node for node in self.g.get_nodes_by_type("door") if not "pre" in node.name and + node.name in self.g2o.objects] + # Iterate over door nodes + if self.security_polygon.containsPoint(robot_point, Qt.OddEvenFill): + for door_node in door_nodes: + try: + is_door_valid = door_node.attrs["valid"].value + if is_door_valid: + door_measured_rt = door_node.attrs["rt_translation"].value + if door_measured_rt[0] != 0.0 or door_measured_rt[1] != 0.0: + self.g2o.add_landmark(door_measured_rt[0], door_measured_rt[1], 0.05 * np.eye(2), + pose_id=self.g2o.vertex_count - 1, + landmark_id=self.g2o.objects[door_node.name]) + else: + print("Door is not valid") + except KeyError: + print("Door node does not have valid attribute") + chi_value = self.g2o.optimize(iterations=50, verbose=False) last_vertex = self.g2o.optimizer.vertices()[self.g2o.vertex_count - 1] opt_translation = last_vertex.estimate().translation() opt_orientation = last_vertex.estimate().rotation().angle() - # Substract pi/2 to opt_orientation and keep the number between -pi and pi - if opt_orientation > np.pi: - opt_orientation -= np.pi - elif opt_orientation < -np.pi: - opt_orientation += np.pi # print("Optimized translation:", opt_translation, "Optimized orientation:", opt_orientation) # cov_matrix = self.get_covariance_matrix(last_vertex) # print("Covariance matrix:", cov_matrix) # self.visualizer.update_graph(self.g2o) - # rt_robot_edge = Edge(room_node.id, robot_node.id, "RT", self.agent_id) - # rt_robot_edge.attrs['rt_translation'] = [opt_translation[0], opt_translation[1], .0] - # rt_robot_edge.attrs['rt_rotation_euler_xyz'] = [.0, .0, opt_orientation] - # # rt_robot_edge.attrs['rt_se2_covariance'] = cov_matrix - # self.g.insert_or_assign_edge(rt_robot_edge) - # self.rt_api.insert_or_assign_edge_RT(room_node, robot_node.id, [opt_translation[0], opt_translation[1], 0], [0, 0, opt_orientation]) + # print("No valid corners counter:", no_valid_corners_counter, self.last_update_with_corners) + # affordance_nodes = [node for node in self.g.get_nodes_by_type("affordance") if node.attrs["active"].value] + # if no_valid_corners_counter > 1 and self.security_polygon.containsPoint(robot_point, Qt.OddEvenFill): + # if time.time() - self.last_update_with_corners > 3: + # print("No affordance nodes active. Rotating robot") + # opt_orientation += np.pi/4 + # else: + # self.last_update_with_corners = time.time() + # + # # Substract pi/2 to opt_orientation and keep the number between -pi and pi + # if opt_orientation > np.pi: + # opt_orientation -= np.pi + # elif opt_orientation < -np.pi: + # opt_orientation += np.pi + rt_robot_edge = Edge(robot_node.id, room_node.id, "RT", self.agent_id) rt_robot_edge.attrs['rt_translation'] = Attribute(np.array([opt_translation[0], opt_translation[1], .0],dtype=np.float32), self.agent_id) rt_robot_edge.attrs['rt_rotation_euler_xyz'] = Attribute(np.array([.0, .0, opt_orientation],dtype=np.float32), self.agent_id) self.g.insert_or_assign_edge(rt_robot_edge) self.last_odometry = robot_odometry # Save last odometry # print("Time elapsed compute:", timfe.time() - init_time) + return - elif self.first_rt_set and self.current_edge_set and self.translation_to_set is not None and self.rotation_to_set is not None: - print("Initializing g2o graph") + elif (self.first_rt_set and self.current_edge_set and self.translation_to_set is not None and self.rotation_to_set is not None) or time.time() - self.rt_set_last_time > 3: + # print("Initializing g2o graph") # if self.last_room_id is not None: # self.g.delete_edge(self.g.get_node("room_"+str(self.last_room_id)).id, self.g.get_node("Shadow").id, "RT") self.initialize_g2o_graph() @@ -212,21 +382,31 @@ def add_noise(self, value, std_dev): return value + np.random.normal(0, std_dev) def initialize_g2o_graph(self): - print("Initializing g2o graph") + # print("Initializing g2o graph") + # get robot pose in room + """ + Initializes the G2O graph by setting fixed poses and adding nominal corners + based on the robot's pose, room geometry, and door nodes. It also updates + the actual room ID and checks for valid node attributes. + + Returns: + bool|None: True if successful and initializes a G2O graph, otherwise + it returns None or False indicating failure. + + """ + self.g2o.clear_graph() + robot_node = self.g.get_node("Shadow") room_nodes = [node for node in self.g.get_nodes_by_type("room") if self.g.get_edge(node.id, node.id, "current")] if len(room_nodes) > 0: - self.g2o.clear_graph() room_node = room_nodes[0] - if self.actual_room_id is not None and self.actual_room_id != room_node.attrs['room_id'].value: + room_node_id = room_node.attrs['room_id'].value + if self.actual_room_id is not None and self.actual_room_id != room_node_id: self.last_room_id = self.actual_room_id self.actual_room_id = room_node.attrs['room_id'].value - print("###########################################################") - print("INITIALIZ>INDºG G2O GRAPH") - print("Room changed to", self.actual_room_id) - print("###########################################################") - - # get robot pose in room - robot_node = self.g.get_node("Shadow") + # print("###########################################################") + # print("INITIALIZ>INDºG G2O GRAPH") + # print("Room changed to", self.actual_room_id) + # print("###########################################################") self.odometry_node_id = robot_node.id # Check if room and robot nodes exist if room_node is None or robot_node is None: @@ -246,43 +426,192 @@ def initialize_g2o_graph(self): self.last_odometry = (.0, .0, .0, int(time.time()*1000)) print("Fixed pose added to g2o graph", robot_tx, robot_ty, robot_rz) - # Get corner values from room node - # corner_nodes = self.g.get_nodes_by_type("corner") - # Order corner nodes by id - corner_list = [] - corner_list_measured = [] - for i in range(4): - corner_list.append(self.g.get_node("corner_"+str(i)+"_"+str(self.actual_room_id))) - corner_list_measured.append(self.g.get_node("corner_"+str(i)+"_measured")) # Generate QPolygonF with corner values self.room_polygon = QPolygonF() - + room_center = QPointF(0, 0) for i in range(4): - corner_edge_measured_rt = self.rt_api.get_edge_RT(robot_node, corner_list_measured[i].id) - corner_measured_tx, corner_measured_ty, _ = corner_edge_measured_rt.attrs['rt_translation'].value - corner_edge_rt = self.inner_api.transform(room_node.name, corner_list[i].name) + corner_node = self.g.get_node("corner_"+str(i)+"_"+str(self.actual_room_id)) + corner_edge_rt = self.inner_api.transform(room_node.name, corner_node.name) corner_tx, corner_ty, _ = corner_edge_rt - print("Nominal corners", corner_tx, corner_ty) - print("Measured corners", corner_measured_tx, corner_measured_ty) + corner_list.append(corner_edge_rt) + self.room_polygon.append(QPointF(corner_tx, corner_ty)) + room_center += self.room_polygon.at(i) + # Insert in security polygon the same point but with and offset towards the room center (0, 0) + + # Calculate room center + room_center /= 4 + + # Get room_polygon shortest side # TODO: set security polygon as a parameter that depends on room dimensions + room_poly_bounding = self.room_polygon.boundingRect() + d = 350 + self.security_polygon = QPolygonF() + if self.room_polygon is not None: landmark_information = np.array([[0.05, 0.0], [0.0, 0.05]]) - print("Eye matrix", 0.1 * np.eye(2)) - print("Landmark information:", landmark_information) - if corner_tx != 0.0 or corner_ty != 0.0: - # self.g2o.add_landmark(corner_tx, corner_ty, 0.1 * np.eye(2), pose_id=0) - self.g2o.add_nominal_corner(corner_edge_rt, - corner_edge_measured_rt.attrs['rt_translation'].value, - landmark_information, pose_id=0) - # Add corner to polygon - self.room_polygon.append(QPointF(corner_tx, corner_ty)) - return True + robot_point = QPointF(robot_tx, robot_ty) + if self.room_polygon.containsPoint(robot_point, Qt.OddEvenFill): + for i in range(4): + + # Variables for security polygon + dir_vector = self.room_polygon.at(i) - room_center + dir_vector /= np.linalg.norm(np.array([dir_vector.x(), dir_vector.y()])) + corner_in = self.room_polygon.at(i) - d * dir_vector + self.security_polygon.append(corner_in) + # print("Corner in:", corner_in, "corresponding to corner", corner_list[i]) + corner_measured_node = self.g.get_node("corner_"+str(i)+"_measured") + if corner_measured_node is not None: + is_corner_valid = corner_measured_node.attrs["valid"].value + if is_corner_valid: + corner_edge_measured_rt = self.rt_api.get_edge_RT(robot_node, corner_measured_node.id) + # print("Eye matrix", 0.1 * np.eye(2)) + # print("Landmark information:", landmark_information) + if corner_tx != 0.0 or corner_ty != 0.0: + # self.g2o.add_landmark(corner_tx, corner_ty, 0.1 * np.eye(2), pose_id=0) + self.g2o.add_nominal_corner(corner_list[i], + corner_edge_measured_rt.attrs['rt_translation'].value, + landmark_information, pose_id=0) + else: + print("Corner is not valid") + self.g2o.add_nominal_corner(corner_list[i], + None, + landmark_information, pose_id=0) + + door_nodes = [node for node in self.g.get_nodes_by_type("door") if not "pre" in node.name and + node.attrs["room_id"].value == self.actual_room_id] + # Iterate over door nodes + for door_node in door_nodes: + door_room_rt = self.inner_api.transform(room_node.name, door_node.name) + door_tx, door_ty, _ = door_room_rt + # Check if door is valid + try: + is_door_valid = door_node.attrs["valid"].value + if is_door_valid: + if door_tx != 0.0 or door_ty != 0.0: + door_measured_rt = door_node.attrs["rt_translation"].value + self.g2o.add_nominal_corner(door_room_rt, + door_measured_rt, + landmark_information, pose_id=0) + else: + print("Door is not valid") + self.g2o.add_nominal_corner(door_room_rt, + None, + landmark_information, pose_id=0) + + except KeyError: + print("Door node does not have valid attribute") + self.g2o.add_nominal_corner(door_room_rt, + None, + landmark_information, pose_id=0) + self.g2o.objects[door_node.name] = self.g2o.vertex_count - 1 + return True + elif robot_node.attrs["parent"].value != 100: + robot_parent = robot_node.attrs["parent"].value + room_node = self.g.get_node(robot_parent) + if self.translation_to_set is None and self.rotation_to_set is None: + robot_edge_rt = self.rt_api.get_edge_RT(room_node, robot_node.id) + robot_tx, robot_ty, _ = robot_edge_rt.attrs['rt_translation'].value + _, _, robot_rz = robot_edge_rt.attrs['rt_rotation_euler_xyz'].value + else: + robot_tx, robot_ty, _ = self.translation_to_set + _, _, robot_rz = self.rotation_to_set + + # Add fixed pose to g2o + self.g2o.add_fixed_pose(g2o.SE2(robot_tx, robot_ty, robot_rz)) + self.last_odometry = (.0, .0, .0, int(time.time()*1000)) + print("Fixed pose added to g2o graph", robot_tx, robot_ty, robot_rz) else: print("Room node does not exist. g2o graph cannot be initialized") return False + # def initialize_g2o_graph(self): + # print("Initializing g2o graph") + # room_nodes = [node for node in self.g.get_nodes_by_type("room") if self.g.get_edge(node.id, node.id, "current")] + # if len(room_nodes) > 0: + # self.g2o.clear_graph() + # room_node = room_nodes[0] + # if self.actual_room_id is not None and self.actual_room_id != room_node.attrs['room_id'].value: + # self.last_room_id = self.actual_room_id + # self.actual_room_id = room_node.attrs['room_id'].value + # # print("###########################################################") + # # print("INITIALIZ>INDºG G2O GRAPH") + # # print("Room changed to", self.actual_room_id) + # # print("###########################################################") + # + # # get robot pose in room + # robot_node = self.g.get_node("Shadow") + # self.odometry_node_id = robot_node.id + # # Check if room and robot nodes exist + # if room_node is None or robot_node is None: + # print("Room or robot node does not exist. g2o graph cannot be initialized") + # return False + # + # if self.translation_to_set is None and self.rotation_to_set is None: + # robot_edge_rt = self.rt_api.get_edge_RT(room_node, robot_node.id) + # robot_tx, robot_ty, _ = robot_edge_rt.attrs['rt_translation'].value + # _, _, robot_rz = robot_edge_rt.attrs['rt_rotation_euler_xyz'].value + # else: + # robot_tx, robot_ty, _ = self.translation_to_set + # _, _, robot_rz = self.rotation_to_set + # + # # Add fixed pose to g2o + # self.g2o.add_fixed_pose(g2o.SE2(robot_tx, robot_ty, robot_rz)) + # self.last_odometry = (.0, .0, .0, int(time.time()*1000)) + # print("Fixed pose added to g2o graph", robot_tx, robot_ty, robot_rz) + # + # # Get corner values from room node + # # corner_nodes = self.g.get_nodes_by_type("corner") + # # Order corner nodes by id + # + # corner_list = [] + # corner_list_measured = [] + # for i in range(4): + # corner_list.append(self.g.get_node("corner_"+str(i)+"_"+str(self.actual_room_id))) + # corner_list_measured.append(self.g.get_node("corner_"+str(i)+"_measured")) + # + # # Generate QPolygonF with corner values + # self.room_polygon = QPolygonF() + # + # for i in range(4): + # corner_edge_measured_rt = self.rt_api.get_edge_RT(robot_node, corner_list_measured[i].id) + # corner_measured_tx, corner_measured_ty, _ = corner_edge_measured_rt.attrs['rt_translation'].value + # corner_edge_rt = self.inner_api.transform(room_node.name, corner_list[i].name) + # corner_tx, corner_ty, _ = corner_edge_rt + # print("Nominal corners", corner_tx, corner_ty) + # print("Measured corners", corner_measured_tx, corner_measured_ty) + # landmark_information = np.array([[0.05, 0.0], + # [0.0, 0.05]]) + # print("Eye matrix", 0.1 * np.eye(2)) + # print("Landmark information:", landmark_information) + # if corner_tx != 0.0 or corner_ty != 0.0: + # # self.g2o.add_landmark(corner_tx, corner_ty, 0.1 * np.eye(2), pose_id=0) + # self.g2o.add_nominal_corner(corner_edge_rt, + # corner_edge_measured_rt.attrs['rt_translation'].value, + # landmark_information, pose_id=0) + # # Add corner to polygon + # self.room_polygon.append(QPointF(corner_tx, corner_ty)) + # return True + # else: + # print("Room node does not exist. g2o graph cannot be initialized") + # return False + def get_displacement(self, odometry): + """ + Calculates displacement values (lateral, advance, and angular) based on + odometry data stored in a queue, using timestamps to match consecutive + readings and applying a filtering factor (0.8). + + Args: + odometry (Tuple[float, float, float, float]): Assumed to represent the + current odometry data. + + Returns: + Tuple[float,float,float]: A tuple containing three float values + representing the lateral displacement, advance displacement, and angular + displacement respectively. + + """ desplazamiento_avance = 0 desplazamiento_lateral = 0 desplazamiento_angular = 0 @@ -302,6 +631,21 @@ def get_displacement(self, odometry): return desplazamiento_lateral, desplazamiento_avance, desplazamiento_angular def get_covariance_matrix(self, vertex): + """ + Computes and returns the covariance matrix for a given vertex using the + g2o optimizer's compute_marginals function, and prints relevant messages + indicating whether computation was successful or not. + + Args: + vertex (object): Expected to have attributes such as `hessian_index`. + + Returns: + Tuple[Optional[str],Optional[Dict[int,float]]|None: A tuple containing + two values: the first one is an optional string ("Covariance computed" + or "Covariance not computed") and the second one is an optional + covariance matrix (or None). + + """ cov_vertices = [(vertex.hessian_index(), vertex.hessian_index())] covariances, covariances_result = self.g2o.optimizer.compute_marginals(cov_vertices) if covariances_result: @@ -315,6 +659,16 @@ def get_covariance_matrix(self, vertex): return (covariances_result, None) def visualize_g2o_realtime(self, optimizer): + """ + Visualizes the estimated positions of vertices and edges from a G2O + optimization process in real-time using matplotlib's 3D plotting functionality. + + Args: + optimizer (g2o::OptimizationAlgorithm): Responsible for optimizing the + graph represented by vertices and edges. It contains methods to + load, iterate over vertices and edges, as well as other operations. + + """ plt.ion() fig = plt.figure() ax = fig.add_subplot(111, projection='3d') @@ -360,6 +714,21 @@ def update_node_att(self, id: int, attribute_names: [str]): # # self.init_graph = True # print("INIT GRAPH") + """ + Updates node attributes and appends data to a queue when the specified ID + matches the odometry node ID, processing relevant attribute values from + the Shadow node. + + Args: + id (int): Required when calling this function. It represents an + identifier that determines which node attributes will be updated, + specifically the odometry node if its value matches the self.odometry_node_id. + attribute_names ([str]): Not used within the provided code snippet. + Its presence suggests that it may be intended for future use or + as a placeholder for some functionality, but its purpose remains + unclear. + + """ if id == self.odometry_node_id: odom_node = self.g.get_node("Shadow") odom_attrs = odom_node.attrs @@ -377,9 +746,31 @@ def update_node(self, id: int, type: str): # if not self.room_initialized: # self.room_initialized = True if self.initialize_g2o_graph() else False # self.init_graph = True + """ + Updates a node in a graph based on its ID and type, possibly initializing + the graph if necessary for certain types of nodes, such as room corners. + + Args: + id (int): Required. It specifies the unique identifier for the node + to be updated in the graph. + type (str): Expected to be either "corner" or any other string value. + This information will help determine how the node with given `id` + should be updated. + + """ pass def delete_node(self, id: int): + """ + Deletes a node based on its id and resets the `room_initialized` flag to + False after deletion, implying that the room's initialization is undone + as a result of deleting a node. + + Args: + id (int): Expected to receive an integer value representing the ID of + a node to be deleted. + + """ pass # if type == "room": # # TODO: reset graph and wait until new room appears @@ -389,6 +780,22 @@ def delete_node(self, id: int): # console.print(f"DELETE NODE:: {id} ", style='green') def update_edge(self, fr: int, to: int, type: str): + """ + Updates the current room ID and initializes/updates RT (Rotation Translation) + settings based on the edge type ("current", "RT") and node attributes. It + also prints status messages for debugging purposes. + + Args: + fr (int): Referred to as "from" or source node. It represents the node + from which an edge originates in a graph, specifically a room in + the context of this function. + to (int): Used as an edge destination node ID when updating edges of + graph `g`. + type (str): Used to determine the action to be taken when updating an + edge in the graph. It can have one of two values: "current" or + "RT", depending on the type of update being performed. + + """ if type == "current" and self.g.get_node(fr).type == "room": self.room_initialized = False # Get number after last "_" in room name @@ -417,5 +824,17 @@ def update_edge_att(self, fr: int, to: int, type: str, attribute_names: [str]): def delete_edge(self, fr: int, to: int, type: str): + """ + Removes an edge from the graph. The method takes three parameters: fr + (from), to (to) and type, representing the edge's endpoints and type respectively. + + Args: + fr (int): Expected to represent the starting vertex of an edge in a + graph. It specifies the node from which the edge originates. + to (int): Likely an identifier for a node or vertex in a graph. + type (str): Intended to specify the type or nature of the edge being + deleted between nodes fr and to. + + """ pass # console.print(f"DELETE EDGE: {fr} to {type} {type}", style='green') diff --git a/agents/long_term_spatial_memory_agent/scripts/long_term_graph.py b/agents/long_term_spatial_memory_agent/scripts/long_term_graph.py index 3b823b92..e3b0884a 100644 --- a/agents/long_term_spatial_memory_agent/scripts/long_term_graph.py +++ b/agents/long_term_spatial_memory_agent/scripts/long_term_graph.py @@ -13,7 +13,32 @@ class LongTermGraph: + """ + Draws a graph of long-term spatial mobility data using PyQt and Matplotlib. + It provides methods to visualize rooms, doors, walls, and edges in the graph. + + Attributes: + g (Graph): Used to represent the graph object that contains the rooms, + doors, and walls to be visualized. + read_graph (instance): Used to read a graph from a file specified by the + user. It takes a string path as input and reads the graph data from it. + fig (instance): A reference to the figure object that will be used to draw + the graph. + ax (MatplotlibFigure): Used to represent the axis object for the graph. + It provides methods for adding patches, lines, and other visual elements + to the graph. + + """ def __init__(self, file_name): + """ + Initializes an object of `LongTermGraph` class, loading a graph from a + file using the `read_graph` method and displaying its summary. + + Args: + file_name (str): Used to specify the name of a file containing a graph + represented as an adjacency matrix. + + """ self.g = self.read_graph(file_name, directed=True) print("Graph read from", file_name, self.g.summary()) @@ -189,6 +214,17 @@ def check_point_in_map(self, rooms_map: dict, point: QPoint): return None def draw_graph(self, only_rooms=True): + """ + Generates a graphical representation of a subgraph within a larger graph, + based on node and edge properties. It creates a figure and axis object, + sets the title, and draws the nodes and edges using different colors for + each type of node or edge. + + Args: + only_rooms (bool): Used to filter the nodes in the graph based on their + types, only showing rooms and doors. + + """ fig1, ax1 = plt.subplots() ax1.set_title('LTSM graph') fig1.canvas.draw() diff --git a/agents/long_term_spatial_memory_agent/scripts/main.py b/agents/long_term_spatial_memory_agent/scripts/main.py index ce380928..70c42ec2 100644 --- a/agents/long_term_spatial_memory_agent/scripts/main.py +++ b/agents/long_term_spatial_memory_agent/scripts/main.py @@ -4,6 +4,15 @@ def draw_graph(graph): + """ + Generates a graph based on a provided adjacency matrix using Kamada-Kawai + layout algorithm, and adds node names and edges with arrowheads. + + Args: + graph (AbstractGraph): Used to represent a graph object that contains + vertices and edges. + + """ fig1, ax1 = plt.subplots() ax1.set_title('LTSM graph') fig1.canvas.draw() @@ -39,12 +48,43 @@ def draw_graph(graph): ax1.set_ylim([min(y) - 2, max(y) + 2]) def find_edge_with_attribute(graph, attribute, value): + """ + Searches through a graph's edges for an edge with a specific attribute equal + to a given value. If such an edge is found, it returns it; otherwise, it returns + `None`. + + Args: + graph (Graph): Represented as an object that contains a collection of + edges, where each edge represents a connection between two nodes in + the graph. + attribute (attribute): Used to specify the attribute of interest for finding + an edge in a graph. + value (object): Used to search for an edge in a graph based on a specific + attribute. + + Returns: + edge: An untyped reference to a graph edge that has the specified attribute + equal to the provided value. + + """ for edge in graph.es: if edge[attribute] == value: return edge return None def get_room_edges(graph): + """ + Iterates over the edges in a graph and adds to an output list any edge connecting + nodes with "door" in their names. + + Args: + graph (Graph): Represented as g, which contains a collection of nodes and + edges that define a graph structure. + + Returns: + list: A collection of edges from the given graph. + + """ edges = [] for edge in g.es: source_node = g.vs[edge.source] @@ -57,6 +97,19 @@ def get_room_edges(graph): def get_connected_door_nodes(graph, node): # Base case: if the node is a door node, return it + """ + In Java code recursively queries the graph for all nodes connected to a given + node via doors, returning a list of such nodes. + + Args: + graph (Graph): Used to represent a graph structure. + node (GraphNode): Referred to as a node in the graph. + + Returns: + list: A collection of nodes that are connected to a specific node through + doors. + + """ if "door" in node["name"] and node["connected_room_name"] is not None: return [node] @@ -72,6 +125,21 @@ def get_connected_door_nodes(graph, node): return door_nodes def traverse_graph(graph, current_room, visited=None): + """ + Navigates through a graph by starting from a given room and visiting all other + rooms reachable through doors. It keeps track of visited rooms using a list + and prints information about each room it visits. + + Args: + graph (Graph): Used to represent a graph with nodes and edges. + current_room (dict): Represents the current room to be traversed in the graph. + visited (list): Used to keep track of the rooms that have been visited + during the traversal process, initialized to an empty list if None. + + Returns: + list: A collection of strings representing the rooms that have been visited. + + """ if visited is None: visited = list() diff --git a/agents/long_term_spatial_memory_agent/src/genericworker.py b/agents/long_term_spatial_memory_agent/src/genericworker.py index c0a9979a..c78d937a 100644 --- a/agents/long_term_spatial_memory_agent/src/genericworker.py +++ b/agents/long_term_spatial_memory_agent/src/genericworker.py @@ -42,13 +42,38 @@ class GenericWorker(QtWidgets.QWidget): + """ + Manages a timer and a signal to stop its own execution. It has methods to + change the timer period and to emit the signal to stop itself. + + Attributes: + kill (QtCoreQObjectSlot): Used to emit a signal that can be caught by any + connected slots to stop the worker's execution. + ui (Ui_guiDlg): Used to setup the user interface of the class. + mutex (QMutex): Used to protect the worker's state from concurrent access. + Period (int): Used to set the time interval for the timer signal emitted + by the `setPeriod()` method, which changes its value on each call. + timer (QtCoreQTimer): Used to start a timer that emits the `kill` signal + after a specified period. + + """ kill = QtCore.Signal() def __init__(self, mprx): + """ + Initializes an object of the `GenericWorker` class, setting up a UI widget, + creating a mutex for synchronization, and defining a timer with a period + of 500 milliseconds. + + Args: + mprx (Ui_guiDlg): Used as the parent widget for the GenericWorker + object's UI. + + """ super(GenericWorker, self).__init__() - self.ui = Ui_guiDlg()ss + self.ui = Ui_guiDlg() self.ui.setupUi(self) # self.show() @@ -59,6 +84,10 @@ def __init__(self, mprx): @QtCore.Slot() def killYourSelf(self): + """ + Emits the `kill` signal, indicating that the object should be terminated. + + """ rDebug("Killing myself") self.kill.emit() @@ -66,6 +95,13 @@ def killYourSelf(self): # @param per Period in ms @QtCore.Slot(int) def setPeriod(self, p): + """ + Sets the period of a timer and updates the internal variable `Period`. + + Args: + p (int): Used to set the new period for the timer. + + """ print("Period changed", p) self.Period = p self.timer.start(self.Period) diff --git a/agents/long_term_spatial_memory_agent/src/long_term_graph.py b/agents/long_term_spatial_memory_agent/src/long_term_graph.py index 1eb1eab3..614f9652 100644 --- a/agents/long_term_spatial_memory_agent/src/long_term_graph.py +++ b/agents/long_term_spatial_memory_agent/src/long_term_graph.py @@ -13,13 +13,48 @@ class LongTermGraph: + """ + Provides a user interface for visualizing and exploring a graph, including + rooms and doors. It allows for adding nodes, edges, and doors, as well as + displaying room names and distances between nodes. + + Attributes: + g (str|int): Used to specify the color of the graph's edges. It can be set + to a valid matplotlib color name or an integer value between 0 and 1, + representing the transparency level of the edge. + read_graph (Callable[[str],Dict[str,float]]): Used to read a graph from a + file specified by the filename parameter. It returns a dictionary + containing the graph data as key-value pairs where keys are node or + edge indices and values are the corresponding coordinates or weights. + fig (matplotlibfigureFigure): Used to represent the figure object that + will be drawn with the graph. It contains information about the figure, + such as its size, layout, and any additional elements that will be + displayed in it. + ax (matplotlibpyplotAxes): Used to represent a 2D axes object that displays + the graphical representation of the long-term graph. + fig_2 (matplotlibfigureFigure): Used to store the figure object for the + second graph. + ax_2 (AxesSubplot|MatplotlibFigure): Used to store a secondary axes object, + which can be used to display additional visualizations or metrics + alongside the primary graph. + + """ def __init__(self, file_name): + """ + Reads a graph from a file, creates an igraph object, and displays both the + original graph and its metric reconstruction using matplotlib. + + Args: + file_name (str): Used to specify the name of the graph file that + contains the LTSM data to be reconstructed. + + """ try: self.g = self.read_graph(file_name, directed=True) print("Graph read from", file_name, self.g.summary()) except FileNotFoundError: print("File not found") - self.g = None + self.g = ig.Graph(directed=True) plt.ion() self.fig, self.ax = plt.subplots() @@ -65,7 +100,8 @@ def get_room_objects_by_type_recursive(self, node, object_type): objects = [] for succ in self.g.successors(node): - objects += self.get_room_objects_by_type_recursive(self.g.vs[succ], object_type) + if self.g.vs[succ]["room_id"] == node["room_id"]: + objects += self.get_room_objects_by_type_recursive(self.g.vs[succ], object_type) return objects def get_room_objects_transform_matrices(self, room_name, object_type) -> list: @@ -77,6 +113,7 @@ def get_room_objects_transform_matrices(self, room_name, object_type) -> list: rts = [] for object in objects: path = self.g.get_shortest_path(room, object, weights=None, mode='out', output='epath', algorithm='auto') + path = self.check_path(path) rt = sm.SE3() for edge_id in path: edge = self.g.es[edge_id] @@ -94,6 +131,7 @@ def get_room_objects_transform_matrices_with_name(self, room_name, object_type) rts = [] for object in objects: path = self.g.get_shortest_path(room, object, weights=None, mode='out', output='epath', algorithm='auto') + path = self.check_path(path) rt = sm.SE3() for edge_id in path: edge = self.g.es[edge_id] @@ -108,10 +146,8 @@ def compute_room_map(self, target_room_name, origin_room_name): transform = self.transform_room(target_room_name, origin_room_name) # corners = self.get_room_objects_coordinates(origin_room_name, "corner") corners_transf = self.get_room_objects_transform_matrices(origin_room_name, "corner") - corners_in_room = [transform.A @ np.array(c_transf.A[:, -1]) for c_transf in corners_transf] # corners_in_room = [transform.A @ np.array(corner) for corner in corners] - x_coords = [corner[0] for corner in corners_in_room] y_coords = [corner[1] for corner in corners_in_room] points = [QPoint(x, y) for x, y in zip(x_coords, y_coords)] @@ -141,11 +177,17 @@ def compute_door_map(self, target_room_name, origin_room_name): def compute_element_pose(self, robot_pose, target_room_name, origin_room_name): """ Computes the robot pose from origin room wrt a target room. Returns a QPoint object""" + """ Computes the robot pose from origin room wrt a target room. Returns a QPoint object""" + pose = sm.SE3(robot_pose[0], robot_pose[1], 0) * sm.SE3.Rz(robot_pose[2], unit='rad') transform = self.transform_room(target_room_name, origin_room_name) - # transform robot pose to target room frame - robot_pose_transformed = transform.A @ robot_pose + element_pose_transformed = transform * pose + rpy = element_pose_transformed.rpy() + xyz = element_pose_transformed.A[:, -1] + + print("Element pose transformed", xyz, rpy) + # return QPoint object - return QPoint(robot_pose_transformed[0], robot_pose_transformed[1]) + return np.array([xyz[0], xyz[1], rpy[2]]) def transform_room(self, target_room_name, origin_room_name) -> sm.SE3: """ Computes the transformation matrix to express the origin room in the target room frame""" @@ -156,6 +198,7 @@ def transform_room(self, target_room_name, origin_room_name) -> sm.SE3: # get the transformation chain from the current room to the connected room path = self.g.get_shortest_path(target_room, source_room, weights=None, mode='all', output='vpath', algorithm='auto') + path = self.check_path(path) # compute the transformation matrices from the edges contents inverse = False @@ -164,6 +207,7 @@ def transform_room(self, target_room_name, origin_room_name) -> sm.SE3: for a, b in zip(path, itertools.islice(path, 1, None)): # first from path, second from path (islice) edge_id = self.g.get_eid(a, b, directed=False) edge = self.g.es(edge_id) + tr = edge["rt"][0] rot = edge["rotation"][0] if tr is not None: @@ -185,6 +229,35 @@ def transform_room(self, target_room_name, origin_room_name) -> sm.SE3: door = True return final + # Create a function that, given a path, check if sequence of (door, wall, door) exists + def check_path(self, path): + """ Check if a path contains a sequence of (door, wall, door) """ + # Copy path + n_insertions = 0 + path_copy = path.copy() + if len(path) < 3: + return path + + for i in range(len(path) - 2): + if self.g.vs[path[i]]["type"] == "door" and self.g.vs[path[i + 1]]["type"] == "wall" and self.g.vs[path[i + 2]]["type"] == "door": + # get wall_node parent id + wall_node = self.g.vs[path[i + 1]] + # Get room_id attr + room_id = wall_node["room_id"] + # Search for the room node + room_node = self.g.vs.find("room_"+str(room_id)) + # Get the wall node id + room_id = room_node.index + # insert room_id in the copied path after the wall node + path_copy.insert(i + 2 + n_insertions, room_id) + # insert another wall_id after the room_id + path_copy.insert(i + 3 + n_insertions, path[i + 1]) + n_insertions += 2 + print("######################## JUMP DETECTED #########################") + print("Room node id", room_id, "name", room_node["name"], "inserted", "wall id", path[i + 1]) + print("Path after check", path_copy) + return path_copy + def compute_metric_map(self, base_room_name): """ Computes the metric map of the environment. Returns a dictionary with the rooms and doors. The structure of the dictionary is as follows: @@ -212,6 +285,15 @@ def check_point_in_map(self, rooms_map: dict, point: QPoint): return None def draw_graph(self, only_rooms=True): + """ + Generates a graph based on a subgraph of the original graph, with only + certain types of nodes and edges visible. + + Args: + only_rooms (bool): Used to filter out nodes that are not rooms or + doors, and edges that do not connect rooms or doors. + + """ self.ax_2.clear() self.ax_2.set_title('LTSM graph') self.ax_2.set_xlabel('X-axis') @@ -271,14 +353,21 @@ def draw_metric_map(self, rooms_map: dict, current_room_name=None): self.fig.canvas.draw() self.fig.canvas.flush_events() - def draw_point(self, point: QPoint): + def draw_point(self, point: QPoint, point_in_direction: QPoint= None ): """ Draws a point in the map """ - circle = patches.Circle((float(point.x()), float(point.y())), 100, edgecolor='g', facecolor='none') + circle = patches.Circle((float(point.x()), float(point.y())), 50, edgecolor='g', facecolor='none') self.ax.add_patch(circle) + + if point_in_direction is not None: + print("Drawing arrow") + print(point.x(), point.y(), point_in_direction.x(), point_in_direction.y()) + self.ax.arrow(point.x(), point.y(), point_in_direction.x() - point.x(), point_in_direction.y() - point.y(), head_width=20, head_length=15, fc='r', ec='r') + self.fig.canvas.draw() self.fig.canvas.flush_events() + def draw_room(self, room_name, room_polygon, current=False): """ Draws the room polygon """ diff --git a/agents/long_term_spatial_memory_agent/src/specificworker.py b/agents/long_term_spatial_memory_agent/src/specificworker.py index 634d6ae7..e9d71615 100644 --- a/agents/long_term_spatial_memory_agent/src/specificworker.py +++ b/agents/long_term_spatial_memory_agent/src/specificworker.py @@ -30,6 +30,9 @@ import setproctitle import math import pickle +import subprocess +from collections import deque +import json from long_term_graph import LongTermGraph @@ -47,7 +50,105 @@ from pydsr import * class SpecificWorker(GenericWorker): + """ + Simulates a robot navigating through a virtual environment by traversing a + graph, updating node and edge attributes, and performing tasks such as door + associations, room creation, and path planning. + + Attributes: + Period (None): 100 by default, indicating a period of time (in milliseconds) + for computing tasks. It is used to set the timer interval for periodic + computations. + agent_id (int): 13, which represents the ID of a specific agent in the + robotic environment. It is used as an identifier for the agent's + internal graph. + g (igraphGraph): Used to interact with the graph library igraph. It contains + methods to insert, delete, update nodes and edges in the graph, as + well as traverse the graph. + startup_check (None|bool): Set by default to None. It is a simple flag + that is used for checking if the startup process has completed successfully. + rt_api (object): Used to get edge RT (Rotation and Translation) from the + `room_exit_door_id` to the robot node. + inner_api (object): Used for internal API operations, likely related to + robot pose transformations, door connections, or other internal + mechanisms. The exact functionality depends on the implementation details. + testing (Attribute): 2. It seems to be used as a flag for testing mode, + controlling certain behavior or actions within the class. + robot_name (str): Used as a name for the robot node in the graph. It appears + to be used to identify the robot's location within the graph. + robot_id (int): 13, as initialized in the `__init__` method. It appears + to be a unique identifier for the robot node in the graph. + last_robot_pose (npfloat64): 3-dimensional, representing the last known + pose (position and orientation) of the robot in the room. It is used + to store the previous position of the robot before updating it with + new data. + robot_exit_pose (npndarray[npfloat64,1D]): 3-dimensional representing a + pose (x, y, z) of the robot when exiting a room. It stores the final + affordance pose transformed to the global reference system. + state (str|None): Used to keep track of the current state of the worker, + such as "idle", "crossing", "known_room", etc., which determines how + it should behave in different situations. + affordance_node_active_id (int): 13 by default, which corresponds to the + agent's ID. This variable seems to keep track of the active affordance + node in the graph. + exit_door_id (int): Used as a reference to identify the ID of the door + that represents the exit from a room. + room_exit_door_id (int): 13, as specified in the class initialization + method (`__init__`). It represents the ID of the exit door node in the + graph. + enter_room_node_id (int): Set when a new room is entered. It holds the ID + of the node that represents the current room being explored by the robot. + vertex_size (int): 0 initially. It increments by 1 each time a new vertex + (node) is inserted into the graph, keeping track of the number of + vertices created so far. + not_required_attrs (List[str]): Used to store the names of attributes that + are not required for vertex objects in the graph, such as "parent", + "timestamp_alivetime", etc. These attributes are ignored when inserting + vertices into the graph. + last_save_time (float): Used to store the timestamp when the worker's state + was last saved or updated. + long_term_graph (igraphGraph): Used to store and manipulate the long-term + graph data structure, which represents the agent's internal model of + its environment. + room_number (int): 1-based indexing for the room ID. It represents the + unique identifier of a room node in the graph data structure. + room_number_limit (None|int): 0 by default. It seems to be related to the + limit of rooms in a generated apartment, used for procedural room generation. + insert_current_edge (None): Used to set a current edge between two nodes + in the graph, updating the room state to idle. + pending_doors_to_stabilize (List[tuple[str,str]]): Initialized with a + deque(maxlen=10) to store door names to be stabilized later. It keeps + track of doors that need to be connected in the graph based on certain + conditions. + timer (QTimer): Used for scheduling the execution of a method after a + certain time delay or interval (set by the `start` method). + compute (None|Callable[[Any],Any]): Used as a slot for handling timer + events. It contains a method that gets called at regular intervals, + performing specific tasks depending on the state of the worker. + update_node (None|int,str): Used to update a node with the given id and type. + update_edge (NoneNonefrint,toint,typestr): Used to update edges in the + graph, specifically when the edge's destination is the robot ID, source + node is not the room exit door, type is "RT" and there are no current + edges. + + """ def __init__(self, proxy_map, startup_check=False): + """ + Initializes various attributes, sets up graph data structures, and connects + signals to methods for updating nodes and edges. It also starts a timer + that triggers the compute method at regular intervals. + + Args: + proxy_map (object): Passed to the superclass constructor using the + `super()` function, indicating that it should be used for + initialization. Its purpose and content are not specified within + this code block. + startup_check (bool): Set to False by default. It determines whether + a startup check should be performed or not. If set to True, it + calls the `startup_check` method; otherwise, it initializes the + other parts of the class. + + """ super(SpecificWorker, self).__init__(proxy_map) self.Period = 100 @@ -55,27 +156,19 @@ def __init__(self, proxy_map, startup_check=False): self.agent_id = 13 self.g = DSRGraph(0, "LongTermSpatialMemory_agent", self.agent_id) - try: - #signals.connect(self.g, signals.UPDATE_NODE_ATTR, self.update_node_att) - signals.connect(self.g, signals.UPDATE_NODE, self.update_node) - #signals.connect(self.g, signals.DELETE_NODE, self.delete_node) - signals.connect(self.g, signals.UPDATE_EDGE, self.update_edge) - #signals.connect(self.g, signals.UPDATE_EDGE_ATTR, self.update_edge_att) - #signals.connect(self.g, signals.DELETE_EDGE, self.delete_edge) - console.print("signals connected") - except RuntimeError as e: - print(e) - if startup_check: self.startup_check() else: self.rt_api = rt_api(self.g) self.inner_api = inner_api(self.g) + self.testing = False + # Robot node variables self.robot_name = "Shadow" self.robot_id = self.g.get_node(self.robot_name).id self.last_robot_pose = [0, 0, 0] + self.robot_exit_pose = [0, 0, 0] # Variable for designing the state machine self.state = "idle" @@ -90,13 +183,37 @@ def __init__(self, proxy_map, startup_check=False): self.enter_room_node_id = None # Enter room node ID # Graph variables - self.graph = ig.Graph() self.vertex_size = 0 self.not_required_attrs = ["parent", "timestamp_alivetime", "timestamp_creation", "rt", "valid", "obj_checked", "name", "id"] + self.last_save_time = time.time() # Global map variables self.long_term_graph = LongTermGraph("graph.pkl") - self.global_map = None + # Check if self.long_term_graph.g is empty + # if self.long_term_graph.g.vcount() != 0: + # # Draw graph from file + # self.long_term_graph.draw_graph(False) + # # Compute metric map and draw it + # g_map = self.long_term_graph.compute_metric_map("room_1") + # self.long_term_graph.draw_metric_map(g_map) + # self.initialize_room_from_igraph() + # self.update_robot_pose_in_igraph() + + if self.testing: + # Get ground truth map from json + root_node = self.g.get_node("root") + path_attribute = root_node.attrs["path"].value + if path_attribute: + # Get string between "generatedRooms/" and "/ApartmentFloorPlan.stl" + self.room_number = path_attribute.split("generatedRooms/")[1].split("/ApartmentFloorPlan.stl")[0] + print("Room number", self.room_number) + # Get data from apartmentData.json file in '/home/robocomp/robocomp/components/proceduralRoomGeneration/generatedRooms' + with open(f"/home/robocomp/robocomp/components/proceduralRoomGeneration/generatedRooms/{self.room_number}/apartmentData.json") as f: + data = json.load(f) + self.room_number_limit = len(data["rooms"]) + # Print the number of rooms in the ground truth map + print("Room number limit", self.room_number_limit) + # In case the room node exists but the current edge is not set, set it room_nodes = self.g.get_nodes_by_type("room") @@ -106,15 +223,43 @@ def __init__(self, proxy_map, startup_check=False): if not "measured" in room_nodes[0].name: self.insert_current_edge(room_nodes[0].id) + self.pending_doors_to_stabilize = deque(maxlen=10) + self.timer.timeout.connect(self.compute) self.timer.start(self.Period) + try: + # signals.connect(self.g, signals.UPDATE_NODE_ATTR, self.update_node_att) + signals.connect(self.g, signals.UPDATE_NODE, self.update_node) + # signals.connect(self.g, signals.DELETE_NODE, self.delete_node) + signals.connect(self.g, signals.UPDATE_EDGE, self.update_edge) + # signals.connect(self.g, signals.UPDATE_EDGE_ATTR, self.update_edge_att) + # signals.connect(self.g, signals.DELETE_EDGE, self.delete_edge) + console.print("signals connected") + except RuntimeError as e: + print(e) + def __del__(self): """Destructor""" def setParams(self, params): + """ + Sets parameters for a scenario and performs subsequent actions on doors, + including removing self-edges, storing door IDs, updating door attributes, + and adding an attribute to an entrance door node. + + Args: + params (Dict[any, any]): Expected to contain various parameters used + for setting up the current room state in the game. + + Returns: + bool: True. + + """ return True + + # PROTOcode # Check if there is a node of type aff_cross and it has it's valid attribute to true # if so, check if aff_cross status attribute is completed, was active and robot pose is outside the room polygon, @@ -128,23 +273,66 @@ def setParams(self, params): # - Read entrance door node and add an attribute other_side_door with the name of the exit door in the new room @QtCore.Slot() def compute(self): - # rt_edges = self.g.get_edges_by_type("RT") - # if len(rt_edges) > 0: - # print("RT edges") - # for edge in rt_edges: - # # get node of edge.origin and edge.destination - # origin_node = self.g.get_node(edge.origin) - # destination_node = self.g.get_node(edge.destination) - # #check if the origin node is a room and the destination node is not none - # if origin_node is not None and destination_node is not None: - # print(self.g.get_node(edge.origin).name, self.g.get_node(edge.destination).name) - # #get level of the origin node and the destination node - # origin_level = origin_node.attrs["level"].value - # destination_level = origin_node.attrs["level"].value - # #check if the origin level is not none and the destination level is not none - # if origin_level is not None and destination_level is not None: - # print(origin_level, destination_level) + # Exectue function self.update_robot_pose_in_igraph each second + # if time.time() - self.last_save_time > 1: + # self.update_robot_pose_in_igraph() + + """ + Computes the position and orientation of rooms and doors, generates JSON + data for each room, saves it to a file, and then terminates the process + if testing mode is enabled. + + """ + if self.testing: + # # Get room nodes number in igraph + try: + room_nodes = self.long_term_graph.g.vs.select(type_eq="room") + door_nodes = self.long_term_graph.g.vs.select(type_eq="door") + # Check if every door node has a "connected_room_name" attribute + connected = True + for door in door_nodes: + # Check if door has "connected_room_name" attribute and return if is None + if not door["other_side_door_name"]: + connected = False + print("Door", door["name"], "has no connected_room_name attribute") + break + if len(room_nodes) == self.room_number_limit and connected: + dict = {} + # Create a dictionary with the room names. Every room name is a key which value is a dictionary with the room attributes + try: + dict["rooms"] = [{"name" : room["name"], "x" : room["width"], "y" : room["depth"], "room_id" : room["room_id"]} for room in room_nodes] + + # For each room, insert an attribute with the room center in the global reference system + for room in dict["rooms"]: + room_center = self.long_term_graph.compute_element_pose(np.array([0., 0., 0.], dtype=np.float64), "room_1", room["name"]) + print("ROOM CENTER POSE VALUE", room_center) + room["global_center"] = (room_center[0], room_center[1]) + if (2 * math.pi/6) < abs(room_center[2]) < (4*math.pi/6): + room["x"], room["y"] = room["y"], room["x"] + dict["doors"] = [{"name" : door["name"], "width" : door["width"], "room_id" : door["room_id"], "other_side_door_name" : door["other_side_door_name"], "connected_room_name" : door["connected_room_name"]} for door in self.long_term_graph.g.vs.select(type_eq="door")] + for door in dict["doors"]: + door_center = self.long_term_graph.compute_element_pose(np.array([0., 0., 0.], dtype=np.float64), "room_1", door["name"]) + door["global_center"] = (door_center[0], door_center[1]) + # Divide door name by "_" and get the second and the forth element to get the wall_id + wall_id = "wall_" + door["name"].split("_")[1] + "_" + door["name"].split("_")[3] + door_center = self.long_term_graph.compute_element_pose(np.array([0., 0., 0.], dtype=np.float64), wall_id, door["name"]) + door["pose"] = (door_center[0], door_center[1]) + print(dict) + # print(dict["doors"]) + # Get file number in "/home/robocomp/robocomp/components/proceduralRoomGeneration/generatedRooms/{self.room_number} " + file_number = len([name for name in os.listdir(f"/home/robocomp/robocomp/components/proceduralRoomGeneration/generatedRooms/{self.room_number}") if os.path.isfile(os.path.join(f"/home/robocomp/robocomp/components/proceduralRoomGeneration/generatedRooms/{self.room_number}", name))]) + # Save the dictionary in a .json file inside "tests" folder. The name of the file is dependent of the number of files in "tests" + with open(f"/home/robocomp/robocomp/components/proceduralRoomGeneration/generatedRooms/{self.room_number}/generated_data_" + str(file_number) + ".json", "w+") as f: + json.dump(dict, f) + self.kill_everything() + except Exception as e: + print(e) + exit(0) + except Exception as e: + print(e) + pass + # print(self.state) match self.state: case "idle": self.idle() @@ -163,9 +351,113 @@ def compute(self): case "removing": self.removing() + def initialize_room_from_igraph(self): + # Remove RT edge between "root" and "Shadow" + """ + Initializes a room structure from an igraph object, creates a root node + and edges, updates node attributes, and inserts or assigns new edges into + the graph. + + """ + self.g.delete_edge(100, self.robot_id, "RT") + # Check in igraph the Shadow parent node + robot_node = self.long_term_graph.g.vs.find(name=self.robot_name) + robot_node_neighbors = self.long_term_graph.g.neighbors(robot_node) + # Get first value and get node + actual_room_igraph = self.long_term_graph.g.vs(robot_node_neighbors[0])[0] + # Insert the room node in the DSR graph + self.insert_dsr_vertex("root", actual_room_igraph) + self.insert_dsr_edge(None, actual_room_igraph) + self.traverse_igraph(actual_room_igraph) + + # Get room node from graph + room_node = self.g.get_node(actual_room_igraph["name"]) + new_edge = Edge(self.robot_id, room_node.id, "RT", self.agent_id) + # Get igraph edge from room to robot + robot_rt_igraph = self.long_term_graph.g.es.find(_source=actual_room_igraph.index, _target=robot_node.index) + # Get "translation" and "rotation" attributes + rt_robot = robot_rt_igraph["traslation"] + door_rotation = robot_rt_igraph["rotation"] + new_edge.attrs["rt_translation"] = Attribute(np.array(rt_robot, dtype=np.float32), self.agent_id) + # Get z rotation value and substract 180 degrees. then, keep the value between -pi and pi + + new_edge.attrs["rt_rotation_euler_xyz"] = Attribute( + np.array(door_rotation, dtype=np.float32), + self.agent_id) + self.g.insert_or_assign_edge(new_edge) + robot_node = self.g.get_node(self.robot_name) + # Modify parent attribute of robot node + robot_node.attrs["parent"] = Attribute(room_node.id, self.agent_id) + self.g.update_node(robot_node) + + def update_robot_pose_in_igraph(self): + # # Check if graph exists + """ + Updates the robot's pose in an igraph graph by reflecting changes from a + long-term graph to the current graph and saving the updated graph. It + handles cases where the robot moves between rooms or when it is not found + in the igraph. + + """ + if self.long_term_graph.g: + # Get room with "current" edge + current_edges = [edge for edge in self.g.get_edges_by_type("current") if self.g.get_node(edge.destination).type == "room" and self.g.get_node(edge.origin).type == "room"] + if len(current_edges) == 1: + actual_room_node = self.g.get_node(current_edges[0].origin) + # Get robot pose + robot_rt = self.rt_api.get_edge_RT(actual_room_node, self.robot_id) + # Check if robot node exists in graph + try: + robot_node = self.long_term_graph.g.vs.find(name=self.robot_name) + # Get robot antecessor + robot_node_neighbors = self.long_term_graph.g.neighbors(robot_node) + print("Robot node neighbors", robot_node_neighbors) + # Get first value and get node + actual_room_igraph = self.long_term_graph.g.vs.find(robot_node_neighbors[0]) + + try: + robot_rt_igraph = self.long_term_graph.g.es.find(_source=actual_room_igraph.index, _target=robot_node.index) + # Print rt and rotation values + print("RT edge from room to robot", actual_room_igraph["name"], robot_node["name"]) + print("Robot RT", robot_rt_igraph["traslation"], robot_rt_igraph["rotation"]) + if actual_room_igraph["name"] != actual_room_node.name: + print("Robot node is not in the same room as the robot") + # Get new room node + try: + new_room_node = self.long_term_graph.g.vs.find(name=actual_room_node.name) + self.long_term_graph.g.delete_edges([robot_rt_igraph]) + self.insert_igraph_edge(robot_rt) + except: + print("No room node found in igraph. waiting") + # Get igraph edge + else: + pass + # Update rt data + robot_rt_igraph["traslation"] = robot_rt.attrs["rt_translation"].value + robot_rt_igraph["rotation"] = robot_rt.attrs["rt_rotation_euler_xyz"].value + + except Exception as e: + print(e) + except Exception as e: + print(e) + print("No robot node found in igraph. Inserting") + robot_node_dsr = self.g.get_node(self.robot_name) + self.insert_igraph_vertex(robot_node_dsr) + self.insert_igraph_edge(robot_rt) + self.long_term_graph.draw_graph(False) + # Save graph to file + with open("graph.pkl", "wb") as f: + pickle.dump(self.long_term_graph.g, f) + self.last_save_time = time.time() def idle(self): # Check if there is a node of type aff_cross and it has it's valid attribute to true using comprehension list + """ + Handles affordance nodes, stabilizes doors, and computes robot poses for + crossing between rooms. It updates the graph state and prints relevant + information during its execution. + + """ aff_cross_nodes = [node for node in self.g.get_nodes_by_type("affordance") if node.attrs["active"].value == True] # Check if not empty if len(aff_cross_nodes) == 0 or len(aff_cross_nodes) > 1: @@ -174,16 +466,29 @@ def idle(self): else: # Check if any "current" edge exists current_edges = [edge for edge in self.g.get_edges_by_type("current") if self.g.get_node(edge.destination).type == "room" and self.g.get_node(edge.origin).type == "room"] - if len(current_edges) == 1: + to_stabilize_doors = [node for node in self.g.get_nodes_by_type("door") if "pre" in node.name] + if len(current_edges) == 1 and to_stabilize_doors == []: # From current edge, get the origin of the edge to get room node id self.room_exit_door_id = current_edges[0].origin exit_room_node = self.g.get_node(self.room_exit_door_id) # Store DSR graph in igraph self.store_graph() - # Load graph from file - self.long_term_graph.g = self.long_term_graph.read_graph("graph.pkl") + # Check if there are pending doors to stabilize + print("Pending doors to stabilize", self.pending_doors_to_stabilize) + if self.pending_doors_to_stabilize: + pending_door = self.pending_doors_to_stabilize.popleft() + print("Pending door to stabilize", pending_door) + # Try to associate + try: + self.associate_doors(pending_door[0], pending_door[1]) + except: + print("Error associating doors. One of the doors was not found in the global map") + self.pending_doors_to_stabilize.append(pending_door) + + # Add data to json file + # Draw graph from file - self.long_term_graph.draw_graph() + self.long_term_graph.draw_graph(False) # Compute metric map and draw it g_map = self.long_term_graph.compute_metric_map("room_1") self.long_term_graph.draw_metric_map(g_map, exit_room_node.name) @@ -202,15 +507,25 @@ def idle(self): final_robot_affordance_pose = self.inner_api.transform(exit_room_node.name, np.array([0., 1000., 0.], dtype=np.float64), exit_door_node.name) - # Append a 1 to the last_robot_pose array to make it a 3D array - final_robot_affordance_pose = np.append(final_robot_affordance_pose, 1) + wall_room_rt = self.rt_api.get_edge_RT(exit_room_node, exit_door_node.attrs["parent"].value) + wall_room_rotation = wall_room_rt.attrs["rt_rotation_euler_xyz"].value + robot_pose = [final_robot_affordance_pose[0], final_robot_affordance_pose[1], wall_room_rotation[2]] + # Transform final affordance pose to global reference - final_robot_affordance_pose_in_room_reference = self.long_term_graph.compute_element_pose(final_robot_affordance_pose, "room_1", + print("Final affordance pose in room reference", robot_pose) + final_robot_affordance_pose_in_room_reference = self.long_term_graph.compute_element_pose(robot_pose, "room_1", exit_room_node.name) + + print("Final affordance pose in global reference", final_robot_affordance_pose_in_room_reference) + pose_point = QPoint(final_robot_affordance_pose_in_room_reference[0], + final_robot_affordance_pose_in_room_reference[1]) + # Check if robot is in a room in the global map - other_side_room = self.long_term_graph.check_point_in_map(g_map, final_robot_affordance_pose_in_room_reference) + other_side_room = self.long_term_graph.check_point_in_map(g_map, pose_point) # Draw the transformed point in global map - self.long_term_graph.draw_point(final_robot_affordance_pose_in_room_reference) + second_point = QPoint(pose_point.x() - (250 * np.sin(final_robot_affordance_pose_in_room_reference[2])), pose_point.y() + (250 * np.cos(final_robot_affordance_pose_in_room_reference[2]))) + print("Pose point", pose_point, "Second point", second_point) + self.long_term_graph.draw_point(pose_point, second_point) # In case the robot is going to cross to a known room... if other_side_room != None: # Get exit door center pose @@ -218,34 +533,33 @@ def idle(self): exit_door_node.name) # Convert to np.array exit_door_pose = np.array(exit_door_pose, dtype=np.float32) - # Add a 1 to the exit_door_pose array to make it a 3D array - exit_door_pose = np.append(exit_door_pose, 1) print("Exit door pose", exit_door_pose) # Transform exit door pose to other_side_room reference exit_door_in_room_reference = self.long_term_graph.compute_element_pose(exit_door_pose, other_side_room, exit_room_node.name) + self.robot_exit_pose = self.long_term_graph.compute_element_pose(robot_pose, + other_side_room, + exit_room_node.name) # Get door nodes connected to room other_side_room doors = self.long_term_graph.get_room_objects_transform_matrices_with_name(other_side_room, "door") - closer_pose = None # Variable to set the closest door to the exit one + closer_pose = ("", np.finfo(np.float32).max) # Variable to set the closest door to the exit one # Iterate over known room doors for i in doors: # Get difference pose between robot and door door_pose = i[1].t - pose_difference = math.sqrt((exit_door_in_room_reference.x() - door_pose[0]) ** 2 + ( - exit_door_in_room_reference.y() - door_pose[1]) ** 2) - if closer_pose is None: + pose_difference = math.sqrt((exit_door_in_room_reference[0] - door_pose[0]) ** 2 + ( + exit_door_in_room_reference[1] - door_pose[1]) ** 2) + if pose_difference < closer_pose[1] and pose_difference < 1200: closer_pose = (i[0], pose_difference) - else: - if pose_difference < closer_pose[1]: - closer_pose = (i[0], pose_difference) - # Associate both doors data in igraph - self.associate_doors((closer_pose[0], other_side_room), - (exit_door_node.name, exit_room_node.name)) + if closer_pose[0] != "": + # Associate both doors data in igraph + self.associate_doors((closer_pose[0], other_side_room), + (exit_door_node.name, exit_room_node.name)) # Set to the exit door DSR node the attributes of the matched door in the new room exit_door_node.attrs["other_side_door_name"] = Attribute(closer_pose[0], self.agent_id) - # Insert the last number in the name of the room to the connected_room_id attribute + # Set to the exit door DSR node the connected room name exit_door_node.attrs["connected_room_name"] = Attribute(other_side_room, self.agent_id) self.g.update_node(exit_door_node) @@ -257,6 +571,11 @@ def idle(self): def crossed(self): # Get parent node of affordance node + """ + Updates the room state and door connections based on the current affordance + node and exit door node in the graph. + + """ affordance_node = self.g.get_node(self.affordance_node_active_id) if not affordance_node.attrs["parent"].value: # print("Affordance node has no parent") @@ -268,7 +587,7 @@ def crossed(self): self.g.delete_edge(self.room_exit_door_id, self.room_exit_door_id, "current") if exit_door_id_node: try: - if exit_door_id_node.attrs["other_side_door_name"].value: + if exit_door_id_node.attrs["connected_room_name"].value: self.state = "known_room" print("INSERTING KNOWN ROOM") except: @@ -278,6 +597,13 @@ def crossed(self): def initializing_room(self): # Get room nodes + """ + Retrieves a list of room nodes, selects the first node, sets it as the + current room, and inserts an edge representing this change into a graph. + The method then changes its internal state to "initializing_doors" and + prints a status message. + + """ room_nodes = [node for node in self.g.get_nodes_by_type("room") if node.id != self.room_exit_door_id and not "measured" in node.name] if len(room_nodes) == 0: # print("No room nodes different from the exit one found") @@ -291,76 +617,112 @@ def initializing_room(self): # def known_room(self): # Get other side door name attribute + """ + Navigates from the current room to another connected room through an exit + door, updates the graph representation of the new room and door, and adjusts + the robot's position and orientation accordingly. + + """ other_side_door_node = self.g.get_node(self.exit_door_id) - other_side_door_name = other_side_door_node.attrs["other_side_door_name"].value # TODO: Get directly the connected_room_name - # Search in self.graph for the node with the name of the other side door + other_side_room_name = other_side_door_node.attrs["connected_room_name"].value # TODO: Get directly the connected_room_name + # Search in self.long_term_graph.g for the node with the name of the other side door + print("known room", other_side_room_name) try: - new_door_node = self.graph.vs.find(name=other_side_door_name) - # Get room_id attribute of the other side door node - new_door_room_id = new_door_node["room_id"] - print("new_door_room_id", new_door_room_id) - try: - # Search in self.graph for the node with the room_id of the other side door - other_side_door_room_node = self.graph.vs.find(name="room_"+str(new_door_room_id)) - other_side_door_room_node_index = other_side_door_room_node.index - print("other_side_room_graph_name", other_side_door_room_node["name"]) - - # Insert the room node in the DSR graph - self.insert_dsr_vertex("root", other_side_door_room_node) - self.insert_dsr_edge(None, other_side_door_room_node) - - self.traverse_igraph(other_side_door_room_node) - - # Delete RT edge from room node t oShadow - self.g.delete_edge(self.room_exit_door_id, self.robot_id, "RT") - new_room_id = self.g.get_node(other_side_door_room_node["name"]).id - new_edge = Edge(self.robot_id, new_room_id, "RT", self.agent_id) - - rt_robot = self.inner_api.transform(other_side_door_room_node["name"], np.array([0. , -1000., 0.], dtype=np.float64), new_door_node["name"]) - print("sale por la puta puerta", new_door_node["name"]) - door_node = self.g.get_node(new_door_node["name"]) - door_parent_id = door_node.attrs["parent"].value - door_parent_node = self.g.get_node(door_parent_id) - print("Door parent name ", door_parent_node.name) - # get door parent node - # get rt from room node to door parent node - rt_room_wall = self.rt_api.get_edge_RT(self.g.get_node(other_side_door_room_node["name"]), door_parent_id) - # get rt_rotation_euler_xyz from rt_room_wall - door_rotation = rt_room_wall.attrs["rt_rotation_euler_xyz"].value - print("WALL ROTATION", door_rotation) - new_edge.attrs["rt_translation"] = Attribute(np.array(rt_robot, dtype=np.float32), self.agent_id) - # Get z rotation value and substract 180 degrees. then, keep the value between -pi and pi - new_z_value = (door_rotation[2] - math.pi) - if new_z_value > math.pi: - new_z_value = new_z_value - 2 * math.pi - elif new_z_value < -math.pi: - new_z_value = new_z_value + 2 * math.pi - - new_edge.attrs["rt_rotation_euler_xyz"] = Attribute(np.array([door_rotation[0], door_rotation[1], new_z_value], dtype=np.float32), - self.agent_id) - print("FIRST ROBOT RT", rt_robot, [door_rotation[0], door_rotation[1], new_z_value]) - self.g.insert_or_assign_edge(new_edge) - robot_node = self.g.get_node(self.robot_name) - # Modify parent attribute of robot node - robot_node.attrs["parent"] = Attribute(new_room_id, self.agent_id) - self.g.update_node(robot_node) - - # Insert current edge - self.insert_current_edge(new_room_id) + # Search in self.long_term_graph.g for the node with the room_id of the other side door + other_side_door_room_node = self.long_term_graph.g.vs.find(name=other_side_room_name) + print("other_side_room_graph_name", other_side_door_room_node["name"]) + + # Insert the room node in the DSR graph + self.insert_dsr_vertex("root", other_side_door_room_node) + self.insert_dsr_edge(None, other_side_door_room_node) + print("TRAVERSING GRAPH") + self.traverse_igraph(other_side_door_room_node) + print("TRAVERSED GRAPH") + exit_room_node = self.g.get_node(self.room_exit_door_id) + + # Delete RT edge from room node t oShadow + self.g.delete_edge(self.room_exit_door_id, self.robot_id, "RT") + new_room_id = self.g.get_node(other_side_door_room_node["name"]).id + new_edge = Edge(self.robot_id, new_room_id, "RT", self.agent_id) + # Get new door name + new_door_name = other_side_door_node.attrs["other_side_door_name"].value + if new_door_name == "": + print("No new door name found. Probably the associated door was not found in the global map. Take into account this as a future mission") + print("Setting as objetive the last affordance pose transformed to the global reference system") + + # final_robot_affordance_pose = self.inner_api.transform(exit_room_node.name, + # np.array([0., 1000., 0.], dtype=np.float64), + # other_side_door_node.name) + # wall_room_rt = self.rt_api.get_edge_RT(exit_room_node, exit_door_node.attrs["parent"].value) + # wall_room_rotation = wall_room_rt.attrs["rt_rotation_euler_xyz"].value + # # Append a 1 to the last_robot_pose array to make it a 3D array + # final_robot_pose = [final_robot_affordance_pose[0], final_robot_affordance_pose[1], wall_room_rotation[2]] + # # Transform final affordance pose to global reference + # final_robot_affordance_pose_in_room_reference = self.long_term_graph.compute_element_pose( + # final_robot_pose, "room_1", + # exit_room_node.name) + exit_robot_pose = self.robot_exit_pose + rt_robot = np.array([exit_robot_pose[0], exit_robot_pose[1], 0.0], dtype=np.float64) + door_rotation = [0., 0., exit_robot_pose[2]] + + else: + print("Going out door", new_door_name) + try: + new_door_node = self.long_term_graph.g.vs.find(name=new_door_name) + rt_robot = self.inner_api.transform(other_side_door_room_node["name"], np.array([0. , -1000., 0.], dtype=np.float64), new_door_node["name"]) + door_node = self.g.get_node(new_door_node["name"]) + door_parent_id = door_node.attrs["parent"].value + door_parent_node = self.g.get_node(door_parent_id) + print("Door parent name ", door_parent_node.name) + # get door parent node + # get rt from room node to door parent node + rt_room_wall = self.rt_api.get_edge_RT(self.g.get_node(other_side_door_room_node["name"]), + door_parent_id) + # get rt_rotation_euler_xyz from rt_room_wall + door_rotation = rt_room_wall.attrs["rt_rotation_euler_xyz"].value + new_z_value = (door_rotation[2] - math.pi) + if new_z_value > math.pi: + new_z_value = new_z_value - 2 * math.pi + elif new_z_value < -math.pi: + new_z_value = new_z_value + 2 * math.pi + door_rotation[2] = new_z_value + print("WALL ROTATION", door_rotation) + except: + print("No door node found") + return + + + new_edge.attrs["rt_translation"] = Attribute(np.array(rt_robot, dtype=np.float32), self.agent_id) + # Get z rotation value and substract 180 degrees. then, keep the value between -pi and pi + + new_edge.attrs["rt_rotation_euler_xyz"] = Attribute( + np.array(door_rotation, dtype=np.float32), + self.agent_id) + print("FIRST ROBOT RT", rt_robot, door_rotation) + self.g.insert_or_assign_edge(new_edge) + robot_node = self.g.get_node(self.robot_name) + # Modify parent attribute of robot node + robot_node.attrs["parent"] = Attribute(new_room_id, self.agent_id) + self.g.update_node(robot_node) + + # Insert current edge + self.insert_current_edge(new_room_id) - except Exception as e: - print("No other side door room node found") - print(e) - return except Exception as e: - print("No other side door node found") + print("No other side door room node found") print(e) return self.state = "removing" def initializing_doors(self): # Check if node called "room_entry" of type room exists + """ + Initializes and associates exit doors with their corresponding rooms, + storing the door names and connected room names as attributes. It also + updates the graph and sets a state to "removing". + + """ exit_edges = [edge for edge in self.g.get_edges_by_type("exit") if edge.destination == self.exit_door_id] if len(exit_edges) > 0: # Check if edge of type "same" exists between door_entry and enter_room_node @@ -392,25 +754,40 @@ def initializing_doors(self): other_side_door_node.attrs["connected_room_name"] = Attribute(connected_room_name_enter_door, self.agent_id) self.g.update_node(exit_door_node) self.g.update_node(other_side_door_node) - self.associate_doors((other_side_door_node.name, connected_room_name_exit_door), (exit_door_node.name, connected_room_name_enter_door)) - # Find each door in igraph self.state = "removing" def associate_doors(self, door_1, door_2): # Find each door in igraph and update attributes + """ + Associates two doors by adding an edge between their corresponding nodes + in the long-term graph, and sets additional attributes on each node to + store information about the other door and connected room. + + Args: + door_1 (Tuple[str, str]): Expected to represent a pair of door names + with connected rooms' names as its elements. + door_2 (Tuple[str, str]): Expected to contain the name of the second + door and its connected room. It is used to find the node corresponding + to this door in the graph and associate it with the first door. + + """ try: - door_1_node = self.graph.vs.find(name=door_1[0]) + door_1_node = self.long_term_graph.g.vs.find(name=door_1[0]) except: print("No door node found in igraph", door_1[0]) + self.pending_doors_to_stabilize.append((door_1, door_2)) + print("Pending doors to stabilize", self.pending_doors_to_stabilize) return try: - door_2_node = self.graph.vs.find(name=door_2[0]) + door_2_node = self.long_term_graph.g.vs.find(name=door_2[0]) except: print("No door node found in igraph", door_2[0]) + self.pending_doors_to_stabilize.append((door_1, door_2)) + print("Pending doors to stabilize", self.pending_doors_to_stabilize) return - self.graph.add_edge(door_1_node, door_2_node) + self.long_term_graph.g.add_edge(door_1_node, door_2_node) door_1_node["other_side_door_name"] = door_2[0] door_1_node["connected_room_name"] = door_2[1] door_2_node["other_side_door_name"] = door_1[0] @@ -418,10 +795,16 @@ def associate_doors(self, door_1, door_2): def store_graph(self): + """ + Stores the current state of a graph, represented by an igraph object, into + a pickle file named "graph.pkl". It first retrieves and verifies the + existence of a room node in the graph before storing it. + + """ actual_room_node = self.g.get_node(self.room_exit_door_id) # Check if node in igraph with the same name exists try: - room_node = self.graph.vs.find(name=actual_room_node.name) + room_node = self.long_term_graph.g.vs.find(name=actual_room_node.name) print("Room node found in igraph") except Exception as e: print("No room node found in igraph. Inserting room") @@ -429,10 +812,17 @@ def store_graph(self): # Save graph to file with open("graph.pkl", "wb") as f: - pickle.dump(self.graph, f) + pickle.dump(self.long_term_graph.g, f) def removing(self): # # Get last number in the name of the room + """ + Deletes nodes and edges from the graph based on certain conditions. It + first identifies nodes connected to a specified room exit door, then removes + nodes with no outgoing edges and finally removes destinations of remaining + edges sorted by their values in descending order. + + """ room_number = self.g.get_node(self.room_exit_door_id).attrs["room_id"].value # # Get all RT edges rt_edges = self.g.get_edges_by_type("RT") @@ -454,11 +844,21 @@ def removing(self): continue self.g.delete_node(item.destination) - self.long_term_graph.draw_graph() + self.long_term_graph.draw_graph(False) self.state = "idle" def traverse_graph(self, node_id): # Mark the current node as visited and print it + """ + Recursively traverses a graph, starting from a specified node, and inserts + nodes and edges into an igraph object. It identifies RT-type edges with + destinations other than the robot ID and explores them further. + + Args: + node_id (int): Expected to be an identifier for a node within the graph + data structure (`self.g`). + + """ node = self.g.get_node(node_id) rt_children = [edge for edge in self.g.get_edges_by_type("RT") if edge.origin == node_id and edge.destination != self.robot_id] self.insert_igraph_vertex(node) @@ -468,10 +868,22 @@ def traverse_graph(self, node_id): self.insert_igraph_edge(i) def traverse_igraph(self, node): - vertex_successors = self.graph.successors(node.index) + """ + Traverses the long-term graph, starting from a given node, and recursively + explores its successors, inserting new vertices and edges into a data + structure whenever it finds a suitable successor with higher level and + same room_id as the current node. + + Args: + node (igraph.vs): Passed to this function. It represents an individual + vertex (node) within the graph and contains information about the + node, such as its index, name, room_id, and level. + + """ + vertex_successors = self.long_term_graph.g.successors(node.index) # Recur for all the vertices adjacent to thisvertex for i in vertex_successors: - sucessor = self.graph.vs[i] + sucessor = self.long_term_graph.g.vs[i] if sucessor["room_id"] == node["room_id"] and sucessor["level"] > node["level"]: self.insert_dsr_vertex(node["name"], sucessor) # Check if node with id i room_id attribute is the same as the room_id attribute of the room node @@ -481,21 +893,33 @@ def traverse_igraph(self, node): continue def insert_igraph_vertex(self, node): - self.graph.add_vertex(name=node.name, id=node.id, type=node.type) + """ + Adds a vertex to an igraph graph object, long_term_graph.g, with attributes + from a given node object. It also checks for specific attributes and adds + edges between vertices if necessary. + + Args: + node (object): Assumed to have attributes such as `name`, `id`, `type`, + and possibly others like `attrs`. The `node` parameter is used to + add vertices to an igraph graph and also potentially establish + edges between them. + + """ + self.long_term_graph.g.add_vertex(name=node.name, id=node.id, type=node.type) # print("Inserting vertex", node.name, node.id) for attr in node.attrs: if attr in self.not_required_attrs: continue - self.graph.vs[self.vertex_size][attr] = node.attrs[attr].value + self.long_term_graph.g.vs[self.vertex_size][attr] = node.attrs[attr].value # Check if current attribute is other_side_door_name and, if it has value, check if the node with that name exists in the graph if attr == "other_side_door_name" and node.attrs[attr].value: try: print("Matched other_side_door_name", node.attrs[attr].value) - origin_node = self.graph.vs.find(id=node.id) + origin_node = self.long_term_graph.g.vs.find(id=node.id) try: - other_side_door_node = self.graph.vs.find(name=node.attrs[attr].value) + other_side_door_node = self.long_term_graph.g.vs.find(name=node.attrs[attr].value) try: - self.graph.add_edge(origin_node, other_side_door_node) + self.long_term_graph.g.add_edge(origin_node, other_side_door_node) print("Matched other_side_door_name", node.attrs[attr].value, other_side_door_node) except Exception as e: print("No other_side_door_name node found", node.attrs[attr].value) @@ -508,15 +932,29 @@ def insert_igraph_vertex(self, node): # Check if current attribute is connected_room_name and, if it has value, check if the node with that name exists in the graph # if attr == "connected_room_name" and node.attrs[attr].value: # try: - # connescted_room_node = self.graph.vs.find(name=node.attrs[attr].value) + # connescted_room_node = self.long_term_graph.g.vs.find(name=node.attrs[attr].value) # if connected_room_node: - # self.graph.add_edge(self.vertex_size, connected_room_node.id) + # self.long_term_graph.g.add_edge(self.vertex_size, connected_room_node.id) # except: # print("No connected_room_name attribute found") self.vertex_size += 1 def insert_dsr_vertex(self, parent_name, node): # print("Inserting vertex", node["name"], node["type"]) + """ + Inserts a new node into a graph, creating it from a given dictionary and + linking it to an existing parent node with the provided name. It also + copies over some attributes from the original node. + + Args: + parent_name (str): Used to get an existing node from the graph using + its name, which is then used as the parent for the new vertex being + inserted into the graph. + node (Dict): Expected to contain attributes such as "type", "name" + that are used to create a new Node object. The values in this + dictionary are used to set the properties of the newly created node. + + """ new_node = Node(agent_id=self.agent_id, type=node["type"], name=node["name"]) # Check if the node is a room node @@ -530,12 +968,32 @@ def insert_dsr_vertex(self, parent_name, node): new_node.attrs[attr] = Attribute(node[attr], self.agent_id) id_result = self.g.insert_node(new_node) def insert_igraph_edge(self, edge): + """ + Inserts an edge into the long-term graph based on the origin and destination + nodes, considering translation and rotation attributes from the provided + edge. It also applies special handling for door destinations. + + Args: + edge (object): Likely an instance of a class representing an edge in + a graph, possibly containing information about its origin, + destination, translation, rotation, and other attributes. + + """ + origin_node_dsr = self.g.get_node(edge.origin) + destination_node_dsr = self.g.get_node(edge.destination) # Search for the origin and destination nodes in the graph - origin_node = self.graph.vs.find(id=edge.origin) - destination_node = self.graph.vs.find(id=edge.destination) + origin_node = self.long_term_graph.g.vs.find(name=origin_node_dsr.name) + destination_node = self.long_term_graph.g.vs.find(name=destination_node_dsr.name) # Add the edge to the graph - self.graph.add_edge(origin_node, destination_node, rt=edge.attrs["rt_translation"].value, rotation=edge.attrs["rt_rotation_euler_xyz"].value) + if destination_node_dsr.name != self.robot_name: + # Check if destination_node["type"] == "door" + traslation = edge.attrs["rt_translation"].value + if destination_node["type"] == "door": + traslation[1] += 100 + self.long_term_graph.g.add_edge(origin_node, destination_node, rt=traslation, rotation=edge.attrs["rt_rotation_euler_xyz"].value) + else: + self.long_term_graph.g.add_edge(origin_node, destination_node, traslation=edge.attrs["rt_translation"].value, rotation=edge.attrs["rt_rotation_euler_xyz"].value) # print("Inserting igraph edge", origin_node["name"], destination_node["name"]) # print("RT", edge.attrs["rt_translation"].value) # print("Rotation", edge.attrs["rt_rotation_euler_xyz"].value) @@ -544,6 +1002,20 @@ def insert_igraph_edge(self, edge): def insert_dsr_edge(self, org, dest): # print("ORG::", org) # self.insert_dsr_vertex(dest) + """ + Inserts or updates an edge between two nodes in a graph based on whether + one node (org) exists or not. It retrieves relevant data, creates a new + edge with attributes and inserts/assigns it to the graph. + + Args: + org (object | None): Used to specify whether a root node should be + created or an existing node from which to create a new edge. + dest (Dict[str, int]): Used to represent a destination node in the + graph. It contains a "name" key with a string value representing + the name of the node, and an "index" key with an integer value + representing the index of the node. + + """ if org is None: root_node = self.g.get_node("root") org_id = root_node.id @@ -551,8 +1023,8 @@ def insert_dsr_edge(self, org, dest): orientation = [0, 0, 0] else: # print("Inserting DSR edge", org["name"], dest["name"]) - edge_id = self.graph.get_eid(org.index, dest.index) - edge = self.graph.es[edge_id] + edge_id = self.long_term_graph.g.get_eid(org.index, dest.index) + edge = self.long_term_graph.g.es[edge_id] rt_value = edge["rt"] orientation = edge["rotation"] org_name = org["name"] @@ -571,32 +1043,21 @@ def insert_dsr_edge(self, org, dest): # print("Rotation", orientation) self.g.insert_or_assign_edge(new_edge) - def draw_graph(self): - self.ax.clear() - # Obtener las coordenadas de los vértices - layout = self.graph.layout("kamada_kawai") # Utiliza el layout Kamada-Kawai - # Dibujar los vértices - x, y = zip(*layout) - self.ax.scatter(x, y, s=100) # Ajustar el tamaño de los vértices con el parámetro 's' - # Dibujar las aristas - for edge in self.graph.get_edgelist(): - # Print rt_translation attribute - # Get edge data - # edge_data = self.graph.es[self.graph.get_eid(edge[0], edge[1])] - # print(edge_data["rt"]) - self.ax.plot([x[edge[0]], x[edge[1]]], [y[edge[0]], y[edge[1]]], color="grey") - # add arrow to the edge - self.ax.annotate("", xy=(x[edge[1]], y[edge[1]]), xytext=(x[edge[0]], y[edge[0]]), arrowprops=dict(arrowstyle="->", lw=2)) - - for i, txt in enumerate([f"Node {i}" for i in range(self.graph.vcount())]): - # Get name attribute - name = self.graph.vs[i]["name"] - self.ax.annotate(name, (x[i], y[i]), textcoords="offset points", xytext=(0, 10), ha='center') - # Adapt ax to the graph - self.ax.set_xlim([min(x) - 2, max(x) + 2]) - self.ax.set_ylim([min(y) - 2, max(y) + 2]) - def check_element_room_number(self, node_id): + """ + Retrieves the room ID associated with a node identified by its node ID + from a graph, and returns it if found; otherwise, it returns -1. The method + handles potential exceptions that may occur during the retrieval process. + + Args: + node_id (int | str): Required for this function. It represents an + identifier that uniquely identifies a node within a graph data structure. + + Returns: + str|int: Either a room ID associated with a node or -1 if no room ID + is found for that node. + + """ node = self.g.get_node(node_id) try: room_id = node.attrs["room_id"].value @@ -606,6 +1067,22 @@ def check_element_room_number(self, node_id): return -1 def check_element_level(self, node_id): + """ + Retrieves an element level from the attributes of a node with the given + ID in the graph, and returns it if found; otherwise, prints an error message + and returns -1. + + Args: + node_id (object): Expected to be the ID of a node stored in the graph + `self.g`. It is used to retrieve information about the node from + the graph. + + Returns: + int|str: 1) the value of "level" attribute of node with given id if + present, or 2) -1 and a print statement indicating that no such attribute + was found. + + """ node = self.g.get_node(node_id) try: element_level = node.attrs["level"].value @@ -650,6 +1127,16 @@ def check_element_level(self, node_id): def generate_room_picture(self, room_node_id): + """ + Retrieves information about a given room node, identifies edges of type + "RT" that are connected to it, and extracts translation data from these edges. + + Args: + room_node_id (int): Used to retrieve a specific node from a graph + object (`self.g`). This node represents a room, and its id is + needed to generate a picture for this room. + + """ room_node = self.g.get_node(room_node_id) # Get room node room_id attribute room_id = room_node.attrs["room_id"].value @@ -685,9 +1172,39 @@ def generate_room_picture(self, room_node_id): def insert_current_edge(self, room_id): # Insert current edge to the room + """ + Inserts or updates an edge in a graph (self.g). The edge represents a + current connection between two rooms with given IDs, identified by the + agent ID. It is created based on room_id and its mirrored version. + + Args: + room_id (int): Required for the creation of an `Edge` object. It + represents the ID of a room that will be part of the edge being + inserted into the graph. + + """ current_edge = Edge(room_id, room_id, "current", self.agent_id) self.g.insert_or_assign_edge(current_edge) + def kill_everything(self): + # Ruta al script que deseas ejecutar + """ + Executes a shell script, sets its executable permissions, runs it with the + argument 'true', and captures any output or errors. + + """ + script_path = '/home/robocomp/robocomp/components/robocomp-shadow/tools/room_and_door_kill_and_restart_dsr.sh' # Asegúrate de que el script tenga permisos de ejecución + # Ejecutar el script + subprocess.run(['chmod', '+x', script_path]) + result = subprocess.run([script_path, 'true'], capture_output=True, text=True) + # Imprimir la salida del scriptprint('Salida del script:')print(result.stdout) + # Imprimir los errores (si los hay) + if result.stderr: + print('Errores del script:') + print(result.stderr) + + # def add_data_to_json(self, room_node_id): + def startup_check(self): QTimer.singleShot(200, QApplication.instance().quit) @@ -698,6 +1215,61 @@ def update_node_att(self, id: int, attribute_names: [str]): console.print(f"UPDATE NODE ATT: {id} {attribute_names}", style='green') def update_node(self, id: int, type: str): + """ + Updates the node with given id and type in the graph. If the type is "door", + it inserts the door node into an igraph graph and adds edges between nodes. + If the id matches the affordance node active id, it checks the state of + the affordance node and changes its state if necessary. + + Args: + id (int): Used as an identifier for nodes in a graph, specifically to + identify either a door node or an affordance node based on the + value of the `type` parameter. + type (str): Used to determine what action should be taken when updating + a node with the given id. The valid values for this parameter are + "door" or other types that may be added in the future. + + """ + if type == "door": + # Get door name + door_node = self.g.get_node(id) + if not "pre" in door_node.name: + # Check if door exists in igraph yet + try: + door_igraph = self.long_term_graph.g.vs.find(name=door_node.name) + # print("Door node found in igraph. Returning.") + return + except: + # print("No door node found in igraph. Checking if room node exists") + # Get room node + room_id = door_node.attrs["room_id"].value + try: + room_node = self.long_term_graph.g.vs.find(name="room_" + str(room_id)) + print("Room node found in igraph. Inserting door") + parent_id = door_node.attrs["parent"].value + # print("Parent id", parent_id) + door_parent_node = self.g.get_node(parent_id) + # print("Door parent name", door_parent_node.name) + # Insert door node in igraph + self.insert_igraph_vertex(door_node) + # print("Door inserted in igraph") + # Get RT from door_parent to door + # rt_door = self.rt_api.get_edge_RT(door_parent_node, door_node.id) + + rt_door = self.g.get_edge(door_parent_node.id, door_node.id, "RT") + # print("RT DOOR", rt_door.attrs["rt_translation"].value, rt_door.attrs["rt_rotation_euler_xyz"].value) + # print("Arigin name", door_parent_node.name, "Destination name", door_node.name) + # print("IDS", door_parent_node.id, door_node.id) + self.insert_igraph_edge(rt_door) + with open("graph.pkl", "wb") as f: + pickle.dump(self.long_term_graph.g, f) + # print("Door inserted in igraph") + self.long_term_graph.draw_graph(False) + except Exception as e: + # print("No room node found in igraph. Not possible to insert door") + # print(e) + return + if id == self.affordance_node_active_id: print("Affordance node is active") affordance_node = self.g.get_node(id) @@ -716,6 +1288,20 @@ def update_edge(self, fr: int, to: int, type: str): # Insert current edge to the room # TODO: Posible problema si tocas el nodo room en la interfaz gráfica + """ + Updates the current edge when a specific condition is met: if there are + no current edges, the update edge originates from the room exit door, and + its type is "RT". It inserts the edge into the graph and prints a message. + + Args: + fr (int): Likely to represent the from ID, indicating the starting + point of an edge in the graph. + to (int): Used to specify the destination of an edge in a graph, which + represents the ID of another node in the graph. + type (str): Expected to be set with value "RT". This indicates that + it is an exit door edge, which leads to the robot's current room. + + """ if to == self.robot_id and fr != self.room_exit_door_id and type == "RT" and len(self.g.get_edges_by_type("current")) == 0: print(self.room_exit_door_id) # if len(self.g.get_nodes_by_type("room")) == 1 and type == "room" and not "measured" in self.g.get_node(id).name: diff --git a/agents/long_term_spatial_memory_agent/src/specificworker_sec.py b/agents/long_term_spatial_memory_agent/src/specificworker_sec.py new file mode 100644 index 00000000..b27f7a61 --- /dev/null +++ b/agents/long_term_spatial_memory_agent/src/specificworker_sec.py @@ -0,0 +1,1100 @@ +#!/usr/bin/python3 +# -*- coding: utf-8 -*- +# +# Copyright (C) 2024 by YOUR NAME HERE +# +# This file is part of RoboComp +# +# RoboComp is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# RoboComp is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with RoboComp. If not, see . +# +import numpy as np +from PySide2.QtCore import QTimer +from PySide2.QtWidgets import QApplication +from rich.console import Console +from genericworker import * +import igraph as ig +import interfaces as ifaces +import matplotlib.pyplot as plt +import time +import setproctitle +import math +import pickle + +from long_term_graph import LongTermGraph + +import cv2 + +sys.path.append('/opt/robocomp/lib') +console = Console(highlight=False) + +try: + setproctitle.setproctitle(os.path.basename(os.getcwd())) + print("Process title set to", os.path.basename(os.getcwd())) +except: + pass + +from pydsr import * + +class SpecificWorker(GenericWorker): + """ + Manages a graph representation of a robot navigating through a room, handling + various actions such as inserting, updating, and deleting nodes and edges, as + well as tracking the current state of the room. + + Attributes: + Period (Union[float,int]): Used to represent the time period for which the + worker is active. + agent_id (int|str): Used to store the unique identifier of the agent in + the simulation. + g (igraphGraph): Used to represent the graph of nodes and edges in the + environment. It is used for various operations such as inserting, + deleting, updating nodes and edges, and drawing the graph. + update_node (Dict[str,Any]): Used to update the node attributes of a + specific node in the graph based on its ID. It takes two arguments: + `id` (int) which is the ID of the node to be updated, and `type` (str) + which can be either "door" or "room" indicating whether the node is a + door or room node. The function performs different actions depending + on the type of node being updated. + update_edge (Union[int,str]): Used to update the edge attributes of a + robot-room pair based on certain conditions, such as when there is no + current edge and the room node exists. + startup_check (bool|str): Used to check if the worker should perform startup + tasks such as inserting a node and edge for a room and robot, and + setting the state to "crossed". + rt_api (str|int): Used to store the ID of the robot that the worker is + associated with, allowing the worker to perform RT-based actions. + inner_api (Dict[str,Any]): Used to store additional APIs that are specific + to this worker. + robot_name (str|int): Used to store the ID of the robot that the worker + is associated with. + robot_id (int): Used to identify the robot that the worker is controlling. + last_robot_pose (Tuple[float,float,float]): Used to store the last known + pose of the robot in the environment, which can be used for various + tasks such as path planning and obstacle avoidance. + robot_exit_pose (str|int): Used to store the exit pose of a robot, which + is the position and orientation of the robot when it exits a room. + state (str|int): Used to keep track of the worker's current state (either + "idle", "crossed", or "completed"). + affordance_node_active_id (int|bool): Used to keep track of the active + affordance node ID in the graph. It is used to determine when the + affordance node has been completed and can transition to the crossed + state. + exit_door_id (int|str): Used to keep track of the id of the door node that + leads from a room to the outside world, which is used for various + purposes such as routing, affordance detection, and edge insertion. + room_exit_door_id (int|str): Used to represent the ID of the door node + that marks the exit of a room in the graph. It is used for updating + edges and inserting current edges. + enter_room_node_id (int|str): Used to store the ID of the room node that + the worker enters when it completes its task. + vertex_size (float|int): Used to control the size of the vertices in the + graph. It determines the width or height of each vertex in the graph, + which can be useful for visualization purposes. + not_required_attrs (List[str]): Used to store a list of attribute names + that are not required for the worker's functionality, i.e., they are + optional or non-essential attributes. + long_term_graph (igraphGraph): Used to store the long-term graph of the + environment, which can be different from the short-term graph represented + by the `g` attribute. + graph (igraphGraph): Used to store the current state of the graph, which + can be updated and manipulated during the execution of the worker's methods. + insert_current_edge (List[Edge]): Used to insert a new edge into the graph + with the specified from and to nodes, and with the "current" type. + timer (int|str): Used to store a timer for the worker, indicating how long + it has been running. + compute (str|int): Used to store the ID of the node that should be updated + or inserted in the graph during computation. + + """ + def __init__(self, proxy_map, startup_check=False): + """ + Initializes the worker's internal state, including its graph, node ID, and + affordance node active ID. It also connects signals for updating nodes and + edges and sets up a timer to call the `compute` method periodically. + + Args: + proxy_map (Dict[str, Any]): Used to store a map of proxy nodes for the + specific worker. + startup_check (bool): Used to check if the graph has already been + initialized before creating its inner API. It is set to False by + default, meaning no check is performed. + + """ + super(SpecificWorker, self).__init__(proxy_map) + self.Period = 100 + + # YOU MUST SET AN UNIQUE ID FOR THIS AGENT IN YOUR DEPLOYMENT. "_CHANGE_THIS_ID_" for a valid unique integer + self.agent_id = 13 + self.g = DSRGraph(0, "LongTermSpatialMemory_agent", self.agent_id) + + try: + #signals.connect(self.g, signals.UPDATE_NODE_ATTR, self.update_node_att) + signals.connect(self.g, signals.UPDATE_NODE, self.update_node) + #signals.connect(self.g, signals.DELETE_NODE, self.delete_node) + signals.connect(self.g, signals.UPDATE_EDGE, self.update_edge) + #signals.connect(self.g, signals.UPDATE_EDGE_ATTR, self.update_edge_att) + #signals.connect(self.g, signals.DELETE_EDGE, self.delete_edge) + console.print("signals connected") + except RuntimeError as e: + print(e) + + if startup_check: + self.startup_check() + else: + self.rt_api = rt_api(self.g) + self.inner_api = inner_api(self.g) + + # Robot node variables + self.robot_name = "Shadow" + self.robot_id = self.g.get_node(self.robot_name).id + self.last_robot_pose = [0, 0, 0] + self.robot_exit_pose = [0, 0, 0] + + # Variable for designing the state machine + self.state = "idle" + print("Starting in IDLE state") + + # ID variables + self.affordance_node_active_id = None # Affordance node ID + self.exit_door_id = None # Exit door node ID + + # Room nodes variables + self.room_exit_door_id = -1 # Exit door node ID + self.enter_room_node_id = None # Enter room node ID + + # Graph variables + self.vertex_size = 0 + self.not_required_attrs = ["parent", "timestamp_alivetime", "timestamp_creation", "rt", "valid", "obj_checked", "name", "id"] + + # Global map variables + self.long_term_graph = LongTermGraph("graph.pkl") + if self.long_term_graph.g: + print("Graph exists") + self.graph = self.long_term_graph.g + self.long_term_graph.draw_graph(False) + # Compute metric map and draw it + g_map = self.long_term_graph.compute_metric_map("room_1") + self.long_term_graph.draw_metric_map(g_map) + else: + print("Graph does not exist. Creating a new one") + self.graph = ig.Graph() + + # In case the room node exists but the current edge is not set, set it + room_nodes = self.g.get_nodes_by_type("room") + current_room_nodes = [node for node in room_nodes if self.g.get_edge(node.id, node.id, "current")] + if len(current_room_nodes) == 0 and len(room_nodes) == 1: + print("Room node exists but no current edge. Setting as current") + if not "measured" in room_nodes[0].name: + self.insert_current_edge(room_nodes[0].id) + + self.timer.timeout.connect(self.compute) + self.timer.start(self.Period) + + def __del__(self): + """Destructor""" + + def setParams(self, params): + """ + Sets the parameters passed as an argument, then modifies the room by + removing a self-edge and adding attributes to doors. + + Args: + params (bool): Passed to set the parameters of the Room object. + + Returns: + bool: True. + + """ + return True + + # PROTOcode + # Check if there is a node of type aff_cross and it has it's valid attribute to true + # if so, check if aff_cross status attribute is completed, was active and robot pose is outside the room polygon, + + # - then remove "current" self-edge from the room + # - Store in a variable the ID of the exit door + # - wait until a new room is stabilized + # - when new room is stabilized, check for the door used to get inside + # - store in both doors the other_side_door name attribute + # - Read exit door node and add an attribute other_side_door with the name of the entrance door in the new room + # - Read entrance door node and add an attribute other_side_door with the name of the exit door in the new room + @QtCore.Slot() + def compute(self): + # Check if graph exists + """ + Computes the RT of a robot in a graph, taking into account the current + edges and long-term graph information. + + """ + if self.long_term_graph.g: + # Get room with "current" edge + current_edges = [edge for edge in self.g.get_edges_by_type("current") if self.g.get_node(edge.destination).type == "room" and self.g.get_node(edge.origin).type == "room"] + if len(current_edges) == 1: + actual_room_node = self.g.get_node(current_edges[0].origin) + # Get robot pose + robot_rt = self.rt_api.get_edge_RT(actual_room_node, self.robot_id) + # Check if robot node exists in graph + try: + robot_node = self.graph.vs.find(name=self.robot_name) + # Get robot antecessor + robot_node_successors = self.g.successors(robot_node) + + except: + print("No robot node found in igraph. Inserting") + robot_node_dsr = self.g.get_node(self.robot_name) + self.insert_igraph_vertex(robot_node_dsr) + self.insert_igraph_edge(robot_rt) + self.long_term_graph.draw_graph(False) + + + + match self.state: + case "idle": + self.idle() + case "crossing": + pass + case "crossed": + self.crossed() + case "initializing_room": + self.initializing_room() + case "known_room": + self.known_room() + case "initializing_doors": + self.initializing_doors() + case "store_graph": + self.store_graph() + case "removing": + self.removing() + + + def idle(self): + # Check if there is a node of type aff_cross and it has it's valid attribute to true using comprehension list + """ + Checks if there are any "current" edges or affordance nodes in the graph, + and performs actions based on their existence. + + """ + aff_cross_nodes = [node for node in self.g.get_nodes_by_type("affordance") if node.attrs["active"].value == True] + # Check if not empty + if len(aff_cross_nodes) == 0 or len(aff_cross_nodes) > 1: + # print("No aff_cross nodes with valid attribute or more than one valid affordance") + return + else: + # Check if any "current" edge exists + current_edges = [edge for edge in self.g.get_edges_by_type("current") if self.g.get_node(edge.destination).type == "room" and self.g.get_node(edge.origin).type == "room"] + to_stabilize_doors = [node for node in self.g.get_nodes_by_type("door") if "pre" in node.name] + if len(current_edges) == 1 and to_stabilize_doors == []: + # From current edge, get the origin of the edge to get room node id + self.room_exit_door_id = current_edges[0].origin + exit_room_node = self.g.get_node(self.room_exit_door_id) + # Store DSR graph in igraph + self.store_graph() + # Load graph from file + self.long_term_graph.g = self.long_term_graph.read_graph("graph.pkl") + # Draw graph from file + self.long_term_graph.draw_graph(False) + # Compute metric map and draw it + g_map = self.long_term_graph.compute_metric_map("room_1") + self.long_term_graph.draw_metric_map(g_map, exit_room_node.name) + # Get affordance node + self.affordance_node_active_id = aff_cross_nodes[0].id + affordance_node = self.g.get_node(self.affordance_node_active_id) + if not affordance_node.attrs["parent"].value: + print("Affordance node has no parent") + return + else: + # Considering that the final affordance pose at crossing a door is at point at 1000 meters in the y axis + # in the normal to the door pointing to the center of the new room, transform this pose to the + # global reference system + self.exit_door_id = affordance_node.attrs["parent"].value + exit_door_node = self.g.get_node(self.exit_door_id) + final_robot_affordance_pose = self.inner_api.transform(exit_room_node.name, + np.array([0., 1000., 0.], dtype=np.float64), + exit_door_node.name) + wall_room_rt = self.rt_api.get_edge_RT(exit_room_node, exit_door_node.attrs["parent"].value) + wall_room_rotation = wall_room_rt.attrs["rt_rotation_euler_xyz"].value + robot_pose = [final_robot_affordance_pose[0], final_robot_affordance_pose[1], wall_room_rotation[2]] + + # Transform final affordance pose to global reference + print("Final affordance pose in room reference", robot_pose) + final_robot_affordance_pose_in_room_reference = self.long_term_graph.compute_element_pose(robot_pose, "room_1", + exit_room_node.name) + + print("Final affordance pose in global reference", final_robot_affordance_pose_in_room_reference) + pose_point = QPoint(final_robot_affordance_pose_in_room_reference[0], + final_robot_affordance_pose_in_room_reference[1]) + + # Check if robot is in a room in the global map + other_side_room = self.long_term_graph.check_point_in_map(g_map, pose_point) + # Draw the transformed point in global map + second_point = QPoint(pose_point.x() - (250 * np.sin(final_robot_affordance_pose_in_room_reference[2])), pose_point.y() + (250 * np.cos(final_robot_affordance_pose_in_room_reference[2]))) + print("Pose point", pose_point, "Second point", second_point) + self.long_term_graph.draw_point(pose_point, second_point) + # In case the robot is going to cross to a known room... + if other_side_room != None: + # Get exit door center pose + exit_door_pose = self.inner_api.transform(exit_room_node.name, + exit_door_node.name) + # Convert to np.array + exit_door_pose = np.array(exit_door_pose, dtype=np.float32) + print("Exit door pose", exit_door_pose) + # Transform exit door pose to other_side_room reference + exit_door_in_room_reference = self.long_term_graph.compute_element_pose(exit_door_pose, + other_side_room, + exit_room_node.name) + + self.robot_exit_pose = self.long_term_graph.compute_element_pose(robot_pose, + other_side_room, + exit_room_node.name) + # Get door nodes connected to room other_side_room + doors = self.long_term_graph.get_room_objects_transform_matrices_with_name(other_side_room, "door") + closer_pose = ("", np.finfo(np.float32).max) # Variable to set the closest door to the exit one + # Iterate over known room doors + for i in doors: + # Get difference pose between robot and door + door_pose = i[1].t + pose_difference = math.sqrt((exit_door_in_room_reference[0] - door_pose[0]) ** 2 + ( + exit_door_in_room_reference[1] - door_pose[1]) ** 2) + if pose_difference < closer_pose[1] and pose_difference < 1200: + closer_pose = (i[0], pose_difference) + if closer_pose[0] != "": + # Associate both doors data in igraph + self.associate_doors((closer_pose[0], other_side_room), + (exit_door_node.name, exit_room_node.name)) + # Set to the exit door DSR node the attributes of the matched door in the new room + exit_door_node.attrs["other_side_door_name"] = Attribute(closer_pose[0], self.agent_id) + # Set to the exit door DSR node the connected room name + exit_door_node.attrs["connected_room_name"] = Attribute(other_side_room, + self.agent_id) + self.g.update_node(exit_door_node) + self.state = "crossing" + print("CROSSING") + else: + print("No current room") + return + + def crossed(self): + # Get parent node of affordance node + """ + Determines the current room and updates the state of the worker based on + whether it is a known or unknown room. + + """ + affordance_node = self.g.get_node(self.affordance_node_active_id) + if not affordance_node.attrs["parent"].value: + # print("Affordance node has no parent") + return + else: + self.exit_door_id = affordance_node.attrs["parent"].value + exit_door_id_node = self.g.get_node(self.exit_door_id) + # Remove "current" self-edge from the room + self.g.delete_edge(self.room_exit_door_id, self.room_exit_door_id, "current") + if exit_door_id_node: + try: + if exit_door_id_node.attrs["connected_room_name"].value: + self.state = "known_room" + print("INSERTING KNOWN ROOM") + except: + self.state = "initializing_room" + print("INITIALIZING ROOM") + + def initializing_room(self): + + # Get room nodes + """ + Initializes the room nodes in the graph, identifying and selecting the + entrance node based on the exit door ID, and setting the current state to + "initializing doors". + + """ + room_nodes = [node for node in self.g.get_nodes_by_type("room") if node.id != self.room_exit_door_id and not "measured" in node.name] + if len(room_nodes) == 0: + # print("No room nodes different from the exit one found") + return + else: + # Get the enter room node id + self.enter_room_node_id = room_nodes[0].id + self.insert_current_edge(self.enter_room_node_id) + self.state = "initializing_doors" + print("INITIALIZING DOORS") + # + def known_room(self): + # Get other side door name attribute + """ + Determines the room the robot is currently in, based on the global map and + the robot's position, and updates the graph with the appropriate edges and + nodes. + + """ + other_side_door_node = self.g.get_node(self.exit_door_id) + other_side_room_name = other_side_door_node.attrs["connected_room_name"].value # TODO: Get directly the connected_room_name + # Search in self.graph for the node with the name of the other side door + print("known room", other_side_room_name) + try: + # Search in self.graph for the node with the room_id of the other side door + other_side_door_room_node = self.graph.vs.find(name=other_side_room_name) + print("other_side_room_graph_name", other_side_door_room_node["name"]) + + # Insert the room node in the DSR graph + self.insert_dsr_vertex("root", other_side_door_room_node) + self.insert_dsr_edge(None, other_side_door_room_node) + print("TRAVERSING GRAPH") + self.traverse_igraph(other_side_door_room_node) + print("TRAVERSED GRAPH") + exit_room_node = self.g.get_node(self.room_exit_door_id) + + # Delete RT edge from room node t oShadow + self.g.delete_edge(self.room_exit_door_id, self.robot_id, "RT") + new_room_id = self.g.get_node(other_side_door_room_node["name"]).id + new_edge = Edge(self.robot_id, new_room_id, "RT", self.agent_id) + # Get new door name + new_door_name = other_side_door_node.attrs["other_side_door_name"].value + if new_door_name == "": + print("No new door name found. Probably the associated door was not found in the global map. Take into account this as a future mission") + print("Setting as objetive the last affordance pose transformed to the global reference system") + + # final_robot_affordance_pose = self.inner_api.transform(exit_room_node.name, + # np.array([0., 1000., 0.], dtype=np.float64), + # other_side_door_node.name) + # wall_room_rt = self.rt_api.get_edge_RT(exit_room_node, exit_door_node.attrs["parent"].value) + # wall_room_rotation = wall_room_rt.attrs["rt_rotation_euler_xyz"].value + # # Append a 1 to the last_robot_pose array to make it a 3D array + # final_robot_pose = [final_robot_affordance_pose[0], final_robot_affordance_pose[1], wall_room_rotation[2]] + # # Transform final affordance pose to global reference + # final_robot_affordance_pose_in_room_reference = self.long_term_graph.compute_element_pose( + # final_robot_pose, "room_1", + # exit_room_node.name) + exit_robot_pose = self.robot_exit_pose + rt_robot = np.array([exit_robot_pose[0], exit_robot_pose[1], 0.0], dtype=np.float64) + door_rotation = [0., 0., exit_robot_pose[2]] + + else: + print("Going out door", new_door_name) + try: + new_door_node = self.graph.vs.find(name=new_door_name) + rt_robot = self.inner_api.transform(other_side_door_room_node["name"], np.array([0. , -1000., 0.], dtype=np.float64), new_door_node["name"]) + door_node = self.g.get_node(new_door_node["name"]) + door_parent_id = door_node.attrs["parent"].value + door_parent_node = self.g.get_node(door_parent_id) + print("Door parent name ", door_parent_node.name) + # get door parent node + # get rt from room node to door parent node + rt_room_wall = self.rt_api.get_edge_RT(self.g.get_node(other_side_door_room_node["name"]), + door_parent_id) + # get rt_rotation_euler_xyz from rt_room_wall + door_rotation = rt_room_wall.attrs["rt_rotation_euler_xyz"].value + new_z_value = (door_rotation[2] - math.pi) + if new_z_value > math.pi: + new_z_value = new_z_value - 2 * math.pi + elif new_z_value < -math.pi: + new_z_value = new_z_value + 2 * math.pi + door_rotation[2] = new_z_value + print("WALL ROTATION", door_rotation) + except: + print("No door node found") + return + + + new_edge.attrs["rt_translation"] = Attribute(np.array(rt_robot, dtype=np.float32), self.agent_id) + # Get z rotation value and substract 180 degrees. then, keep the value between -pi and pi + + new_edge.attrs["rt_rotation_euler_xyz"] = Attribute( + np.array(door_rotation, dtype=np.float32), + self.agent_id) + print("FIRST ROBOT RT", rt_robot, door_rotation) + self.g.insert_or_assign_edge(new_edge) + robot_node = self.g.get_node(self.robot_name) + # Modify parent attribute of robot node + robot_node.attrs["parent"] = Attribute(new_room_id, self.agent_id) + self.g.update_node(robot_node) + + # Insert current edge + self.insert_current_edge(new_room_id) + + + except Exception as e: + print("No other side door room node found") + print(e) + return + self.state = "removing" + + def initializing_doors(self): + # Check if node called "room_entry" of type room exists + """ + 1) identifies exit edges in the graph, 2) finds matching doors, and 3) + associates them to create a connected room hierarchy. + + """ + exit_edges = [edge for edge in self.g.get_edges_by_type("exit") if edge.destination == self.exit_door_id] + if len(exit_edges) > 0: + # Check if edge of type "same" exists between door_entry and enter_room_node + same_edges = self.g.get_edges_by_type("match") + if len(same_edges) == 0: + # print("No same edges found") + return + else: + # Get the other side door id TODO: the edge comes from door_entry to nominal door (set in door_detector) + other_side_door_id = same_edges[0].origin + other_side_door_node = self.g.get_node(other_side_door_id) + exit_door_id = same_edges[0].destination + exit_door_node = self.g.get_node(exit_door_id) + + print(other_side_door_node.name, exit_door_node.name) + + # Read exit door node and add an attribute other_side_door with the name of the entrance door in the new room + + connected_room_name_exit_door = self.g.get_node(self.g.get_node(other_side_door_node.attrs["parent"].value).attrs["parent"].value).name + connected_room_name_enter_door = self.g.get_node(self.room_exit_door_id).name + + exit_door_node.attrs["other_side_door_name"] = Attribute(other_side_door_node.name, self.agent_id) + # Insert the last number in the name of the room to the connected_room_id attribute + exit_door_node.attrs["connected_room_name"] = Attribute(connected_room_name_exit_door, self.agent_id) + + # Read entrance door node and add an attribute other_side_door with the name of the exit door in the new room + + other_side_door_node.attrs["other_side_door_name"] = Attribute(exit_door_node.name, self.agent_id) + other_side_door_node.attrs["connected_room_name"] = Attribute(connected_room_name_enter_door, self.agent_id) + self.g.update_node(exit_door_node) + self.g.update_node(other_side_door_node) + + self.associate_doors((other_side_door_node.name, connected_room_name_exit_door), (exit_door_node.name, connected_room_name_enter_door)) + + # Find each door in igraph + self.state = "removing" + + def associate_doors(self, door_1, door_2): + # Find each door in igraph and update attributes + """ + Connects two doors in a graph by adding an edge between them and setting + their "other side door name" and "connected room name" attributes. + + Args: + door_1 (str): A string representing the name of the first door to be + associated with another door. + door_2 (str | int): Used to represent the name of the second door that + needs to be associated with the first door. + + """ + try: + door_1_node = self.graph.vs.find(name=door_1[0]) + except: + print("No door node found in igraph", door_1[0]) + return + try: + door_2_node = self.graph.vs.find(name=door_2[0]) + except: + print("No door node found in igraph", door_2[0]) + return + self.graph.add_edge(door_1_node, door_2_node) + door_1_node["other_side_door_name"] = door_2[0] + door_1_node["connected_room_name"] = door_2[1] + door_2_node["other_side_door_name"] = door_1[0] + door_2_node["connected_room_name"] = door_1[1] + + + def store_graph(self): + """ + Saves the graph data to a file using pickling. + + """ + actual_room_node = self.g.get_node(self.room_exit_door_id) + # Check if node in igraph with the same name exists + try: + room_node = self.graph.vs.find(name=actual_room_node.name) + print("Room node found in igraph") + except Exception as e: + print("No room node found in igraph. Inserting room") + self.traverse_graph(self.room_exit_door_id) + + # Save graph to file + with open("graph.pkl", "wb") as f: + pickle.dump(self.graph, f) + + def removing(self): + # # Get last number in the name of the room + """ + Removes edges from the long-term graph that have been labeled as RT or + has, based on room numbers. It also deletes nodes in the graph that + correspond to shadow nodes. + + """ + room_number = self.g.get_node(self.room_exit_door_id).attrs["room_id"].value + # # Get all RT edges + rt_edges = self.g.get_edges_by_type("RT") + # # Get all RT edges which last number in name is the same as the room number and generate a dictionary with the origin node as key and the origin node level as value + old_room_rt_edges = [edge for edge in rt_edges if self.check_element_room_number(edge.origin) == room_number or self.check_element_room_number(edge.destination) == room_number] + has_edges = self.g.get_edges_by_type("has") + old_room_has_edges = [edge for edge in has_edges if self.check_element_room_number(edge.origin) == room_number] + for edge in old_room_has_edges: + self.g.delete_node(edge.destination) + + old_room_dict = {edge: int(self.check_element_room_number(edge.destination)) for edge in old_room_rt_edges} + # Order dictionary by level value in descending order + old_room_dict = dict(sorted(old_room_dict.items(), key=lambda item: item[1], reverse=True)) + # iterate over the dictionary in descending order + for item in old_room_dict: + # self.g.delete_node(self.g.get_node(item.origin).id) + if item.origin == 200 or item.destination == 200: + print("SHADOW NODES") + continue + + self.g.delete_node(item.destination) + self.long_term_graph.draw_graph(False) + self.state = "idle" + + def traverse_graph(self, node_id): + # Mark the current node as visited and print it + """ + Traverses a directed graph represented by an igraph object, starting from + a given node ID. It recursively visits all reachable nodes and inserts + edges from the root node to each visited node. + + Args: + node_id (int): Representing the unique identifier of a node in the + graph to be traversed. + + """ + node = self.g.get_node(node_id) + rt_children = [edge for edge in self.g.get_edges_by_type("RT") if edge.origin == node_id and edge.destination != self.robot_id] + self.insert_igraph_vertex(node) + # Recur for all the vertices adjacent to this vertex + for i in rt_children: + self.traverse_graph(i.destination) + self.insert_igraph_edge(i) + + def traverse_igraph(self, node): + """ + Iterates through the graph's successors of a given vertex, and for each + successor, it checks if the successor's level is higher than the current + vertex's level, and if so, it inserts a new vertex and edge in the DSR and + recursively traverses the graph. + + Args: + node (igraph.Vertex): Used to represent a specific vertex in the graph. + + """ + vertex_successors = self.graph.successors(node.index) + # Recur for all the vertices adjacent to thisvertex + for i in vertex_successors: + sucessor = self.graph.vs[i] + if sucessor["room_id"] == node["room_id"] and sucessor["level"] > node["level"]: + self.insert_dsr_vertex(node["name"], sucessor) + # Check if node with id i room_id attribute is the same as the room_id attribute of the room node + self.insert_dsr_edge(node, sucessor) + self.traverse_igraph(sucessor) + else: + continue + + def insert_igraph_vertex(self, node): + """ + Adds a vertex to an igraph graph, based on attributes provided by a node + object. It also tries to find matching vertices using specific attribute + values and adds edges between them if found. + + Args: + node (igraph.Node | dict): Used to add a new vertex to an igraph graph + with specified attributes. + + """ + self.graph.add_vertex(name=node.name, id=node.id, type=node.type) + # print("Inserting vertex", node.name, node.id) + for attr in node.attrs: + if attr in self.not_required_attrs: + continue + self.graph.vs[self.vertex_size][attr] = node.attrs[attr].value + # Check if current attribute is other_side_door_name and, if it has value, check if the node with that name exists in the graph + if attr == "other_side_door_name" and node.attrs[attr].value: + try: + print("Matched other_side_door_name", node.attrs[attr].value) + origin_node = self.graph.vs.find(id=node.id) + try: + other_side_door_node = self.graph.vs.find(name=node.attrs[attr].value) + try: + self.graph.add_edge(origin_node, other_side_door_node) + print("Matched other_side_door_name", node.attrs[attr].value, other_side_door_node) + except Exception as e: + print("No other_side_door_name node found", node.attrs[attr].value) + print(e) + except: + print("No other_side_door_name node found", node.attrs[attr].value) + except: + print("No origin node found") + + # Check if current attribute is connected_room_name and, if it has value, check if the node with that name exists in the graph + # if attr == "connected_room_name" and node.attrs[attr].value: + # try: + # connescted_room_node = self.graph.vs.find(name=node.attrs[attr].value) + # if connected_room_node: + # self.graph.add_edge(self.vertex_size, connected_room_node.id) + # except: + # print("No connected_room_name attribute found") + self.vertex_size += 1 + + def insert_dsr_vertex(self, parent_name, node): + # print("Inserting vertex", node["name"], node["type"]) + """ + Inserts a new vertex into a graph, updating the parent node's attribute + with the worker's ID and copying over non-optional attributes from the + input node to the new vertex. + + Args: + parent_name (str | str): Used to specify the name of the parent node + to insert the new node as. + node (Node): Passed as an instance of the class `Node`. + + """ + new_node = Node(agent_id=self.agent_id, type=node["type"], name=node["name"]) + # Check if the node is a room node + + parent_node = self.g.get_node(parent_name) + new_node.attrs['parent'] = Attribute(int(parent_node.id), self.agent_id) + + # Iterate over the attributes of the node + for attr in node.attributes(): + if node[attr] is not None and attr not in self.not_required_attrs: + # Add the attribute to the node + new_node.attrs[attr] = Attribute(node[attr], self.agent_id) + id_result = self.g.insert_node(new_node) + def insert_igraph_edge(self, edge): + """ + Adds an edge to an existing graph based on information provided by the + edge attribute, including translation and rotation values. + + Args: + edge (igraph.Edge | GraphElement): An instance representing a single + edge to be inserted into the graph. + + """ + origin_node = self.g.get_node(edge.origin) + destination_node = self.g.get_node(edge.destination) + + # Search for the origin and destination nodes in the graph + origin_node = self.graph.vs.find(name=origin_node.name) + destination_node = self.graph.vs.find(name=destination_node.name) + # Add the edge to the graph + self.graph.add_edge(origin_node, destination_node, rt=edge.attrs["rt_translation"].value, rotation=edge.attrs["rt_rotation_euler_xyz"].value) + # print("Inserting igraph edge", origin_node["name"], destination_node["name"]) + # print("RT", edge.attrs["rt_translation"].value) + # print("Rotation", edge.attrs["rt_rotation_euler_xyz"].value) + # Print origin and destination nodes + + def insert_dsr_edge(self, org, dest): + # print("ORG::", org) + # self.insert_dsr_vertex(dest) + """ + Inserts or updates an edge in a graph based on the distance-sensitive + roadmap (DSR) algorithm, considering the RT translation and rotation of + the edge's endpoints. + + Args: + org (Node | None): Used to specify the source node of the edge being + inserted. If `org` is None, it means the root node of the graph. + dest (Node | str): Used to specify the destination node or name in the + graph for which to create a new edge with RT attributes. + + """ + if org is None: + root_node = self.g.get_node("root") + org_id = root_node.id + rt_value = [0, 0, 0] + orientation = [0, 0, 0] + else: + # print("Inserting DSR edge", org["name"], dest["name"]) + edge_id = self.graph.get_eid(org.index, dest.index) + edge = self.graph.es[edge_id] + rt_value = edge["rt"] + orientation = edge["rotation"] + org_name = org["name"] + org_id = self.g.get_node(org_name).id + + + # print("RT_VALUE", rt_value) + # print(dest["name"], org["name"], "RT") + dest_id = self.g.get_node(dest["name"]).id + new_edge = Edge(dest_id, org_id, "RT", self.agent_id) + new_edge.attrs["rt_translation"] = Attribute(np.array(rt_value, dtype=np.float32), self.agent_id) + new_edge.attrs["rt_rotation_euler_xyz"] = Attribute(np.array(orientation, dtype=np.float32), self.agent_id) + + + # print("RT", rt_value) + # print("Rotation", orientation) + self.g.insert_or_assign_edge(new_edge) + + def draw_graph(self): + """ + Generates a graph based on the layout of a Kamada-Kawai graph, and adds + node labels and edge annotations. + + """ + self.ax.clear() + # Obtener las coordenadas de los vértices + layout = self.graph.layout("kamada_kawai") # Utiliza el layout Kamada-Kawai + # Dibujar los vértices + x, y = zip(*layout) + self.ax.scatter(x, y, s=100) # Ajustar el tamaño de los vértices con el parámetro 's' + # Dibujar las aristas + for edge in self.graph.get_edgelist(): + # Print rt_translation attribute + # Get edge data + # edge_data = self.graph.es[self.graph.get_eid(edge[0], edge[1])] + # print(edge_data["rt"]) + self.ax.plot([x[edge[0]], x[edge[1]]], [y[edge[0]], y[edge[1]]], color="grey") + # add arrow to the edge + self.ax.annotate("", xy=(x[edge[1]], y[edge[1]]), xytext=(x[edge[0]], y[edge[0]]), arrowprops=dict(arrowstyle="->", lw=2)) + + for i, txt in enumerate([f"Node {i}" for i in range(self.graph.vcount())]): + # Get name attribute + name = self.graph.vs[i]["name"] + self.ax.annotate(name, (x[i], y[i]), textcoords="offset points", xytext=(0, 10), ha='center') + # Adapt ax to the graph + self.ax.set_xlim([min(x) - 2, max(x) + 2]) + self.ax.set_ylim([min(y) - 2, max(y) + 2]) + + def check_element_room_number(self, node_id): + """ + Retrieves the room ID associated with a given node ID using the Graph + object's `get_node` method and attribute access, and returns the room ID + if found, or -1 otherwise. + + Args: + node_id (int): Used to identify the element for which the room number + needs to be checked. + + Returns: + int: The room ID of a given element if the element has a "room_id" + attribute, or -1 otherwise. + + """ + node = self.g.get_node(node_id) + try: + room_id = node.attrs["room_id"].value + return room_id + except: + # print("No room_id attribute found") + return -1 + + def check_element_level(self, node_id): + """ + Determines the level of an element with a given ID in the internal graph, + handles exceptions, and adjusts door connections based on the room similarity. + + Args: + node_id (int): Used to identify the node being checked for its element + level attribute value. + + Returns: + int: The level of an element with the given node ID, or -1 if no such + attribute is found. + + """ + node = self.g.get_node(node_id) + try: + element_level = node.attrs["level"].value + return element_level + except: + print("No element_level attribute found") + return -1 + + + + + + # # check it there is an active goto edge connecting the robot and a door + # # if so, then check the robot position. If it is at the door + # # signal that the current room is not anymore by removing self-edge + # # wait for a new room to be created + # # when new room is created, check for the first door in it. Should be the door used to get in + # # connect both doors with a new edge "same" + # ### loop-closure PARA LA SEGUNDA VUELTA (gist es una imagen 360 de la habitación usada para reconocerla rápidamente) + # # check if the current room "gist" is similar to the gists of the other rooms in the agent's internal graph. + # # if so, then replace the current room by the matching nominal room in the internal graph + # # and adjust door connections + # # if not, then check if there is a room not marked as "current" that has been there for at least 1 minute + # # if so, then replace the not-current room by a proxy empty room node with the doors as children nodes + # # save the old room in the agent's internal graph + # # save a gist of the room in the agent's internal graph + # # connect the door connecting both rooms with a new edge "same" + # + # # check it there is an active goto edge connecting the robot and a door + # goto_edges = self.g.get_edges_by_type("goto") + # if len(goto_edges) > 0: + # # search if one of the edges in goto_edges goes to a door + # for edge in goto_edges: + # if edge.fr.name == "robot" and edge.to.name == "door": + # # get door coordinates transformed to robot coordinates are smaller than 100mm + # door_coords_in_robot = self.inner_api(edge.fr.name, edge.to.name) + # # check that door_coords_in_robot are smaller than 100mm + # if np.sqrt(np.power(door_coords_in_robot[0], 2) + np.power(door_coords_in_robot[1], 2)) < 100: + # # signal that the current room is not anymore by removing self-edge + # self.g.remove_edge(edge.fr.name, edge.to.name, "current") + # # wait for a new room to be created + + def generate_room_picture(self, room_node_id): + + """ + Retrieves information about a room and its RT edges, then draws the room + polygon and doors on an image. + + Args: + room_node_id (str | int): Represented as an integer or string, + representing the node ID of the room to be drawn. + + """ + room_node = self.g.get_node(room_node_id) + # Get room node room_id attribute + room_id = room_node.attrs["room_id"].value + # Get RT edges from the room node + old_room_rt_edges = self.g.get_edges_by_type("RT") + #Iterate over the RT edges + for edge in old_room_rt_edges: + print(edge.origin, edge.destination) + # Get destination node + origin_node = self.g.get_node(edge.origin) + # Get room_id attribute + try: + print(origin_node.attrs) + origin_room_id = origin_node.attrs["room_id"].value + # Check if the room_id attribute is the same as the room_id attribute of the room node + if origin_room_id == room_id: + # Get translation attribute + translation = edge.attrs["rt_translation"].value + print(translation) + except: + print("No room_id attribute found") + + + # # Create a black image which size is proportional to the room size + # room_image = np.zeros((int(corners[1] - corners[3]), int(corners[0] - corners[2]), 3), np.uint8) + # # Draw the room polygon + # cv2.fillPoly(room_image, [np.array(corners, np.int32)], (255, 255, 255)) + # # Draw the doors + # for door in doors: + # cv2.circle(room_image, (int(door[0]), int(door[1])), 5, (0, 0, 255), -1) + # # Save the image + # cv2.imwrite("room_image.jpg", room_image) + + def insert_current_edge(self, room_id): + # Insert current edge to the room + """ + Inserts or assigns an edge to the graph representing the current room of + the agent, with the source and destination being the same agent ID. + + Args: + room_id (str): Passed as an argument to Edge constructor, representing + the ID of the current room that the agent is in. + + """ + current_edge = Edge(room_id, room_id, "current", self.agent_id) + self.g.insert_or_assign_edge(current_edge) + + def startup_check(self): + QTimer.singleShot(200, QApplication.instance().quit) + + # =============== DSR SLOTS ================ + # ============================================= + + def update_node_att(self, id: int, attribute_names: [str]): + console.print(f"UPDATE NODE ATT: {id} {attribute_names}", style='green') + + def update_node(self, id: int, type: str): + """ + Updates a node's information based on its type and other factors, such as + checking if a door node exists and inserting it into the graph if necessary, + or handling an affordance node's state change. + + Args: + id (int): Used as an identifier for a node in the graph. + type (str): Used to indicate the type of node being updated. + + """ + if type == "door": + # Get door name + door_node = self.g.get_node(id) + if not "pre" in door_node.name: + # Check if door exists in igraph yet + try: + door_igraph = self.graph.vs.find(name=door_node.name) + # print("Door node found in igraph. Returning.") + return + except: + print("No door node found in igraph. Checking if room node exists") + # Get room node + room_id = door_node.attrs["room_id"].value + try: + room_node = self.graph.vs.find(name="room_" + str(room_id)) + print("Room node found in igraph. Inserting door") + parent_id = door_node.attrs["parent"].value + print("Parent id", parent_id) + door_parent_node = self.g.get_node(parent_id) + print("Door parent name", door_parent_node.name) + # Insert door node in igraph + self.insert_igraph_vertex(door_node) + print("Door inserted in igraph") + # Get RT from door_parent to door + # rt_door = self.rt_api.get_edge_RT(door_parent_node, door_node.id) + + rt_door = self.g.get_edge(door_parent_node.id, door_node.id, "RT") + print("RT DOOR", rt_door.attrs["rt_translation"].value, rt_door.attrs["rt_rotation_euler_xyz"].value) + print("Arigin name", door_parent_node.name, "Destination name", door_node.name) + print("IDS", door_parent_node.id, door_node.id) + self.insert_igraph_edge(rt_door) + with open("graph.pkl", "wb") as f: + pickle.dump(self.graph, f) + print("Door inserted in igraph") + self.long_term_graph.draw_graph(False) + except Exception as e: + print("No room node found in igraph. Not possible to insert door") + print(e) + return + + if id == self.affordance_node_active_id: + print("Affordance node is active") + affordance_node = self.g.get_node(id) + print(affordance_node.attrs["bt_state"].value, affordance_node.attrs["active"].value) + if affordance_node.attrs["bt_state"].value == "completed" and affordance_node.attrs[ + "active"].value == False: + print("Affordance node is completed and not active. Go to crossed state") + # Remove "current" self-edge from the room + self.state = "crossed" + + + def delete_node(self, id: int): + console.print(f"DELETE NODE:: {id} ", style='green') + + def update_edge(self, fr: int, to: int, type: str): + # Insert current edge to the room + # TODO: Posible problema si tocas el nodo room en la interfaz gráfica + + """ + Updates an edge in the graph based on the current node, type, and other conditions. + + Args: + fr (int): Referred to as "from room" indicating that it represents the + starting point of an edge in the graph, specifically a room node. + to (int): The id of the target node to which the edge is being updated. + type (str): Used to specify the type of edge being updated (either + "RT" or "FT"). + + """ + if to == self.robot_id and fr != self.room_exit_door_id and type == "RT" and len(self.g.get_edges_by_type("current")) == 0: + print(self.room_exit_door_id) + # if len(self.g.get_nodes_by_type("room")) == 1 and type == "room" and not "measured" in self.g.get_node(id).name: + self.insert_current_edge(fr) + print("Room node exists but no current edge. Setting as current") + + def update_edge_att(self, fr: int, to: int, type: str, attribute_names: [str]): + console.print(f"UPDATE EDGE ATT: {fr} to {type} {attribute_names}", style='green') + + def delete_edge(self, fr: int, to: int, type: str): + console.print(f"DELETE EDGE: {fr} to {type} {type}", style='green')